diff --git a/canopen_402_driver/include/canopen_402_driver/cia402_driver.hpp b/canopen_402_driver/include/canopen_402_driver/cia402_driver.hpp index 8a6505da..87fc6e14 100644 --- a/canopen_402_driver/include/canopen_402_driver/cia402_driver.hpp +++ b/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(); } diff --git a/canopen_402_driver/include/canopen_402_driver/motor.hpp b/canopen_402_driver/include/canopen_402_driver/motor.hpp index cf040f97..1a741a0b 100644 --- a/canopen_402_driver/include/canopen_402_driver/motor.hpp +++ b/canopen_402_driver/include/canopen_402_driver/motor.hpp @@ -174,6 +174,10 @@ class Motor402 : public MotorBase registerMode(MotorBase::Cyclic_Synchronous_Torque, driver); } + double get_effort() const + { + return (double)this->driver->universal_get_value(0x6077, 0); + } double get_speed() const { return (double)this->driver->universal_get_value(0x606C, 0); } double get_position() const { diff --git a/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp b/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp index 13a01a01..9eed0ce1 100644 --- a/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp +++ b/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp @@ -57,6 +57,7 @@ class NodeCanopen402Driver : public NodeCanopenProxyDriver 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 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_; } diff --git a/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp b/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp index 0df26964..c36a8f2b 100644 --- a/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp +++ b/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp @@ -195,6 +195,7 @@ void NodeCanopen402Driver::configure(bool calle std::optional scale_pos_from_dev; std::optional scale_vel_to_dev; std::optional scale_vel_from_dev; + std::optional scale_eff_from_dev; std::optional switching_state; try { @@ -225,6 +226,13 @@ void NodeCanopen402Driver::configure(bool calle { } try + { + scale_eff_from_dev = std::optional(this->config_["scale_eff_from_dev"].as()); + } + catch (...) + { + } + try { switching_state = std::optional(this->config_["switching_state"].as()); } @@ -238,6 +246,7 @@ void NodeCanopen402Driver::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::configure(bool called_from_base) std::optional scale_pos_from_dev; std::optional scale_vel_to_dev; std::optional scale_vel_from_dev; + std::optional scale_eff_from_dev; std::optional switching_state; try { @@ -284,6 +294,13 @@ void NodeCanopen402Driver::configure(bool called_from_base) { } try + { + scale_eff_from_dev = std::optional(this->config_["scale_eff_from_dev"].as()); + } + catch (...) + { + } + try { switching_state = std::optional(this->config_["switching_state"].as()); } @@ -297,12 +314,15 @@ void NodeCanopen402Driver::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 @@ -336,7 +356,7 @@ void NodeCanopen402Driver::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); } diff --git a/canopen_ros2_control/include/canopen_ros2_control/cia402_data.hpp b/canopen_ros2_control/include/canopen_ros2_control/cia402_data.hpp index 20eb070e..fd962876 100644 --- a/canopen_ros2_control/include/canopen_ros2_control/cia402_data.hpp +++ b/canopen_ros2_control/include/canopen_ros2_control/cia402_data.hpp @@ -45,6 +45,7 @@ struct Cia402Data // FROM MOTOR double actual_position = std::numeric_limits::quiet_NaN(); double actual_velocity = std::numeric_limits::quiet_NaN(); + double actual_effort = std::numeric_limits::quiet_NaN(); // TO MOTOR double target_position = std::numeric_limits::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() diff --git a/canopen_ros2_control/include/canopen_ros2_control/cia402_system.hpp b/canopen_ros2_control/include/canopen_ros2_control/cia402_system.hpp index bcf7ebfa..f24ee8b3 100644 --- a/canopen_ros2_control/include/canopen_ros2_control/cia402_system.hpp +++ b/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; diff --git a/canopen_ros2_control/src/cia402_system.cpp b/canopen_ros2_control/src/cia402_system.cpp index 48b4b99b..7e69f981 100644 --- a/canopen_ros2_control/src/cia402_system.cpp +++ b/canopen_ros2_control/src/cia402_system.cpp @@ -136,6 +136,9 @@ std::vector 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;