declare on_configure

Update joint_state_topic_hardware_interface.cpp

also export the limited commands

remove print

add limited commands array too

Update joint_state_topic_hardware_interface.hpp

Apply suggestions from code review

Author: bmagyar

joint_state_topic_hardware_interface/include/joint_state_topic_hardware_interface/joint_state_topic_hardware_interface.hpp

@@ -38,7 +38,6 @@ struct VelocityLimits
   explicit VelocityLimits() = default;
   explicit VelocityLimits(const hardware_interface::InterfaceInfo& info)
   {
-    std::cout << "********************* MIN IS " << info.min << " AND MAX IS " << info.max << std::endl;
     if(not info.min.empty())
     {
       min = std::stod(info.min);
@@ -58,6 +57,8 @@ class JointStateTopicSystem : public hardware_interface::SystemInterface
 public:
   CallbackReturn on_init(const hardware_interface::HardwareComponentInterfaceParams& params) override;
 
+  CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
+
   std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
 
   std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
@@ -92,6 +93,7 @@ class JointStateTopicSystem : public hardware_interface::SystemInterface
 
   bool enable_command_limiting_ = false;
   std::vector<double> nonlimited_velocity_commands_;
+  std::vector<double> limited_velocity_commands_;
   std::vector<VelocityLimits> velocity_limits_;
 
   // If the difference between the current joint state and joint command is less than this value,

joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp

@@ -59,6 +59,7 @@ CallbackReturn JointStateTopicSystem::on_init(const hardware_interface::Hardware
   joint_commands_.resize(standard_interfaces_.size());
   joint_states_.resize(standard_interfaces_.size());
   velocity_limits_.resize(get_hardware_info().joints.size());
+  limited_velocity_commands_.resize(get_hardware_info().joints.size());
   nonlimited_velocity_commands_.resize(get_hardware_info().joints.size());
   for (auto i = 0u; i < standard_interfaces_.size(); i++)
   {
@@ -155,7 +156,8 @@ CallbackReturn JointStateTopicSystem::on_configure(const rclcpp_lifecycle::State
 {
   REGISTER_ROS2_CONTROL_INTROSPECTION("nonlimited_left_wheel_velocity", &nonlimited_velocity_commands_[0]);
   REGISTER_ROS2_CONTROL_INTROSPECTION("nonlimited_right_wheel_velocity", &nonlimited_velocity_commands_[1]);
-
+  REGISTER_ROS2_CONTROL_INTROSPECTION("limited_left_wheel_velocity", &limited_velocity_commands_[0]);
+  REGISTER_ROS2_CONTROL_INTROSPECTION("limited_right_wheel_velocity", &limited_velocity_commands_[1]);
   return CallbackReturn::SUCCESS;
 }
 
@@ -285,14 +287,12 @@ hardware_interface::return_type JointStateTopicSystem::write(const rclcpp::Time&
       }
       else if (interface.name == hardware_interface::HW_IF_VELOCITY)
       {
+        nonlimited_velocity_commands_[i] = joint_commands_[VELOCITY_INTERFACE_INDEX][i];
         if(enable_command_limiting_)
         {
-          //RCLCPP_WARN(get_node()->get_logger(),"Before clamp: %f",joint_commands_[VELOCITY_INTERFACE_INDEX][i]);
-          nonlimited_velocity_commands_[i] = joint_commands_[VELOCITY_INTERFACE_INDEX][i];
-          joint_commands_[VELOCITY_INTERFACE_INDEX][i] = std::clamp(nonlimited_velocity_commands_[i], velocity_limits_[i].min, velocity_limits_[i].max);
-          //RCLCPP_WARN(get_node()->get_logger(),"After clamp: %f",joint_commands_[VELOCITY_INTERFACE_INDEX][i]);
+          limited_velocity_commands_[i] = std::clamp(nonlimited_velocity_commands_[i], velocity_limits_[i].min, velocity_limits_[i].max);
         }
-        joint_state.velocity.push_back(joint_commands_[VELOCITY_INTERFACE_INDEX][i]);
+        joint_state.velocity.push_back(limited_velocity_commands_[i]);
       }
       else if (interface.name == hardware_interface::HW_IF_EFFORT)
       {