From 8e923b5b2d76444cda119da4397f6a701d77b0ac Mon Sep 17 00:00:00 2001 From: ksotebeer Date: Tue, 14 Jun 2022 16:19:27 -0600 Subject: [PATCH 1/2] implemented perform_command_mode_switch override in GazeboSystem --- .../gazebo_ros2_control/gazebo_system.hpp | 5 +++ gazebo_ros2_control/src/gazebo_system.cpp | 40 +++++++++++++++++-- 2 files changed, 42 insertions(+), 3 deletions(-) diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp index 4349bed4..8230a4ec 100644 --- a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp +++ b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp @@ -55,6 +55,11 @@ class GazeboSystem : public GazeboSystemInterface // Documentation Inherited CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + // Documentation Inherited + hardware_interface::return_type perform_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) override; + // Documentation Inherited hardware_interface::return_type read( const rclcpp::Time & time, diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index defd014c..73500982 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -252,7 +252,6 @@ void GazeboSystem::registerJoints( for (unsigned int i = 0; i < joint_info.command_interfaces.size(); i++) { if (joint_info.command_interfaces[i].name == "position") { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t position"); - this->dataPtr->joint_control_methods_[j] |= POSITION; this->dataPtr->command_interfaces_.emplace_back( joint_name + suffix, hardware_interface::HW_IF_POSITION, @@ -263,7 +262,6 @@ void GazeboSystem::registerJoints( } if (joint_info.command_interfaces[i].name == "velocity") { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity"); - this->dataPtr->joint_control_methods_[j] |= VELOCITY; this->dataPtr->command_interfaces_.emplace_back( joint_name + suffix, hardware_interface::HW_IF_VELOCITY, @@ -273,7 +271,6 @@ void GazeboSystem::registerJoints( } } if (joint_info.command_interfaces[i].name == "effort") { - this->dataPtr->joint_control_methods_[j] |= EFFORT; RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t effort"); this->dataPtr->command_interfaces_.emplace_back( joint_name + suffix, @@ -454,6 +451,43 @@ CallbackReturn GazeboSystem::on_deactivate(const rclcpp_lifecycle::State & previ return CallbackReturn::SUCCESS; } +hardware_interface::return_type +GazeboSystem::perform_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) +{ + for (unsigned int j = 0; j < this->dataPtr->joint_names_.size(); j++) { + for (const std::string & key : stop_interfaces) { + // Clear joint control method bits corresponding to stop interfaces + if (key == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_POSITION)) { + this->dataPtr->joint_control_methods_[j] &= static_cast(VELOCITY & EFFORT); + } + if (key == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_VELOCITY)) { + this->dataPtr->joint_control_methods_[j] &= static_cast(POSITION & EFFORT); + } + if (key == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_EFFORT)) { + this->dataPtr->joint_control_methods_[j] &= + static_cast(POSITION & VELOCITY); + } + } + + // Set joint control method bits corresponding to start interfaces + for (const std::string & key : start_interfaces) { + if (key == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_POSITION)) { + this->dataPtr->joint_control_methods_[j] |= POSITION; + } + if (key == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_VELOCITY)) { + this->dataPtr->joint_control_methods_[j] |= VELOCITY; + } + if (key == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_EFFORT)) { + this->dataPtr->joint_control_methods_[j] |= EFFORT; + } + } + } + + return hardware_interface::return_type::OK; +} + hardware_interface::return_type GazeboSystem::read( const rclcpp::Time & time, const rclcpp::Duration & period) From 0bab2b8f883ff7687d2215da133e9735a1cb8ef1 Mon Sep 17 00:00:00 2001 From: ksotebeer Date: Tue, 28 Jun 2022 09:11:56 -0600 Subject: [PATCH 2/2] Update variable names in perform_command_mode_switch method --- gazebo_ros2_control/src/gazebo_system.cpp | 28 ++++++++++++++++------- 1 file changed, 20 insertions(+), 8 deletions(-) diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index 73500982..9280e246 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -457,29 +457,41 @@ GazeboSystem::perform_command_mode_switch( const std::vector & stop_interfaces) { for (unsigned int j = 0; j < this->dataPtr->joint_names_.size(); j++) { - for (const std::string & key : stop_interfaces) { + for (const std::string & interface_name : stop_interfaces) { // Clear joint control method bits corresponding to stop interfaces - if (key == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_POSITION)) { + if (interface_name == (this->dataPtr->joint_names_[j] + "/" + + hardware_interface::HW_IF_POSITION)) + { this->dataPtr->joint_control_methods_[j] &= static_cast(VELOCITY & EFFORT); } - if (key == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_VELOCITY)) { + if (interface_name == (this->dataPtr->joint_names_[j] + "/" + + hardware_interface::HW_IF_VELOCITY)) + { this->dataPtr->joint_control_methods_[j] &= static_cast(POSITION & EFFORT); } - if (key == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_EFFORT)) { + if (interface_name == (this->dataPtr->joint_names_[j] + "/" + + hardware_interface::HW_IF_EFFORT)) + { this->dataPtr->joint_control_methods_[j] &= static_cast(POSITION & VELOCITY); } } // Set joint control method bits corresponding to start interfaces - for (const std::string & key : start_interfaces) { - if (key == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_POSITION)) { + for (const std::string & interface_name : start_interfaces) { + if (interface_name == (this->dataPtr->joint_names_[j] + "/" + + hardware_interface::HW_IF_POSITION)) + { this->dataPtr->joint_control_methods_[j] |= POSITION; } - if (key == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_VELOCITY)) { + if (interface_name == (this->dataPtr->joint_names_[j] + "/" + + hardware_interface::HW_IF_VELOCITY)) + { this->dataPtr->joint_control_methods_[j] |= VELOCITY; } - if (key == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_EFFORT)) { + if (interface_name == (this->dataPtr->joint_names_[j] + "/" + + hardware_interface::HW_IF_EFFORT)) + { this->dataPtr->joint_control_methods_[j] |= EFFORT; } }