From acb118f748b7549f5a4d3c42c9dff74f2f133277 Mon Sep 17 00:00:00 2001 From: Dan Zimmerman Date: Tue, 12 Nov 2024 01:37:00 -0500 Subject: [PATCH] Implement position offsets --- .../node_canopen_402_driver.hpp | 4 +- .../node_canopen_402_driver_impl.hpp | 52 ++++++++++++++++--- 2 files changed, 47 insertions(+), 9 deletions(-) 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..88570c14 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,7 @@ 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 0df26964..96c53dd5 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,14 @@ 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 +272,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 +304,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 +331,14 @@ 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( + 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 +370,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 +482,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 @@ -665,7 +701,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