Skip to content
This repository was archived by the owner on Jul 10, 2025. It is now read-only.

Port wheel slip plugin to ros2 (forward port from eloquent) #1148

Merged
scpeters merged 3 commits into from
Aug 17, 2020
Merged

Port wheel slip plugin to ros2 (forward port from eloquent) #1148

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
e3aee68
Port wheel slip plugin to ros2 (eloquent) (#1099)
scpeters Aug 13, 2020
44ebb39
Update gazebo_plugins/CMakeLists.txt
scpeters Aug 17, 2020
6c7b71f
uncrustify
scpeters Aug 17, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
26 changes: 26 additions & 0 deletions gazebo_plugins/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -448,6 +448,24 @@ 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
)
target_include_directories(gazebo_ros_wheel_slip PUBLIC include)
ament_export_libraries(gazebo_ros_wheel_slip)
Comment thread
scpeters marked this conversation as resolved.
endif()

ament_export_include_directories(include)
ament_export_dependencies(rclcpp)
ament_export_dependencies(gazebo_dev)
Expand Down Expand Up @@ -505,6 +523,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}/
Expand Down
83 changes: 83 additions & 0 deletions gazebo_plugins/include/gazebo_plugins/gazebo_ros_wheel_slip.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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 <gazebo/common/Plugin.hh>
#include <gazebo/plugins/WheelSlipPlugin.hh>

#include <memory>

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}
<!-- Plugin to set wheel slip parameters according to wheel speed -->
<plugin name="projector" filename="libgazebo_ros_wheel_speed.so">
<ros>
<namespace>wheel_slip_front</namespace>
</ros>
<wheel link_name="wheel_front_left">
<slip_compliance_lateral>0</slip_compliance_lateral>
<slip_compliance_longitudinal>0.1</slip_compliance_longitudinal>
<wheel_normal_force>100</wheel_normal_force>
</wheel>
<wheel link_name="wheel_front_right">
<slip_compliance_lateral>0</slip_compliance_lateral>
<slip_compliance_longitudinal>0.1</slip_compliance_longitudinal>
<wheel_normal_force>100</wheel_normal_force>
</wheel>
</plugin>
\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<GazeboRosWheelSlipPrivate> impl_;
};
} // namespace gazebo_plugins

#endif // GAZEBO_PLUGINS__GAZEBO_ROS_WHEEL_SLIP_HPP_
100 changes: 100 additions & 0 deletions gazebo_plugins/src/gazebo_ros_wheel_slip.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,100 @@
// 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 <gazebo/physics/Model.hh>
#include <gazebo/physics/World.hh>

#include <gazebo/transport/transport.hh>
#include <gazebo_plugins/gazebo_ros_wheel_slip.hpp>
#include <gazebo_ros/node.hpp>
#include <std_msgs/msg/bool.hpp>

#include <memory>
#include <string>
#include <vector>

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<GazeboRosWheelSlipPrivate>())
{
}

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<rclcpp::Parameter> 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
132 changes: 132 additions & 0 deletions gazebo_plugins/worlds/gazebo_ros_wheel_slip_demo.world
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,132 @@
<?xml version="1.0" ?>
<!--
Gazebo ROS wheel slip plugin demo

Try for example:

To change slip compliance of a group of wheels:

ros2 param set /trisphere_cycle_slip0/wheel_slip_rear slip_compliance_unitless_longitudinal 1.0
ros2 param set /trisphere_cycle_slip1/wheel_slip_rear slip_compliance_unitless_longitudinal 0.0

To get the current slip compliance of a group of wheels:

ros2 param get /trisphere_cycle_slip1/wheel_slip_rear slip_compliance_unitless_longitudinal
-->
<sdf version="1.6">
<world name="default">
<gravity>-2 0 -9.8</gravity>

<include>
<uri>model://ground_plane</uri>
</include>
<include>
<uri>model://sun</uri>
</include>

