From e3aee6857d616a172ba12650c14dcdfa20e72eab Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 12 Aug 2020 22:42:41 -0700 Subject: [PATCH 1/3] Port wheel slip plugin to ros2 (eloquent) (#1099) Initial port of gazebo_ros_wheel_slip_plugin to ros2. It includes a similar change to #1111, which moved the WheelSlipPlugin::Load call before the parameter callback so that the values from SDF/URDF are set first. Then the callback is changed to ignore negative values, and the default slip parameter values are set to -1, so that the SDF/URDF values are still preferred unless a different parameter value is specified in a launch file. Signed-off-by: Steve Peters --- gazebo_plugins/CMakeLists.txt | 25 ++++ .../gazebo_plugins/gazebo_ros_wheel_slip.hpp | 83 +++++++++++ gazebo_plugins/src/gazebo_ros_wheel_slip.cpp | 97 +++++++++++++ .../worlds/gazebo_ros_wheel_slip_demo.world | 132 ++++++++++++++++++ 4 files changed, 337 insertions(+) create mode 100644 gazebo_plugins/include/gazebo_plugins/gazebo_ros_wheel_slip.hpp create mode 100644 gazebo_plugins/src/gazebo_ros_wheel_slip.cpp create mode 100644 gazebo_plugins/worlds/gazebo_ros_wheel_slip_demo.world diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index e650d93aa..59d752947 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -448,6 +448,23 @@ if(ENABLE_PROFILER) target_link_libraries(gazebo_ros_projector ${ignition-common3_LIBRARIES}) endif() +# gazebo_ros_wheel_slip +if(NOT GAZEBO_VERSION VERSION_LESS 9.5) + add_library(gazebo_ros_wheel_slip SHARED + src/gazebo_ros_wheel_slip.cpp + ) + ament_target_dependencies(gazebo_ros_wheel_slip + "gazebo_dev" + "gazebo_ros" + "rclcpp" + "std_msgs" + ) + target_link_libraries(gazebo_ros_wheel_slip + WheelSlipPlugin + ) + ament_export_libraries(gazebo_ros_wheel_slip) +endif() + ament_export_include_directories(include) ament_export_dependencies(rclcpp) ament_export_dependencies(gazebo_dev) @@ -505,6 +522,14 @@ install(TARGETS LIBRARY DESTINATION lib RUNTIME DESTINATION bin) +if(NOT GAZEBO_VERSION VERSION_LESS 9.5) +install(TARGETS + gazebo_ros_wheel_slip + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) +endif() + install(DIRECTORY worlds DESTINATION share/${PROJECT_NAME}/ diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_wheel_slip.hpp b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_wheel_slip.hpp new file mode 100644 index 000000000..267445f74 --- /dev/null +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_wheel_slip.hpp @@ -0,0 +1,83 @@ +// Copyright 2019 Open Source Robotics Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GAZEBO_PLUGINS__GAZEBO_ROS_WHEEL_SLIP_HPP_ +#define GAZEBO_PLUGINS__GAZEBO_ROS_WHEEL_SLIP_HPP_ + +#include +#include + +#include + +namespace gazebo_plugins +{ +class GazeboRosWheelSlipPrivate; + +/// A plugin for adjusting wheel slip parameters in gazebo. +/// The plugin can set separate longitudinal and lateral wheel slip compliance +/// parameters for separate wheel links. +/// 1. slip_compliance_unitless_lateral +/// - Type: double +/// - Description: Unitless slip compliance (slip / friction) in the +/// lateral direction. This value is applied to all wheels declared +/// in the WheelSlipPlugin. +/// +/// 2. slip_compliance_unitless_longitudinal +/// - Type: double +/// - Description: Unitless slip compliance (slip / friction) in the +/// longitudinal direction. This value is applied to all wheels declared +/// in the WheelSlipPlugin. +/// See the WheelSlipPlugin documentation at the following location for more details: +/// http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1WheelSlipPlugin.html#details +/** + Example Usage: + \code{.xml} + + + + wheel_slip_front + + + 0 + 0.1 + 100 + + + 0 + 0.1 + 100 + + + \endcode +*/ +class GazeboRosWheelSlip : public gazebo::WheelSlipPlugin +{ +public: + /// Constructor + GazeboRosWheelSlip(); + + /// Destructor + ~GazeboRosWheelSlip(); + +protected: + // Documentation inherited + void Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr _sdf) override; + +private: + /// Private data pointer + std::unique_ptr impl_; +}; +} // namespace gazebo_plugins + +#endif // GAZEBO_PLUGINS__GAZEBO_ROS_WHEEL_SLIP_HPP_ diff --git a/gazebo_plugins/src/gazebo_ros_wheel_slip.cpp b/gazebo_plugins/src/gazebo_ros_wheel_slip.cpp new file mode 100644 index 000000000..5d263c5af --- /dev/null +++ b/gazebo_plugins/src/gazebo_ros_wheel_slip.cpp @@ -0,0 +1,97 @@ +// Copyright 2020 Open Source Robotics Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace gazebo_plugins +{ +class GazeboRosWheelSlipPrivate +{ +public: + /// A pointer to the GazeboROS node. + gazebo_ros::Node::SharedPtr ros_node_; + + /// Handle to parameters callback + rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_handle_; +}; + +GazeboRosWheelSlip::GazeboRosWheelSlip() +: impl_(std::make_unique()) +{ +} + +GazeboRosWheelSlip::~GazeboRosWheelSlip() +{ +} + +void GazeboRosWheelSlip::Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr _sdf) +{ + // Initialize the WheelSlipPlugin first so its values are preferred unless the ros + // parameters are overridden by a launch file. + WheelSlipPlugin::Load(_model, _sdf); + + // Initialize ROS node + impl_->ros_node_ = gazebo_ros::Node::Get(_sdf); + + auto param_change_callback = + [this](std::vector parameters) { + auto result = rcl_interfaces::msg::SetParametersResult(); + result.successful = true; + for (const auto & parameter : parameters) { + std::string param_name = parameter.get_name(); + if (param_name == "slip_compliance_unitless_lateral") { + double slip = parameter.as_double(); + if (slip >= 0.) { + RCLCPP_INFO(impl_->ros_node_->get_logger(), + "New lateral slip compliance: %.3e", slip); + this->SetSlipComplianceLateral(slip); + } + } else if (param_name == "slip_compliance_unitless_longitudinal") { + double slip = parameter.as_double(); + if (slip >= 0.) { + RCLCPP_INFO(impl_->ros_node_->get_logger(), + "New longitudinal slip compliance: %.3e", slip); + this->SetSlipComplianceLongitudinal(slip); + } + } else { + RCLCPP_WARN(impl_->ros_node_->get_logger(), + "Unrecognized parameter name[%s]", param_name); + result.successful = false; + } + } + return result; + }; + + impl_->on_set_parameters_callback_handle_ = impl_->ros_node_->add_on_set_parameters_callback( + param_change_callback); + + // Declare parameters after adding callback so that callback will trigger immediately. + // Set negative values by default, which are ignored by the callback. + // This approach allows values specified in a launch file to override the SDF/URDF values. + impl_->ros_node_->declare_parameter("slip_compliance_unitless_lateral", -1.); + impl_->ros_node_->declare_parameter("slip_compliance_unitless_longitudinal", -1.); +} + +GZ_REGISTER_MODEL_PLUGIN(GazeboRosWheelSlip) +} // namespace gazebo_plugins diff --git a/gazebo_plugins/worlds/gazebo_ros_wheel_slip_demo.world b/gazebo_plugins/worlds/gazebo_ros_wheel_slip_demo.world new file mode 100644 index 000000000..e4fad156b --- /dev/null +++ b/gazebo_plugins/worlds/gazebo_ros_wheel_slip_demo.world @@ -0,0 +1,132 @@ + + + + + -2 0 -9.8 + + + model://ground_plane + + + model://sun + + + + model://trisphere_cycle + trisphere_cycle_slip0 + 0 0 0 0 0 0 + + + trisphere_cycle_slip0 + + + 0 + 0 + 77 + + + + + trisphere_cycle_slip0 + + + 0 + 0 + 32 + + + 0 + 0 + 32 + + + + + wheel_front_steer + 0 + 9 0 0.1 + + + wheel_rear_left_spin + wheel_rear_right_spin + 6.0 + 9 0 0 + + + wheel_rear_left_spin + wheel_rear_right_spin + 2.15 + + + + + + model://trisphere_cycle + trisphere_cycle_slip1 + 0 2 0 0 0 0 + + + trisphere_cycle_slip1 + + + 1 + 1 + 77 + + + + + trisphere_cycle_slip1 + + + 1 + 1 + 32 + + + 1 + 1 + 32 + + + + + wheel_front_steer + 0 + 9 0 0.1 + + + wheel_rear_left_spin + wheel_rear_right_spin + 6.0 + 9 0 0 + + + wheel_rear_left_spin + wheel_rear_right_spin + 2.15 + + + + + + + 1.5 -4 2.5 0 0.5 1.6 + orbit + + + + From 44ebb39d9a19d704c32e9cb10487997326a17eeb Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 17 Aug 2020 09:42:39 -0700 Subject: [PATCH 2/3] Update gazebo_plugins/CMakeLists.txt Co-authored-by: Jacob Perron --- gazebo_plugins/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index 59d752947..ffa6dcedb 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -462,6 +462,7 @@ if(NOT GAZEBO_VERSION VERSION_LESS 9.5) target_link_libraries(gazebo_ros_wheel_slip WheelSlipPlugin ) + target_include_directories(gazebo_ros_wheel_slip PUBLIC include) ament_export_libraries(gazebo_ros_wheel_slip) endif() From 6c7b71fb7f1eaf2c3bac9396b20cb52c5eb8fc6b Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 17 Aug 2020 11:13:53 -0700 Subject: [PATCH 3/3] uncrustify Signed-off-by: Steve Peters --- gazebo_plugins/src/gazebo_ros_wheel_slip.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/gazebo_plugins/src/gazebo_ros_wheel_slip.cpp b/gazebo_plugins/src/gazebo_ros_wheel_slip.cpp index 5d263c5af..849fd2ce6 100644 --- a/gazebo_plugins/src/gazebo_ros_wheel_slip.cpp +++ b/gazebo_plugins/src/gazebo_ros_wheel_slip.cpp @@ -63,19 +63,22 @@ void GazeboRosWheelSlip::Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr if (param_name == "slip_compliance_unitless_lateral") { double slip = parameter.as_double(); if (slip >= 0.) { - RCLCPP_INFO(impl_->ros_node_->get_logger(), + RCLCPP_INFO( + impl_->ros_node_->get_logger(), "New lateral slip compliance: %.3e", slip); this->SetSlipComplianceLateral(slip); } } else if (param_name == "slip_compliance_unitless_longitudinal") { double slip = parameter.as_double(); if (slip >= 0.) { - RCLCPP_INFO(impl_->ros_node_->get_logger(), + RCLCPP_INFO( + impl_->ros_node_->get_logger(), "New longitudinal slip compliance: %.3e", slip); this->SetSlipComplianceLongitudinal(slip); } } else { - RCLCPP_WARN(impl_->ros_node_->get_logger(), + RCLCPP_WARN( + impl_->ros_node_->get_logger(), "Unrecognized parameter name[%s]", param_name); result.successful = false; }