diff --git a/example_10/description/ros2_control/rrbot.ros2_control.xacro b/example_10/description/ros2_control/rrbot.ros2_control.xacro index 77611c089..1b3b2be5c 100644 --- a/example_10/description/ros2_control/rrbot.ros2_control.xacro +++ b/example_10/description/ros2_control/rrbot.ros2_control.xacro @@ -21,28 +21,45 @@ + double -1 1 - + + double + + double -1 1 - + + double + - - - - + + double + + + double + + + double + + + double + - + + double + + double 1.0 diff --git a/example_10/hardware/include/ros2_control_demo_example_10/rrbot.hpp b/example_10/hardware/include/ros2_control_demo_example_10/rrbot.hpp index 9f6b2c62f..9e5ff76df 100644 --- a/example_10/hardware/include/ros2_control_demo_example_10/rrbot.hpp +++ b/example_10/hardware/include/ros2_control_demo_example_10/rrbot.hpp @@ -68,11 +68,31 @@ class RRBotSystemWithGPIOHardware : public hardware_interface::SystemInterface private: // Parameters for the RRBot simulation - // Store the command and state interfaces for the simulated robot - std::vector hw_commands_; - std::vector hw_states_; - std::vector hw_gpio_in_; - std::vector hw_gpio_out_; + struct FlangeVacuum + { + explicit FlangeVacuum(const std::string & gpio_name) : name(gpio_name) {} + // delete move constructor since would throw because of const std::string members + // but we dont want to move this anyways so const for member is ok i guess + FlangeVacuum(FlangeVacuum && other) = delete; + + const std::string name; + const std::string vacuum = name + "/vacuum"; + }; + + struct FlangeIOs + { + explicit FlangeIOs(const std::string & gpio_name) : name(gpio_name) {} + // delete move constructor since would throw because of const std::string members + // but we dont want to move this anyways so const for member is ok i guess + FlangeIOs(FlangeIOs && other) = delete; + + const std::string name; + const std::string out = name + "/analog_output1"; + const std::string input_1 = name + "/analog_input1"; + const std::string input_2 = name + "/analog_input2"; + }; + std::unique_ptr flange_vacuum_; + std::unique_ptr flange_ios_; }; } // namespace ros2_control_demo_example_10 diff --git a/example_10/hardware/rrbot.cpp b/example_10/hardware/rrbot.cpp index d8e9bd84a..4d9708eda 100644 --- a/example_10/hardware/rrbot.cpp +++ b/example_10/hardware/rrbot.cpp @@ -35,9 +35,6 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_init( return hardware_interface::CallbackReturn::ERROR; } - hw_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - for (const hardware_interface::ComponentInfo & joint : info_.joints) { // RRBotSystemPositionOnly has exactly one state and command interface on each joint @@ -77,6 +74,27 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_init( return hardware_interface::CallbackReturn::ERROR; } } + // check if we have same command and state interfaces for joint this makes iterating easier + // first check if size is equal then we only need to iterate over one of them + if (joint_state_interfaces_.size() != joint_command_interfaces_.size()) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemWithSensorHardware"), + "Expect joint CommandInterface and joint StateInterfaces to be of equal size."); + return hardware_interface::CallbackReturn::ERROR; + } + for (const auto & [state_itf_name, state_itf_descr] : joint_state_interfaces_) + { + if (joint_command_interfaces_.find(state_itf_name) == joint_command_interfaces_.end()) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemWithSensorHardware"), + "Expect joint CommandInterface and joint StateInterfaces to be equal but StateInterface " + "includes<%s> which is not included in CommandInterfaces.", + state_itf_name.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + } // RRBotSystemWithGPIOHardware has exactly two GPIO components if (info_.gpios.size() != 2) @@ -116,6 +134,9 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_init( info_.gpios[0].state_interfaces.size(), 1); return hardware_interface::CallbackReturn::ERROR; } + // set gpios + flange_ios_ = std::make_unique(info_.gpios[0].name); + flange_vacuum_ = std::make_unique(info_.gpios[1].name); return hardware_interface::CallbackReturn::SUCCESS; } @@ -128,69 +149,23 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_configure( // END: This part here is for exemplary purposes - Please do not copy to your production code // reset values always when configuring hardware - std::fill(hw_states_.begin(), hw_states_.end(), 0); - std::fill(hw_commands_.begin(), hw_commands_.end(), 0); - std::fill(hw_gpio_in_.begin(), hw_gpio_in_.end(), 0); - std::fill(hw_gpio_out_.begin(), hw_gpio_out_.end(), 0); - - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Successfully configured!"); - - return hardware_interface::CallbackReturn::SUCCESS; -} - -std::vector -RRBotSystemWithGPIOHardware::export_state_interfaces() -{ - std::vector state_interfaces; - for (uint i = 0; i < info_.joints.size(); i++) - { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i])); - } - - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "State interfaces:"); - hw_gpio_in_.resize(4); - size_t ct = 0; - for (size_t i = 0; i < info_.gpios.size(); i++) + for (const auto & [itf_name, itf_descr] : joint_state_interfaces_) { - for (auto state_if : info_.gpios.at(i).state_interfaces) - { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.gpios.at(i).name, state_if.name, &hw_gpio_in_[ct++])); - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Added %s/%s", - info_.gpios.at(i).name.c_str(), state_if.name.c_str()); - } + set_state(itf_name, 0.0); + set_command(itf_name, 0.0); } - - return state_interfaces; -} - -std::vector -RRBotSystemWithGPIOHardware::export_command_interfaces() -{ - std::vector command_interfaces; - for (uint i = 0; i < info_.joints.size(); i++) + for (const auto & [gpio_state, gpio_state_desc] : gpio_state_interfaces_) { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i])); + set_state(gpio_state, 0.0); } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Command interfaces:"); - hw_gpio_out_.resize(2); - size_t ct = 0; - for (size_t i = 0; i < info_.gpios.size(); i++) + for (const auto & [gpio_cmd, gpio_cmd_desc] : gpio_command_interfaces_) { - for (auto command_if : info_.gpios.at(i).command_interfaces) - { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.gpios.at(i).name, command_if.name, &hw_gpio_out_[ct++])); - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Added %s/%s", - info_.gpios.at(i).name.c_str(), command_if.name.c_str()); - } + set_command(gpio_cmd, 0.0); } - return command_interfaces; + RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Successfully configured!"); + + return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_activate( @@ -201,9 +176,9 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_activate( // END: This part here is for exemplary purposes - Please do not copy to your production code // command and state should be equal when starting - for (uint i = 0; i < hw_states_.size(); i++) + for (const auto & [itf_name, itf_descr] : joint_state_interfaces_) { - hw_commands_[i] = hw_states_[i]; + set_command(itf_name, get_state(itf_name)); } RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Successfully activated!"); @@ -227,26 +202,26 @@ hardware_interface::return_type RRBotSystemWithGPIOHardware::read( // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Reading..."); - for (uint i = 0; i < hw_states_.size(); i++) + for (const auto & [itf_name, itf_descr] : joint_state_interfaces_) { // Simulate RRBot's movement - hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]); + set_state(itf_name, get_state(itf_name) + (get_command(itf_name) - get_state(itf_name))); } // mirror GPIOs back - hw_gpio_in_[0] = hw_gpio_out_[0]; - hw_gpio_in_[3] = hw_gpio_out_[1]; + set_state(flange_ios_->out, get_command(flange_ios_->out)); + set_state(flange_vacuum_->vacuum, get_command(flange_vacuum_->vacuum)); // random inputs unsigned int seed = time(NULL) + 1; - hw_gpio_in_[1] = static_cast(rand_r(&seed)); + set_state(flange_ios_->input_1, static_cast(rand_r(&seed))); seed = time(NULL) + 2; - hw_gpio_in_[2] = static_cast(rand_r(&seed)); + set_state(flange_ios_->input_2, static_cast(rand_r(&seed))); - for (uint i = 0; i < hw_gpio_in_.size(); i++) + for (const auto & [gpio_state, gpio_state_desc] : gpio_state_interfaces_) { RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Read %.1f from GP input %d!", - hw_gpio_in_[i], i); + rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Read %.1f from GP input %s!", + get_state(gpio_state), gpio_state.c_str()); } RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "GPIOs successfully read!"); // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -260,11 +235,11 @@ hardware_interface::return_type RRBotSystemWithGPIOHardware::write( // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Writing..."); - for (uint i = 0; i < hw_gpio_out_.size(); i++) + for (const auto & [gpio_cmd, gpio_cmd_desc] : gpio_command_interfaces_) { RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Got command %.1f for GP output %d!", - hw_gpio_out_[i], i); + rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Got command %.1f for GP output %s!", + get_command(gpio_cmd), gpio_cmd.c_str()); } RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "GPIOs successfully written!"); // END: This part here is for exemplary purposes - Please do not copy to your production code diff --git a/example_14/description/ros2_control/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.ros2_control.xacro b/example_14/description/ros2_control/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.ros2_control.xacro index 739ffc8e5..b2b0d0f2d 100644 --- a/example_14/description/ros2_control/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.ros2_control.xacro +++ b/example_14/description/ros2_control/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.ros2_control.xacro @@ -17,7 +17,9 @@ 23286 - + + double + @@ -29,7 +31,9 @@ 23287 - + + double + @@ -42,6 +46,7 @@ + double -1 1 @@ -56,6 +61,7 @@ + double -1 1 diff --git a/example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp b/example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp index 775d4d592..fb37ed39a 100644 --- a/example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp +++ b/example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp @@ -74,9 +74,7 @@ class RRBotActuatorWithoutFeedback : public hardware_interface::ActuatorInterfac // Parameters for the RRBot simulation double hw_start_sec_; double hw_stop_sec_; - - // Store the command for the simulated robot - double hw_joint_command_; + std::string vel_itf_; // Fake "mechanical connection" between actuator and sensor using sockets struct sockaddr_in address_; diff --git a/example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp b/example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp index 20e9a51e9..fa8895f8d 100644 --- a/example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp +++ b/example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp @@ -80,7 +80,7 @@ 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_; + std::string pos_itf_; // Timestamps to calculate position for velocity rclcpp::Clock clock_; diff --git a/example_14/hardware/rrbot_actuator_without_feedback.cpp b/example_14/hardware/rrbot_actuator_without_feedback.cpp index 6feb8067c..4bd22e8c3 100644 --- a/example_14/hardware/rrbot_actuator_without_feedback.cpp +++ b/example_14/hardware/rrbot_actuator_without_feedback.cpp @@ -49,8 +49,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::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) @@ -71,6 +69,8 @@ hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_init( return hardware_interface::CallbackReturn::ERROR; } + vel_itf_ = joint.name + "/" + joint.command_interfaces[0].name; + // START: 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); @@ -111,24 +111,6 @@ hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_shutdown( return hardware_interface::CallbackReturn::SUCCESS; } -std::vector -RRBotActuatorWithoutFeedback::export_state_interfaces() -{ - std::vector state_interfaces; - return state_interfaces; -} - -std::vector -RRBotActuatorWithoutFeedback::export_command_interfaces() -{ - std::vector command_interfaces; - - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[0].name, hardware_interface::HW_IF_VELOCITY, &hw_joint_command_)); - - return command_interfaces; -} - hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { @@ -145,9 +127,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_)) + if (!command_holds_value(vel_itf_) || std::isnan(get_command(vel_itf_))) { - hw_joint_command_ = 0; + set_command(vel_itf_, 0.0); } RCLCPP_INFO(rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Successfully activated!"); @@ -183,15 +165,16 @@ hardware_interface::return_type RRBotActuatorWithoutFeedback::read( hardware_interface::return_type ros2_control_demo_example_14::RRBotActuatorWithoutFeedback::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - if (std::isfinite(hw_joint_command_)) + if (std::isfinite(get_command(vel_itf_))) { // START: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO( - rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Writing command: %f", hw_joint_command_); + rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Writing command: %f", + get_command(vel_itf_)); // Simulate sending commands to the hardware std::ostringstream data; - data << hw_joint_command_; + data << get_command(vel_itf_); RCLCPP_INFO( rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Sending data command: %s", data.str().c_str()); diff --git a/example_14/hardware/rrbot_sensor_for_position_feedback.cpp b/example_14/hardware/rrbot_sensor_for_position_feedback.cpp index 395c922c6..0984e6696 100644 --- a/example_14/hardware/rrbot_sensor_for_position_feedback.cpp +++ b/example_14/hardware/rrbot_sensor_for_position_feedback.cpp @@ -52,8 +52,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::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) @@ -73,6 +71,7 @@ hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_init( joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; } + pos_itf_ = joint.name + "/" + joint.state_interfaces[0].name; clock_ = rclcpp::Clock(); @@ -184,25 +183,15 @@ hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_shutdown( return hardware_interface::CallbackReturn::SUCCESS; } -std::vector -RRBotSensorPositionFeedback::export_state_interfaces() -{ - std::vector 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*/) { // set some default values for joints - if (std::isnan(hw_joint_state_)) + if (!state_holds_value(pos_itf_) || std::isnan(get_state(pos_itf_))) { - hw_joint_state_ = 0; + set_state(pos_itf_, 0.0); } + last_measured_velocity_ = 0; // In general after a hardware is configured it can be read @@ -269,10 +258,11 @@ hardware_interface::return_type RRBotSensorPositionFeedback::read( RCLCPP_INFO( rclcpp::get_logger("RRBotSensorPositionFeedback"), "Got measured velocity %.5f", measured_velocity); - hw_joint_state_ += (last_measured_velocity_ * duration.seconds()) / hw_slowdown_; + set_state( + pos_itf_, get_state(pos_itf_) + (last_measured_velocity_ * duration.seconds()) / hw_slowdown_); RCLCPP_INFO( rclcpp::get_logger("RRBotSensorPositionFeedback"), "Got state %.5f for joint '%s'!", - hw_joint_state_, info_.joints[0].name.c_str()); + get_state(pos_itf_), info_.joints[0].name.c_str()); RCLCPP_INFO(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Joints successfully read!"); // END: This part here is for exemplary purposes - Please do not copy to your production code diff --git a/example_3/description/ros2_control/rrbot_system_multi_interface.ros2_control.xacro b/example_3/description/ros2_control/rrbot_system_multi_interface.ros2_control.xacro index b0be21fac..a3028b6a2 100644 --- a/example_3/description/ros2_control/rrbot_system_multi_interface.ros2_control.xacro +++ b/example_3/description/ros2_control/rrbot_system_multi_interface.ros2_control.xacro @@ -21,37 +21,55 @@ + double -1 1 + double -1 1 + double -1 1 - - - + + double + + + double + + + double + + double -1 1 + double -1 1 + double -1 1 - - - + + double + + + double + + + double + diff --git a/example_3/hardware/include/ros2_control_demo_example_3/rrbot_system_multi_interface.hpp b/example_3/hardware/include/ros2_control_demo_example_3/rrbot_system_multi_interface.hpp index 578b07eab..55b3f07ea 100644 --- a/example_3/hardware/include/ros2_control_demo_example_3/rrbot_system_multi_interface.hpp +++ b/example_3/hardware/include/ros2_control_demo_example_3/rrbot_system_multi_interface.hpp @@ -77,14 +77,6 @@ class RRBotSystemMultiInterfaceHardware : public hardware_interface::SystemInter double hw_stop_sec_; double hw_slowdown_; - // Store the commands for the simulated robot - std::vector hw_commands_positions_; - std::vector hw_commands_velocities_; - std::vector hw_commands_accelerations_; - std::vector hw_states_positions_; - std::vector hw_states_velocities_; - std::vector hw_states_accelerations_; - // Enum defining at which control level we are // Dumb way of maintaining the command_interface type per joint. enum integration_level_t : std::uint8_t @@ -95,8 +87,25 @@ class RRBotSystemMultiInterfaceHardware : public hardware_interface::SystemInter ACCELERATION = 3 }; - // Active control mode for each actuator - std::vector control_level_; + struct Joint + { + explicit Joint(const std::string & joint_name) : name_(joint_name) {} + + const std::string & name() const { return name_; } + const std::string & position() const { return position_; } + const std::string & velocity() const { return velocity_; } + const std::string & acceleration() const { return acceleration_; } + + protected: + std::string name_; + std::string position_ = name_ + "/" + hardware_interface::HW_IF_POSITION; + std::string velocity_ = name_ + "/" + hardware_interface::HW_IF_VELOCITY; + std::string acceleration_ = name_ + "/" + hardware_interface::HW_IF_ACCELERATION; + }; + + std::vector joints_; + // All joints have same mode + integration_level_t control_level_; }; } // namespace ros2_control_demo_example_3 diff --git a/example_3/hardware/rrbot_system_multi_interface.cpp b/example_3/hardware/rrbot_system_multi_interface.cpp index ee6f4d37b..0f2bee8c1 100644 --- a/example_3/hardware/rrbot_system_multi_interface.cpp +++ b/example_3/hardware/rrbot_system_multi_interface.cpp @@ -41,13 +41,6 @@ hardware_interface::CallbackReturn RRBotSystemMultiInterfaceHardware::on_init( hw_stop_sec_ = stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); hw_slowdown_ = stod(info_.hardware_parameters["example_param_hw_slowdown"]); // END: This part here is for exemplary purposes - Please do not copy to your production code - hw_states_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_states_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_states_accelerations_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_commands_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_commands_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_commands_accelerations_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - control_level_.resize(info_.joints.size(), integration_level_t::POSITION); for (const hardware_interface::ComponentInfo & joint : info_.joints) { @@ -96,42 +89,35 @@ hardware_interface::CallbackReturn RRBotSystemMultiInterfaceHardware::on_init( } } - return hardware_interface::CallbackReturn::SUCCESS; -} - -std::vector -RRBotSystemMultiInterfaceHardware::export_state_interfaces() -{ - std::vector state_interfaces; - for (std::size_t i = 0; i < info_.joints.size(); i++) + // check if we have same command and state interfaces for joint this makes iterating easier + // first check if size is equal then we only need to iterate over one of them + if (joint_state_interfaces_.size() != joint_command_interfaces_.size()) { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_positions_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_states_velocities_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &hw_states_accelerations_[i])); + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), + "Expect joint CommandInterface and joint StateInterfaces to be of equal size."); + return hardware_interface::CallbackReturn::ERROR; + } + for (const auto & [state_itf_name, state_itf_descr] : joint_state_interfaces_) + { + if (joint_command_interfaces_.find(state_itf_name) == joint_command_interfaces_.end()) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), + "Expect joint CommandInterface and joint StateInterfaces to be equal but StateInterface " + "includes<%s> which is not included in CommandInterfaces.", + state_itf_name.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } } - return state_interfaces; -} - -std::vector -RRBotSystemMultiInterfaceHardware::export_command_interfaces() -{ - std::vector command_interfaces; - for (std::size_t i = 0; i < info_.joints.size(); i++) + // fill joints vector + for (const auto & joint : info_.joints) { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_positions_[i])); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_velocities_[i])); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, - &hw_commands_accelerations_[i])); + joints_.emplace_back(joint.name); } - return command_interfaces; + return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::return_type RRBotSystemMultiInterfaceHardware::prepare_command_mode_switch( @@ -178,21 +164,21 @@ hardware_interface::return_type RRBotSystemMultiInterfaceHardware::prepare_comma { if (key.find(info_.joints[i].name) != std::string::npos) { - hw_commands_velocities_[i] = 0; - hw_commands_accelerations_[i] = 0; - control_level_[i] = integration_level_t::UNDEFINED; // Revert to undefined + set_command(key + "/" + hardware_interface::HW_IF_VELOCITY, 0.0); + set_command(key + "/" + hardware_interface::HW_IF_ACCELERATION, 0.0); + control_level_ = integration_level_t::UNDEFINED; // Revert to undefined } } } // Set the new command modes for (std::size_t i = 0; i < info_.joints.size(); i++) { - if (control_level_[i] != integration_level_t::UNDEFINED) + if (control_level_ != integration_level_t::UNDEFINED) { // Something else is using the joint! Abort! return hardware_interface::return_type::ERROR; } - control_level_[i] = new_modes[i]; + control_level_ = new_modes[0]; } return hardware_interface::return_type::OK; } @@ -214,38 +200,24 @@ hardware_interface::CallbackReturn RRBotSystemMultiInterfaceHardware::on_activat // END: This part here is for exemplary purposes - Please do not copy to your production code // Set some default values - for (std::size_t i = 0; i < hw_states_positions_.size(); i++) + // we checked that joint_state_interfaces_ == joint_command_interfaces_ + for (const auto & [itf_name, itf_descr] : joint_state_interfaces_) { - if (std::isnan(hw_states_positions_[i])) + if (!state_holds_value(itf_name) || std::isnan(get_state(itf_name))) { - hw_states_positions_[i] = 0; + set_state(itf_name, 0.0); } - if (std::isnan(hw_states_velocities_[i])) + if (!command_holds_value(itf_name) || std::isnan(get_command(itf_name))) { - hw_states_velocities_[i] = 0; + set_command(itf_name, 0.0); } - if (std::isnan(hw_states_accelerations_[i])) - { - hw_states_accelerations_[i] = 0; - } - if (std::isnan(hw_commands_positions_[i])) - { - hw_commands_positions_[i] = 0; - } - if (std::isnan(hw_commands_velocities_[i])) - { - hw_commands_velocities_[i] = 0; - } - if (std::isnan(hw_commands_accelerations_[i])) - { - hw_commands_accelerations_[i] = 0; - } - control_level_[i] = integration_level_t::UNDEFINED; } + control_level_ = integration_level_t::UNDEFINED; + RCLCPP_INFO( rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), "System successfully activated! %u", - control_level_[0]); + control_level_); return hardware_interface::CallbackReturn::SUCCESS; } @@ -273,9 +245,10 @@ hardware_interface::CallbackReturn RRBotSystemMultiInterfaceHardware::on_deactiv hardware_interface::return_type RRBotSystemMultiInterfaceHardware::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { - for (std::size_t i = 0; i < hw_states_positions_.size(); i++) + // we checked that joint_state_interfaces_ == joint_command_interfaces_ + for (const auto & joint : joints_) { - switch (control_level_[i]) + switch (control_level_) { case integration_level_t::UNDEFINED: RCLCPP_INFO( @@ -284,27 +257,36 @@ hardware_interface::return_type RRBotSystemMultiInterfaceHardware::read( return hardware_interface::return_type::OK; break; case integration_level_t::POSITION: - hw_states_accelerations_[i] = 0; - hw_states_velocities_[i] = 0; - hw_states_positions_[i] += - (hw_commands_positions_[i] - hw_states_positions_[i]) / hw_slowdown_; + set_state(joint.acceleration(), 0.0); + set_state(joint.velocity(), 0.0); + set_state( + joint.position(), + get_state(joint.position()) + + (get_command(joint.position()) - get_state(joint.position()) / hw_slowdown_)); break; case integration_level_t::VELOCITY: - hw_states_accelerations_[i] = 0; - hw_states_velocities_[i] = hw_commands_velocities_[i]; - hw_states_positions_[i] += (hw_states_velocities_[i] * period.seconds()) / hw_slowdown_; + set_state(joint.acceleration(), 0.0); + set_state(joint.velocity(), get_command(joint.velocity())); + set_state( + joint.position(), get_state(joint.position()) + + (get_state(joint.velocity()) * period.seconds()) / hw_slowdown_); break; case integration_level_t::ACCELERATION: - hw_states_accelerations_[i] = hw_commands_accelerations_[i]; - hw_states_velocities_[i] += (hw_states_accelerations_[i] * period.seconds()) / hw_slowdown_; - hw_states_positions_[i] += (hw_states_velocities_[i] * period.seconds()) / hw_slowdown_; + set_state(joint.acceleration(), get_command(joint.acceleration())); + set_state( + joint.velocity(), + get_state(joint.velocity()) + + (get_command(joint.acceleration()) * period.seconds()) / hw_slowdown_); + set_state( + joint.position(), get_state(joint.position()) + + (get_state(joint.velocity()) * period.seconds()) / hw_slowdown_); break; } // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO( rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), - "Got pos: %.5f, vel: %.5f, acc: %.5f for joint %lu!", hw_states_positions_[i], - hw_states_velocities_[i], hw_states_accelerations_[i], i); + "Got pos: %.5f, vel: %.5f, acc: %.5f for joint %s!", get_state(joint.position()), + get_state(joint.velocity()), get_state(joint.acceleration()), joint.name().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code } return hardware_interface::return_type::OK; @@ -314,14 +296,14 @@ hardware_interface::return_type RRBotSystemMultiInterfaceHardware::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - for (std::size_t i = 0; i < hw_commands_positions_.size(); i++) + for (const auto & joint : joints_) { // Simulate sending commands to the hardware RCLCPP_INFO( rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), - "Got the commands pos: %.5f, vel: %.5f, acc: %.5f for joint %lu, control_lvl:%u", - hw_commands_positions_[i], hw_commands_velocities_[i], hw_commands_accelerations_[i], i, - control_level_[i]); + "Got the commands pos: %.5f, vel: %.5f, acc: %.5f for joint %s, control_lvl:%u", + get_state(joint.position()), get_state(joint.velocity()), get_state(joint.acceleration()), + joint.name().c_str(), control_level_); } // END: This part here is for exemplary purposes - Please do not copy to your production code diff --git a/example_4/description/ros2_control/rrbot_system_with_sensor.ros2_control.xacro b/example_4/description/ros2_control/rrbot_system_with_sensor.ros2_control.xacro index 7b821e010..ec058545a 100644 --- a/example_4/description/ros2_control/rrbot_system_with_sensor.ros2_control.xacro +++ b/example_4/description/ros2_control/rrbot_system_with_sensor.ros2_control.xacro @@ -22,17 +22,23 @@ + double -1 1 - + + double + + double -1 1 - + + double + diff --git a/example_4/hardware/include/ros2_control_demo_example_4/rrbot_system_with_sensor.hpp b/example_4/hardware/include/ros2_control_demo_example_4/rrbot_system_with_sensor.hpp index de6b1f3d1..b6a79f54a 100644 --- a/example_4/hardware/include/ros2_control_demo_example_4/rrbot_system_with_sensor.hpp +++ b/example_4/hardware/include/ros2_control_demo_example_4/rrbot_system_with_sensor.hpp @@ -74,11 +74,6 @@ class RRBotSystemWithSensorHardware : public hardware_interface::SystemInterface double hw_stop_sec_; double hw_slowdown_; double hw_sensor_change_; - - // Store the command for the simulated robot - std::vector hw_joint_commands_; - std::vector hw_joint_states_; - std::vector hw_sensor_states_; }; } // namespace ros2_control_demo_example_4 diff --git a/example_4/hardware/rrbot_system_with_sensor.cpp b/example_4/hardware/rrbot_system_with_sensor.cpp index eea370e1f..8d796a36e 100644 --- a/example_4/hardware/rrbot_system_with_sensor.cpp +++ b/example_4/hardware/rrbot_system_with_sensor.cpp @@ -46,11 +46,6 @@ hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_init( hw_sensor_change_ = stod(info_.hardware_parameters["example_param_max_sensor_change"]); // END: This part here is for exemplary purposes - Please do not copy to your production code - hw_joint_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_joint_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_sensor_states_.resize( - info_.sensors[0].state_interfaces.size(), std::numeric_limits::quiet_NaN()); - for (const hardware_interface::ComponentInfo & joint : info_.joints) { // RRBotSystemWithSensor has exactly one state and command interface on each joint @@ -91,6 +86,28 @@ hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_init( } } + // check if we have same command and state interfaces for joint this makes iterating easier + // first check if size is equal then we only need to iterate over one of them + if (joint_state_interfaces_.size() != joint_command_interfaces_.size()) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemWithSensorHardware"), + "Expect joint CommandInterface and joint StateInterfaces to be of equal size."); + return hardware_interface::CallbackReturn::ERROR; + } + for (const auto & [state_itf_name, state_itf_descr] : joint_state_interfaces_) + { + if (joint_command_interfaces_.find(state_itf_name) == joint_command_interfaces_.end()) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemWithSensorHardware"), + "Expect joint CommandInterface and joint StateInterfaces to be equal but StateInterface " + "includes<%s> which is not included in CommandInterfaces.", + state_itf_name.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + } + return hardware_interface::CallbackReturn::SUCCESS; } @@ -110,10 +127,11 @@ hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_configure( // END: This part here is for exemplary purposes - Please do not copy to your production code // reset values always when configuring hardware - for (uint i = 0; i < hw_joint_states_.size(); i++) + // we checked before that joint_state_itfs_ == joint_command_itfs_; + for (const auto & [itf_name, itf_desc] : joint_state_interfaces_) { - hw_joint_states_[i] = 0; - hw_joint_commands_[i] = 0; + set_state(itf_name, 0.0); + set_command(itf_name, 0.0); } RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Successfully configured!"); @@ -121,39 +139,6 @@ hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_configure( return hardware_interface::CallbackReturn::SUCCESS; } -std::vector -RRBotSystemWithSensorHardware::export_state_interfaces() -{ - std::vector state_interfaces; - for (uint i = 0; i < info_.joints.size(); i++) - { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_joint_states_[i])); - } - - // export sensor state interface - for (uint i = 0; i < info_.sensors[0].state_interfaces.size(); i++) - { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.sensors[0].name, info_.sensors[0].state_interfaces[i].name, &hw_sensor_states_[i])); - } - - return state_interfaces; -} - -std::vector -RRBotSystemWithSensorHardware::export_command_interfaces() -{ - std::vector command_interfaces; - for (uint i = 0; i < info_.joints.size(); i++) - { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_joint_commands_[i])); - } - - return command_interfaces; -} - hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { @@ -170,15 +155,18 @@ hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_activate( // END: This part here is for exemplary purposes - Please do not copy to your production code // command and state should be equal when starting - for (uint i = 0; i < hw_joint_states_.size(); i++) + for (const auto & [itf_name, itf_desc] : joint_state_interfaces_) { - hw_joint_commands_[i] = hw_joint_states_[i]; + set_command(itf_name, get_state(itf_name)); } // set default value for sensor - if (std::isnan(hw_sensor_states_[0])) + for (const auto & [sensor_itf_name, sensor_itf_desc] : sensor_state_interfaces_) { - hw_sensor_states_[0] = 0; + if (!state_holds_value(sensor_itf_name) || std::isnan(get_state(sensor_itf_name))) + { + set_state(sensor_itf_name, 0.0); + } } RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Successfully activated!"); @@ -213,25 +201,30 @@ hardware_interface::return_type RRBotSystemWithSensorHardware::read( // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Reading...please wait..."); - for (uint i = 0; i < hw_joint_states_.size(); i++) + for (const auto & [itf_name, itf_desc] : joint_state_interfaces_) { // Simulate RRBot's movement - hw_joint_states_[i] += (hw_joint_commands_[i] - hw_joint_states_[i]) / hw_slowdown_; + auto old_state = get_state(itf_name); + auto new_state = old_state + (get_command(itf_name) - old_state) / hw_slowdown_; + set_state(itf_name, new_state); RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Got state %.5f for joint %u!", - hw_joint_states_[i], i); + rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Got state %.5f for joint %s!", + get_state(itf_name), itf_name.c_str()); } RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Joints successfully read!"); - for (uint i = 0; i < hw_sensor_states_.size(); i++) + int i = 0; + for (const auto & [sensor_itf_name, sensor_itf_desc] : sensor_state_interfaces_) { // Simulate RRBot's sensor data unsigned int seed = time(NULL) + i; - hw_sensor_states_[i] = - static_cast(rand_r(&seed)) / (static_cast(RAND_MAX / hw_sensor_change_)); + set_state( + sensor_itf_name, + static_cast(rand_r(&seed)) / (static_cast(RAND_MAX / hw_sensor_change_))); RCLCPP_INFO( rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Got value %e for interface %s!", - hw_sensor_states_[i], info_.sensors[0].state_interfaces[i].name.c_str()); + get_state(sensor_itf_name), sensor_itf_name.c_str()); + ++i; } RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Sensors successfully read!"); // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -245,12 +238,12 @@ hardware_interface::return_type ros2_control_demo_example_4::RRBotSystemWithSens // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Writing...please wait..."); - for (uint i = 0; i < hw_joint_commands_.size(); i++) + for (const auto & [itf_name, itf_desc] : joint_command_interfaces_) { // Simulate sending commands to the hardware RCLCPP_INFO( rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Got command %.5f for joint %u!", - hw_joint_commands_[i], i); + get_command(itf_name), itf_name); } RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Joints successfully written!"); // END: This part here is for exemplary purposes - Please do not copy to your production code diff --git a/example_5/hardware/include/ros2_control_demo_example_5/rrbot.hpp b/example_5/hardware/include/ros2_control_demo_example_5/rrbot.hpp index c3d053464..448b61837 100644 --- a/example_5/hardware/include/ros2_control_demo_example_5/rrbot.hpp +++ b/example_5/hardware/include/ros2_control_demo_example_5/rrbot.hpp @@ -70,10 +70,6 @@ class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterfa double hw_start_sec_; double hw_stop_sec_; double hw_slowdown_; - - // Store the command for the simulated robot - std::vector hw_commands_; - std::vector hw_states_; }; } // namespace ros2_control_demo_example_5 diff --git a/example_5/hardware/rrbot.cpp b/example_5/hardware/rrbot.cpp index 35fe1c0f2..4aeea73b8 100644 --- a/example_5/hardware/rrbot.cpp +++ b/example_5/hardware/rrbot.cpp @@ -40,9 +40,6 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_init( hw_stop_sec_ = stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); hw_slowdown_ = stod(info_.hardware_parameters["example_param_hw_slowdown"]); // END: This part here is for exemplary purposes - Please do not copy to your production code - hw_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - for (const hardware_interface::ComponentInfo & joint : info_.joints) { // RRBotSystemPositionOnly has exactly one state and command interface on each joint @@ -83,6 +80,28 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_init( } } + // check if we have same command and state interfaces for joint this makes iterating easier + // first check if size is equal then we only need to iterate over one of them + if (joint_state_interfaces_.size() != joint_command_interfaces_.size()) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), + "Expect joint CommandInterface and joint StateInterfaces to be of equal size."); + return hardware_interface::CallbackReturn::ERROR; + } + for (const auto & [state_itf_name, state_itf_descr] : joint_state_interfaces_) + { + if (joint_command_interfaces_.find(state_itf_name) == joint_command_interfaces_.end()) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), + "Expect joint CommandInterface and joint StateInterfaces to be equal but StateInterface " + "includes<%s> which is not included in CommandInterfaces.", + state_itf_name.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + } + return hardware_interface::CallbackReturn::SUCCESS; } @@ -103,10 +122,11 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_configure // END: This part here is for exemplary purposes - Please do not copy to your production code // reset values always when configuring hardware - for (uint i = 0; i < hw_states_.size(); i++) + // we checked before that joint_state_itfs_ == joint_command_itfs_; + for (const auto & [itf_name, itf_desc] : joint_state_interfaces_) { - hw_states_[i] = 0; - hw_commands_[i] = 0; + set_state(itf_name, 0.0); + set_command(itf_name, 0.0); } RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully configured!"); @@ -114,32 +134,6 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_configure return hardware_interface::CallbackReturn::SUCCESS; } -std::vector -RRBotSystemPositionOnlyHardware::export_state_interfaces() -{ - std::vector state_interfaces; - for (uint i = 0; i < info_.joints.size(); i++) - { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i])); - } - - return state_interfaces; -} - -std::vector -RRBotSystemPositionOnlyHardware::export_command_interfaces() -{ - std::vector command_interfaces; - for (uint i = 0; i < info_.joints.size(); i++) - { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i])); - } - - return command_interfaces; -} - hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { @@ -157,9 +151,9 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_activate( // END: This part here is for exemplary purposes - Please do not copy to your production code // command and state should be equal when starting - for (uint i = 0; i < hw_states_.size(); i++) + for (const auto & [itf_name, itf_desc] : joint_state_interfaces_) { - hw_commands_[i] = hw_states_[i]; + set_command(itf_name, get_state(itf_name)); } RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully activated!"); @@ -194,13 +188,15 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::read( // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Reading..."); - for (uint i = 0; i < hw_states_.size(); i++) + for (const auto & [itf_name, itf_desc] : joint_state_interfaces_) { // Simulate RRBot's movement - hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]) / hw_slowdown_; + auto old_state = get_state(itf_name); + auto new_state = old_state + (get_command(itf_name) - old_state) / hw_slowdown_; + set_state(itf_name, new_state); RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Got state %.5f for joint %d!", - hw_states_[i], i); + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Got state %.5f for joint %s!", + get_state(itf_name), itf_name.c_str()); } RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Joints successfully read!"); // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -214,12 +210,12 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::write( // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Writing..."); - for (uint i = 0; i < hw_commands_.size(); i++) + for (const auto & [itf_name, itf_desc] : joint_command_interfaces_) { // Simulate sending commands to the hardware RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Got command %.5f for joint %d!", - hw_commands_[i], i); + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Got command %.5f for joint %u!", + get_command(itf_name), itf_name); } RCLCPP_INFO( rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Joints successfully written!"); diff --git a/example_6/hardware/rrbot_actuator.cpp b/example_6/hardware/rrbot_actuator.cpp index 2fe1fdf7c..2a02a618f 100644 --- a/example_6/hardware/rrbot_actuator.cpp +++ b/example_6/hardware/rrbot_actuator.cpp @@ -91,7 +91,7 @@ hardware_interface::CallbackReturn RRBotModularJoint::on_init( rclcpp::get_logger("RRBotModularJoint"), "Joint '%s' has state interface with name<%s> but not found in CommandInterfaces. Make " "sure the configuration exports same State- and CommandInterfaces", - joint.name.c_str(), joint.state_itf_name.c_str()); + joint.name.c_str(), state_itf_name.c_str()); return hardware_interface::CallbackReturn::ERROR; } } @@ -115,7 +115,7 @@ hardware_interface::CallbackReturn RRBotModularJoint::on_activate( // set some default values for joints for (const auto & [cmd_itf_name, cmd_itf_descr] : joint_command_interfaces_) { - if (!valid_command(cmd_itf_name) || std::isnan(get_command(cmd_itf_name))) + if (!command_holds_value(cmd_itf_name) || std::isnan(get_command(cmd_itf_name))) { set_command(cmd_itf_name, 0.0); } @@ -123,7 +123,7 @@ hardware_interface::CallbackReturn RRBotModularJoint::on_activate( for (const auto & [state_itf_name, state_itf_descr] : joint_state_interfaces_) { - if (!valid_state(state_itf_name) || std::isnan(get_state(state_itf_name))) + if (!command_holds_value(state_itf_name) || std::isnan(get_state(state_itf_name))) { set_state(state_itf_name, 0.0); } diff --git a/example_7/hardware/include/ros2_control_demo_example_7/r6bot_hardware.hpp b/example_7/hardware/include/ros2_control_demo_example_7/r6bot_hardware.hpp index 1c8aa7bdc..c609c419a 100644 --- a/example_7/hardware/include/ros2_control_demo_example_7/r6bot_hardware.hpp +++ b/example_7/hardware/include/ros2_control_demo_example_7/r6bot_hardware.hpp @@ -52,6 +52,10 @@ class RobotSystem : public hardware_interface::SystemInterface struct FTS_Sensor { explicit FTS_Sensor(const std::string & sensor_name) : name(sensor_name) {} + // delete move constructor since would throw because of const std::string members + // but we dont want to move this anyways so const for member is ok i guess + FTS_Sensor(FTS_Sensor && other) = delete; + const std::string name; const std::string force_x = "force.x"; const std::string force_y = "force.y";