<include>
<uri>model://trisphere_cycle</uri>
<name>trisphere_cycle_slip0</name>
<pose>0 0 0 0 0 0</pose>
<plugin name="wheel_slip_front" filename="libgazebo_ros_wheel_slip.so">
<ros>
<namespace>trisphere_cycle_slip0</namespace>
</ros>
<wheel link_name="wheel_front">
<slip_compliance_lateral>0</slip_compliance_lateral>
<slip_compliance_longitudinal>0</slip_compliance_longitudinal>
<wheel_normal_force>77</wheel_normal_force>
</wheel>
</plugin>
<plugin name="wheel_slip_rear" filename="libgazebo_ros_wheel_slip.so">
<ros>
<namespace>trisphere_cycle_slip0</namespace>
</ros>
<wheel link_name="wheel_rear_left">
<slip_compliance_lateral>0</slip_compliance_lateral>
<slip_compliance_longitudinal>0</slip_compliance_longitudinal>
<wheel_normal_force>32</wheel_normal_force>
</wheel>
<wheel link_name="wheel_rear_right">
<slip_compliance_lateral>0</slip_compliance_lateral>
<slip_compliance_longitudinal>0</slip_compliance_longitudinal>
<wheel_normal_force>32</wheel_normal_force>
</wheel>
</plugin>
<plugin name="joint_control" filename="libJointControlPlugin.so">
<controller type="position">
<joint>wheel_front_steer</joint>
<target>0</target>
<pid_gains>9 0 0.1</pid_gains>
</controller>
<controller type="velocity">
<joint>wheel_rear_left_spin</joint>
<joint>wheel_rear_right_spin</joint>
<target>6.0</target>
<pid_gains>9 0 0</pid_gains>
</controller>
<controller type="force">
<joint>wheel_rear_left_spin</joint>
<joint>wheel_rear_right_spin</joint>
<target>2.15</target>
</controller>
</plugin>
</include>

<include>
<uri>model://trisphere_cycle</uri>
<name>trisphere_cycle_slip1</name>
<pose>0 2 0 0 0 0</pose>
<plugin name="wheel_slip_front" filename="libgazebo_ros_wheel_slip.so">
<ros>
<namespace>trisphere_cycle_slip1</namespace>
</ros>
<wheel link_name="wheel_front">
<slip_compliance_lateral>1</slip_compliance_lateral>
<slip_compliance_longitudinal>1</slip_compliance_longitudinal>
<wheel_normal_force>77</wheel_normal_force>
</wheel>
</plugin>
<plugin name="wheel_slip_rear" filename="libgazebo_ros_wheel_slip.so">
<ros>
<namespace>trisphere_cycle_slip1</namespace>
</ros>
<wheel link_name="wheel_rear_left">
<slip_compliance_lateral>1</slip_compliance_lateral>
<slip_compliance_longitudinal>1</slip_compliance_longitudinal>
<wheel_normal_force>32</wheel_normal_force>
</wheel>
<wheel link_name="wheel_rear_right">
<slip_compliance_lateral>1</slip_compliance_lateral>
<slip_compliance_longitudinal>1</slip_compliance_longitudinal>
<wheel_normal_force>32</wheel_normal_force>
</wheel>
</plugin>
<plugin name="joint_control" filename="libJointControlPlugin.so">
<controller type="position">
<joint>wheel_front_steer</joint>
<target>0</target>
<pid_gains>9 0 0.1</pid_gains>
</controller>
<controller type="velocity">
<joint>wheel_rear_left_spin</joint>
<joint>wheel_rear_right_spin</joint>
<target>6.0</target>
<pid_gains>9 0 0</pid_gains>
</controller>
<controller type="force">
<joint>wheel_rear_left_spin</joint>
<joint>wheel_rear_right_spin</joint>
<target>2.15</target>
</controller>
</plugin>
</include>

<gui fullscreen='0'>
<camera name='user_camera'>
<pose>1.5 -4 2.5 0 0.5 1.6</pose>
<view_controller>orbit</view_controller>
</camera>
</gui>
</world>
</sdf>