diff --git a/CMakeLists.txt b/CMakeLists.txt index 07849c6..469395a 100644 --- a/CMakeLists.txt +++ b/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 $ diff --git a/README.md b/README.md index 97cd034..48d01cf 100644 --- a/README.md +++ b/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 diff --git a/cmake b/cmake index e1a7152..7faf6f9 160000 --- a/cmake +++ b/cmake @@ -1 +1 @@ -Subproject commit e1a71520cd2f0e6f2a611e1a70df4d8edf4d5a65 +Subproject commit 7faf6f9c4e981111bf0ac5dff4970f0959c11f69 diff --git a/include/ros2_hardware_interface_odri/system_odri.hpp b/include/ros2_hardware_interface_odri/system_odri.hpp index 0d96b8a..8d75451 100644 --- a/include/ros2_hardware_interface_odri/system_odri.hpp +++ b/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 &start_interfaces, const std::vector &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" diff --git a/package.xml b/package.xml index a4c96dc..b883562 100644 --- a/package.xml +++ b/package.xml @@ -37,6 +37,7 @@ hardware_interface pluginlib rclcpp + rclcpp_lifecycle odri_control_interface controller_interface diff --git a/src/system_odri.cpp b/src/system_odri.cpp index df443bf..588a8e4 100644 --- a/src/system_odri.cpp +++ b/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 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,19 +139,15 @@ 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 vec_des_start_pos; if (info_.hardware_parameters.find("desired_starting_position") == @@ -154,7 +155,7 @@ return_type SystemOdriHardware::read_desired_starting_position() { 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 &start_interfaces, const std::vector &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 @@ -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,10 +485,15 @@ 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++; } @@ -492,7 +501,7 @@ hardware_interface::CallbackReturn SystemOdriHardware::on_activate( } 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(); @@ -500,7 +509,7 @@ hardware_interface::CallbackReturn SystemOdriHardware::on_deactivate( } 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