Variants ex14

example_14/description/ros2_control/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.ros2_control.xacro

@@ -5,7 +5,7 @@
     params="name prefix use_sim:=^|false slowdown:=2.0">
 
     <!-- NOTE generally is the order of hardware definition not relevant. But in this case the
-    sensors are listening on socket to which the actuators are connecting. Therefore, the sensors
+    sensors are listening on sockets to which the actuators are connecting. Therefore, the sensors
     has to be initialized and started first. -->
 
     <ros2_control name="RRBotModularPositionSensorJoint1" type="sensor">
@@ -31,7 +31,7 @@
       <joint name="joint2">
         <state_interface name="position"/>
       </joint>
-      </ros2_control>
+    </ros2_control>
 
     <ros2_control name="RRBotModularJoint1" type="actuator">
       <hardware>

example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp

@@ -41,9 +41,8 @@ class RRBotActuatorWithoutFeedback : public hardware_interface::ActuatorInterfac
   hardware_interface::CallbackReturn on_init(
     const hardware_interface::HardwareInfo & info) override;
 
-  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
-
-  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
+  hardware_interface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
 
   hardware_interface::CallbackReturn on_activate(
     const rclcpp_lifecycle::State & previous_state) override;
@@ -65,9 +64,6 @@ class RRBotActuatorWithoutFeedback : public hardware_interface::ActuatorInterfac
   double hw_start_sec_;
   double hw_stop_sec_;
 
-  // Store the command for the simulated robot
-  double hw_joint_command_;
-
   // Fake "mechanical connection" between actuator and sensor using sockets
   struct sockaddr_in address_;
   int socket_port_;

example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp

@@ -45,8 +45,6 @@ class RRBotSensorPositionFeedback : public hardware_interface::SensorInterface
   hardware_interface::CallbackReturn on_init(
     const hardware_interface::HardwareInfo & info) override;
 
-  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
-
   hardware_interface::CallbackReturn on_configure(
     const rclcpp_lifecycle::State & previous_state) override;
 
@@ -71,7 +69,6 @@ class RRBotSensorPositionFeedback : public hardware_interface::SensorInterface
   // Store the command for the simulated robot
   double measured_velocity;  // Local variable, but avoid initialization on each read
   double last_measured_velocity_;
-  double hw_joint_state_;
 
   // Timestamps to calculate position for velocity
   rclcpp::Clock clock_;

example_14/hardware/rrbot_actuator_without_feedback.cpp

@@ -53,8 +53,6 @@ hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_init(
   socket_port_ = std::stoi(info_.hardware_parameters["example_param_socket_port"]);
   // END: This part here is for exemplary purposes - Please do not copy to your production code
 
-  hw_joint_command_ = std::numeric_limits<double>::quiet_NaN();
-
   const hardware_interface::ComponentInfo & joint = info_.joints[0];
   // RRBotActuatorWithoutFeedback has exactly one command interface and one joint
   if (joint.command_interfaces.size() != 1)
@@ -74,7 +72,7 @@ hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_init(
     return hardware_interface::CallbackReturn::ERROR;
   }
 
-  // START: This part here is for exemplary purposes - Please do not copy to your production code
+  // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
   // Initialize objects for fake mechanical connection
   sock_ = socket(AF_INET, SOCK_STREAM, 0);
   if (sock_ < 0)
@@ -129,34 +127,29 @@ hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_init(
     RCLCPP_INFO(get_logger(), "Successfully connected to port %d.", socket_port_);
   }
   // END: This part here is for exemplary purposes - Please do not copy to your production code
-
   return hardware_interface::CallbackReturn::SUCCESS;
 }
 
-hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_shutdown(
+hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_configure(
   const rclcpp_lifecycle::State & /*previous_state*/)
 {
-  shutdown(sock_, SHUT_RDWR);  // shutdown socket
+  RCLCPP_INFO(get_logger(), "Configuring ...please wait...");
 
-  return hardware_interface::CallbackReturn::SUCCESS;
-}
+  // reset values always when configuring hardware
+  for (const auto & [name, descr] : joint_command_interfaces_)
+  {
+    set_command(name, 0.0);
+  }
 
-std::vector<hardware_interface::StateInterface>
-RRBotActuatorWithoutFeedback::export_state_interfaces()
-{
-  std::vector<hardware_interface::StateInterface> state_interfaces;
-  return state_interfaces;
+  return hardware_interface::CallbackReturn::SUCCESS;
 }
 
-std::vector<hardware_interface::CommandInterface>
-RRBotActuatorWithoutFeedback::export_command_interfaces()
+hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_shutdown(
+  const rclcpp_lifecycle::State & /*previous_state*/)
 {
-  std::vector<hardware_interface::CommandInterface> command_interfaces;
-
-  command_interfaces.emplace_back(hardware_interface::CommandInterface(
-    info_.joints[0].name, hardware_interface::HW_IF_VELOCITY, &hw_joint_command_));
+  shutdown(sock_, SHUT_RDWR);  // shutdown socket
 
-  return command_interfaces;
+  return hardware_interface::CallbackReturn::SUCCESS;
 }
 
 hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_activate(
@@ -173,9 +166,9 @@ hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_activate(
   // END: This part here is for exemplary purposes - Please do not copy to your production code
 
   // set some default values for joints
-  if (std::isnan(hw_joint_command_))
+  for (const auto & [name, descr] : joint_command_interfaces_)
   {
-    hw_joint_command_ = 0;
+    set_command(name, 0.0);
   }
 
   RCLCPP_INFO(get_logger(), "Successfully activated!");
@@ -212,12 +205,15 @@ hardware_interface::return_type ros2_control_demo_example_14::RRBotActuatorWitho
 {
   // START: This part here is for exemplary purposes - Please do not copy to your production code
   std::stringstream ss;
+  std::ostringstream data;
+
   ss << "Writing..." << std::endl;
   ss << std::fixed << std::setprecision(2);
-  ss << "Writing command: " << hw_joint_command_ << std::endl;
 
-  std::ostringstream data;
-  data << hw_joint_command_;
+  auto name = info_.joints[0].name + "/" + hardware_interface::HW_IF_VELOCITY;
+  ss << "Writing command: " << get_command(name) << std::endl;
+
+  data << get_command(name);
   ss << "Sending data command: " << data.str() << std::endl;
   RCLCPP_INFO(get_logger(), ss.str().c_str());
 

example_14/hardware/rrbot_sensor_for_position_feedback.cpp

@@ -55,8 +55,6 @@ hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_init(
   socket_port_ = std::stoi(info_.hardware_parameters["example_param_socket_port"]);
   // END: This part here is for exemplary purposes - Please do not copy to your production code
 
-  hw_joint_state_ = std::numeric_limits<double>::quiet_NaN();
-
   const hardware_interface::ComponentInfo & joint = info_.joints[0];
   // RRBotSensorPositionFeedback has exactly one state interface and one joint
   if (joint.state_interfaces.size() != 1)
@@ -169,34 +167,16 @@ hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_init(
   return hardware_interface::CallbackReturn::SUCCESS;
 }
 
-hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_shutdown(
-  const rclcpp_lifecycle::State & /*previous_state*/)
-{
-  incoming_data_thread_.join();      // stop reading thread
-  shutdown(sock_, SHUT_RDWR);        // shutdown socket
-  shutdown(obj_socket_, SHUT_RDWR);  // shutdown socket
-
-  return hardware_interface::CallbackReturn::SUCCESS;
-}
-
-std::vector<hardware_interface::StateInterface>
-RRBotSensorPositionFeedback::export_state_interfaces()
-{
-  std::vector<hardware_interface::StateInterface> state_interfaces;
-
-  state_interfaces.emplace_back(hardware_interface::StateInterface(
-    info_.joints[0].name, hardware_interface::HW_IF_POSITION, &hw_joint_state_));
-
-  return state_interfaces;
-}
-
 hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_configure(
   const rclcpp_lifecycle::State & /*previous_state*/)
 {
+  RCLCPP_INFO(get_logger(), "Configuring ...please wait...");
+
   // set some default values for joints
-  if (std::isnan(hw_joint_state_))
+  // reset values always when configuring hardware
+  for (const auto & [name, descr] : sensor_state_interfaces_)
   {
-    hw_joint_state_ = 0;
+    set_state(name, 0.0);
   }
   last_measured_velocity_ = 0;
 
@@ -207,6 +187,16 @@ hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_configure(
   return hardware_interface::CallbackReturn::SUCCESS;
 }
 
+hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_shutdown(
+  const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  incoming_data_thread_.join();      // stop reading thread
+  shutdown(sock_, SHUT_RDWR);        // shutdown socket
+  shutdown(obj_socket_, SHUT_RDWR);  // shutdown socket
+
+  return hardware_interface::CallbackReturn::SUCCESS;
+}
+
 hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_activate(
   const rclcpp_lifecycle::State & /*previous_state*/)
 {
@@ -254,17 +244,21 @@ hardware_interface::return_type RRBotSensorPositionFeedback::read(
   std::stringstream ss;
   ss << "Reading..." << std::endl;
 
-  // Simulate RRBot's movement
+  // Sensor reading
   measured_velocity = *(rt_incomming_data_ptr_.readFromRT());
   if (!std::isnan(measured_velocity))
   {
     last_measured_velocity_ = measured_velocity;
   }
-  hw_joint_state_ += (last_measured_velocity_ * duration.seconds()) / hw_slowdown_;
+
+  // integrate velocity to position
+  auto name = info_.joints[0].name + "/" + hardware_interface::HW_IF_POSITION;
+  auto new_value = get_state(name) + (last_measured_velocity_ * duration.seconds()) / hw_slowdown_;
+  set_state(name, new_value);
 
   ss << std::fixed << std::setprecision(2);
   ss << "Got measured velocity " << measured_velocity << std::endl;
-  ss << "Got state " << hw_joint_state_ << " for joint '" << info_.joints[0].name << "'"
+  ss << "Got state(position) " << new_value << " for joint '" << info_.joints[0].name << "'"
      << std::endl;
   RCLCPP_INFO(get_logger(), ss.str().c_str());
   // END: This part here is for exemplary purposes - Please do not copy to your production code