diff --git a/canopen/sphinx/user-guide/cia402-driver.rst b/canopen/sphinx/user-guide/cia402-driver.rst index 29df0c30..514e4699 100644 --- a/canopen/sphinx/user-guide/cia402-driver.rst +++ b/canopen/sphinx/user-guide/cia402-driver.rst @@ -127,12 +127,18 @@ Additional parameters that can be used in bus.yml for this driver. * - scale_pos_to_dev - double - Scaling factor to convert from SI units to device units for position. + * - offset_pos_to_dev + - double + - Offset in device units added to scaled position commands sent to the device. * - scale_vel_to_dev - double - Scaling factor to convert from SI units to device units for velocity. * - scale_pos_from_dev - double - Scaling factor to convert from device units to SI units for position. + * - offset_pos_from_dev + - double + - Offset in SI units to added to scaled position reports from the device. * - scale_vel_from_dev - double - Scaling factor to convert from device units to SI units for velocity. 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 36f31741..5810457c 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,8 @@ class NodeCanopen402Driver : public NodeCanopenProxyDriver double scale_pos_from_dev_; double scale_vel_to_dev_; double scale_vel_from_dev_; + double offset_pos_to_dev_; + double offset_pos_from_dev_; ros2_canopen::State402::InternalState switching_state_; void publish(); @@ -74,7 +76,10 @@ class NodeCanopen402Driver : public NodeCanopenProxyDriver virtual double get_speed() { return motor_->get_speed() * scale_vel_from_dev_; } - virtual double get_position() { return motor_->get_position() * scale_pos_from_dev_; } + virtual double get_position() + { + return motor_->get_position() * scale_pos_from_dev_ + offset_pos_from_dev_; + } virtual uint16_t get_mode() { return motor_->getMode(); } 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 fcd2d35d..1d30860a 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,8 @@ 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 offset_pos_to_dev; + std::optional offset_pos_from_dev; std::optional switching_state; try { @@ -225,6 +227,20 @@ void NodeCanopen402Driver::configure(bool calle { } try + { + offset_pos_to_dev = std::optional(this->config_["offset_pos_to_dev"].as()); + } + catch (...) + { + } + try + { + offset_pos_from_dev = std::optional(this->config_["offset_from_to_dev"].as()); + } + catch (...) + { + } + try { switching_state = std::optional(this->config_["switching_state"].as()); } @@ -238,12 +254,16 @@ 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); + offset_pos_to_dev_ = offset_pos_to_dev.value_or(0.0); + offset_pos_from_dev_ = offset_pos_from_dev.value_or(0.0); 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\noffset_pos_to_dev_ %f\noffset_pos_from_dev_ %f\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_); } template <> @@ -254,6 +274,8 @@ 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 offset_pos_to_dev; + std::optional offset_pos_from_dev; std::optional switching_state; try { @@ -284,6 +306,20 @@ void NodeCanopen402Driver::configure(bool called_from_base) { } try + { + offset_pos_to_dev = std::optional(this->config_["offset_pos_to_dev"].as()); + } + catch (...) + { + } + try + { + offset_pos_from_dev = std::optional(this->config_["offset_from_to_dev"].as()); + } + catch (...) + { + } + try { switching_state = std::optional(this->config_["switching_state"].as()); } @@ -297,12 +333,16 @@ 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); + offset_pos_to_dev_ = offset_pos_to_dev.value_or(0.0); + offset_pos_from_dev_ = offset_pos_from_dev.value_or(0.0); 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\noffset_pos_to_dev_ %f\noffset_pos_from_dev_ %f\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_); } template @@ -334,7 +374,7 @@ void NodeCanopen402Driver::publish() { sensor_msgs::msg::JointState js_msg; js_msg.name.push_back(this->node_->get_name()); - js_msg.position.push_back(motor_->get_position() * scale_pos_from_dev_); + 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); publish_joint_state->publish(js_msg); @@ -446,7 +486,7 @@ void NodeCanopen402Driver::handle_set_target( (mode == MotorBase::Profiled_Position) or (mode == MotorBase::Cyclic_Synchronous_Position) or (mode == MotorBase::Interpolated_Position)) { - target = request->target * scale_pos_to_dev_; + target = request->target * scale_pos_to_dev_ + offset_pos_to_dev_; } else if ( (mode == MotorBase::Velocity) or (mode == MotorBase::Profiled_Velocity) or @@ -532,7 +572,7 @@ bool NodeCanopen402Driver::set_target(double target) (mode == MotorBase::Profiled_Position) or (mode == MotorBase::Cyclic_Synchronous_Position) or (mode == MotorBase::Interpolated_Position)) { - scaled_target = target * scale_pos_to_dev_; + scaled_target = target * scale_pos_to_dev_ + offset_pos_to_dev_; } else if ( (mode == MotorBase::Velocity) or (mode == MotorBase::Profiled_Velocity) or