From 068454cefc33b0ab67b6ebb7edb211c11558c069 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 29 Dec 2022 10:36:33 +0100 Subject: [PATCH 01/58] Rename hw info class type to plugin name (#169) API break from https://github.com/ros-controls/ros2_control/pull/780 --- gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp index 3e4ceab1..c03eddef 100644 --- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -287,7 +287,7 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element } for (unsigned int i = 0; i < control_hardware_info.size(); i++) { - std::string robot_hw_sim_type_str_ = control_hardware_info[i].hardware_class_type; + std::string robot_hw_sim_type_str_ = control_hardware_info[i].hardware_plugin_name; auto gazeboSystem = std::unique_ptr( impl_->robot_hw_sim_loader_->createUnmanagedInstance(robot_hw_sim_type_str_)); From 6b90ed972472d86301e0204aa18d89a25ef6b761 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 29 Dec 2022 13:10:31 +0100 Subject: [PATCH 02/58] Improve error message if robot_description_ param is wrong (#168) --- gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp index c03eddef..e4f14706 100644 --- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -420,14 +420,13 @@ std::string GazeboRosControlPrivate::getURDF(std::string param_name) const // search and wait for robot_description on param server while (urdf_string.empty()) { - std::string search_param_name; RCLCPP_DEBUG(model_nh_->get_logger(), "param_name %s", param_name.c_str()); try { auto f = parameters_client->get_parameters({param_name}); f.wait(); std::vector values = f.get(); - urdf_string = values[0].as_string(); + urdf_string = values.at(0).as_string(); } catch (const std::exception & e) { RCLCPP_ERROR(model_nh_->get_logger(), "%s", e.what()); } @@ -437,7 +436,7 @@ std::string GazeboRosControlPrivate::getURDF(std::string param_name) const } else { RCLCPP_ERROR( model_nh_->get_logger(), "gazebo_ros2_control plugin is waiting for model" - " URDF in parameter [%s] on the ROS param server.", search_param_name.c_str()); + " URDF in parameter [%s] on the ROS param server.", param_name.c_str()); } usleep(100000); } From cc5746bad6820a39d158eec87c0100126f78a6ab Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 5 Jan 2023 21:54:59 +0100 Subject: [PATCH 03/58] Force setting use_sim_time parameter when using plugin. (#171) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero --- gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp index e4f14706..45504786 100644 --- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -348,6 +348,9 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element " s) is slower than the gazebo simulation period (" << gazebo_period.seconds() << " s)."); } + // Force setting of use_sime_time parameter + impl_->controller_manager_->set_parameter( + rclcpp::Parameter("use_sim_time", rclcpp::ParameterValue(true))); impl_->stop_ = false; auto spin = [this]() From e0a125af8451e6676fb02cb53a9fed7a2e1d49cf Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 6 Jan 2023 10:05:52 +0000 Subject: [PATCH 04/58] Update changelogs --- gazebo_ros2_control/CHANGELOG.rst | 10 ++++++++++ gazebo_ros2_control_demos/CHANGELOG.rst | 5 +++++ 2 files changed, 15 insertions(+) diff --git a/gazebo_ros2_control/CHANGELOG.rst b/gazebo_ros2_control/CHANGELOG.rst index 50de1a39..6ca5e9ef 100644 --- a/gazebo_ros2_control/CHANGELOG.rst +++ b/gazebo_ros2_control/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package gazebo_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Force setting use_sim_time parameter when using plugin. (`#171 `_) +* Improve error message if robot_description\_ param is wrong (`#168 `_) +* Rename hw info class type to plugin name (`#169 `_) +* Removed warning (`#162 `_) +* Mimic joint should have the same control mode as mimicked joint. (`#154 `_) +* Enable loading params from multiple yaml files (`#149 `_) +* Contributors: Alejandro Hernández Cordero, Christoph Fröhlich, Denis Štogl, Tony Najjar + 0.4.0 (2022-08-09) ------------------ * Implemented perform_command_mode_switch override in GazeboSystem (`#136 `_) diff --git a/gazebo_ros2_control_demos/CHANGELOG.rst b/gazebo_ros2_control_demos/CHANGELOG.rst index d80896d6..1c0a2189 100644 --- a/gazebo_ros2_control_demos/CHANGELOG.rst +++ b/gazebo_ros2_control_demos/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gazebo_ros2_control_demos ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add tricycle controller demo (`#145 `_) +* Contributors: Tony Najjar + 0.4.0 (2022-08-09) ------------------ * fix demo launch From 6718ae9e60a43befc816968a2a1aa05395fc691b Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 6 Jan 2023 10:06:17 +0000 Subject: [PATCH 05/58] 0.5.0 --- gazebo_ros2_control/CHANGELOG.rst | 4 ++-- gazebo_ros2_control/package.xml | 2 +- gazebo_ros2_control_demos/CHANGELOG.rst | 4 ++-- gazebo_ros2_control_demos/package.xml | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/gazebo_ros2_control/CHANGELOG.rst b/gazebo_ros2_control/CHANGELOG.rst index 6ca5e9ef..89b490f7 100644 --- a/gazebo_ros2_control/CHANGELOG.rst +++ b/gazebo_ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.0 (2023-01-06) +------------------ * Force setting use_sim_time parameter when using plugin. (`#171 `_) * Improve error message if robot_description\_ param is wrong (`#168 `_) * Rename hw info class type to plugin name (`#169 `_) diff --git a/gazebo_ros2_control/package.xml b/gazebo_ros2_control/package.xml index 024ec20e..e19e3ac7 100644 --- a/gazebo_ros2_control/package.xml +++ b/gazebo_ros2_control/package.xml @@ -1,7 +1,7 @@ gazebo_ros2_control - 0.4.0 + 0.5.0 gazebo_ros2_control Alejandro Hernandez diff --git a/gazebo_ros2_control_demos/CHANGELOG.rst b/gazebo_ros2_control_demos/CHANGELOG.rst index 1c0a2189..296f4488 100644 --- a/gazebo_ros2_control_demos/CHANGELOG.rst +++ b/gazebo_ros2_control_demos/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_ros2_control_demos ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.0 (2023-01-06) +------------------ * Add tricycle controller demo (`#145 `_) * Contributors: Tony Najjar diff --git a/gazebo_ros2_control_demos/package.xml b/gazebo_ros2_control_demos/package.xml index 421c441f..38670440 100644 --- a/gazebo_ros2_control_demos/package.xml +++ b/gazebo_ros2_control_demos/package.xml @@ -1,7 +1,7 @@ gazebo_ros2_control_demos - 0.4.0 + 0.5.0 gazebo_ros2_control_demos Alejandro Hernandez From 9f7a06b53a3ba7dcd3af4a53d7ffa3be127d133f Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Sat, 14 Jan 2023 22:46:29 +0100 Subject: [PATCH 06/58] [CI] Add dependabot configuration to automatically update actions. --- .github/dependabot.yml | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100644 .github/dependabot.yml diff --git a/.github/dependabot.yml b/.github/dependabot.yml new file mode 100644 index 00000000..05a48fc6 --- /dev/null +++ b/.github/dependabot.yml @@ -0,0 +1,13 @@ +# To get started with Dependabot version updates, you'll need to specify which +# package ecosystems to update and where the package manifests are located. +# Please see the documentation for all configuration options: +# https://docs.github.com/github/administering-a-repository/configuration-options-for-dependency-updates + +version: 2 +updates: + - package-ecosystem: "github-actions" + # Workflow files stored in the + # default location of `.github/workflows` + directory: "/" + schedule: + interval: "weekly" From d89f7ca5077c5e71b2fbf22bfe27191c8fb0066c Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Sat, 14 Jan 2023 23:43:23 +0100 Subject: [PATCH 07/58] Bump actions/checkout from 2 to 3 (#174) Bumps [actions/checkout](https://github.com/actions/checkout) from 2 to 3. - [Release notes](https://github.com/actions/checkout/releases) - [Changelog](https://github.com/actions/checkout/blob/main/CHANGELOG.md) - [Commits](https://github.com/actions/checkout/compare/v2...v3) --- updated-dependencies: - dependency-name: actions/checkout dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 4e9f0f1f..dd473e30 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -11,7 +11,7 @@ jobs: container: image: osrf/ros:rolling-desktop steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 - name: Setup colcon workspace id: configure run: | @@ -46,7 +46,7 @@ jobs: container: image: osrf/ros:rolling-desktop steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 - name: Setup colcon workspace id: configure run: | From 1b4ecb6f5f05e48467ca0898ca5c2b480a1a9d56 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 3 Feb 2023 09:21:40 -0600 Subject: [PATCH 08/58] Various bug fixes (#177) --- gazebo_ros2_control/src/gazebo_system.cpp | 41 ++++++++++++++--------- 1 file changed, 25 insertions(+), 16 deletions(-) diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index 71ba3a9f..ddb7868a 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -100,6 +100,9 @@ class gazebo_ros2_control::GazeboSystemPrivate /// \brief mapping of mimicked joints to index of joint they mimic std::vector mimic_joints_; + + /// \brief Gain which converts position error to a velocity command + double position_proportional_gain_; }; namespace gazebo_ros2_control @@ -125,6 +128,17 @@ bool GazeboSystem::initSim( return false; } + constexpr double default_gain = 0.1; + if (!this->nh_->get_parameter_or( + "position_proportional_gain", + this->dataPtr->position_proportional_gain_, default_gain)) + { + RCLCPP_WARN_STREAM( + this->nh_->get_logger(), + "The position_proportional_gain parameter was not defined, defaulting to: " << + default_gain); + } + registerJoints(hardware_info, parent_model); registerSensors(hardware_info, parent_model); @@ -475,13 +489,11 @@ GazeboSystem::perform_command_mode_switch( hardware_interface::HW_IF_POSITION)) { this->dataPtr->joint_control_methods_[j] &= static_cast(VELOCITY & EFFORT); - } - if (interface_name == (this->dataPtr->joint_names_[j] + "/" + + } else if (interface_name == (this->dataPtr->joint_names_[j] + "/" + // NOLINT hardware_interface::HW_IF_VELOCITY)) { this->dataPtr->joint_control_methods_[j] &= static_cast(POSITION & EFFORT); - } - if (interface_name == (this->dataPtr->joint_names_[j] + "/" + + } else if (interface_name == (this->dataPtr->joint_names_[j] + "/" + // NOLINT hardware_interface::HW_IF_EFFORT)) { this->dataPtr->joint_control_methods_[j] &= @@ -495,13 +507,11 @@ GazeboSystem::perform_command_mode_switch( hardware_interface::HW_IF_POSITION)) { this->dataPtr->joint_control_methods_[j] |= POSITION; - } - if (interface_name == (this->dataPtr->joint_names_[j] + "/" + + } else if (interface_name == (this->dataPtr->joint_names_[j] + "/" + // NOLINT hardware_interface::HW_IF_VELOCITY)) { this->dataPtr->joint_control_methods_[j] |= VELOCITY; - } - if (interface_name == (this->dataPtr->joint_names_[j] + "/" + + } else if (interface_name == (this->dataPtr->joint_names_[j] + "/" + // NOLINT hardware_interface::HW_IF_EFFORT)) { this->dataPtr->joint_control_methods_[j] |= EFFORT; @@ -575,15 +585,13 @@ hardware_interface::return_type GazeboSystem::write( this->dataPtr->joint_position_cmd_[mimic_joint.joint_index] = mimic_joint.multiplier * this->dataPtr->joint_position_cmd_[mimic_joint.mimicked_joint_index]; - } - if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & VELOCITY && + } else if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & VELOCITY && // NOLINT this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & VELOCITY) { this->dataPtr->joint_velocity_cmd_[mimic_joint.joint_index] = mimic_joint.multiplier * this->dataPtr->joint_velocity_cmd_[mimic_joint.mimicked_joint_index]; - } - if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & EFFORT && + } else if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & EFFORT && // NOLINT this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & EFFORT) { this->dataPtr->joint_effort_cmd_[mimic_joint.joint_index] = @@ -595,12 +603,13 @@ hardware_interface::return_type GazeboSystem::write( if (this->dataPtr->sim_joints_[j]) { if (this->dataPtr->joint_control_methods_[j] & POSITION) { this->dataPtr->sim_joints_[j]->SetPosition(0, this->dataPtr->joint_position_cmd_[j], true); - } - if (this->dataPtr->joint_control_methods_[j] & VELOCITY) { + } else if (this->dataPtr->joint_control_methods_[j] & VELOCITY) { // NOLINT this->dataPtr->sim_joints_[j]->SetVelocity(0, this->dataPtr->joint_velocity_cmd_[j]); - } - if (this->dataPtr->joint_control_methods_[j] & EFFORT) { + } else if (this->dataPtr->joint_control_methods_[j] & EFFORT) { // NOLINT this->dataPtr->sim_joints_[j]->SetForce(0, this->dataPtr->joint_effort_cmd_[j]); + } else { + // Fallback case is a velocity command of zero + this->dataPtr->sim_joints_[j]->SetVelocity(0, 0.0); } } } From 0bab72ddbcd3d2e974d93ad9255585e32a3c1bb1 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 7 Feb 2023 16:24:03 +0000 Subject: [PATCH 09/58] Update changelogs --- gazebo_ros2_control/CHANGELOG.rst | 5 +++++ gazebo_ros2_control_demos/CHANGELOG.rst | 3 +++ 2 files changed, 8 insertions(+) diff --git a/gazebo_ros2_control/CHANGELOG.rst b/gazebo_ros2_control/CHANGELOG.rst index 89b490f7..371f04d5 100644 --- a/gazebo_ros2_control/CHANGELOG.rst +++ b/gazebo_ros2_control/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gazebo_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Various bug fixes (`#177 `_) +* Contributors: AndyZe + 0.5.0 (2023-01-06) ------------------ * Force setting use_sim_time parameter when using plugin. (`#171 `_) diff --git a/gazebo_ros2_control_demos/CHANGELOG.rst b/gazebo_ros2_control_demos/CHANGELOG.rst index 296f4488..67bf274a 100644 --- a/gazebo_ros2_control_demos/CHANGELOG.rst +++ b/gazebo_ros2_control_demos/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gazebo_ros2_control_demos ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.5.0 (2023-01-06) ------------------ * Add tricycle controller demo (`#145 `_) From cea0546bb4cfde13ce5e88a44fb0f42b61f85bf5 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 7 Feb 2023 16:25:42 +0000 Subject: [PATCH 10/58] 0.5.1 --- gazebo_ros2_control/CHANGELOG.rst | 4 ++-- gazebo_ros2_control/package.xml | 2 +- gazebo_ros2_control_demos/CHANGELOG.rst | 4 ++-- gazebo_ros2_control_demos/package.xml | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/gazebo_ros2_control/CHANGELOG.rst b/gazebo_ros2_control/CHANGELOG.rst index 371f04d5..2818166f 100644 --- a/gazebo_ros2_control/CHANGELOG.rst +++ b/gazebo_ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.1 (2023-02-07) +------------------ * Various bug fixes (`#177 `_) * Contributors: AndyZe diff --git a/gazebo_ros2_control/package.xml b/gazebo_ros2_control/package.xml index e19e3ac7..e2c55d18 100644 --- a/gazebo_ros2_control/package.xml +++ b/gazebo_ros2_control/package.xml @@ -1,7 +1,7 @@ gazebo_ros2_control - 0.5.0 + 0.5.1 gazebo_ros2_control Alejandro Hernandez diff --git a/gazebo_ros2_control_demos/CHANGELOG.rst b/gazebo_ros2_control_demos/CHANGELOG.rst index 67bf274a..fc3352c8 100644 --- a/gazebo_ros2_control_demos/CHANGELOG.rst +++ b/gazebo_ros2_control_demos/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_ros2_control_demos ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.1 (2023-02-07) +------------------ 0.5.0 (2023-01-06) ------------------ diff --git a/gazebo_ros2_control_demos/package.xml b/gazebo_ros2_control_demos/package.xml index 38670440..70366f38 100644 --- a/gazebo_ros2_control_demos/package.xml +++ b/gazebo_ros2_control_demos/package.xml @@ -1,7 +1,7 @@ gazebo_ros2_control_demos - 0.5.0 + 0.5.1 gazebo_ros2_control_demos Alejandro Hernandez From 49ad216d5ef26c94352aeefb927b4bfd3856f241 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 16 May 2023 13:42:17 +0200 Subject: [PATCH 11/58] Remove publish_rate parameter (#179) It was removed https://github.com/ros-controls/ros2_controllers/pull/468 --- gazebo_ros2_control_demos/config/tricycle_drive_controller.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/gazebo_ros2_control_demos/config/tricycle_drive_controller.yaml b/gazebo_ros2_control_demos/config/tricycle_drive_controller.yaml index 3519bfb9..2ffabcd5 100644 --- a/gazebo_ros2_control_demos/config/tricycle_drive_controller.yaml +++ b/gazebo_ros2_control_demos/config/tricycle_drive_controller.yaml @@ -24,7 +24,6 @@ tricycle_controller: # Odometry odom_frame_id: odom base_frame_id: base_link - publish_rate: 50.0 # publish rate of odom and tf open_loop: false # if True, uses cmd_vel instead of hardware interface feedback to compute odometry enable_odom_tf: true # If True, publishes odom<-base_link TF odom_only_twist: false # If True, publishes on /odom only linear.x and angular.z; Useful for computing odometry in another node, e.g robot_localization's ekf From 0521c13c8005539aff0c495f8949b830ac966891 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 16 May 2023 14:01:50 +0200 Subject: [PATCH 12/58] Export all dependencies (#183) (#184) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The ament_export_dependencies exports dependencies to downstream packages. This is necessary so that the user of the library does not have to call find_package for those dependencies. (cherry picked from commit 47255ae70342e1401bc4caff57db803046d3b095) Co-authored-by: Adrian Zwiener Co-authored-by: Alejandro Hernández Cordero --- gazebo_ros2_control/CMakeLists.txt | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/gazebo_ros2_control/CMakeLists.txt b/gazebo_ros2_control/CMakeLists.txt index 2440eaf1..1dffa02a 100644 --- a/gazebo_ros2_control/CMakeLists.txt +++ b/gazebo_ros2_control/CMakeLists.txt @@ -62,8 +62,18 @@ ament_export_include_directories( include ) -ament_export_dependencies(ament_cmake) -ament_export_dependencies(rclcpp) +ament_export_dependencies( + ament_cmake + angles + controller_manager + gazebo_dev + gazebo_ros + hardware_interface + pluginlib + rclcpp + yaml_cpp_vendor +) + ament_export_libraries( ${PROJECT_NAME} gazebo_hardware_plugins From c311225789931a082d7da666d6818b5e6f7318f9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 16 May 2023 15:50:16 +0200 Subject: [PATCH 13/58] Fixed rolling compilation (#195) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- gazebo_ros2_control/CMakeLists.txt | 4 ++++ gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp | 4 ++++ 2 files changed, 8 insertions(+) diff --git a/gazebo_ros2_control/CMakeLists.txt b/gazebo_ros2_control/CMakeLists.txt index 1dffa02a..6805852b 100644 --- a/gazebo_ros2_control/CMakeLists.txt +++ b/gazebo_ros2_control/CMakeLists.txt @@ -39,6 +39,10 @@ ament_target_dependencies(${PROJECT_NAME} yaml_cpp_vendor ) +if("$ENV{ROS_DISTRO}" STREQUAL "rolling") + add_definitions(-DROLLING) +endif() + add_library(gazebo_hardware_plugins SHARED src/gazebo_system.cpp ) diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp index 45504786..d5bad32d 100644 --- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -287,7 +287,11 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element } for (unsigned int i = 0; i < control_hardware_info.size(); i++) { +#ifndef ROLLING + std::string robot_hw_sim_type_str_ = control_hardware_info[i].hardware_class_type; +#else std::string robot_hw_sim_type_str_ = control_hardware_info[i].hardware_plugin_name; +#endif auto gazeboSystem = std::unique_ptr( impl_->robot_hw_sim_loader_->createUnmanagedInstance(robot_hw_sim_type_str_)); From 970b929872eeca23737057b334163c567d33f017 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Noel=20Jim=C3=A9nez=20Garc=C3=ADa?= Date: Mon, 22 May 2023 11:27:11 +0200 Subject: [PATCH 14/58] add copy operator to SafeEnum (#197) --- .../include/gazebo_ros2_control/gazebo_system_interface.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system_interface.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system_interface.hpp index b0750b07..fe98597c 100644 --- a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system_interface.hpp +++ b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system_interface.hpp @@ -44,6 +44,7 @@ class SafeEnum : mFlags(original.mFlags) {} ~SafeEnum() = default; + SafeEnum & operator=(const SafeEnum & original) = default; SafeEnum & operator|=(ENUM addValue) {mFlags |= addValue; return *this;} SafeEnum operator|(ENUM addValue) {SafeEnum result(*this); result |= addValue; return result;} SafeEnum & operator&=(ENUM maskValue) {mFlags &= maskValue; return *this;} From eed506c6f7a4b6470a2e02fc6c78fdb319cecd31 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 22 May 2023 12:51:49 +0200 Subject: [PATCH 15/58] Clean shutdown position example (#196) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- gazebo_ros2_control_demos/examples/example_position.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/gazebo_ros2_control_demos/examples/example_position.cpp b/gazebo_ros2_control_demos/examples/example_position.cpp index 417a34dc..8d2d4c0b 100644 --- a/gazebo_ros2_control_demos/examples/example_position.cpp +++ b/gazebo_ros2_control_demos/examples/example_position.cpp @@ -149,6 +149,8 @@ int main(int argc, char * argv[]) rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node->get_logger(), "send goal call failed :("); + action_client.reset(); + node.reset(); return 1; } RCLCPP_INFO(node->get_logger(), "send goal call ok :)"); @@ -157,6 +159,8 @@ int main(int argc, char * argv[]) goal_handle = goal_handle_future.get(); if (!goal_handle) { RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server"); + action_client.reset(); + node.reset(); return 1; } RCLCPP_INFO(node->get_logger(), "Goal was accepted by server"); @@ -171,7 +175,8 @@ int main(int argc, char * argv[]) return 1; } - std::cout << "async_send_goal" << std::endl; + action_client.reset(); + node.reset(); rclcpp::shutdown(); return 0; From 65c3cf6e21e9ceba8edc019e0baf17198b0ae266 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 23 May 2023 20:39:25 +0100 Subject: [PATCH 16/58] Update changelogs --- gazebo_ros2_control/CHANGELOG.rst | 7 +++++++ gazebo_ros2_control_demos/CHANGELOG.rst | 6 ++++++ 2 files changed, 13 insertions(+) diff --git a/gazebo_ros2_control/CHANGELOG.rst b/gazebo_ros2_control/CHANGELOG.rst index 2818166f..ce72e373 100644 --- a/gazebo_ros2_control/CHANGELOG.rst +++ b/gazebo_ros2_control/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package gazebo_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* add copy operator to SafeEnum (`#197 `_) +* Fixed rolling compilation (`#195 `_) +* Export all dependencies (`#183 `_) (`#184 `_) +* Contributors: Alejandro Hernández Cordero, Noel Jiménez García, Adrian Zwiener + 0.5.1 (2023-02-07) ------------------ * Various bug fixes (`#177 `_) diff --git a/gazebo_ros2_control_demos/CHANGELOG.rst b/gazebo_ros2_control_demos/CHANGELOG.rst index fc3352c8..168a8cae 100644 --- a/gazebo_ros2_control_demos/CHANGELOG.rst +++ b/gazebo_ros2_control_demos/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package gazebo_ros2_control_demos ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Clean shutdown position example (`#196 `_) +* Remove publish_rate parameter (`#179 `_) +* Contributors: Alejandro Hernández Cordero, Tony Najjar + 0.5.1 (2023-02-07) ------------------ From ba0c5f8ab7a4495aaa9df00ea9b21f383afcd689 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 23 May 2023 20:39:41 +0100 Subject: [PATCH 17/58] 0.6.0 --- gazebo_ros2_control/CHANGELOG.rst | 4 ++-- gazebo_ros2_control/package.xml | 2 +- gazebo_ros2_control_demos/CHANGELOG.rst | 4 ++-- gazebo_ros2_control_demos/package.xml | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/gazebo_ros2_control/CHANGELOG.rst b/gazebo_ros2_control/CHANGELOG.rst index ce72e373..c0ab7837 100644 --- a/gazebo_ros2_control/CHANGELOG.rst +++ b/gazebo_ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.6.0 (2023-05-23) +------------------ * add copy operator to SafeEnum (`#197 `_) * Fixed rolling compilation (`#195 `_) * Export all dependencies (`#183 `_) (`#184 `_) diff --git a/gazebo_ros2_control/package.xml b/gazebo_ros2_control/package.xml index e2c55d18..8e78b995 100644 --- a/gazebo_ros2_control/package.xml +++ b/gazebo_ros2_control/package.xml @@ -1,7 +1,7 @@ gazebo_ros2_control - 0.5.1 + 0.6.0 gazebo_ros2_control Alejandro Hernandez diff --git a/gazebo_ros2_control_demos/CHANGELOG.rst b/gazebo_ros2_control_demos/CHANGELOG.rst index 168a8cae..b7fc4afc 100644 --- a/gazebo_ros2_control_demos/CHANGELOG.rst +++ b/gazebo_ros2_control_demos/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_ros2_control_demos ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.6.0 (2023-05-23) +------------------ * Clean shutdown position example (`#196 `_) * Remove publish_rate parameter (`#179 `_) * Contributors: Alejandro Hernández Cordero, Tony Najjar diff --git a/gazebo_ros2_control_demos/package.xml b/gazebo_ros2_control_demos/package.xml index 70366f38..782f957b 100644 --- a/gazebo_ros2_control_demos/package.xml +++ b/gazebo_ros2_control_demos/package.xml @@ -1,7 +1,7 @@ gazebo_ros2_control_demos - 0.5.1 + 0.6.0 gazebo_ros2_control_demos Alejandro Hernandez From 6639e53aef4aaf8997186967f861e7d12cbc3e27 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 26 May 2023 11:24:38 +0200 Subject: [PATCH 18/58] Compile with ROS iron and rolling (#202) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- gazebo_ros2_control/CMakeLists.txt | 4 ---- gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp | 4 ---- 2 files changed, 8 deletions(-) diff --git a/gazebo_ros2_control/CMakeLists.txt b/gazebo_ros2_control/CMakeLists.txt index 6805852b..1dffa02a 100644 --- a/gazebo_ros2_control/CMakeLists.txt +++ b/gazebo_ros2_control/CMakeLists.txt @@ -39,10 +39,6 @@ ament_target_dependencies(${PROJECT_NAME} yaml_cpp_vendor ) -if("$ENV{ROS_DISTRO}" STREQUAL "rolling") - add_definitions(-DROLLING) -endif() - add_library(gazebo_hardware_plugins SHARED src/gazebo_system.cpp ) diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp index d5bad32d..45504786 100644 --- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -287,11 +287,7 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element } for (unsigned int i = 0; i < control_hardware_info.size(); i++) { -#ifndef ROLLING - std::string robot_hw_sim_type_str_ = control_hardware_info[i].hardware_class_type; -#else std::string robot_hw_sim_type_str_ = control_hardware_info[i].hardware_plugin_name; -#endif auto gazeboSystem = std::unique_ptr( impl_->robot_hw_sim_loader_->createUnmanagedInstance(robot_hw_sim_type_str_)); From f2cf686a1a97cefc9b5e3daa115e0c4854ea5707 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 7 Jun 2023 09:58:45 +0200 Subject: [PATCH 19/58] Add pre-commit and CI-format (#206) * Add pre-commit and ci-format --- .github/workflows/ci-format.yaml | 23 +++ .pre-commit-config.yaml | 136 ++++++++++++++++++ README.md | 4 +- .../gazebo_system_interface.hpp | 2 +- .../src/gazebo_ros2_control_plugin.cpp | 2 +- gazebo_ros2_control/src/gazebo_system.cpp | 2 +- .../config/tricycle_drive_controller.yaml | 4 +- 7 files changed, 166 insertions(+), 7 deletions(-) create mode 100644 .github/workflows/ci-format.yaml create mode 100644 .pre-commit-config.yaml diff --git a/.github/workflows/ci-format.yaml b/.github/workflows/ci-format.yaml new file mode 100644 index 00000000..c26fc77a --- /dev/null +++ b/.github/workflows/ci-format.yaml @@ -0,0 +1,23 @@ +# This is a format job. Pre-commit has a first-party GitHub action, so we use +# that: https://github.com/pre-commit/action + +name: Format + +on: + workflow_dispatch: + pull_request: + +jobs: + pre-commit: + name: Format + runs-on: ubuntu-22.04 + steps: + - uses: actions/checkout@v3 + - uses: actions/setup-python@v4 + with: + python-version: 3.10.6 + - name: Install system hooks + run: sudo apt install -qq cppcheck ament-cmake-uncrustify ament-cmake-pep257 + - uses: pre-commit/action@v3.0.0 + with: + extra_args: --all-files --hook-stage manual diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 00000000..f03ed362 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,136 @@ +# To use: +# +# pre-commit run -a +# +# Or: +# +# pre-commit install # (runs every time you commit in git) +# +# To update this file: +# +# pre-commit autoupdate +# +# See https://github.com/pre-commit/pre-commit + +repos: + # Standard hooks + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.1.0 + hooks: + - id: check-added-large-files + - id: check-ast + - id: check-case-conflict + - id: check-docstring-first + - id: check-merge-conflict + - id: check-symlinks + - id: check-xml + - id: check-yaml + - id: debug-statements + - id: end-of-file-fixer + - id: mixed-line-ending + - id: trailing-whitespace + exclude_types: [rst] + - id: fix-byte-order-marker + + + # Python hooks + - repo: https://github.com/asottile/pyupgrade + rev: v2.31.1 + hooks: + - id: pyupgrade + args: [--py36-plus] + + # # PEP 257 + - repo: local + hooks: + - id: ament_pep257 + name: ament_pep257 + description: Format files with pep257. + entry: ament_pep257 + language: system + args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] + + - repo: https://github.com/pycqa/flake8 + rev: 4.0.1 + hooks: + - id: flake8 + args: ["--extend-ignore=E501"] + + # CPP hooks + - repo: local + hooks: + - id: ament_uncrustify + name: ament_uncrustify + description: Format files with uncrustify. + entry: ament_uncrustify + language: system + files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ + args: ["--reformat"] + + - repo: local + hooks: + - id: ament_cppcheck + name: ament_cppcheck + description: Static code analysis of C/C++ files. + stages: [commit] + entry: ament_cppcheck + language: system + files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + + # Maybe use https://github.com/cpplint/cpplint instead + - repo: local + hooks: + - id: ament_cpplint + name: ament_cpplint + description: Static code analysis of C/C++ files. + stages: [commit] + entry: ament_cpplint + language: system + files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + args: ["--linelength=100", "--filter=-whitespace/newline"] + + # Cmake hooks + - repo: local + hooks: + - id: ament_lint_cmake + name: ament_lint_cmake + description: Check format of CMakeLists.txt files. + stages: [commit] + entry: ament_lint_cmake + language: system + files: CMakeLists\.txt$ + + # Copyright + - repo: local + hooks: + - id: ament_copyright + name: ament_copyright + description: Check if copyright notice is available in all files. + stages: [commit] + entry: ament_copyright + language: system + + # Docs - RestructuredText hooks + - repo: https://github.com/PyCQA/doc8 + rev: 0.10.1 + hooks: + - id: doc8 + args: ['--max-line-length=100', '--ignore=D001'] + exclude: CHANGELOG\.rst$ + + - repo: https://github.com/pre-commit/pygrep-hooks + rev: v1.9.0 + hooks: + - id: rst-backticks + exclude: CHANGELOG\.rst$ + - id: rst-directive-colons + - id: rst-inline-touching-normal + + # Spellcheck in comments and docs + # skipping of *.svg files is not working... + - repo: https://github.com/codespell-project/codespell + rev: v2.1.0 + hooks: + - id: codespell + args: ['--write-changes'] + exclude: CHANGELOG\.rst|\.(svg|pyc)$ diff --git a/README.md b/README.md index 39a5ef09..ba909192 100644 --- a/README.md +++ b/README.md @@ -142,7 +142,7 @@ robot hardware interfaces between `ros2_control` and Gazebo. The `gazebo_ros2_control` `` tag also has the following optional child elements: - ``: The location of the `robot_description` (URDF) on the parameter server, defaults to `robot_description` - - ``: Name of the node where the `robot_param` is located, defauls to `robot_state_publisher` + - ``: Name of the node where the `robot_param` is located, defaults to `robot_state_publisher` - ``: YAML file with the configuration of the controllers #### Default gazebo_ros2_control Behavior @@ -229,7 +229,7 @@ ros2 launch gazebo_ros2_control_demos tricycle_drive.launch.py Send example commands: -When the Gazebo world is launched you can run some of the following commads to move the cart. +When the Gazebo world is launched you can run some of the following commands to move the cart. ```bash ros2 run gazebo_ros2_control_demos example_position diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system_interface.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system_interface.hpp index fe98597c..8a4cf1fb 100644 --- a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system_interface.hpp +++ b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system_interface.hpp @@ -61,7 +61,7 @@ class GazeboSystemInterface : public hardware_interface::SystemInterface { public: - /// \brief Initilize the system interface + /// \brief Initialize the system interface /// param[in] model_nh pointer to the ros2 node /// param[in] parent_model pointer to the model /// param[in] control_hardware vector filled with information about robot's control resources diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp index 45504786..49d85b7c 100644 --- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -444,7 +444,7 @@ std::string GazeboRosControlPrivate::getURDF(std::string param_name) const usleep(100000); } RCLCPP_INFO( - model_nh_->get_logger(), "Recieved urdf from param server, parsing..."); + model_nh_->get_logger(), "Received urdf from param server, parsing..."); return urdf_string; } diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index ddb7868a..eaaa984b 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -207,7 +207,7 @@ void GazeboSystem::registerJoints( } RCLCPP_INFO_STREAM( this->nh_->get_logger(), - "Joint '" << joint_name << "'is mimicing joint '" << mimicked_joint << + "Joint '" << joint_name << "'is mimicking joint '" << mimicked_joint << "' with mutiplier: " << mimic_joint.multiplier); this->dataPtr->mimic_joints_.push_back(mimic_joint); suffix = "_mimic"; diff --git a/gazebo_ros2_control_demos/config/tricycle_drive_controller.yaml b/gazebo_ros2_control_demos/config/tricycle_drive_controller.yaml index 2ffabcd5..5083a29c 100644 --- a/gazebo_ros2_control_demos/config/tricycle_drive_controller.yaml +++ b/gazebo_ros2_control_demos/config/tricycle_drive_controller.yaml @@ -13,7 +13,7 @@ joint_state_broadcaster: ros__parameters: extra_joints: ["right_wheel_joint", "left_wheel_joint"] -tricycle_controller: +tricycle_controller: ros__parameters: # Model traction_joint_name: traction_joint # Name of traction joint in URDF @@ -30,7 +30,7 @@ tricycle_controller: pose_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Need to be set if fusing odom with other localization source twist_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Need to be set if fusing odom with other localization source velocity_rolling_window_size: 10 # Rolling window size of rcppmath::RollingMeanAccumulator applied on linear and angular speeds published on odom - + # Rate Limiting traction: # All values should be positive # min_velocity: 0.0 From eeb738537d24892680fe74f61a69685b6020ca7e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 7 Jun 2023 14:46:44 +0200 Subject: [PATCH 20/58] ReST: Move documentation to doc/index.rst (#205) * Move documentation to doc/index.rst --- README.md | 270 +--------------- {img => doc/img}/cart.gif | Bin .../img}/gazebo_ros2_control_diff_drive.gif | Bin .../img}/gazebo_ros2_control_position.gif | Bin {img => doc/img}/gripper.gif | Bin {img => doc/img}/moveit2.gif | Bin doc/index.rst | 297 ++++++++++++++++++ 7 files changed, 306 insertions(+), 261 deletions(-) rename {img => doc/img}/cart.gif (100%) rename {img => doc/img}/gazebo_ros2_control_diff_drive.gif (100%) rename {img => doc/img}/gazebo_ros2_control_position.gif (100%) rename {img => doc/img}/gripper.gif (100%) rename {img => doc/img}/moveit2.gif (100%) create mode 100644 doc/index.rst diff --git a/README.md b/README.md index ba909192..ca4ea7b4 100644 --- a/README.md +++ b/README.md @@ -1,268 +1,16 @@ # gazebo_ros2_control -This is a ROS 2 package for integrating the `ros2_control` controller architecture with the [Gazebo](http://gazebosim.org/) simulator. +This is a ROS 2 package for integrating the `ros2_control` controller architecture with the [Gazebo Classic](http://gazebosim.org/) simulator. This package provides a Gazebo plugin which instantiates a `ros2_control` controller manager and connects it to a Gazebo model. -# Usage +## Documentation +See the [documentation file](doc/index.rst) or [control.ros.org](https://control.ros.org/master/doc/simulators/gazebo_ros2_control/doc/index.html) -This repository contains the contents for testing gazebo_ros2_control +## Build status -It is running Gazebo and some other ROS 2 nodes. - -## Video + Pictures - -![](img/gazebo_ros2_control_position.gif) - -![](img/gazebo_ros2_control_diff_drive.gif) - -## Running - -### Modifying or building your own - -```bash -cd Docker -docker build -t gazebo_ros2_control . -``` - -### To run the demo - -#### Using Docker - -Docker allows us to run the demo without GUI if we don't configure it properly. The following command runs the demo without GUI: - -```bash -docker run -it --rm --name gazebo_ros2_control_demo --net host gazebo_ros2_control ros2 launch gazebo_ros2_control_demos cart_example_position.launch.py gui:=false -``` - -The in your local machine you can run the Gazebo client: - -```bash -gzclient -``` - -#### Using Rocker - -To run the demo with GUI we are going to use [rocker](https://github.com/osrf/rocker/) which is a tool to run docker -images with customized local support injected for things like nvidia support. And user id specific files for cleaner -mounting file permissions. You can install this tool with the following [instructions](https://github.com/osrf/rocker/#installation). - -The following command will launch Gazebo: - -```bash -rocker --x11 --nvidia --name gazebo_ros2_control_demo gazebo_ros2_control:latest -``` - -The following commands allow to move the cart in the rail: - -```bash -docker exec -it gazebo_ros2_control_demo bash -source /home/ros2_ws/install/setup.bash -ros2 run gazebo_ros2_control_demos example_position -``` - - -## Add ros2_control tag to a URDF - -To use `ros2_control` with your robot, you need to add some additional elements to your URDF. -You should include the tag `` to access and control the robot interfaces. We should -include - - - a specific `` for our robot - - `` tag including the robot controllers: commands and states. - -```xml - - - gazebo_ros2_control/GazeboSystem - - - - -1000 - 1000 - - - 1.0 - - - - - -``` - -### Using mimic joints in simulation - -To use `mimic` joints in `gazebo_ros2_control` you should define its parameters to your URDF. -We should include: - -- `` tag to the mimicked joint ([detailed manual(https://wiki.ros.org/urdf/XML/joint)) -- `mimic` and `multiplier` parameters to joint definition in `` tag - -```xml - - - - - - - - -``` - -```xml - - right_finger_joint - 1 - - - - - -``` - - -## Add the gazebo_ros2_control plugin - -In addition to the `ros2_control` tags, a Gazebo plugin needs to be added to your URDF that -actually parses the `ros2_control` tags and loads the appropriate hardware interfaces and -controller manager. By default the `gazebo_ros2_control` plugin is very simple, though it is also -extensible via an additional plugin architecture to allow power users to create their own custom -robot hardware interfaces between `ros2_control` and Gazebo. - -```xml - - - robot_description - robot_state_publisher - $(find gazebo_ros2_control_demos)/config/cartpole_controller.yaml - - -``` - -The `gazebo_ros2_control` `` tag also has the following optional child elements: - - - ``: The location of the `robot_description` (URDF) on the parameter server, defaults to `robot_description` - - ``: Name of the node where the `robot_param` is located, defaults to `robot_state_publisher` - - ``: YAML file with the configuration of the controllers - -#### Default gazebo_ros2_control Behavior - -By default, without a `` tag, `gazebo_ros2_control` will attempt to get all of the information it needs to interface with a ros2_control-based controller out of the URDF. This is sufficient for most cases, and good for at least getting started. - -The default behavior provides the following ros2_control interfaces: - - - hardware_interface::JointStateInterface - - hardware_interface::EffortJointInterface - - hardware_interface::VelocityJointInterface - -#### Advanced: custom gazebo_ros2_control Simulation Plugins - -The `gazebo_ros2_control` Gazebo plugin also provides a pluginlib-based interface to implement custom interfaces between Gazebo and `ros2_control` for simulating more complex mechanisms (nonlinear springs, linkages, etc). - -These plugins must inherit `gazebo_ros2_control::GazeboSystemInterface` which implements a simulated `ros2_control` -`hardware_interface::SystemInterface`. SystemInterface provides API-level access to read and command joint properties. - -The respective GazeboSystemInterface sub-class is specified in a URDF model and is loaded when the -robot model is loaded. For example, the following XML will load the default plugin: -```xml - - - gazebo_ros2_control/GazeboSystem - - ... - - - - ... - - -``` - -#### Set up controllers - -Use the tag `` inside `` to set the YAML file with the controller configuration. - -```xml - - - $(find gazebo_ros2_control_demos)/config/cartpole_controller.yaml - - -``` - -This controller publishes the state of all resources registered to a -`hardware_interface::StateInterface` to a topic of type `sensor_msgs/msg/JointState`. -The following is a basic configuration of the controller. - -```yaml -joint_state_controller: - ros__parameters: - type: joint_state_controller/JointStateController -``` - -This controller creates an action called `/cart_pole_controller/follow_joint_trajectory` of type `control_msgs::action::FollowJointTrajectory`. - -```yaml -cart_pole_controller: - ros__parameters: - type: joint_trajectory_controller/JointTrajectoryController - joints: - - slider_to_cart - write_op_modes: - - slider_to_cart -``` -#### Executing the examples - -There are some examples in the `gazebo_ros2_control_demos` package. These examples allow to launch a cart in a 30 meter rail. - -![](img/cart.gif) - -You can run some of the configuration running the following commands: - -```bash -ros2 launch gazebo_ros2_control_demos cart_example_position.launch.py -ros2 launch gazebo_ros2_control_demos cart_example_velocity.launch.py -ros2 launch gazebo_ros2_control_demos cart_example_effort.launch.py -ros2 launch gazebo_ros2_control_demos diff_drive.launch.py -ros2 launch gazebo_ros2_control_demos tricycle_drive.launch.py -``` - -Send example commands: - -When the Gazebo world is launched you can run some of the following commands to move the cart. - -```bash -ros2 run gazebo_ros2_control_demos example_position -ros2 run gazebo_ros2_control_demos example_velocity -ros2 run gazebo_ros2_control_demos example_effort -ros2 run gazebo_ros2_control_demos example_diff_drive -ros2 run gazebo_ros2_control_demos example_tricycle_drive -``` - -The following example shows parallel gripper with mimic joint: - -![](img/gripper.gif) - - -```bash -ros2 launch gazebo_ros2_control_demos gripper_mimic_joint_example.launch.py -``` - -Send example commands: - -```bash -ros2 run gazebo_ros2_control_demos example_gripper -``` - -#### Gazebo + Moveit2 + ROS 2 - -This example works with [ROS 2 Foxy](https://index.ros.org/doc/ros2/Installation/Foxy/). -You should install Moveit2 from sources, the instructions are available in this [link](https://moveit.ros.org/install-moveit2/source/). - -The repository with all the required packages are in the [gazebo_ros_demos](https://github.com/ros-controls/gazebo_ros_demos/tree/ahcorde/port/ros2). - -```bash -ros2 launch rrbot_moveit_demo_nodes rrbot_demo.launch.py -``` - -![](img/moveit2.gif) +ROS 2 Distro | Branch | Build status | Documentation +:----------: | :----: | :----------: | :-----------: +**Rolling** | [`master`](https://github.com/ros-controls/gazebo_ros2_control/tree/master) | [![Gazebo ros2 control CI](https://github.com/ros-controls/gazebo_ros2_control/actions/workflows/ci.yaml/badge.svg?branch=master)](https://github.com/ros-controls/gazebo_ros2_control/actions/workflows/ci.yaml) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) +**Iron** | [`master`](https://github.com/ros-controls/gazebo_ros2_control/tree/master) | [![Gazebo ros2 control CI](https://github.com/ros-controls/gazebo_ros2_control/actions/workflows/ci.yaml/badge.svg?branch=master)](https://github.com/ros-controls/gazebo_ros2_control/actions/workflows/ci.yaml) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) +**Humble** | [`humble`](https://github.com/ros-controls/gazebo_ros2_control/tree/humble) | [![Gazebo ros2 control CI](https://github.com/ros-controls/gazebo_ros2_control/actions/workflows/ci.yaml/badge.svg?branch=humble)](https://github.com/ros-controls/gazebo_ros2_control/actions/workflows/ci.yaml) | [Documentation](https://control.ros.org/humble/index.html)
[API Reference](https://control.ros.org/humble/doc/api/index.html) diff --git a/img/cart.gif b/doc/img/cart.gif similarity index 100% rename from img/cart.gif rename to doc/img/cart.gif diff --git a/img/gazebo_ros2_control_diff_drive.gif b/doc/img/gazebo_ros2_control_diff_drive.gif similarity index 100% rename from img/gazebo_ros2_control_diff_drive.gif rename to doc/img/gazebo_ros2_control_diff_drive.gif diff --git a/img/gazebo_ros2_control_position.gif b/doc/img/gazebo_ros2_control_position.gif similarity index 100% rename from img/gazebo_ros2_control_position.gif rename to doc/img/gazebo_ros2_control_position.gif diff --git a/img/gripper.gif b/doc/img/gripper.gif similarity index 100% rename from img/gripper.gif rename to doc/img/gripper.gif diff --git a/img/moveit2.gif b/doc/img/moveit2.gif similarity index 100% rename from img/moveit2.gif rename to doc/img/moveit2.gif diff --git a/doc/index.rst b/doc/index.rst new file mode 100644 index 00000000..b4f98f98 --- /dev/null +++ b/doc/index.rst @@ -0,0 +1,297 @@ +:github_url: https://github.com/ros-controls/gazebo_ros2_control/blob/{github_branch}/doc/index.rst + +.. _gazebo_ros2_control: + +===================== +gazebo_ros2_control +===================== + +This is a ROS 2 package for integrating the *ros2_control* controller architecture with the `Gazebo Classic `__ simulator. + +This package provides a Gazebo plugin which instantiates a *ros2_control* controller manager and connects it to a Gazebo model. + +.. image:: img/gazebo_ros2_control_position.gif + :alt: Cart + +.. image:: img/gazebo_ros2_control_diff_drive.gif + :alt: DiffBot + +Usage +====== + +Modifying or building your own +--------------------------------- + +.. code-block:: shell + + cd Docker + docker build -t gazebo_ros2_control . + + +To run the demo +--------------------------------- + +1. Using Docker + + Docker allows us to run the demo without GUI if we don't configure it properly. The following command runs the demo without GUI: + + .. code-block:: shell + + docker run -it --rm --name gazebo_ros2_control_demo --net host gazebo_ros2_control ros2 launch gazebo_ros2_control_demos cart_example_position.launch.py gui:=false + + The in your local machine you can run the Gazebo Classic client: + + .. code-block:: shell + + gzclient + +2. Using Rocker + + To run the demo with GUI we are going to use `rocker `__ which is a tool to run docker + images with customized local support injected for things like nvidia support. And user id specific files for cleaner + mounting file permissions. You can install this tool with the following `instructions `__. + + The following command will launch Gazebo Classic: + + .. code-block:: shell + + rocker --x11 --nvidia --name gazebo_ros2_control_demo gazebo_ros2_control:latest + + The following commands allow to move the cart in the rail: + + .. code-block:: shell + + docker exec -it gazebo_ros2_control_demo bash + source /home/ros2_ws/install/setup.bash + ros2 run gazebo_ros2_control_demos example_position + +Add ros2_control tag to a URDF +========================================== + +Simple setup +----------------------------------------------------------- + +To use *ros2_control* with your robot, you need to add some additional elements to your URDF. +You should include the tag ```` to access and control the robot interfaces. We should +include + +* a specific ```` for our robot +* ```` tag including the robot controllers: commands and states. + +.. code-block:: xml + + + + gazebo_ros2_control/GazeboSystem + + + + -1000 + 1000 + + + 1.0 + + + + + + + +Using mimic joints in simulation +----------------------------------------------------------- + +To use ``mimic`` joints in *gazebo_ros2_control* you should define its parameters to your URDF. +We should include: + +* ```` tag to the mimicked joint `detailed manual `__ +* ``mimic`` and ``multiplier`` parameters to joint definition in ```` tag + +.. code-block:: xml + + + + + + + + + + + +.. code-block:: xml + + + right_finger_joint + 1 + + + + + + + +Add the gazebo_ros2_control plugin +========================================== + +In addition to the *ros2_control* tags, a Gazebo plugin needs to be added to your URDF that +actually parses the *ros2_control* tags and loads the appropriate hardware interfaces and +controller manager. By default the *gazebo_ros2_control* plugin is very simple, though it is also +extensible via an additional plugin architecture to allow power users to create their own custom +robot hardware interfaces between *ros2_control* and Gazebo Classic. + +.. code-block:: xml + + + + robot_description + robot_state_publisher + $(find gazebo_ros2_control_demos)/config/cartpole_controller.yaml + + + +The *gazebo_ros2_control* ```` tag also has the following optional child elements: + +* ````: The location of the ``robot_description`` (URDF) on the parameter server, defaults to ``robot_description`` +* ````: Name of the node where the ``robot_param`` is located, defaults to ``robot_state_publisher`` +* ````: YAML file with the configuration of the controllers + +Default gazebo_ros2_control Behavior +----------------------------------------------------------- + +By default, without a ```` tag, *gazebo_ros2_control* will attempt to get all of the information it needs to interface with a ros2_control-based controller out of the URDF. This is sufficient for most cases, and good for at least getting started. + +The default behavior provides the following ros2_control interfaces: + +* hardware_interface::JointStateInterface +* hardware_interface::EffortJointInterface +* hardware_interface::VelocityJointInterface + +Advanced: custom gazebo_ros2_control Simulation Plugins +----------------------------------------------------------- + +The *gazebo_ros2_control* Gazebo plugin also provides a pluginlib-based interface to implement custom interfaces between Gazebo Classic and *ros2_control* for simulating more complex mechanisms (nonlinear springs, linkages, etc). + +These plugins must inherit ``gazebo_ros2_control::GazeboSystemInterface`` which implements a simulated *ros2_control* +``hardware_interface::SystemInterface``. SystemInterface provides API-level access to read and command joint properties. + +The respective GazeboSystemInterface sub-class is specified in a URDF model and is loaded when the +robot model is loaded. For example, the following XML will load the default plugin: + +.. code-block:: xml + + + + gazebo_ros2_control/GazeboSystem + + ... + + + + ... + + + + +Set up controllers +----------------------------------------------------------- + +Use the tag ```` inside ```` to set the YAML file with the controller configuration. + +.. code-block:: xml + + + + $(find gazebo_ros2_control_demos)/config/cartpole_controller.yaml + + + +This controller publishes the state of all resources registered to a +``hardware_interface::StateInterface`` to a topic of type ``sensor_msgs/msg/JointState``. +The following is a basic configuration of the controller. + +.. code-block:: yaml + + joint_state_controller: + ros__parameters: + type: joint_state_controller/JointStateController + + +This controller creates an action called ``/cart_pole_controller/follow_joint_trajectory`` of type ``control_msgs::action::FollowJointTrajectory``. + +.. code-block:: yaml + + cart_pole_controller: + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - slider_to_cart + write_op_modes: + - slider_to_cart + +gazebo_ros2_control_demos +========================================== + +This package contains the contents for testing gazebo_ros2_control. It is running Gazebo Classic and some other ROS 2 nodes. + +There are some examples in the *Gazebo_ros2_control_demos* package. These examples allow to launch a cart in a 30 meter rail. + +.. image:: img/cart.gif + :alt: Cart + +You can run some of the configuration running the following commands: + +.. code-block:: shell + + ros2 launch gazebo_ros2_control_demos cart_example_position.launch.py + ros2 launch gazebo_ros2_control_demos cart_example_velocity.launch.py + ros2 launch gazebo_ros2_control_demos cart_example_effort.launch.py + ros2 launch gazebo_ros2_control_demos diff_drive.launch.py + ros2 launch gazebo_ros2_control_demos tricycle_drive.launch.py + + +When the Gazebo world is launched you can run some of the following commands to move the cart. + +.. code-block:: shell + + ros2 run gazebo_ros2_control_demos example_position + ros2 run gazebo_ros2_control_demos example_velocity + ros2 run gazebo_ros2_control_demos example_effort + ros2 run gazebo_ros2_control_demos example_diff_drive + ros2 run gazebo_ros2_control_demos example_tricycle_drive + + +The following example shows parallel gripper with mimic joint: + +.. image:: img/gripper.gif + :alt: Cart + + +.. code-block:: shell + + ros2 launch gazebo_ros2_control_demos gripper_mimic_joint_example.launch.py + + +Send example commands: + + +.. code-block:: shell + + ros2 run gazebo_ros2_control_demos example_gripper + + +Gazebo Classic + Moveit2 + ROS 2 +========================================== + +This example works with `ROS 2 Foxy `__. +You should install Moveit2 from sources, the instructions are available in this `link `__. + +The repository with all the required packages are in the `gazebo_ros_demos `__. + +.. code-block:: shell + + ros2 launch rrbot_moveit_demo_nodes rrbot_demo.launch.py + + +.. image:: img/moveit2.gif + :alt: moveit2 From b6e86a211c71b44cbda6179d304f6a9be6080952 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 8 Jun 2023 23:30:31 +0200 Subject: [PATCH 21/58] Update github_url link (#210) --- doc/index.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/index.rst b/doc/index.rst index b4f98f98..f3aee796 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -1,4 +1,4 @@ -:github_url: https://github.com/ros-controls/gazebo_ros2_control/blob/{github_branch}/doc/index.rst +:github_url: https://github.com/ros-controls/gazebo_ros2_control/blob/{REPOS_FILE_BRANCH}/doc/index.rst .. _gazebo_ros2_control: From 419d16a7f41f78546a903da2735e81b2fa0e8081 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 9 Jun 2023 14:11:27 +0200 Subject: [PATCH 22/58] 0.6.1 --- gazebo_ros2_control/CHANGELOG.rst | 7 +++++++ gazebo_ros2_control/package.xml | 2 +- gazebo_ros2_control_demos/CHANGELOG.rst | 6 ++++++ gazebo_ros2_control_demos/package.xml | 2 +- 4 files changed, 15 insertions(+), 2 deletions(-) diff --git a/gazebo_ros2_control/CHANGELOG.rst b/gazebo_ros2_control/CHANGELOG.rst index c0ab7837..5feec522 100644 --- a/gazebo_ros2_control/CHANGELOG.rst +++ b/gazebo_ros2_control/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package gazebo_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.6.1 (2023-06-09) +------------------ +* Add pre-commit and CI-format (`#206 `_) + * Add pre-commit and ci-format +* Compile with ROS iron and rolling (`#202 `_) +* Contributors: Alejandro Hernández Cordero, Christoph Fröhlich + 0.6.0 (2023-05-23) ------------------ * add copy operator to SafeEnum (`#197 `_) diff --git a/gazebo_ros2_control/package.xml b/gazebo_ros2_control/package.xml index 8e78b995..93244eed 100644 --- a/gazebo_ros2_control/package.xml +++ b/gazebo_ros2_control/package.xml @@ -1,7 +1,7 @@ gazebo_ros2_control - 0.6.0 + 0.6.1 gazebo_ros2_control Alejandro Hernandez diff --git a/gazebo_ros2_control_demos/CHANGELOG.rst b/gazebo_ros2_control_demos/CHANGELOG.rst index b7fc4afc..717ed28c 100644 --- a/gazebo_ros2_control_demos/CHANGELOG.rst +++ b/gazebo_ros2_control_demos/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package gazebo_ros2_control_demos ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.6.1 (2023-06-09) +------------------ +* Add pre-commit and CI-format (`#206 `_) + * Add pre-commit and ci-format +* Contributors: Christoph Fröhlich + 0.6.0 (2023-05-23) ------------------ * Clean shutdown position example (`#196 `_) diff --git a/gazebo_ros2_control_demos/package.xml b/gazebo_ros2_control_demos/package.xml index 782f957b..29832af2 100644 --- a/gazebo_ros2_control_demos/package.xml +++ b/gazebo_ros2_control_demos/package.xml @@ -1,7 +1,7 @@ gazebo_ros2_control_demos - 0.6.0 + 0.6.1 gazebo_ros2_control_demos Alejandro Hernandez From 3e950618a1f82c72097f7c90a6b5d2ea2e32b7b8 Mon Sep 17 00:00:00 2001 From: gwalck Date: Fri, 23 Jun 2023 10:40:58 +0200 Subject: [PATCH 23/58] Forced zero vel in position mode to avoid sagging (#213) --- gazebo_ros2_control/src/gazebo_system.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index eaaa984b..f659f7f3 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -603,6 +603,7 @@ hardware_interface::return_type GazeboSystem::write( if (this->dataPtr->sim_joints_[j]) { if (this->dataPtr->joint_control_methods_[j] & POSITION) { this->dataPtr->sim_joints_[j]->SetPosition(0, this->dataPtr->joint_position_cmd_[j], true); + this->dataPtr->sim_joints_[j]->SetVelocity(0, 0.0); } else if (this->dataPtr->joint_control_methods_[j] & VELOCITY) { // NOLINT this->dataPtr->sim_joints_[j]->SetVelocity(0, this->dataPtr->joint_velocity_cmd_[j]); } else if (this->dataPtr->joint_control_methods_[j] & EFFORT) { // NOLINT From c15af63cb036cd1f36cffbc56e5e5bdb5224c7e2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 23 Jun 2023 19:56:35 +0200 Subject: [PATCH 24/58] Remove plugin export from ROS 1 (#212) --- gazebo_ros2_control/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/gazebo_ros2_control/package.xml b/gazebo_ros2_control/package.xml index 93244eed..0c9131a9 100644 --- a/gazebo_ros2_control/package.xml +++ b/gazebo_ros2_control/package.xml @@ -34,6 +34,5 @@ ament_cmake - From 6458276ca1eaf3ee646afa8e0833386f9785e585 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 1 Jul 2023 14:19:46 +0200 Subject: [PATCH 25/58] Add CI workflow to check docs (#216) --- .github/workflows/ci-check-docs.yml | 12 ++++++++++++ 1 file changed, 12 insertions(+) create mode 100644 .github/workflows/ci-check-docs.yml diff --git a/.github/workflows/ci-check-docs.yml b/.github/workflows/ci-check-docs.yml new file mode 100644 index 00000000..7d281ca4 --- /dev/null +++ b/.github/workflows/ci-check-docs.yml @@ -0,0 +1,12 @@ +name: Check Docs + +on: + workflow_dispatch: + pull_request: + +jobs: + check-docs: + name: Check Docs + uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@master + with: + GAZEBO_ROS2_CONTROL_PR: ${{ github.ref }} From ddc61d54b745239e78d3e3bd637a55ecc3beb07e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 14 Aug 2023 12:18:42 +0200 Subject: [PATCH 26/58] Remove non-working examples (#222) --- doc/index.rst | 17 ----------------- 1 file changed, 17 deletions(-) diff --git a/doc/index.rst b/doc/index.rst index f3aee796..829f71da 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -278,20 +278,3 @@ Send example commands: .. code-block:: shell ros2 run gazebo_ros2_control_demos example_gripper - - -Gazebo Classic + Moveit2 + ROS 2 -========================================== - -This example works with `ROS 2 Foxy `__. -You should install Moveit2 from sources, the instructions are available in this `link `__. - -The repository with all the required packages are in the `gazebo_ros_demos `__. - -.. code-block:: shell - - ros2 launch rrbot_moveit_demo_nodes rrbot_demo.launch.py - - -.. image:: img/moveit2.gif - :alt: moveit2 From 174e6b85f82774e9e802a5540382999066734421 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 15 Aug 2023 06:21:58 +0100 Subject: [PATCH 27/58] Removed unused var (#220) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- gazebo_ros2_control/src/gazebo_system.cpp | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index f659f7f3..b325d46b 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -100,9 +100,6 @@ class gazebo_ros2_control::GazeboSystemPrivate /// \brief mapping of mimicked joints to index of joint they mimic std::vector mimic_joints_; - - /// \brief Gain which converts position error to a velocity command - double position_proportional_gain_; }; namespace gazebo_ros2_control @@ -128,17 +125,6 @@ bool GazeboSystem::initSim( return false; } - constexpr double default_gain = 0.1; - if (!this->nh_->get_parameter_or( - "position_proportional_gain", - this->dataPtr->position_proportional_gain_, default_gain)) - { - RCLCPP_WARN_STREAM( - this->nh_->get_logger(), - "The position_proportional_gain parameter was not defined, defaulting to: " << - default_gain); - } - registerJoints(hardware_info, parent_model); registerSensors(hardware_info, parent_model); From c915939bfc13a43b2ab0e30029725f6c8023f3ca Mon Sep 17 00:00:00 2001 From: "David V. Lu!!" Date: Tue, 15 Aug 2023 03:28:09 -0400 Subject: [PATCH 28/58] Update diff_drive_controller.yaml (#224) The wrong base frame is set. The name of the link in the URDF is chassis. --- gazebo_ros2_control_demos/config/diff_drive_controller.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo_ros2_control_demos/config/diff_drive_controller.yaml b/gazebo_ros2_control_demos/config/diff_drive_controller.yaml index d14a6e3f..52e49dc6 100644 --- a/gazebo_ros2_control_demos/config/diff_drive_controller.yaml +++ b/gazebo_ros2_control_demos/config/diff_drive_controller.yaml @@ -24,7 +24,7 @@ diff_drive_base_controller: publish_rate: 50.0 odom_frame_id: odom - base_frame_id: base_link + base_frame_id: chassis pose_covariance_diagonal : [0.001, 0.001, 0.0, 0.0, 0.0, 0.01] twist_covariance_diagonal: [0.001, 0.0, 0.0, 0.0, 0.0, 0.01] From 6da415cf82a75e2a5e9f9a41400957ad45b2be84 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 15 Aug 2023 18:26:04 +0100 Subject: [PATCH 29/58] Set the C++ version to 17 (#221) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- gazebo_ros2_control/CMakeLists.txt | 6 ++++-- gazebo_ros2_control_demos/CMakeLists.txt | 5 +++-- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/gazebo_ros2_control/CMakeLists.txt b/gazebo_ros2_control/CMakeLists.txt index 1dffa02a..cfba1298 100644 --- a/gazebo_ros2_control/CMakeLists.txt +++ b/gazebo_ros2_control/CMakeLists.txt @@ -11,10 +11,12 @@ find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(yaml_cpp_vendor REQUIRED) -# Default to C++14 +# Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) endif() + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wconversion -Wno-sign-conversion -Wpedantic -Wnon-virtual-dtor -Woverloaded-virtual) endif() diff --git a/gazebo_ros2_control_demos/CMakeLists.txt b/gazebo_ros2_control_demos/CMakeLists.txt index 92f49653..6823378a 100644 --- a/gazebo_ros2_control_demos/CMakeLists.txt +++ b/gazebo_ros2_control_demos/CMakeLists.txt @@ -5,9 +5,10 @@ project(gazebo_ros2_control_demos) if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 11) endif() -# Default to C++14 +# Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) endif() if(NOT WIN32) From f8a475d3092e67b77846d76738ffad0861c680c1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 18 Aug 2023 16:29:15 +0200 Subject: [PATCH 30/58] Catch pluginlib exceptions (#229) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero --- .../src/gazebo_ros2_control_plugin.cpp | 25 ++++++++++++++++--- 1 file changed, 21 insertions(+), 4 deletions(-) diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp index 49d85b7c..b0a8bb07 100644 --- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -288,10 +288,24 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element for (unsigned int i = 0; i < control_hardware_info.size(); i++) { std::string robot_hw_sim_type_str_ = control_hardware_info[i].hardware_plugin_name; - auto gazeboSystem = std::unique_ptr( - impl_->robot_hw_sim_loader_->createUnmanagedInstance(robot_hw_sim_type_str_)); - - rclcpp::Node::SharedPtr node_ros2 = std::dynamic_pointer_cast(impl_->model_nh_); + RCLCPP_DEBUG( + impl_->model_nh_->get_logger(), "Load hardware interface %s ...", + robot_hw_sim_type_str_.c_str()); + std::unique_ptr gazeboSystem; + try { + gazeboSystem = std::unique_ptr( + impl_->robot_hw_sim_loader_->createUnmanagedInstance(robot_hw_sim_type_str_)); + } catch (pluginlib::PluginlibException & ex) { + RCLCPP_ERROR( + impl_->model_nh_->get_logger(), "The plugin failed to load for some reason. Error: %s\n", + ex.what()); + continue; + } + rclcpp::Node::SharedPtr node_ros2 = std::dynamic_pointer_cast( + impl_->model_nh_); + RCLCPP_DEBUG( + impl_->model_nh_->get_logger(), "Loaded hardware interface %s!", + robot_hw_sim_type_str_.c_str()); if (!gazeboSystem->initSim( node_ros2, impl_->parent_model_, @@ -302,6 +316,9 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element impl_->model_nh_->get_logger(), "Could not initialize robot simulation interface"); return; } + RCLCPP_DEBUG( + impl_->model_nh_->get_logger(), "Initialized robot simulation interface %s!", + robot_hw_sim_type_str_.c_str()); resource_manager_->import_component(std::move(gazeboSystem), control_hardware_info[i]); From 9c60869279f2d0f62b408dbd8b3c2a4aed9dab9f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 23 Aug 2023 09:56:48 +0200 Subject: [PATCH 31/58] Changelog MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- gazebo_ros2_control/CHANGELOG.rst | 10 ++++++++++ gazebo_ros2_control_demos/CHANGELOG.rst | 7 +++++++ 2 files changed, 17 insertions(+) diff --git a/gazebo_ros2_control/CHANGELOG.rst b/gazebo_ros2_control/CHANGELOG.rst index 5feec522..200365dc 100644 --- a/gazebo_ros2_control/CHANGELOG.rst +++ b/gazebo_ros2_control/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package gazebo_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.6.2 (2023-08-23) +------------------ +* Catch pluginlib exceptions (`#229 `_) + Co-authored-by: Alejandro Hernández Cordero +* Set the C++ version to 17 (`#221 `_) +* Removed unused var (`#220 `_) +* Remove plugin export from ROS 1 (`#212 `_) +* Forced zero vel in position mode to avoid sagging (`#213 `_) +* Contributors: Alejandro Hernández Cordero, Christoph Fröhlich, gwalck + 0.6.1 (2023-06-09) ------------------ * Add pre-commit and CI-format (`#206 `_) diff --git a/gazebo_ros2_control_demos/CHANGELOG.rst b/gazebo_ros2_control_demos/CHANGELOG.rst index 717ed28c..bd7d85f3 100644 --- a/gazebo_ros2_control_demos/CHANGELOG.rst +++ b/gazebo_ros2_control_demos/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package gazebo_ros2_control_demos ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.6.2 (2023-08-23) +------------------ +* Set the C++ version to 17 (`#221 `_) +* Update diff_drive_controller.yaml (`#224 `_) + The wrong base frame is set. The name of the link in the URDF is chassis. +* Contributors: Alejandro Hernández Cordero, David V. Lu!! + 0.6.1 (2023-06-09) ------------------ * Add pre-commit and CI-format (`#206 `_) From 4afa57b3ba47b0a44f64d5b23535dbbc4673f8e9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 23 Aug 2023 09:57:00 +0200 Subject: [PATCH 32/58] 0.6.2 --- gazebo_ros2_control/package.xml | 2 +- gazebo_ros2_control_demos/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gazebo_ros2_control/package.xml b/gazebo_ros2_control/package.xml index 0c9131a9..ca5d9107 100644 --- a/gazebo_ros2_control/package.xml +++ b/gazebo_ros2_control/package.xml @@ -1,7 +1,7 @@ gazebo_ros2_control - 0.6.1 + 0.6.2 gazebo_ros2_control Alejandro Hernandez diff --git a/gazebo_ros2_control_demos/package.xml b/gazebo_ros2_control_demos/package.xml index 29832af2..84a207f5 100644 --- a/gazebo_ros2_control_demos/package.xml +++ b/gazebo_ros2_control_demos/package.xml @@ -1,7 +1,7 @@ gazebo_ros2_control_demos - 0.6.1 + 0.6.2 gazebo_ros2_control_demos Alejandro Hernandez From 934621ee236dc9b32350b113c6b42a894bfbf092 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 6 Sep 2023 21:50:08 +0200 Subject: [PATCH 33/58] Cleanup controller config (#232) * Remove wrong yaml entries * Rename effort_controller --- .../config/cartpole_controller_effort.yaml | 10 ++-------- .../config/cartpole_controller_velocity.yaml | 6 +----- gazebo_ros2_control_demos/examples/example_effort.cpp | 2 +- .../launch/cart_example_effort.launch.py | 2 +- 4 files changed, 5 insertions(+), 15 deletions(-) diff --git a/gazebo_ros2_control_demos/config/cartpole_controller_effort.yaml b/gazebo_ros2_control_demos/config/cartpole_controller_effort.yaml index 8ea3c67c..baf14bc3 100644 --- a/gazebo_ros2_control_demos/config/cartpole_controller_effort.yaml +++ b/gazebo_ros2_control_demos/config/cartpole_controller_effort.yaml @@ -2,19 +2,13 @@ controller_manager: ros__parameters: update_rate: 100 # Hz - effort_controllers: + effort_controller: type: effort_controllers/JointGroupEffortController joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster -effort_controllers: +effort_controller: ros__parameters: joints: - slider_to_cart - command_interfaces: - - effort - state_interfaces: - - position - - velocity - - effort diff --git a/gazebo_ros2_control_demos/config/cartpole_controller_velocity.yaml b/gazebo_ros2_control_demos/config/cartpole_controller_velocity.yaml index 6a9e240d..cea5cc9e 100644 --- a/gazebo_ros2_control_demos/config/cartpole_controller_velocity.yaml +++ b/gazebo_ros2_control_demos/config/cartpole_controller_velocity.yaml @@ -15,11 +15,7 @@ velocity_controller: ros__parameters: joints: - slider_to_cart - command_interfaces: - - velocity - state_interfaces: - - position - - velocity + imu_sensor_broadcaster: ros__parameters: sensor_name: cart_imu_sensor diff --git a/gazebo_ros2_control_demos/examples/example_effort.cpp b/gazebo_ros2_control_demos/examples/example_effort.cpp index dd4bff8b..ef76a603 100644 --- a/gazebo_ros2_control_demos/examples/example_effort.cpp +++ b/gazebo_ros2_control_demos/examples/example_effort.cpp @@ -29,7 +29,7 @@ int main(int argc, char * argv[]) node = std::make_shared("effort_test_node"); auto publisher = node->create_publisher( - "/effort_controllers/commands", 10); + "/effort_controller/commands", 10); RCLCPP_INFO(node->get_logger(), "node created"); diff --git a/gazebo_ros2_control_demos/launch/cart_example_effort.launch.py b/gazebo_ros2_control_demos/launch/cart_example_effort.launch.py index 3e6c91b8..95caf297 100644 --- a/gazebo_ros2_control_demos/launch/cart_example_effort.launch.py +++ b/gazebo_ros2_control_demos/launch/cart_example_effort.launch.py @@ -63,7 +63,7 @@ def generate_launch_description(): ) load_joint_trajectory_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'effort_controllers'], + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'effort_controller'], output='screen' ) From 3db53a2770ddbe75f09ac32c0ec1ce65aa015e34 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Mon, 11 Sep 2023 08:51:35 +0200 Subject: [PATCH 34/58] Bump actions/checkout from 3 to 4 (#234) --- .github/workflows/ci-format.yaml | 2 +- .github/workflows/ci.yaml | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ci-format.yaml b/.github/workflows/ci-format.yaml index c26fc77a..0712e70b 100644 --- a/.github/workflows/ci-format.yaml +++ b/.github/workflows/ci-format.yaml @@ -12,7 +12,7 @@ jobs: name: Format runs-on: ubuntu-22.04 steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: actions/setup-python@v4 with: python-version: 3.10.6 diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index dd473e30..7c93d08f 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -11,7 +11,7 @@ jobs: container: image: osrf/ros:rolling-desktop steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Setup colcon workspace id: configure run: | @@ -46,7 +46,7 @@ jobs: container: image: osrf/ros:rolling-desktop steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Setup colcon workspace id: configure run: | From 12dad6355f0a1239a0abdfe3011bde31740f56ca Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 25 Sep 2023 19:46:55 +0200 Subject: [PATCH 35/58] Activate CI for humble (#223) --- .github/workflows/ci.yaml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 7c93d08f..99ca38aa 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -3,7 +3,9 @@ name: Gazebo ros2 control CI on: pull_request: push: - branches: [ master ] + branches: + - master + - humble jobs: build: From 5dba0f95a03b136f39145c846ec5ebbfb5a09599 Mon Sep 17 00:00:00 2001 From: Johannes Huemer Date: Wed, 4 Oct 2023 10:41:20 +0200 Subject: [PATCH 36/58] Fix stuck passive joints (#237) --- gazebo_ros2_control/src/gazebo_system.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index b325d46b..a9292bd9 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -59,6 +59,9 @@ class gazebo_ros2_control::GazeboSystemPrivate /// \brief vector with the control method defined in the URDF for each joint. std::vector joint_control_methods_; + /// \brief vector with indication for actuated joints (vs. passive joints) + std::vector is_joint_actuated_; + /// \brief handles to the joints from within Gazebo std::vector sim_joints_; @@ -144,6 +147,7 @@ void GazeboSystem::registerJoints( this->dataPtr->joint_names_.resize(this->dataPtr->n_dof_); this->dataPtr->joint_control_methods_.resize(this->dataPtr->n_dof_); + this->dataPtr->is_joint_actuated_.resize(this->dataPtr->n_dof_); this->dataPtr->joint_position_.resize(this->dataPtr->n_dof_); this->dataPtr->joint_velocity_.resize(this->dataPtr->n_dof_); this->dataPtr->joint_effort_.resize(this->dataPtr->n_dof_); @@ -293,6 +297,9 @@ void GazeboSystem::registerJoints( this->dataPtr->sim_joints_[j]->SetForce(0, initial_effort); } } + + // check if joint is actuated (has command interfaces) or passive + this->dataPtr->is_joint_actuated_[j] = (joint_info.command_interfaces.size() > 0); } } @@ -594,8 +601,8 @@ hardware_interface::return_type GazeboSystem::write( this->dataPtr->sim_joints_[j]->SetVelocity(0, this->dataPtr->joint_velocity_cmd_[j]); } else if (this->dataPtr->joint_control_methods_[j] & EFFORT) { // NOLINT this->dataPtr->sim_joints_[j]->SetForce(0, this->dataPtr->joint_effort_cmd_[j]); - } else { - // Fallback case is a velocity command of zero + } else if (this->dataPtr->is_joint_actuated_[j]) { + // Fallback case is a velocity command of zero (only for actuated joints) this->dataPtr->sim_joints_[j]->SetVelocity(0, 0.0); } } From f991075a672a26d42c49504e07f8dbb46dfcbb4a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 19 Nov 2023 10:26:54 +0100 Subject: [PATCH 37/58] Replace double quotes with single ones (#243) --- .../launch/tricycle_drive.launch.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/gazebo_ros2_control_demos/launch/tricycle_drive.launch.py b/gazebo_ros2_control_demos/launch/tricycle_drive.launch.py index f9a905fe..1a3cf07c 100644 --- a/gazebo_ros2_control_demos/launch/tricycle_drive.launch.py +++ b/gazebo_ros2_control_demos/launch/tricycle_drive.launch.py @@ -69,13 +69,13 @@ def generate_launch_description(): ) rviz = Node( - package="rviz2", - executable="rviz2", + package='rviz2', + executable='rviz2', arguments=[ - "-d", - os.path.join(gazebo_ros2_control_demos_path, "config/config.rviz"), + '-d', + os.path.join(gazebo_ros2_control_demos_path, 'config/config.rviz'), ], - output="screen", + output='screen', ) return LaunchDescription([ From 3c8f5bb4a4cc5eb01240afdbda9094f3ea3263ec Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Mon, 11 Dec 2023 10:10:05 +0100 Subject: [PATCH 38/58] Bump actions/setup-python from 4 to 5 (#246) Bumps [actions/setup-python](https://github.com/actions/setup-python) from 4 to 5. - [Release notes](https://github.com/actions/setup-python/releases) - [Commits](https://github.com/actions/setup-python/compare/v4...v5) --- updated-dependencies: - dependency-name: actions/setup-python dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-format.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-format.yaml b/.github/workflows/ci-format.yaml index 0712e70b..4db51ac9 100644 --- a/.github/workflows/ci-format.yaml +++ b/.github/workflows/ci-format.yaml @@ -13,7 +13,7 @@ jobs: runs-on: ubuntu-22.04 steps: - uses: actions/checkout@v4 - - uses: actions/setup-python@v4 + - uses: actions/setup-python@v5 with: python-version: 3.10.6 - name: Install system hooks From 7682dced640efcd67211e085cefbca23e2d1dab5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 1 Jan 2024 18:52:29 +0100 Subject: [PATCH 39/58] Add `hold_joints` parameter (#251) --- doc/index.rst | 1 + .../src/gazebo_ros2_control_plugin.cpp | 23 +++++++++++++++ gazebo_ros2_control/src/gazebo_system.cpp | 29 ++++++++++++++++++- 3 files changed, 52 insertions(+), 1 deletion(-) diff --git a/doc/index.rst b/doc/index.rst index 829f71da..235ee91d 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -155,6 +155,7 @@ The *gazebo_ros2_control* ```` tag also has the following optional child * ````: The location of the ``robot_description`` (URDF) on the parameter server, defaults to ``robot_description`` * ````: Name of the node where the ``robot_param`` is located, defaults to ``robot_state_publisher`` * ````: YAML file with the configuration of the controllers +* ````: if set to true (default), it will hold the joints' position if their interface was not claimed, e.g., the controller hasn't been activated yet. Default gazebo_ros2_control Behavior ----------------------------------------------------------- diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp index b0a8bb07..2008cacd 100644 --- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -182,6 +182,12 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element } else { impl_->robot_description_node_ = "robot_state_publisher"; // default } + // Hold joints if no control mode is active? + bool hold_joints = true; // default + if (sdf->HasElement("hold_joints")) { + hold_joints = + sdf->GetElement("hold_joints")->Get(); + } // There's currently no direct way to set parameters to the plugin's node // So we have to parse the plugin file manually and set it to the node's context. @@ -306,6 +312,23 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element RCLCPP_DEBUG( impl_->model_nh_->get_logger(), "Loaded hardware interface %s!", robot_hw_sim_type_str_.c_str()); + try { + node_ros2->declare_parameter("hold_joints", rclcpp::ParameterValue(hold_joints)); + } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException & e) { + RCLCPP_ERROR( + impl_->model_nh_->get_logger(), "Parameter 'hold_joints' has already been declared, %s", + e.what()); + } catch (const rclcpp::exceptions::InvalidParametersException & e) { + RCLCPP_ERROR( + impl_->model_nh_->get_logger(), "Parameter 'hold_joints' has invalid name, %s", e.what()); + } catch (const rclcpp::exceptions::InvalidParameterValueException & e) { + RCLCPP_ERROR( + impl_->model_nh_->get_logger(), "Parameter 'hold_joints' value is invalid, %s", e.what()); + } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + RCLCPP_ERROR( + impl_->model_nh_->get_logger(), "Parameter 'hold_joints' value has wrong type, %s", + e.what()); + } if (!gazeboSystem->initSim( node_ros2, impl_->parent_model_, diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index a9292bd9..af3e73be 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -103,6 +103,9 @@ class gazebo_ros2_control::GazeboSystemPrivate /// \brief mapping of mimicked joints to index of joint they mimic std::vector mimic_joints_; + + // Should hold the joints if no control_mode is active + bool hold_joints_ = true; }; namespace gazebo_ros2_control @@ -128,6 +131,30 @@ bool GazeboSystem::initSim( return false; } + try { + this->dataPtr->hold_joints_ = this->nh_->get_parameter("hold_joints").as_bool(); + } catch (rclcpp::exceptions::ParameterUninitializedException & ex) { + RCLCPP_ERROR( + this->nh_->get_logger(), + "Parameter 'hold_joints' not initialized, with error %s", ex.what()); + RCLCPP_WARN_STREAM( + this->nh_->get_logger(), "Using default value: " << this->dataPtr->hold_joints_); + } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) { + RCLCPP_ERROR( + this->nh_->get_logger(), + "Parameter 'hold_joints' not declared, with error %s", ex.what()); + RCLCPP_WARN_STREAM( + this->nh_->get_logger(), "Using default value: " << this->dataPtr->hold_joints_); + } catch (rclcpp::ParameterTypeException & ex) { + RCLCPP_ERROR( + this->nh_->get_logger(), + "Parameter 'hold_joints' has wrong type: %s", ex.what()); + RCLCPP_WARN_STREAM( + this->nh_->get_logger(), "Using default value: " << this->dataPtr->hold_joints_); + } + RCLCPP_DEBUG_STREAM( + this->nh_->get_logger(), "hold_joints (system): " << this->dataPtr->hold_joints_ << std::endl); + registerJoints(hardware_info, parent_model); registerSensors(hardware_info, parent_model); @@ -601,7 +628,7 @@ hardware_interface::return_type GazeboSystem::write( this->dataPtr->sim_joints_[j]->SetVelocity(0, this->dataPtr->joint_velocity_cmd_[j]); } else if (this->dataPtr->joint_control_methods_[j] & EFFORT) { // NOLINT this->dataPtr->sim_joints_[j]->SetForce(0, this->dataPtr->joint_effort_cmd_[j]); - } else if (this->dataPtr->is_joint_actuated_[j]) { + } else if (this->dataPtr->is_joint_actuated_[j] && this->dataPtr->hold_joints_) { // Fallback case is a velocity command of zero (only for actuated joints) this->dataPtr->sim_joints_[j]->SetVelocity(0, 0.0); } From b39074a4a1adf8a9319a6d4378ac26e2aa9e298a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 1 Jan 2024 19:05:27 +0100 Subject: [PATCH 40/58] Rename cartpole (#252) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero --- doc/index.rst | 28 +++++-------------- ...e_controller.yaml => cart_controller.yaml} | 0 ...ffort.yaml => cart_controller_effort.yaml} | 0 ...ity.yaml => cart_controller_velocity.yaml} | 0 .../launch/cart_example_effort.launch.py | 8 +++--- .../launch/cart_example_position.launch.py | 8 +++--- .../launch/cart_example_velocity.launch.py | 8 +++--- .../launch/diff_drive.launch.py | 6 ++-- .../gripper_mimic_joint_example.launch.py | 6 ++-- .../launch/tricycle_drive.launch.py | 6 ++-- .../urdf/test_cart_effort.xacro.urdf | 8 ++---- .../urdf/test_cart_position.xacro.urdf | 8 ++---- .../urdf/test_cart_velocity.xacro.urdf | 8 ++---- 13 files changed, 34 insertions(+), 60 deletions(-) rename gazebo_ros2_control_demos/config/{cartpole_controller.yaml => cart_controller.yaml} (100%) rename gazebo_ros2_control_demos/config/{cartpole_controller_effort.yaml => cart_controller_effort.yaml} (100%) rename gazebo_ros2_control_demos/config/{cartpole_controller_velocity.yaml => cart_controller_velocity.yaml} (100%) diff --git a/doc/index.rst b/doc/index.rst index 235ee91d..ca0a4c82 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -146,7 +146,7 @@ robot hardware interfaces between *ros2_control* and Gazebo Classic. robot_description robot_state_publisher - $(find gazebo_ros2_control_demos)/config/cartpole_controller.yaml + $(find gazebo_ros2_control_demos)/config/cart_controller.yaml @@ -203,32 +203,18 @@ Use the tag ```` inside ```` to set the YAML file with the c - $(find gazebo_ros2_control_demos)/config/cartpole_controller.yaml + $(find gazebo_ros2_control_demos)/config/cart_controller.yaml -This controller publishes the state of all resources registered to a -``hardware_interface::StateInterface`` to a topic of type ``sensor_msgs/msg/JointState``. -The following is a basic configuration of the controller. +The following is a basic configuration of the controllers: -.. code-block:: yaml +- ``joint_state_broadcaster``: This controller publishes the state of all resources registered to a ``hardware_interface::StateInterface`` to a topic of type ``sensor_msgs/msg/JointState``. - joint_state_controller: - ros__parameters: - type: joint_state_controller/JointStateController +- ``joint_trajectory_controller``: This controller creates an action called ``/joint_trajectory_controller/follow_joint_trajectory`` of type ``control_msgs::action::FollowJointTrajectory``. - -This controller creates an action called ``/cart_pole_controller/follow_joint_trajectory`` of type ``control_msgs::action::FollowJointTrajectory``. - -.. code-block:: yaml - - cart_pole_controller: - ros__parameters: - type: joint_trajectory_controller/JointTrajectoryController - joints: - - slider_to_cart - write_op_modes: - - slider_to_cart +.. literalinclude:: ../gazebo_ros2_control_demos/config/cart_controller.yaml + :language: yaml gazebo_ros2_control_demos ========================================== diff --git a/gazebo_ros2_control_demos/config/cartpole_controller.yaml b/gazebo_ros2_control_demos/config/cart_controller.yaml similarity index 100% rename from gazebo_ros2_control_demos/config/cartpole_controller.yaml rename to gazebo_ros2_control_demos/config/cart_controller.yaml diff --git a/gazebo_ros2_control_demos/config/cartpole_controller_effort.yaml b/gazebo_ros2_control_demos/config/cart_controller_effort.yaml similarity index 100% rename from gazebo_ros2_control_demos/config/cartpole_controller_effort.yaml rename to gazebo_ros2_control_demos/config/cart_controller_effort.yaml diff --git a/gazebo_ros2_control_demos/config/cartpole_controller_velocity.yaml b/gazebo_ros2_control_demos/config/cart_controller_velocity.yaml similarity index 100% rename from gazebo_ros2_control_demos/config/cartpole_controller_velocity.yaml rename to gazebo_ros2_control_demos/config/cart_controller_velocity.yaml diff --git a/gazebo_ros2_control_demos/launch/cart_example_effort.launch.py b/gazebo_ros2_control_demos/launch/cart_example_effort.launch.py index 95caf297..7b0c5036 100644 --- a/gazebo_ros2_control_demos/launch/cart_example_effort.launch.py +++ b/gazebo_ros2_control_demos/launch/cart_example_effort.launch.py @@ -53,10 +53,10 @@ def generate_launch_description(): spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py', arguments=['-topic', 'robot_description', - '-entity', 'cartpole'], + '-entity', 'cart'], output='screen') - load_joint_state_controller = ExecuteProcess( + load_joint_state_broadcaster = ExecuteProcess( cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'joint_state_broadcaster'], output='screen' @@ -71,12 +71,12 @@ def generate_launch_description(): RegisterEventHandler( event_handler=OnProcessExit( target_action=spawn_entity, - on_exit=[load_joint_state_controller], + on_exit=[load_joint_state_broadcaster], ) ), RegisterEventHandler( event_handler=OnProcessExit( - target_action=load_joint_state_controller, + target_action=load_joint_state_broadcaster, on_exit=[load_joint_trajectory_controller], ) ), diff --git a/gazebo_ros2_control_demos/launch/cart_example_position.launch.py b/gazebo_ros2_control_demos/launch/cart_example_position.launch.py index d5aae851..b741e849 100644 --- a/gazebo_ros2_control_demos/launch/cart_example_position.launch.py +++ b/gazebo_ros2_control_demos/launch/cart_example_position.launch.py @@ -53,10 +53,10 @@ def generate_launch_description(): spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py', arguments=['-topic', 'robot_description', - '-entity', 'cartpole'], + '-entity', 'cart'], output='screen') - load_joint_state_controller = ExecuteProcess( + load_joint_state_broadcaster = ExecuteProcess( cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'joint_state_broadcaster'], output='screen' @@ -72,12 +72,12 @@ def generate_launch_description(): RegisterEventHandler( event_handler=OnProcessExit( target_action=spawn_entity, - on_exit=[load_joint_state_controller], + on_exit=[load_joint_state_broadcaster], ) ), RegisterEventHandler( event_handler=OnProcessExit( - target_action=load_joint_state_controller, + target_action=load_joint_state_broadcaster, on_exit=[load_joint_trajectory_controller], ) ), diff --git a/gazebo_ros2_control_demos/launch/cart_example_velocity.launch.py b/gazebo_ros2_control_demos/launch/cart_example_velocity.launch.py index c155e34a..ea853dee 100644 --- a/gazebo_ros2_control_demos/launch/cart_example_velocity.launch.py +++ b/gazebo_ros2_control_demos/launch/cart_example_velocity.launch.py @@ -52,10 +52,10 @@ def generate_launch_description(): spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py', arguments=['-topic', 'robot_description', - '-entity', 'cartpole'], + '-entity', 'cart'], output='screen') - load_joint_state_controller = ExecuteProcess( + load_joint_state_broadcaster = ExecuteProcess( cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'joint_state_broadcaster'], output='screen' @@ -75,12 +75,12 @@ def generate_launch_description(): RegisterEventHandler( event_handler=OnProcessExit( target_action=spawn_entity, - on_exit=[load_joint_state_controller], + on_exit=[load_joint_state_broadcaster], ) ), RegisterEventHandler( event_handler=OnProcessExit( - target_action=load_joint_state_controller, + target_action=load_joint_state_broadcaster, on_exit=[load_joint_trajectory_controller], ) ), diff --git a/gazebo_ros2_control_demos/launch/diff_drive.launch.py b/gazebo_ros2_control_demos/launch/diff_drive.launch.py index 97913bde..faa476db 100644 --- a/gazebo_ros2_control_demos/launch/diff_drive.launch.py +++ b/gazebo_ros2_control_demos/launch/diff_drive.launch.py @@ -56,7 +56,7 @@ def generate_launch_description(): '-entity', 'diffbot'], output='screen') - load_joint_state_controller = ExecuteProcess( + load_joint_state_broadcaster = ExecuteProcess( cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'joint_state_broadcaster'], output='screen' @@ -72,12 +72,12 @@ def generate_launch_description(): RegisterEventHandler( event_handler=OnProcessExit( target_action=spawn_entity, - on_exit=[load_joint_state_controller], + on_exit=[load_joint_state_broadcaster], ) ), RegisterEventHandler( event_handler=OnProcessExit( - target_action=load_joint_state_controller, + target_action=load_joint_state_broadcaster, on_exit=[load_diff_drive_base_controller], ) ), diff --git a/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py b/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py index ef3a7f75..85b65398 100644 --- a/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py +++ b/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py @@ -57,7 +57,7 @@ def generate_launch_description(): '-entity', 'gripper'], output='screen') - load_joint_state_controller = ExecuteProcess( + load_joint_state_broadcaster = ExecuteProcess( cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'joint_state_broadcaster'], output='screen' @@ -73,12 +73,12 @@ def generate_launch_description(): RegisterEventHandler( event_handler=OnProcessExit( target_action=spawn_entity, - on_exit=[load_joint_state_controller], + on_exit=[load_joint_state_broadcaster], ) ), RegisterEventHandler( event_handler=OnProcessExit( - target_action=load_joint_state_controller, + target_action=load_joint_state_broadcaster, on_exit=[load_gripper_controller], ) ), diff --git a/gazebo_ros2_control_demos/launch/tricycle_drive.launch.py b/gazebo_ros2_control_demos/launch/tricycle_drive.launch.py index 1a3cf07c..a857248a 100644 --- a/gazebo_ros2_control_demos/launch/tricycle_drive.launch.py +++ b/gazebo_ros2_control_demos/launch/tricycle_drive.launch.py @@ -56,7 +56,7 @@ def generate_launch_description(): '-entity', 'tricycle'], output='screen') - load_joint_state_controller = ExecuteProcess( + load_joint_state_broadcaster = ExecuteProcess( cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'joint_state_broadcaster'], output='screen' @@ -82,12 +82,12 @@ def generate_launch_description(): RegisterEventHandler( event_handler=OnProcessExit( target_action=spawn_entity, - on_exit=[load_joint_state_controller], + on_exit=[load_joint_state_broadcaster], ) ), RegisterEventHandler( event_handler=OnProcessExit( - target_action=load_joint_state_controller, + target_action=load_joint_state_broadcaster, on_exit=[load_tricycle_controller], ) ), diff --git a/gazebo_ros2_control_demos/urdf/test_cart_effort.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_cart_effort.xacro.urdf index 523bca36..35d13c12 100644 --- a/gazebo_ros2_control_demos/urdf/test_cart_effort.xacro.urdf +++ b/gazebo_ros2_control_demos/urdf/test_cart_effort.xacro.urdf @@ -1,9 +1,5 @@ - - - - - + @@ -68,7 +64,7 @@ - $(find gazebo_ros2_control_demos)/config/cartpole_controller_effort.yaml + $(find gazebo_ros2_control_demos)/config/cart_controller_effort.yaml diff --git a/gazebo_ros2_control_demos/urdf/test_cart_position.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_cart_position.xacro.urdf index be1829d0..8ab12f82 100644 --- a/gazebo_ros2_control_demos/urdf/test_cart_position.xacro.urdf +++ b/gazebo_ros2_control_demos/urdf/test_cart_position.xacro.urdf @@ -1,9 +1,5 @@ - - - - - + @@ -67,7 +63,7 @@ - $(find gazebo_ros2_control_demos)/config/cartpole_controller.yaml + $(find gazebo_ros2_control_demos)/config/cart_controller.yaml diff --git a/gazebo_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf index 08e28d1b..09939e8d 100644 --- a/gazebo_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf +++ b/gazebo_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf @@ -1,9 +1,5 @@ - - - - - + @@ -97,7 +93,7 @@ - $(find gazebo_ros2_control_demos)/config/cartpole_controller_velocity.yaml + $(find gazebo_ros2_control_demos)/config/cart_controller_velocity.yaml From 6bf5f06ce402fbef4e823557f015e1b119dc83d0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 2 Jan 2024 19:50:14 +0100 Subject: [PATCH 41/58] replace Twist with TwistStamped (#249) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../config/diff_drive_controller.yaml | 2 +- .../config/tricycle_drive_controller.yaml | 2 +- .../examples/example_diff_drive.cpp | 22 ++++++++++--------- .../examples/example_tricycle_drive.cpp | 20 +++++++++-------- 4 files changed, 25 insertions(+), 21 deletions(-) diff --git a/gazebo_ros2_control_demos/config/diff_drive_controller.yaml b/gazebo_ros2_control_demos/config/diff_drive_controller.yaml index 52e49dc6..fdc04e13 100644 --- a/gazebo_ros2_control_demos/config/diff_drive_controller.yaml +++ b/gazebo_ros2_control_demos/config/diff_drive_controller.yaml @@ -33,7 +33,7 @@ diff_drive_base_controller: cmd_vel_timeout: 0.5 #publish_limited_velocity: true - use_stamped_vel: false + use_stamped_vel: true #velocity_rolling_window_size: 10 # Velocity and acceleration limits diff --git a/gazebo_ros2_control_demos/config/tricycle_drive_controller.yaml b/gazebo_ros2_control_demos/config/tricycle_drive_controller.yaml index 5083a29c..c94b81d5 100644 --- a/gazebo_ros2_control_demos/config/tricycle_drive_controller.yaml +++ b/gazebo_ros2_control_demos/config/tricycle_drive_controller.yaml @@ -51,7 +51,7 @@ tricycle_controller: # cmd_vel input cmd_vel_timeout: 500 # In milliseconds. Timeout to stop if no cmd_vel is received - use_stamped_vel: false # Set to True if using TwistStamped. + use_stamped_vel: true # Set to True if using TwistStamped. # Debug publish_ackermann_command: true # Publishes AckermannDrive. The speed does not comply to the msg definition, it the wheel angular speed in rad/s. diff --git a/gazebo_ros2_control_demos/examples/example_diff_drive.cpp b/gazebo_ros2_control_demos/examples/example_diff_drive.cpp index a3c117ff..5439073f 100644 --- a/gazebo_ros2_control_demos/examples/example_diff_drive.cpp +++ b/gazebo_ros2_control_demos/examples/example_diff_drive.cpp @@ -16,7 +16,7 @@ #include -#include +#include using namespace std::chrono_literals; @@ -27,20 +27,22 @@ int main(int argc, char * argv[]) std::shared_ptr node = std::make_shared("diff_drive_test_node"); - auto publisher = node->create_publisher( - "/diff_drive_base_controller/cmd_vel_unstamped", 10); + auto publisher = node->create_publisher( + "/tricycle_controller/cmd_vel", 10); RCLCPP_INFO(node->get_logger(), "node created"); - geometry_msgs::msg::Twist command; + geometry_msgs::msg::TwistStamped command; - command.linear.x = 0.2; - command.linear.y = 0.0; - command.linear.z = 0.0; + command.header.stamp = node->now(); - command.angular.x = 0.0; - command.angular.y = 0.0; - command.angular.z = 0.1; + command.twist.linear.x = 0.2; + command.twist.linear.y = 0.0; + command.twist.linear.z = 0.0; + + command.twist.angular.x = 0.0; + command.twist.angular.y = 0.0; + command.twist.angular.z = 0.1; while (1) { publisher->publish(command); diff --git a/gazebo_ros2_control_demos/examples/example_tricycle_drive.cpp b/gazebo_ros2_control_demos/examples/example_tricycle_drive.cpp index 9713ff76..1783ce1e 100644 --- a/gazebo_ros2_control_demos/examples/example_tricycle_drive.cpp +++ b/gazebo_ros2_control_demos/examples/example_tricycle_drive.cpp @@ -16,7 +16,7 @@ #include -#include +#include using namespace std::chrono_literals; @@ -27,20 +27,22 @@ int main(int argc, char * argv[]) std::shared_ptr node = std::make_shared("tricycle_drive_test_node"); - auto publisher = node->create_publisher( + auto publisher = node->create_publisher( "/tricycle_controller/cmd_vel", 10); RCLCPP_INFO(node->get_logger(), "node created"); - geometry_msgs::msg::Twist command; + geometry_msgs::msg::TwistStamped command; - command.linear.x = 0.2; - command.linear.y = 0.0; - command.linear.z = 0.0; + command.header.stamp = node->now(); - command.angular.x = 0.0; - command.angular.y = 0.0; - command.angular.z = 0.1; + command.twist.linear.x = 0.2; + command.twist.linear.y = 0.0; + command.twist.linear.z = 0.0; + + command.twist.angular.x = 0.0; + command.twist.angular.y = 0.0; + command.twist.angular.z = 0.1; while (1) { publisher->publish(command); From 2b0ce94be7652bbe2ddc120770484ce0b3240e9b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 4 Jan 2024 11:55:37 +0100 Subject: [PATCH 42/58] Changelog MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- gazebo_ros2_control/CHANGELOG.rst | 6 ++++++ gazebo_ros2_control_demos/CHANGELOG.rst | 11 +++++++++++ 2 files changed, 17 insertions(+) diff --git a/gazebo_ros2_control/CHANGELOG.rst b/gazebo_ros2_control/CHANGELOG.rst index 200365dc..8406c968 100644 --- a/gazebo_ros2_control/CHANGELOG.rst +++ b/gazebo_ros2_control/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package gazebo_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.7.0 (2024-01-04) +------------------ +* Add `hold_joints` parameter (`#251 `_) +* Fix stuck passive joints (`#237 `_) +* Contributors: Christoph Fröhlich, Johannes Huemer + 0.6.2 (2023-08-23) ------------------ * Catch pluginlib exceptions (`#229 `_) diff --git a/gazebo_ros2_control_demos/CHANGELOG.rst b/gazebo_ros2_control_demos/CHANGELOG.rst index bd7d85f3..96d457ca 100644 --- a/gazebo_ros2_control_demos/CHANGELOG.rst +++ b/gazebo_ros2_control_demos/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package gazebo_ros2_control_demos ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.7.0 (2024-01-04) +------------------ +* replace Twist with TwistStamped (`#249 `_) +* Rename cartpole (`#252 `_) + Co-authored-by: Alejandro Hernández Cordero +* Replace double quotes with single ones (`#243 `_) +* Cleanup controller config (`#232 `_) + * Remove wrong yaml entries + * Rename effort_controller +* Contributors: Alejandro Hernández Cordero, Christoph Fröhlich + 0.6.2 (2023-08-23) ------------------ * Set the C++ version to 17 (`#221 `_) From 480abc5e83a4cddbdbfcf3ee52ad0c5536ef6571 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 4 Jan 2024 11:55:50 +0100 Subject: [PATCH 43/58] 0.7.0 --- gazebo_ros2_control/package.xml | 2 +- gazebo_ros2_control_demos/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gazebo_ros2_control/package.xml b/gazebo_ros2_control/package.xml index ca5d9107..29475aae 100644 --- a/gazebo_ros2_control/package.xml +++ b/gazebo_ros2_control/package.xml @@ -1,7 +1,7 @@ gazebo_ros2_control - 0.6.2 + 0.7.0 gazebo_ros2_control Alejandro Hernandez diff --git a/gazebo_ros2_control_demos/package.xml b/gazebo_ros2_control_demos/package.xml index 84a207f5..60458673 100644 --- a/gazebo_ros2_control_demos/package.xml +++ b/gazebo_ros2_control_demos/package.xml @@ -1,7 +1,7 @@ gazebo_ros2_control_demos - 0.6.2 + 0.7.0 gazebo_ros2_control_demos Alejandro Hernandez From 074617a61db57e06c33cb4a3a92a75864bb67a39 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 4 Jan 2024 11:59:32 +0100 Subject: [PATCH 44/58] Update readme and CI jobs for iron (#248) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero --- .github/dependabot.yml | 14 ++++ .github/workflows/ci-humble.yaml | 82 +++++++++++++++++++ .github/workflows/ci-iron.yaml | 82 +++++++++++++++++++ .../workflows/{ci.yaml => ci-rolling.yaml} | 5 +- README.md | 6 +- 5 files changed, 184 insertions(+), 5 deletions(-) create mode 100644 .github/workflows/ci-humble.yaml create mode 100644 .github/workflows/ci-iron.yaml rename .github/workflows/{ci.yaml => ci-rolling.yaml} (97%) diff --git a/.github/dependabot.yml b/.github/dependabot.yml index 05a48fc6..aafd67c2 100644 --- a/.github/dependabot.yml +++ b/.github/dependabot.yml @@ -11,3 +11,17 @@ updates: directory: "/" schedule: interval: "weekly" + - package-ecosystem: "github-actions" + # Workflow files stored in the + # default location of `.github/workflows` + directory: "/" + schedule: + interval: "weekly" + target-branch: "humble" + - package-ecosystem: "github-actions" + # Workflow files stored in the + # default location of `.github/workflows` + directory: "/" + schedule: + interval: "weekly" + target-branch: "iron" diff --git a/.github/workflows/ci-humble.yaml b/.github/workflows/ci-humble.yaml new file mode 100644 index 00000000..54a3b990 --- /dev/null +++ b/.github/workflows/ci-humble.yaml @@ -0,0 +1,82 @@ +name: gazebo_ros2_control CI - Humble + +on: + pull_request: + branches: + - humble + push: + branches: + - humble + +jobs: + build: + runs-on: ubuntu-latest + container: + image: osrf/ros:humble-desktop + steps: + - uses: actions/checkout@v4 + - name: Setup colcon workspace + id: configure + run: | + cd .. + mkdir -p /home/ros2_ws/src + cp -r gazebo_ros2_control /home/ros2_ws/src/ + apt-get update && apt-get upgrade -q -y + apt-get update && apt-get install -q -y --no-install-recommends \ + dirmngr \ + gnupg2 \ + lsb-release \ + python3-colcon-ros + cd /home/ros2_ws/src/ + rosdep update + rosdep install --from-paths ./ -i -y --rosdistro humble \ + --ignore-src + - name: Build project + id: build + run: | + cd /home/ros2_ws/ + . /opt/ros/humble/local_setup.sh + colcon build --packages-up-to gazebo_ros2_control_demos + - name: Run tests + id: test + run: | + cd /home/ros2_ws/ + . /opt/ros/humble/local_setup.sh + colcon test --event-handlers console_direct+ --packages-select gazebo_ros2_control gazebo_ros2_control_demos + colcon test-result + build_testing: + runs-on: ubuntu-latest + container: + image: osrf/ros:humble-desktop + steps: + - uses: actions/checkout@v4 + - name: Setup colcon workspace + id: configure + run: | + cd .. + mkdir -p /home/ros2_ws/src + cp -r gazebo_ros2_control /home/ros2_ws/src/ + sh -c 'echo "deb [arch=$(dpkg --print-architecture)] http://packages.ros.org/ros2-testing/ubuntu $(lsb_release -cs) main" > /etc/apt/sources.list.d/ros2-latest.list' + apt-get update && apt-get upgrade -q -y + apt-get update && apt-get install -q -y --no-install-recommends \ + dirmngr \ + gnupg2 \ + lsb-release \ + python3-colcon-ros + cd /home/ros2_ws/src/ + rosdep update + rosdep install --from-paths ./ -i -y --rosdistro humble \ + --ignore-src + - name: Build project + id: build + run: | + cd /home/ros2_ws/ + . /opt/ros/humble/local_setup.sh + colcon build --packages-up-to gazebo_ros2_control_demos + - name: Run tests + id: test + run: | + cd /home/ros2_ws/ + . /opt/ros/humble/local_setup.sh + colcon test --event-handlers console_direct+ --packages-select gazebo_ros2_control gazebo_ros2_control_demos + colcon test-result diff --git a/.github/workflows/ci-iron.yaml b/.github/workflows/ci-iron.yaml new file mode 100644 index 00000000..12fb68cc --- /dev/null +++ b/.github/workflows/ci-iron.yaml @@ -0,0 +1,82 @@ +name: gazebo_ros2_control CI - iron + +on: + pull_request: + branches: + - iron + push: + branches: + - iron + +jobs: + build: + runs-on: ubuntu-latest + container: + image: osrf/ros:iron-desktop + steps: + - uses: actions/checkout@v4 + - name: Setup colcon workspace + id: configure + run: | + cd .. + mkdir -p /home/ros2_ws/src + cp -r gazebo_ros2_control /home/ros2_ws/src/ + apt-get update && apt-get upgrade -q -y + apt-get update && apt-get install -q -y --no-install-recommends \ + dirmngr \ + gnupg2 \ + lsb-release \ + python3-colcon-ros + cd /home/ros2_ws/src/ + rosdep update + rosdep install --from-paths ./ -i -y --rosdistro iron \ + --ignore-src + - name: Build project + id: build + run: | + cd /home/ros2_ws/ + . /opt/ros/iron/local_setup.sh + colcon build --packages-up-to gazebo_ros2_control_demos + - name: Run tests + id: test + run: | + cd /home/ros2_ws/ + . /opt/ros/iron/local_setup.sh + colcon test --event-handlers console_direct+ --packages-select gazebo_ros2_control gazebo_ros2_control_demos + colcon test-result + build_testing: + runs-on: ubuntu-latest + container: + image: osrf/ros:iron-desktop + steps: + - uses: actions/checkout@v4 + - name: Setup colcon workspace + id: configure + run: | + cd .. + mkdir -p /home/ros2_ws/src + cp -r gazebo_ros2_control /home/ros2_ws/src/ + sh -c 'echo "deb [arch=$(dpkg --print-architecture)] http://packages.ros.org/ros2-testing/ubuntu $(lsb_release -cs) main" > /etc/apt/sources.list.d/ros2-latest.list' + apt-get update && apt-get upgrade -q -y + apt-get update && apt-get install -q -y --no-install-recommends \ + dirmngr \ + gnupg2 \ + lsb-release \ + python3-colcon-ros + cd /home/ros2_ws/src/ + rosdep update + rosdep install --from-paths ./ -i -y --rosdistro iron \ + --ignore-src + - name: Build project + id: build + run: | + cd /home/ros2_ws/ + . /opt/ros/iron/local_setup.sh + colcon build --packages-up-to gazebo_ros2_control_demos + - name: Run tests + id: test + run: | + cd /home/ros2_ws/ + . /opt/ros/iron/local_setup.sh + colcon test --event-handlers console_direct+ --packages-select gazebo_ros2_control gazebo_ros2_control_demos + colcon test-result diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci-rolling.yaml similarity index 97% rename from .github/workflows/ci.yaml rename to .github/workflows/ci-rolling.yaml index 99ca38aa..1c8f95ce 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci-rolling.yaml @@ -1,11 +1,12 @@ -name: Gazebo ros2 control CI +name: gazebo_ros2_control - Rolling on: pull_request: + branches: + - master push: branches: - master - - humble jobs: build: diff --git a/README.md b/README.md index ca4ea7b4..de2e262b 100644 --- a/README.md +++ b/README.md @@ -11,6 +11,6 @@ See the [documentation file](doc/index.rst) or [control.ros.org](https://control ROS 2 Distro | Branch | Build status | Documentation :----------: | :----: | :----------: | :-----------: -**Rolling** | [`master`](https://github.com/ros-controls/gazebo_ros2_control/tree/master) | [![Gazebo ros2 control CI](https://github.com/ros-controls/gazebo_ros2_control/actions/workflows/ci.yaml/badge.svg?branch=master)](https://github.com/ros-controls/gazebo_ros2_control/actions/workflows/ci.yaml) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) -**Iron** | [`master`](https://github.com/ros-controls/gazebo_ros2_control/tree/master) | [![Gazebo ros2 control CI](https://github.com/ros-controls/gazebo_ros2_control/actions/workflows/ci.yaml/badge.svg?branch=master)](https://github.com/ros-controls/gazebo_ros2_control/actions/workflows/ci.yaml) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) -**Humble** | [`humble`](https://github.com/ros-controls/gazebo_ros2_control/tree/humble) | [![Gazebo ros2 control CI](https://github.com/ros-controls/gazebo_ros2_control/actions/workflows/ci.yaml/badge.svg?branch=humble)](https://github.com/ros-controls/gazebo_ros2_control/actions/workflows/ci.yaml) | [Documentation](https://control.ros.org/humble/index.html)
[API Reference](https://control.ros.org/humble/doc/api/index.html) +**Rolling** | [`master`](https://github.com/ros-controls/gazebo_ros2_control/tree/master) | [![Gazebo ros2 control CI](https://github.com/ros-controls/gazebo_ros2_control/actions/workflows/ci-rolling.yaml/badge.svg?branch=master)](https://github.com/ros-controls/gazebo_ros2_control/actions/workflows/ci-rolling.yaml) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) +**Iron** | [`iron`](https://github.com/ros-controls/gazebo_ros2_control/tree/iron) | [![Gazebo ros2 control CI](https://github.com/ros-controls/gazebo_ros2_control/actions/workflows/ci-iron.yaml/badge.svg?branch=iron)](https://github.com/ros-controls/gazebo_ros2_control/actions/workflows/ci-iron.yaml) | [Documentation](https://control.ros.org/iron/index.html)
[API Reference](https://control.ros.org/iron/doc/api/index.html) +**Humble** | [`humble`](https://github.com/ros-controls/gazebo_ros2_control/tree/humble) | [![Gazebo ros2 control CI](https://github.com/ros-controls/gazebo_ros2_control/actions/workflows/ci-humble.yaml/badge.svg?branch=humble)](https://github.com/ros-controls/gazebo_ros2_control/actions/workflows/ci-humble.yaml) | [Documentation](https://control.ros.org/humble/index.html)
[API Reference](https://control.ros.org/humble/doc/api/index.html) From 295bf0843fcd9ae78558bcbd891253903abe8fca Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 10 Jan 2024 15:11:35 +0100 Subject: [PATCH 45/58] Added controller manager xml argument (#255) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- doc/index.rst | 2 ++ gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp | 9 ++++++++- 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/doc/index.rst b/doc/index.rst index ca0a4c82..daff5f2c 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -147,6 +147,7 @@ robot hardware interfaces between *ros2_control* and Gazebo Classic. robot_description robot_state_publisher $(find gazebo_ros2_control_demos)/config/cart_controller.yaml + simulation_controller_manager
@@ -156,6 +157,7 @@ The *gazebo_ros2_control* ```` tag also has the following optional child * ````: Name of the node where the ``robot_param`` is located, defaults to ``robot_state_publisher`` * ````: YAML file with the configuration of the controllers * ````: if set to true (default), it will hold the joints' position if their interface was not claimed, e.g., the controller hasn't been activated yet. +* ````: Set controller manager name (default: ``controller_manager``) Default gazebo_ros2_control Behavior ----------------------------------------------------------- diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp index 2008cacd..23368e2a 100644 --- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -189,6 +189,13 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element sdf->GetElement("hold_joints")->Get(); } + // Get controller manager node name + std::string controllerManagerNodeName{"controller_manager"}; + + if (sdf->HasElement("controller_manager_name")) { + controllerManagerNodeName = sdf->GetElement("controller_manager_name")->Get(); + } + // There's currently no direct way to set parameters to the plugin's node // So we have to parse the plugin file manually and set it to the node's context. auto rcl_context = impl_->model_nh_->get_node_base_interface()->get_context()->get_rcl_context(); @@ -360,7 +367,7 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element new controller_manager::ControllerManager( std::move(resource_manager_), impl_->executor_, - "controller_manager", + controllerManagerNodeName, impl_->model_nh_->get_namespace())); impl_->executor_->add_node(impl_->controller_manager_); From 60e4b75f4a786c9563d7dd97c6d08a31ca69c5b1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 10 Jan 2024 16:59:24 +0100 Subject: [PATCH 46/58] Rolling CI add sign from ROS2 packages (#261) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .github/workflows/ci-rolling.yaml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ci-rolling.yaml b/.github/workflows/ci-rolling.yaml index 1c8f95ce..096856de 100644 --- a/.github/workflows/ci-rolling.yaml +++ b/.github/workflows/ci-rolling.yaml @@ -56,7 +56,8 @@ jobs: cd .. mkdir -p /home/ros2_ws/src cp -r gazebo_ros2_control /home/ros2_ws/src/ - sh -c 'echo "deb [arch=$(dpkg --print-architecture)] http://packages.ros.org/ros2-testing/ubuntu $(lsb_release -cs) main" > /etc/apt/sources.list.d/ros2-latest.list' + curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg + sh -c 'echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2-testing/ubuntu $(lsb_release -cs) main" > /etc/apt/sources.list.d/ros2-latest.list' apt-get update && apt-get upgrade -q -y apt-get update && apt-get install -q -y --no-install-recommends \ dirmngr \ From d44b879615a539fc7c6c53707ec518df7bfd4f47 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Tue, 16 Jan 2024 23:09:08 +0100 Subject: [PATCH 47/58] Fix links in documentation (#263) --- README.md | 4 ++-- gazebo_ros2_control/README.md | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index de2e262b..b1b8d17d 100644 --- a/README.md +++ b/README.md @@ -1,11 +1,11 @@ # gazebo_ros2_control -This is a ROS 2 package for integrating the `ros2_control` controller architecture with the [Gazebo Classic](http://gazebosim.org/) simulator. +This is a ROS 2 package for integrating the `ros2_control` controller architecture with the [Gazebo Classic](https://classic.gazebosim.org/) simulator. This package provides a Gazebo plugin which instantiates a `ros2_control` controller manager and connects it to a Gazebo model. ## Documentation -See the [documentation file](doc/index.rst) or [control.ros.org](https://control.ros.org/master/doc/simulators/gazebo_ros2_control/doc/index.html) +See the [documentation file](doc/index.rst) or [control.ros.org](https://control.ros.org/master/doc/gazebo_ros2_control/doc/index.html). ## Build status diff --git a/gazebo_ros2_control/README.md b/gazebo_ros2_control/README.md index 0dd9548b..11a61a76 100644 --- a/gazebo_ros2_control/README.md +++ b/gazebo_ros2_control/README.md @@ -1,7 +1,7 @@ # Gazebo ros_control Interfaces This is a ROS 2 package for integrating the [ros2_control](https://github.com/ros-controls/ros2_control) controller architecture -with the [Gazebo](http://gazebosim.org/) simulator. +with the [Gazebo Classic](https://classic.gazebosim.org/) simulator. This package provides a Gazebo plugin which instantiates a ros_control controller manager and connects it to a Gazebo model. From cb9aa69d5928f90c2ee6fc54fa4f4a6b5fc055ee Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 22 Jan 2024 10:13:49 +0100 Subject: [PATCH 48/58] Use lexical casts (#260) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero --- gazebo_ros2_control/src/gazebo_system.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index af3e73be..16cd23d0 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -25,6 +25,7 @@ #include "gazebo/sensors/SensorManager.hh" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/lexical_casts.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" struct MimicJoint @@ -218,7 +219,7 @@ void GazeboSystem::registerJoints( hardware_info.joints.begin(), mimicked_joint_it); auto param_it = joint_info.parameters.find("multiplier"); if (param_it != joint_info.parameters.end()) { - mimic_joint.multiplier = std::stod(joint_info.parameters.at("multiplier")); + mimic_joint.multiplier = hardware_interface::stod(joint_info.parameters.at("multiplier")); } else { mimic_joint.multiplier = 1.0; } @@ -234,7 +235,7 @@ void GazeboSystem::registerJoints( auto get_initial_value = [this](const hardware_interface::InterfaceInfo & interface_info) { if (!interface_info.initial_value.empty()) { - double value = std::stod(interface_info.initial_value); + double value = hardware_interface::stod(interface_info.initial_value); RCLCPP_INFO(this->nh_->get_logger(), "\t\t\t found initial value: %f", value); return value; } else { From f5baf71c4c7cb3c0a0af52f988c107b356c95ed0 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 22 Jan 2024 10:15:39 +0100 Subject: [PATCH 49/58] Load the URDF to the resource_manager before parsing it to CM (#262) * Load the URDF to the resource_manager before parsing it to CM constructor (fixes https://github.com/ros-controls/ros2_control/issues/1299) --- gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp index 23368e2a..c1804254 100644 --- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -288,6 +288,13 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element std::unique_ptr resource_manager_ = std::make_unique(); + try { + resource_manager_->load_urdf(urdf_string, false, false); + } catch (...) { + // This error should be normal as the resource manager is not supposed to load and initialize + // them + RCLCPP_ERROR(impl_->model_nh_->get_logger(), "Error initializing URDF to resource manager!"); + } try { impl_->robot_hw_sim_loader_.reset( new pluginlib::ClassLoader( From 9b69ef22b157e1230e968856d8925d870943da44 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 23 Jan 2024 14:55:50 +0100 Subject: [PATCH 50/58] Fix sign CI (#268) (#270) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero (cherry picked from commit 07ceffd4f51675b5485ff72afc9da98a909676ea) Co-authored-by: Alejandro Hernández Cordero --- .github/workflows/ci-humble.yaml | 3 ++- .github/workflows/ci-iron.yaml | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci-humble.yaml b/.github/workflows/ci-humble.yaml index 54a3b990..e7187a6e 100644 --- a/.github/workflows/ci-humble.yaml +++ b/.github/workflows/ci-humble.yaml @@ -56,7 +56,8 @@ jobs: cd .. mkdir -p /home/ros2_ws/src cp -r gazebo_ros2_control /home/ros2_ws/src/ - sh -c 'echo "deb [arch=$(dpkg --print-architecture)] http://packages.ros.org/ros2-testing/ubuntu $(lsb_release -cs) main" > /etc/apt/sources.list.d/ros2-latest.list' + curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg + sh -c 'echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2-testing/ubuntu $(lsb_release -cs) main" > /etc/apt/sources.list.d/ros2-latest.list' apt-get update && apt-get upgrade -q -y apt-get update && apt-get install -q -y --no-install-recommends \ dirmngr \ diff --git a/.github/workflows/ci-iron.yaml b/.github/workflows/ci-iron.yaml index 12fb68cc..592c9b00 100644 --- a/.github/workflows/ci-iron.yaml +++ b/.github/workflows/ci-iron.yaml @@ -56,7 +56,8 @@ jobs: cd .. mkdir -p /home/ros2_ws/src cp -r gazebo_ros2_control /home/ros2_ws/src/ - sh -c 'echo "deb [arch=$(dpkg --print-architecture)] http://packages.ros.org/ros2-testing/ubuntu $(lsb_release -cs) main" > /etc/apt/sources.list.d/ros2-latest.list' + curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg + sh -c 'echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2-testing/ubuntu $(lsb_release -cs) main" > /etc/apt/sources.list.d/ros2-latest.list' apt-get update && apt-get upgrade -q -y apt-get update && apt-get install -q -y --no-install-recommends \ dirmngr \ From 7bb8695bc6d71393308885ac493cc32b77fc2e00 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 24 Jan 2024 10:55:06 +0100 Subject: [PATCH 51/58] Changelog MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- gazebo_ros2_control/CHANGELOG.rst | 10 ++++++++++ gazebo_ros2_control_demos/CHANGELOG.rst | 3 +++ 2 files changed, 13 insertions(+) diff --git a/gazebo_ros2_control/CHANGELOG.rst b/gazebo_ros2_control/CHANGELOG.rst index 8406c968..8eedc4b9 100644 --- a/gazebo_ros2_control/CHANGELOG.rst +++ b/gazebo_ros2_control/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package gazebo_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.7.1 (2024-01-24) +------------------ +* Load the URDF to the resource_manager before parsing it to CM (`#262 `_) + * Load the URDF to the resource_manager before parsing it to CM constructor (fixes https://github.com/ros-controls/ros2_control/issues/1299) +* Use lexical casts (`#260 `_) + Co-authored-by: Alejandro Hernández Cordero +* Fix links in documentation (`#263 `_) +* Added controller manager xml argument (`#255 `_) +* Contributors: Alejandro Hernández Cordero, Christoph Fröhlich, Sai Kishor Kothakota, Silvio Traversaro + 0.7.0 (2024-01-04) ------------------ * Add `hold_joints` parameter (`#251 `_) diff --git a/gazebo_ros2_control_demos/CHANGELOG.rst b/gazebo_ros2_control_demos/CHANGELOG.rst index 96d457ca..86c979fa 100644 --- a/gazebo_ros2_control_demos/CHANGELOG.rst +++ b/gazebo_ros2_control_demos/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gazebo_ros2_control_demos ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.7.1 (2024-01-24) +------------------ + 0.7.0 (2024-01-04) ------------------ * replace Twist with TwistStamped (`#249 `_) From ef4caaac3335ee1d7bd51087d062f6a8c5989dc2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 24 Jan 2024 10:55:13 +0100 Subject: [PATCH 52/58] 0.7.1 --- gazebo_ros2_control/package.xml | 2 +- gazebo_ros2_control_demos/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gazebo_ros2_control/package.xml b/gazebo_ros2_control/package.xml index 29475aae..256a7654 100644 --- a/gazebo_ros2_control/package.xml +++ b/gazebo_ros2_control/package.xml @@ -1,7 +1,7 @@ gazebo_ros2_control - 0.7.0 + 0.7.1 gazebo_ros2_control Alejandro Hernandez diff --git a/gazebo_ros2_control_demos/package.xml b/gazebo_ros2_control_demos/package.xml index 60458673..fda80de2 100644 --- a/gazebo_ros2_control_demos/package.xml +++ b/gazebo_ros2_control_demos/package.xml @@ -1,7 +1,7 @@ gazebo_ros2_control_demos - 0.7.0 + 0.7.1 gazebo_ros2_control_demos Alejandro Hernandez From ac5e215bdbc4267739ec359c73c9a363228ac93c Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Mon, 12 Feb 2024 10:04:30 +0100 Subject: [PATCH 53/58] Bump pre-commit/action from 3.0.0 to 3.0.1 (#281) Bumps [pre-commit/action](https://github.com/pre-commit/action) from 3.0.0 to 3.0.1. - [Release notes](https://github.com/pre-commit/action/releases) - [Commits](https://github.com/pre-commit/action/compare/v3.0.0...v3.0.1) --- updated-dependencies: - dependency-name: pre-commit/action dependency-type: direct:production update-type: version-update:semver-patch ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-format.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-format.yaml b/.github/workflows/ci-format.yaml index 4db51ac9..55586aa4 100644 --- a/.github/workflows/ci-format.yaml +++ b/.github/workflows/ci-format.yaml @@ -18,6 +18,6 @@ jobs: python-version: 3.10.6 - name: Install system hooks run: sudo apt install -qq cppcheck ament-cmake-uncrustify ament-cmake-pep257 - - uses: pre-commit/action@v3.0.0 + - uses: pre-commit/action@v3.0.1 with: extra_args: --all-files --hook-stage manual From cdae6b8a8f638d87146482e99bf76cf36530e5a6 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor <69388767+Wiktor-99@users.noreply.github.com> Date: Mon, 12 Feb 2024 16:31:22 +0100 Subject: [PATCH 54/58] Fix crashing due to an invalid parameter in the initial value. (#271) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero --- gazebo_ros2_control/src/gazebo_system.cpp | 23 +++++++++++++++++------ 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index 16cd23d0..c688541c 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -233,14 +233,25 @@ void GazeboSystem::registerJoints( RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:"); - auto get_initial_value = [this](const hardware_interface::InterfaceInfo & interface_info) { + auto get_initial_value = + [this, joint_name](const hardware_interface::InterfaceInfo & interface_info) { + double initial_value{0.0}; if (!interface_info.initial_value.empty()) { - double value = hardware_interface::stod(interface_info.initial_value); - RCLCPP_INFO(this->nh_->get_logger(), "\t\t\t found initial value: %f", value); - return value; - } else { - return 0.0; + try { + initial_value = hardware_interface::stod(interface_info.initial_value); + RCLCPP_INFO(this->nh_->get_logger(), "\t\t\t found initial value: %f", initial_value); + } catch (std::invalid_argument &) { + RCLCPP_ERROR_STREAM( + this->nh_->get_logger(), + "Failed converting initial_value string to real number for the joint " + << joint_name + << " and state interface " << interface_info.name + << ". Actual value of parameter: " << interface_info.initial_value + << ". Initial value will be set to 0.0"); + throw std::invalid_argument("Failed converting initial_value string"); + } } + return initial_value; }; double initial_position = std::numeric_limits::quiet_NaN(); From 5230b1e657f7326f28413e04630c6964f54544fb Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 13 Feb 2024 10:10:03 +0100 Subject: [PATCH 55/58] set the robot description parameter (#277) (#285) (cherry picked from commit 7f23568a31ec812c3745af89d1f3bc54ac787af0) Co-authored-by: AB --- .../src/gazebo_ros2_control_plugin.cpp | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp index c1804254..1ed72fcb 100644 --- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -196,6 +196,14 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element controllerManagerNodeName = sdf->GetElement("controller_manager_name")->Get(); } + // Read urdf from ros parameter server + std::string urdf_string; + urdf_string = impl_->getURDF(impl_->robot_description_); + if (urdf_string.empty()) { + RCLCPP_ERROR_STREAM(impl_->model_nh_->get_logger(), "An empty URDF was passed. Exiting."); + return; + } + // There's currently no direct way to set parameters to the plugin's node // So we have to parse the plugin file manually and set it to the node's context. auto rcl_context = impl_->model_nh_->get_node_base_interface()->get_context()->get_rcl_context(); @@ -215,6 +223,12 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element impl_->model_nh_->get_logger(), "No parameter file provided. Configuration might be wrong"); } + // set the robot description parameter + // to propagate it among controller manager and controllers + std::string rb_arg = std::string("robot_description:=") + urdf_string; + arguments.push_back(RCL_PARAM_FLAG); + arguments.push_back(rb_arg); + if (sdf->HasElement("ros")) { sdf = sdf->GetElement("ros"); @@ -270,13 +284,10 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element std::chrono::duration( impl_->parent_model_->GetWorld()->Physics()->GetMaxStepSize()))); - // Read urdf from ros parameter server then // setup actuators and mechanism control node. // This call will block if ROS is not properly initialized. - std::string urdf_string; std::vector control_hardware_info; try { - urdf_string = impl_->getURDF(impl_->robot_description_); control_hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf_string); } catch (const std::runtime_error & ex) { RCLCPP_ERROR_STREAM( @@ -465,7 +476,7 @@ std::string GazeboRosControlPrivate::getURDF(std::string param_name) const RCLCPP_ERROR( model_nh_->get_logger(), "Interrupted while waiting for %s service. Exiting.", robot_description_node_.c_str()); - return 0; + return ""; } RCLCPP_ERROR( model_nh_->get_logger(), "%s service not available, waiting again...", From 5e1f9a52bcd1bf4164186dbb3c8b5cf070ed156e Mon Sep 17 00:00:00 2001 From: Tobias Fischer Date: Wed, 14 Feb 2024 18:52:51 +1000 Subject: [PATCH 56/58] Update gazebo_ros2_control_plugin.cpp (#286) --- gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp index 1ed72fcb..6015669a 100644 --- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -36,6 +36,8 @@ #include #include #include +#include +#include #include "gazebo_ros/node.hpp" @@ -506,7 +508,7 @@ std::string GazeboRosControlPrivate::getURDF(std::string param_name) const model_nh_->get_logger(), "gazebo_ros2_control plugin is waiting for model" " URDF in parameter [%s] on the ROS param server.", param_name.c_str()); } - usleep(100000); + std::this_thread::sleep_for(std::chrono::microseconds(100000)); } RCLCPP_INFO( model_nh_->get_logger(), "Received urdf from param server, parsing..."); From 03b853b5337f6b8e9b8d4c0c8a3d814d7f34a97c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 5 Mar 2024 10:37:10 +0100 Subject: [PATCH 57/58] Cleanup of demos (#290) --- gazebo_ros2_control_demos/config/cart_controller.yaml | 1 - gazebo_ros2_control_demos/package.xml | 1 - 2 files changed, 2 deletions(-) diff --git a/gazebo_ros2_control_demos/config/cart_controller.yaml b/gazebo_ros2_control_demos/config/cart_controller.yaml index aa000ca2..f51d2fa5 100644 --- a/gazebo_ros2_control_demos/config/cart_controller.yaml +++ b/gazebo_ros2_control_demos/config/cart_controller.yaml @@ -12,7 +12,6 @@ joint_trajectory_controller: ros__parameters: joints: - slider_to_cart - interface_name: position command_interfaces: - position state_interfaces: diff --git a/gazebo_ros2_control_demos/package.xml b/gazebo_ros2_control_demos/package.xml index fda80de2..497fda86 100644 --- a/gazebo_ros2_control_demos/package.xml +++ b/gazebo_ros2_control_demos/package.xml @@ -38,7 +38,6 @@ launch_ros robot_state_publisher ros2_control - ros2_controllers rclcpp std_msgs velocity_controllers From e402031da6abf8ddb0edb4f19242117467d58954 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 5 Mar 2024 10:37:35 +0100 Subject: [PATCH 58/58] Fix topic (#291) --- gazebo_ros2_control_demos/examples/example_diff_drive.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo_ros2_control_demos/examples/example_diff_drive.cpp b/gazebo_ros2_control_demos/examples/example_diff_drive.cpp index 5439073f..53b518a2 100644 --- a/gazebo_ros2_control_demos/examples/example_diff_drive.cpp +++ b/gazebo_ros2_control_demos/examples/example_diff_drive.cpp @@ -28,7 +28,7 @@ int main(int argc, char * argv[]) std::make_shared("diff_drive_test_node"); auto publisher = node->create_publisher( - "/tricycle_controller/cmd_vel", 10); + "/diff_drive_base_controller/cmd_vel", 10); RCLCPP_INFO(node->get_logger(), "node created");