Rewrite of PR #24

CMakeLists.txt

@@ -44,8 +44,10 @@ check_minimal_cxx_standard(11 ENFORCE)
 # ----------------------------------------------------
 # --- DEPENDENCIES -----------------------------------
 # ----------------------------------------------------
+cmake_policy(SET CMP0057 NEW)
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
 find_package(hardware_interface REQUIRED)
 find_package(odri_control_interface REQUIRED)
 find_package(controller_interface REQUIRED)
@@ -82,7 +84,7 @@ target_link_libraries(
   Eigen3::Eigen rclcpp::rclcpp odri_control_interface::odri_control_interface)
 
 ament_target_dependencies(${PROJECT_NAME} controller_interface
-                          hardware_interface pluginlib rclcpp)
+                          hardware_interface pluginlib rclcpp rclcpp_lifecycle)
 
 target_include_directories(
   ${PROJECT_NAME} PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>

README.md

@@ -83,6 +83,7 @@ Tested on Bolt.
 
 # Credits
 
+- Andreas Bihlmaier (Update to ros2 Humble)
 - Maxime-Ulrich Fansi (04/2022-09/2022) - First working version of gazebo_bolt_ros2_control
 - Benjamin Amsellem (10/2021-02/2022) - First working version of ros2_hardware_bolt
 - Paul Rouanet - (03/2021-09/2021) - Building the LAAS Bolt, starting this repo

include/ros2_hardware_interface_odri/system_odri.hpp

@@ -36,8 +36,6 @@
 #include "ros2_hardware_interface_odri/visibility_control.h"
 #include "semantic_components/imu_sensor.hpp"
 
-using hardware_interface::return_type;
-
 #define rt_printf printf
 
 /**
@@ -89,12 +87,12 @@ class SystemOdriHardware : public hardware_interface::SystemInterface {
       override;
 
   ROS2_CONTROL_ODRI_PUBLIC
-  return_type prepare_command_mode_switch(
+  hardware_interface::return_type prepare_command_mode_switch(
       const std::vector<std::string> &start_interfaces,
       const std::vector<std::string> &stop_interfaces) override;
 
   ROS2_CONTROL_ODRI_PUBLIC
-  return_type calibration();
+  hardware_interface::return_type calibration();
 
   ROS2_CONTROL_ODRI_PUBLIC
   hardware_interface::CallbackReturn on_activate(
@@ -105,15 +103,15 @@ class SystemOdriHardware : public hardware_interface::SystemInterface {
       const rclcpp_lifecycle::State &previous_state) override;
 
   ROS2_CONTROL_ODRI_PUBLIC
-  hardware_interface::return_type read(const rclcpp::Time &,
-                                       const rclcpp::Duration &) override;
+  hardware_interface::return_type read(const rclcpp::Time &time,
+                                       const rclcpp::Duration &period) override;
 
   ROS2_CONTROL_ODRI_PUBLIC
-  hardware_interface::return_type write(const rclcpp::Time &,
-                                        const rclcpp::Duration &) override;
+  hardware_interface::return_type write(
+      const rclcpp::Time &time, const rclcpp::Duration &period) override;
 
   ROS2_CONTROL_ODRI_PUBLIC
-  return_type display();
+  hardware_interface::return_type display();
 
  private:
   // Parameters for the RRBot simulation
@@ -125,10 +123,11 @@ class SystemOdriHardware : public hardware_interface::SystemInterface {
   void display_robot_state();
 
   // Read desired starting position.
-  return_type read_desired_starting_position();
+  hardware_interface::return_type read_desired_starting_position();
 
   // Read default joint cmd and state values
-  return_type read_default_cmd_state_value(std::string &default_joint_cs);
+  hardware_interface::return_type read_default_cmd_state_value(
+      std::string &default_joint_cs);
 
   // Read default cmd or state value.
   // default_joint_cs: "default_joint_cmd" or "default_joint_state"

package.xml

@@ -37,6 +37,7 @@
     <depend>hardware_interface</depend>
     <depend>pluginlib</depend>
     <depend>rclcpp</depend>
+    <depend>rclcpp_lifecycle</depend>
     <depend>odri_control_interface</depend>
     <depend>controller_interface</depend>
 

src/system_odri.cpp

@@ -43,7 +43,8 @@ namespace ros2_control_odri {
 Eigen::Vector6d desired_joint_position = Eigen::Vector6d::Zero();
 Eigen::Vector6d desired_torque = Eigen::Vector6d::Zero();
 
-return_type SystemOdriHardware::read_default_cmd_state_value(
+hardware_interface::return_type
+SystemOdriHardware::read_default_cmd_state_value(
     std::string &default_joint_cs) {
   // Hardware parameters provides a string
   if (info_.hardware_parameters.find(default_joint_cs) ==
@@ -52,18 +53,18 @@ return_type SystemOdriHardware::read_default_cmd_state_value(
         rclcpp::get_logger("SystemOdriHardware"),
         "%s not in the parameter list of ros2_control_odri/SystemOdriHardware!",
         default_joint_cs.c_str());
-    return return_type::ERROR;
+    return hardware_interface::return_type::ERROR;
   }
   std::string str_des_start_pos = info_.hardware_parameters[default_joint_cs];
 
   typedef std::map<std::string, PosVelEffortGains> map_pveg;
-  map_pveg hw_cs;
+  map_pveg *hw_cs;
   if (default_joint_cs == "default_joint_cmd") {
-    hw_cs = hw_commands_;
+    hw_cs = &hw_commands_;
   } else if (default_joint_cs == "default_joint_state") {
-    hw_cs = hw_states_;
+    hw_cs = &hw_states_;
   } else
-    return return_type::ERROR;
+    return hardware_interface::return_type::ERROR;
 
   // Read the parameter through a stream of strings.
   std::istringstream iss_def_cmd_val;
@@ -72,10 +73,14 @@ return_type SystemOdriHardware::read_default_cmd_state_value(
   while (!iss_def_cmd_val.eof()) {
     // Find joint name.
     std::string joint_name;
-    RCLCPP_INFO_STREAM(
-        rclcpp::get_logger("SystemOdriHardware"),
-        " Current value of iss_def_cmd_val:" << iss_def_cmd_val.str().c_str());
+    RCLCPP_INFO_STREAM(rclcpp::get_logger("SystemOdriHardware"),
+                       " Current value of iss_def_cmd_val for "
+                           << default_joint_cs << ":"
+                           << iss_def_cmd_val.str().c_str());
     iss_def_cmd_val >> joint_name;
+    if (joint_name == "" && iss_def_cmd_val.eof()) {
+      break;
+    }
 
     // Find the associate joint
     bool found_joint = false;
@@ -90,40 +95,40 @@ return_type SystemOdriHardware::read_default_cmd_state_value(
             RCLCPP_FATAL(rclcpp::get_logger("SystemOdriHardware"),
                          "default_joint_cmd '%s' no '%s'.", joint_name.c_str(),
                          msg.c_str());
-            return return_type::ERROR;
+            return hardware_interface::return_type::ERROR;
           }
-          return return_type::OK;
+          return hardware_interface::return_type::OK;
         };
 
         std::string amsg("position");
         if (handle_dbl_and_msg(iss_def_cmd_val, joint_name,
-                               hw_cs.at(joint_name).position,
-                               amsg) == return_type::ERROR)
-          return return_type::ERROR;
+                               hw_cs->at(joint_name).position,
+                               amsg) == hardware_interface::return_type::ERROR)
+          return hardware_interface::return_type::ERROR;
 
         amsg = "velocity";
         if (handle_dbl_and_msg(iss_def_cmd_val, joint_name,
-                               hw_cs.at(joint_name).velocity,
-                               amsg) == return_type::ERROR)
-          return return_type::ERROR;
+                               hw_cs->at(joint_name).velocity,
+                               amsg) == hardware_interface::return_type::ERROR)
+          return hardware_interface::return_type::ERROR;
 
         amsg = "effort";
         if (handle_dbl_and_msg(iss_def_cmd_val, joint_name,
-                               hw_cs.at(joint_name).effort,
-                               amsg) == return_type::ERROR)
-          return return_type::ERROR;
+                               hw_cs->at(joint_name).effort,
+                               amsg) == hardware_interface::return_type::ERROR)
+          return hardware_interface::return_type::ERROR;
 
         amsg = "Kp";
         if (handle_dbl_and_msg(iss_def_cmd_val, joint_name,
-                               hw_cs.at(joint_name).Kp,
-                               amsg) == return_type::ERROR)
-          return return_type::ERROR;
+                               hw_cs->at(joint_name).Kp,
+                               amsg) == hardware_interface::return_type::ERROR)
+          return hardware_interface::return_type::ERROR;
 
         amsg = "Kd";
         if (handle_dbl_and_msg(iss_def_cmd_val, joint_name,
-                               hw_cs.at(joint_name).Kd,
-                               amsg) == return_type::ERROR)
-          return return_type::ERROR;
+                               hw_cs->at(joint_name).Kd,
+                               amsg) == hardware_interface::return_type::ERROR)
+          return hardware_interface::return_type::ERROR;
 
         found_joint = true;
         break;  // Found the joint break the loop
@@ -134,27 +139,23 @@ return_type SystemOdriHardware::read_default_cmd_state_value(
       RCLCPP_FATAL(rclcpp::get_logger("SystemOdriHardware"),
                    "Joint '%s' not found in '%s'.", joint_name.c_str(),
                    default_joint_cs.c_str());
-      return return_type::ERROR;
+      return hardware_interface::return_type::ERROR;
     }
   }
-  if (default_joint_cs == "default_joint_cmd")
-    hw_commands_ = hw_cs;
-  else if (default_joint_cs == "default_joint_state")
-    hw_states_ = hw_cs;
-
-  return return_type::OK;
+  return hardware_interface::return_type::OK;
 }
 
 /// Reading desired position
-return_type SystemOdriHardware::read_desired_starting_position() {
+hardware_interface::return_type
+SystemOdriHardware::read_desired_starting_position() {
   std::vector<double> vec_des_start_pos;
 
   if (info_.hardware_parameters.find("desired_starting_position") ==
       info_.hardware_parameters.end()) {
     RCLCPP_FATAL(rclcpp::get_logger("SystemOdriHardware"),
                  "desired_starting_positon not in the parameter list of "
                  "ros2_control_odri/SystemOdriHardware!");
-    return return_type::ERROR;
+    return hardware_interface::return_type::ERROR;
   }
 
   // Hardware parameters provides a string
@@ -185,7 +186,7 @@ return_type SystemOdriHardware::read_desired_starting_position() {
   int idx_dsp = 0;
   for (auto apos : vec_des_start_pos) eig_des_start_pos_[idx_dsp++] = apos;
 
-  return return_type::OK;
+  return hardware_interface::return_type::OK;
 }
 
 hardware_interface::CallbackReturn SystemOdriHardware::on_configure(
@@ -231,7 +232,7 @@ hardware_interface::CallbackReturn SystemOdriHardware::on_configure(
     if (joint.command_interfaces.size() != odri_list_of_cmd_inter.size()) {
       RCLCPP_FATAL(
           rclcpp::get_logger("SystemOdriHardware"),
-          "Joint '%s' has %ld command interfaces found.",  // 5 expected.",
+          "Joint '%s' has %lu command interfaces found.",  // 5 expected.",
           joint.name.c_str(), joint.command_interfaces.size());
       return hardware_interface::CallbackReturn::ERROR;
     }
@@ -257,7 +258,7 @@ hardware_interface::CallbackReturn SystemOdriHardware::on_configure(
     // Check if the state interface list is of the right size
     if (joint.state_interfaces.size() != odri_list_of_state_inter.size()) {
       RCLCPP_FATAL(rclcpp::get_logger("SystemOdriHardware"),
-                   "Joint '%s' has %ld state interface.",  // 5 expected.",
+                   "Joint '%s' has %lu state interface.",  // 5 expected.",
                    joint.name.c_str(), joint.state_interfaces.size());
       return hardware_interface::CallbackReturn::ERROR;
     }
@@ -298,7 +299,7 @@ void SystemOdriHardware::display_robot_state() {
   std::cout << " **************************" << std::endl;
 }
 
-return_type SystemOdriHardware::prepare_command_mode_switch(
+hardware_interface::return_type SystemOdriHardware::prepare_command_mode_switch(
     const std::vector<std::string> &start_interfaces,
     const std::vector<std::string> &stop_interfaces) {
   // Initialize new modes.
@@ -353,11 +354,14 @@ return_type SystemOdriHardware::prepare_command_mode_switch(
       return hardware_interface::return_type::ERROR;
     }
     control_mode_[joint.name] = new_modes_[joint.name];
+    RCLCPP_INFO_STREAM(
+        rclcpp::get_logger("SystemOdriHardware"),
+        "control_mode[" << joint.name << "]: " << control_mode_[joint.name]);
   }
 
   std::cout << "in prepare_command_mode_switch" << std::endl;
   display_robot_state();
-  return return_type::OK;
+  return hardware_interface::return_type::OK;
 }
 
 std::vector<hardware_interface::StateInterface>
@@ -448,7 +452,7 @@ SystemOdriHardware::export_command_interfaces() {
 }
 
 hardware_interface::CallbackReturn SystemOdriHardware::on_activate(
-    const rclcpp_lifecycle::State & /* previous_state */) {
+    const rclcpp_lifecycle::State & /*previous_state*/) {
   //// Read Parameters ////
 
   /// Read odri_config_yaml
@@ -481,26 +485,31 @@ hardware_interface::CallbackReturn SystemOdriHardware::on_activate(
     joint_name_to_array_index_[joint.name] = 0;
   }
 
-  // Then build the index.
+  /// Then build the index.
+  // Warning: depends on order of joint tags within ros2_control tag
+  //          and on order in robot.joint_modules.motor_numbers in
+  //          config_solo12.yaml
   uint idx = 0;
   for (auto it = joint_name_to_array_index_.begin();
        it != joint_name_to_array_index_.end(); ++it) {
+    RCLCPP_INFO_STREAM(rclcpp::get_logger("SystemOdriHardware"),
+                       "joint_name=" << it->first << " -> index=" << idx);
     joint_name_to_array_index_[it->first] = idx++;
   }
 
   return hardware_interface::CallbackReturn::SUCCESS;
 }
 
 hardware_interface::CallbackReturn SystemOdriHardware::on_deactivate(
-    const rclcpp_lifecycle::State & /* previous_state */) {
+    const rclcpp_lifecycle::State & /*previous_state*/) {
   // Stop the MasterBoard
   main_board_ptr_->MasterBoardInterface::Stop();
 
   return hardware_interface::CallbackReturn::SUCCESS;
 }
 
 hardware_interface::return_type SystemOdriHardware::read(
-    const rclcpp::Time &, const rclcpp::Duration &) {
+    const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
   // Data acquisition (with ODRI)
   robot_->ParseSensorData();
 
@@ -549,11 +558,11 @@ hardware_interface::return_type SystemOdriHardware::read(
   imu_states_["IMU"].quater_z = imu_quater[2];
   imu_states_["IMU"].quater_w = imu_quater[3];
 
-  return return_type::OK;
+  return hardware_interface::return_type::OK;
 }
 
 hardware_interface::return_type SystemOdriHardware::write(
-    const rclcpp::Time &, const rclcpp::Duration &) {
+    const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
   Eigen::Vector6d positions;
   Eigen::Vector6d velocities;
   Eigen::Vector6d torques;
@@ -574,6 +583,9 @@ hardware_interface::return_type SystemOdriHardware::write(
           hw_commands_[joint.name].Kp;
       gain_KD[joint_name_to_array_index_[joint.name]] =
           hw_commands_[joint.name].Kd;
+    } else if (control_mode_[joint.name] == control_mode_t::EFFORT) {
+      torques[joint_name_to_array_index_[joint.name]] =
+          hw_commands_[joint.name].effort;
     }
   }
 
@@ -596,7 +608,7 @@ hardware_interface::return_type SystemOdriHardware::write(
 
   robot_->SendCommandAndWaitEndOfCycle(0.00);
 
-  return return_type::OK;
+  return hardware_interface::return_type::OK;
 }
 
 }  // namespace ros2_control_odri