Implement position offsets

ros-industrial · Nov 12, 2024 · acb118f · acb118f
1 parent eb219eb
commit acb118f
Show file tree

Hide file tree

Showing 2 changed files with 47 additions and 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
@@ -57,6 +57,8 @@ class NodeCanopen402Driver : public NodeCanopenProxyDriver<NODETYPE>
   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<NODETYPE>
 
   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/...en_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp b/...en_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp
@@ -195,6 +195,8 @@ 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> offset_pos_to_dev;
+  std::optional<double> offset_pos_from_dev;
   std::optional<int> switching_state;
   try
   {
@@ -225,6 +227,20 @@ void NodeCanopen402Driver<rclcpp_lifecycle::LifecycleNode>::configure(bool calle
   {
   }
   try
+  {
+    offset_pos_to_dev = std::optional(this->config_["offset_pos_to_dev"].as<double>());
+  }
+  catch(...)
+  {
+  }
+   try
+  {
+    offset_pos_from_dev = std::optional(this->config_["offset_from_to_dev"].as<double>());
+  }
+  catch(...)
+  {
+  }
+  try
   {
     switching_state = std::optional(this->config_["switching_state"].as<int>());
   }
@@ -238,12 +254,14 @@ 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);
+  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<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> offset_pos_to_dev;
+  std::optional<double> offset_pos_from_dev;
   std::optional<int> switching_state;
   try
   {
@@ -284,6 +304,20 @@ void NodeCanopen402Driver<rclcpp::Node>::configure(bool called_from_base)
   {
   }
   try
+  {
+    offset_pos_to_dev = std::optional(this->config_["offset_pos_to_dev"].as<double>());
+  }
+  catch(...)
+  {
+  }
+   try
+  {
+    offset_pos_from_dev = std::optional(this->config_["offset_from_to_dev"].as<double>());
+  }
+  catch(...)
+  {
+  }
+  try
   {
     switching_state = std::optional(this->config_["switching_state"].as<int>());
   }
@@ -297,12 +331,14 @@ 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);
+  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 <class NODETYPE>
@@ -334,7 +370,7 @@ void NodeCanopen402Driver<NODETYPE>::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<NODETYPE>::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<NODETYPE>::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