gazebo_ros_wheel_slip plugin

gazebo_plugins/CMakeLists.txt

@@ -76,6 +76,7 @@ generate_dynamic_reconfigure_options(
   cfg/GazeboRosCamera.cfg
   cfg/GazeboRosOpenniKinect.cfg
   cfg/Hokuyo.cfg
+  cfg/WheelSlip.cfg
 )
 
 include_directories(include
@@ -233,6 +234,13 @@ if (NOT GAZEBO_VERSION VERSION_LESS 7.3)
     ${Boost_LIBRARIES} HarnessPlugin ${catkin_LIBRARIES})
 endif()
 
+if (NOT GAZEBO_VERSION VERSION_LESS 9.5)
+  add_library(gazebo_ros_wheel_slip src/gazebo_ros_wheel_slip.cpp)
+  add_dependencies(gazebo_ros_wheel_slip ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)
+  target_link_libraries(gazebo_ros_wheel_slip
+    ${Boost_LIBRARIES} WheelSlipPlugin ${catkin_LIBRARIES})
+endif()
+
 add_library(gazebo_ros_laser src/gazebo_ros_laser.cpp)
 target_link_libraries(gazebo_ros_laser RayPlugin ${catkin_LIBRARIES})
 
@@ -367,6 +375,13 @@ if (NOT GAZEBO_VERSION VERSION_LESS 7.3)
     )
 endif()
 
