Merge remote-tracking branch 'syns/add-pdo-6077-effort-feedback' into dz/update-syns123b-6077-effort

Author: zimmerman-pliant

canopen_402_driver/include/canopen_402_driver/cia402_driver.hpp

@@ -69,6 +69,8 @@ class Cia402Driver : public ros2_canopen::CanopenDriver
     node_canopen_402_driver_->register_rpdo_cb(rpdo_cb);
   }
 
+  double get_effort() { return node_canopen_402_driver_->get_effort(); }
+
   double get_speed() { return node_canopen_402_driver_->get_speed(); }
 
   double get_position() { return node_canopen_402_driver_->get_position(); }

canopen_402_driver/include/canopen_402_driver/motor.hpp

@@ -196,6 +196,10 @@ class Motor402 : public MotorBase
     registerMode<CyclicSynchronousTorqueMode>(MotorBase::Cyclic_Synchronous_Torque, driver);
   }
 
+  double get_effort() const
+  {
+    return (double)this->driver->universal_get_value<int16_t>(0x6077, 0);
+  }
   double get_speed() const { return (double)this->driver->universal_get_value<int32_t>(0x606C, 0); }
   double get_position() const
   {

canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp

@@ -61,6 +61,7 @@ class NodeCanopen402Driver : public NodeCanopenProxyDriver<NODETYPE>
   double scale_vel_from_dev_;
   double offset_pos_to_dev_;
   double offset_pos_from_dev_;
+  double scale_eff_from_dev_;
   ros2_canopen::State402::InternalState switching_state_;
   int homing_timeout_seconds_;
 
@@ -77,6 +78,8 @@ class NodeCanopen402Driver : public NodeCanopenProxyDriver<NODETYPE>
   virtual void deactivate(bool called_from_base) override;
   virtual void add_to_master() override;
 
+  virtual double get_effort() { return motor_->get_effort() * scale_eff_from_dev_; }
+
   virtual double get_speed() { return motor_->get_speed() * scale_vel_from_dev_; }
 
   virtual double get_position()

canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp

@@ -221,6 +221,7 @@ void NodeCanopen402Driver<rclcpp_lifecycle::LifecycleNode>::configure(bool calle
   std::optional<double> scale_vel_from_dev;
   std::optional<double> offset_pos_to_dev;
   std::optional<double> offset_pos_from_dev;
+  std::optional<double> scale_eff_from_dev;
   std::optional<int> switching_state;
   std::optional<int> homing_timeout_seconds;
   try
@@ -266,6 +267,13 @@ void NodeCanopen402Driver<rclcpp_lifecycle::LifecycleNode>::configure(bool calle
   {
   }
   try
+  {
+    scale_eff_from_dev = std::optional(this->config_["scale_eff_from_dev"].as<double>());
+  }
+  catch (...)
+  {
+  }
+  try
   {
     switching_state = std::optional(this->config_["switching_state"].as<int>());
   }
@@ -288,6 +296,7 @@ void NodeCanopen402Driver<rclcpp_lifecycle::LifecycleNode>::configure(bool calle
   scale_vel_from_dev_ = scale_vel_from_dev.value_or(0.001);
   offset_pos_to_dev_ = offset_pos_to_dev.value_or(0.0);
   offset_pos_from_dev_ = offset_pos_from_dev.value_or(0.0);
+  scale_eff_from_dev_ = scale_eff_from_dev.value_or(0.001);
   switching_state_ = (ros2_canopen::State402::InternalState)switching_state.value_or(
     (int)ros2_canopen::State402::InternalState::Operation_Enable);
   homing_timeout_seconds_ = homing_timeout_seconds.value_or(10);
@@ -310,6 +319,7 @@ void NodeCanopen402Driver<rclcpp::Node>::configure(bool called_from_base)
   std::optional<double> scale_vel_from_dev;
   std::optional<double> offset_pos_to_dev;
   std::optional<double> offset_pos_from_dev;
+  std::optional<double> scale_eff_from_dev;
   std::optional<int> switching_state;
   std::optional<int> homing_timeout_seconds;
   try
@@ -355,6 +365,13 @@ void NodeCanopen402Driver<rclcpp::Node>::configure(bool called_from_base)
   {
   }
   try
+  {
+    scale_eff_from_dev = std::optional(this->config_["scale_eff_from_dev"].as<double>());
+  }
+  catch (...)
+  {
+  }
+  try
   {
     switching_state = std::optional(this->config_["switching_state"].as<int>());
   }
@@ -377,16 +394,20 @@ void NodeCanopen402Driver<rclcpp::Node>::configure(bool called_from_base)
   scale_vel_from_dev_ = scale_vel_from_dev.value_or(0.001);
   offset_pos_to_dev_ = offset_pos_to_dev.value_or(0.0);
   offset_pos_from_dev_ = offset_pos_from_dev.value_or(0.0);
+  scale_eff_from_dev_ = scale_eff_from_dev.value_or(0.001);
   switching_state_ = (ros2_canopen::State402::InternalState)switching_state.value_or(
     (int)ros2_canopen::State402::InternalState::Operation_Enable);
   homing_timeout_seconds_ = homing_timeout_seconds.value_or(10);
   RCLCPP_INFO(
     this->node_->get_logger(),
     "scale_pos_to_dev_ %f\nscale_pos_from_dev_ %f\nscale_vel_to_dev_ %f\nscale_vel_from_dev_ "
+    "%f\nscale_eff_from_dev_ "
     "%f\noffset_pos_to_dev_ %f\noffset_pos_from_dev_ "
     "%f\nhoming_timeout_seconds_ %i\n",
     scale_pos_to_dev_, scale_pos_from_dev_, scale_vel_to_dev_, scale_vel_from_dev_,
-    offset_pos_to_dev_, offset_pos_from_dev_, homing_timeout_seconds_);
+    scale_eff_from_dev_,
+    offset_pos_to_dev_, offset_pos_from_dev_, homing_timeout_seconds_,
+    );
 }
 
 template <class NODETYPE>
@@ -420,7 +441,7 @@ void NodeCanopen402Driver<NODETYPE>::publish()
   js_msg.name.push_back(this->node_->get_name());
   js_msg.position.push_back(motor_->get_position() * scale_pos_from_dev_ + offset_pos_from_dev_);
   js_msg.velocity.push_back(motor_->get_speed() * scale_vel_from_dev_);
-  js_msg.effort.push_back(0.0);
+  js_msg.effort.push_back(motor_->get_effort() * scale_eff_from_dev_);
   publish_joint_state->publish(js_msg);
 }
 

canopen_ros2_control/include/canopen_ros2_control/cia402_data.hpp

@@ -45,6 +45,7 @@ struct Cia402Data
   // FROM MOTOR
   double actual_position = std::numeric_limits<double>::quiet_NaN();
   double actual_velocity = std::numeric_limits<double>::quiet_NaN();
+  double actual_effort = std::numeric_limits<double>::quiet_NaN();
 
   // TO MOTOR
   double target_position = std::numeric_limits<double>::quiet_NaN();
@@ -107,6 +108,10 @@ struct Cia402Data
     // actual speed
     state_interfaces.emplace_back(hardware_interface::StateInterface(
       joint_name, hardware_interface::HW_IF_VELOCITY, &actual_velocity));
+
+    // actual effort
+    state_interfaces.emplace_back(hardware_interface::StateInterface(
+      joint_name, hardware_interface::HW_IF_EFFORT, &actual_effort));
   }
 
   void export_command_interface(
@@ -144,6 +149,7 @@ struct Cia402Data
   {
     actual_position = driver->get_position();
     actual_velocity = driver->get_speed();
+    actual_effort = driver->get_effort();
   }
 
   void write_target()

canopen_ros2_control/include/canopen_ros2_control/cia402_system.hpp

@@ -61,6 +61,7 @@ struct MotorNodeData
   // feedback
   double actual_position;
   double actual_speed;
+  double actual_effort;
 
   // basic control
   MotorTriggerCommand init;

canopen_ros2_control/src/cia402_system.cpp

@@ -136,6 +136,9 @@ std::vector<hardware_interface::StateInterface> Cia402System::export_state_inter
     state_interfaces.emplace_back(hardware_interface::StateInterface(
       info_.joints[i].name, hardware_interface::HW_IF_VELOCITY,
       &motor_data_[node_id].actual_speed));
+    // actual effort
+    state_interfaces.emplace_back(hardware_interface::StateInterface(
+      info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &motor_data_[node_id].actual_effort));
   }
 
   return state_interfaces;
@@ -253,6 +256,8 @@ hardware_interface::return_type Cia402System::read(
     motor_data_[it->first].actual_position = motion_controller_driver->get_position();
     // get speed
     motor_data_[it->first].actual_speed = motion_controller_driver->get_speed();
+    // get effort
+    motor_data_[it->first].actual_effort = motion_controller_driver->get_effort();
   }
 
   return ret_val;