add pdo 6077 torque actual value to the joint state interface as effort

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

@@ -174,6 +174,7 @@ 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

@@ -57,6 +57,7 @@ class NodeCanopen402Driver : public NodeCanopenProxyDriver<NODETYPE>
   double scale_pos_from_dev_;
   double scale_vel_to_dev_;
   double scale_vel_from_dev_;
+  double scale_eff_from_dev_;
   ros2_canopen::State402::InternalState switching_state_;
 
   void publish();
@@ -72,6 +73,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() { return motor_->get_position() * scale_pos_from_dev_; }

canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp

@@ -195,6 +195,7 @@ void NodeCanopen402Driver<rclcpp_lifecycle::LifecycleNode>::configure(bool calle
   std::optional<double> scale_pos_from_dev;
   std::optional<double> scale_vel_to_dev;
   std::optional<double> scale_vel_from_dev;
+  std::optional<double> scale_eff_from_dev;
   std::optional<int> switching_state;
   try
   {
@@ -225,6 +226,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>());
   }
@@ -238,6 +246,7 @@ void NodeCanopen402Driver<rclcpp_lifecycle::LifecycleNode>::configure(bool calle
   scale_pos_from_dev_ = scale_pos_from_dev.value_or(0.001);
   scale_vel_to_dev_ = scale_vel_to_dev.value_or(1000.0);
   scale_vel_from_dev_ = scale_vel_from_dev.value_or(0.001);
+  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);
   RCLCPP_INFO(
@@ -254,6 +263,7 @@ void NodeCanopen402Driver<rclcpp::Node>::configure(bool called_from_base)
   std::optional<double> scale_pos_from_dev;
   std::optional<double> scale_vel_to_dev;
   std::optional<double> scale_vel_from_dev;
+  std::optional<double> scale_eff_from_dev;
   std::optional<int> switching_state;
   try
   {
@@ -284,6 +294,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>());
   }
@@ -297,12 +314,13 @@ void NodeCanopen402Driver<rclcpp::Node>::configure(bool called_from_base)
   scale_pos_from_dev_ = scale_pos_from_dev.value_or(0.001);
   scale_vel_to_dev_ = scale_vel_to_dev.value_or(1000.0);
   scale_vel_from_dev_ = scale_vel_from_dev.value_or(0.001);
+  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);
   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\n",
-    scale_pos_to_dev_, scale_pos_from_dev_, scale_vel_to_dev_, scale_vel_from_dev_);
+    "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\n",
+    scale_pos_to_dev_, scale_pos_from_dev_, scale_vel_to_dev_, scale_vel_from_dev_, scale_eff_from_dev_);
 }
 
 template <class NODETYPE>
@@ -336,7 +354,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_);
   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,10 @@ 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 +257,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;