+if (NOT GAZEBO_VERSION VERSION_LESS 9.5)
+  install(TARGETS gazebo_ros_wheel_slip
+    DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    )
+endif()
+
 install(DIRECTORY include/
    DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
    FILES_MATCHING PATTERN "*.h"

gazebo_plugins/cfg/WheelSlip.cfg

@@ -0,0 +1,15 @@
+#! /usr/bin/env python
+
+# Wheel slip plugin configuration
+
+PACKAGE='gazebo_plugins'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+#       Name                                      Type      Reconf level   Description                                                                  Default    Min   Max
+gen.add("slip_compliance_unitless_lateral",       double_t, 0,             "Unitless slip compliance (slip / friction) in the lateral direction.",      0.0,       0.0,  20.0)
+gen.add("slip_compliance_unitless_longitudinal",  double_t, 0,             "Unitless slip compliance (slip / friction) in the longitudinal direction.", 0.0,       0.0,  20.0)
+
+exit(gen.generate(PACKAGE, "wheel_slip",     "WheelSlip"))

gazebo_plugins/include/gazebo_plugins/gazebo_ros_wheel_slip.h

@@ -0,0 +1,84 @@
+/*
+ * Copyright (C) 2017 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_ROS_WHEEL_SLIP_H
+#define GAZEBO_ROS_WHEEL_SLIP_H
+
+// Custom Callback Queue
+#include <ros/callback_queue.h>
+#include <ros/subscribe_options.h>
+
+#include <ros/ros.h>
+#include <std_msgs/Bool.h>
+
+// dynamic reconfigure stuff
+#include <gazebo_plugins/WheelSlipConfig.h>
+#include <dynamic_reconfigure/server.h>
+
+#include <gazebo/plugins/WheelSlipPlugin.hh>
+
+namespace gazebo
+{
+/// \brief See the Gazebo documentation about the WheelSlipPlugin. This ROS
+/// wrapper exposes two parameters via dynamic reconfigure:
+///
+///  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.
+class GazeboRosWheelSlip : public WheelSlipPlugin
+{
+    /// \brief Constructor
+    public: GazeboRosWheelSlip();
+
+    /// \brief Destructor
+    public: virtual ~GazeboRosWheelSlip();
+
+    /// \brief Load the plugin
+    public: virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
+
+    /// \brief Custom callback queue thread
+    private: void QueueThread();
+
+    // Allow dynamic reconfiguration of wheel slip params
+    private: void configCallback(
+                    gazebo_plugins::WheelSlipConfig &config,
+                    uint32_t level);
+
+    /// \brief pointer to ros node
+    private: ros::NodeHandle *rosnode_;
+
+    /// \brief Subscriber to detach control messages.
+    private: ros::Subscriber detachSub_;
+
+    /// \brief Dynamic reconfigure server.
+    private: dynamic_reconfigure::Server<gazebo_plugins::WheelSlipConfig>
+                    *dyn_srv_;
+
+    /// \brief for setting ROS name space
+    private: std::string robotNamespace_;
+    private: ros::CallbackQueue queue_;
+    private: boost::thread callbackQueueThread_;
+};
+}
+#endif

gazebo_plugins/src/gazebo_ros_wheel_slip.cpp

@@ -0,0 +1,119 @@
+/*
+ * Copyright (C) 2017 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/World.hh>
+#include <gazebo/physics/Model.hh>
+#include <sdf/sdf.hh>
+
+#include "gazebo_plugins/gazebo_ros_wheel_slip.h"
+
+namespace gazebo
+{
+
+// Register this plugin with the simulator
+GZ_REGISTER_MODEL_PLUGIN(GazeboRosWheelSlip)
+
+/////////////////////////////////////////////////
+GazeboRosWheelSlip::GazeboRosWheelSlip()
+{
+}
+
+/////////////////////////////////////////////////
+GazeboRosWheelSlip::~GazeboRosWheelSlip()
+{
+  // Custom Callback Queue
+  this->queue_.clear();
+  this->queue_.disable();
+
+  delete this->dyn_srv_;
+
+  this->rosnode_->shutdown();
+  delete this->rosnode_;
+}
+
+/////////////////////////////////////////////////
+void GazeboRosWheelSlip::configCallback(
+  gazebo_plugins::WheelSlipConfig &config, uint32_t level)
+{
+  if (level == ~0)
+  {
+    // don't overwrite initial parameters
+    return;
+  }
+
+  ROS_INFO_NAMED("wheel_slip", "Reconfigure request for the gazebo ros wheel_slip: %s. New slip compliances, lateral: %.3e, longitudinal: %.3e",
+           this->GetParentModel()->GetScopedName().c_str(),
+           config.slip_compliance_unitless_lateral,
+           config.slip_compliance_unitless_longitudinal);
+  this->SetSlipComplianceLateral(config.slip_compliance_unitless_lateral);
+  this->SetSlipComplianceLongitudinal(config.slip_compliance_unitless_longitudinal);
+}
+
+/////////////////////////////////////////////////
+// Load the controller
+void GazeboRosWheelSlip::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
+{
+  // Load the plugin
+  WheelSlipPlugin::Load(_parent, _sdf);
+
+  if (_sdf->HasElement("robotNamespace"))
+  {
+    this->robotNamespace_ = _sdf->Get<std::string>("robotNamespace") + "/";
+  }
+  if (this->robotNamespace_.empty() ||
+      this->robotNamespace_ == "/" ||
+      this->robotNamespace_ == "//")
+  {
+    this->robotNamespace_ = "wheel_slip/";
+  }
+  this->robotNamespace_ = _parent->GetName() + "/" + this->robotNamespace_;
+
+  // Init ROS
+  if (!ros::isInitialized())
+  {
+    ROS_FATAL_STREAM_NAMED("wheel_slip", "Not loading plugin since ROS hasn't been "
+          << "properly initialized.  Try starting gazebo with ros plugin:\n"
+          << "  gazebo -s libgazebo_ros_api_plugin.so\n");
+    return;
+  }
+
+  this->rosnode_ = new ros::NodeHandle(this->robotNamespace_);
+
+  // set up dynamic reconfigure
+  dyn_srv_ =
+    new dynamic_reconfigure::Server<gazebo_plugins::WheelSlipConfig>
+    (*this->rosnode_);
+  dynamic_reconfigure::Server<gazebo_plugins::WheelSlipConfig>
+    ::CallbackType f =
+    boost::bind(&GazeboRosWheelSlip::configCallback, this, _1, _2);
+  dyn_srv_->setCallback(f);
+
+  // Custom Callback Queue
+  this->callbackQueueThread_ =
+    boost::thread(boost::bind(&GazeboRosWheelSlip::QueueThread, this));
+}
+
+/////////////////////////////////////////////////
+void GazeboRosWheelSlip::QueueThread()
+{
+  static const double timeout = 0.01;
+
+  while (this->rosnode_->ok())
+  {
+    this->queue_.callAvailable(ros::WallDuration(timeout));
+  }
+}
+}

gazebo_plugins/test/test_worlds/trisphere_cycle_wheel_slip.world

@@ -0,0 +1,111 @@
+<?xml version="1.0" ?>
+
+<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">
+        <robotNamespace>wheel_slip_front</robotNamespace>
+        <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">
+        <robotNamespace>wheel_slip_rear</robotNamespace>
+        <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">
+        <robotNamespace>wheel_slip_front</robotNamespace>
+        <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">
+        <robotNamespace>wheel_slip_rear</robotNamespace>
+        <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>