diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index 99a7f50c5..033d2f7a0 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/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" diff --git a/gazebo_plugins/cfg/WheelSlip.cfg b/gazebo_plugins/cfg/WheelSlip.cfg new file mode 100755 index 000000000..7e02bafca --- /dev/null +++ b/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")) diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_wheel_slip.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_wheel_slip.h new file mode 100644 index 000000000..1ddfff1ef --- /dev/null +++ b/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 +#include + +#include +#include + +// dynamic reconfigure stuff +#include +#include + +#include + +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 + *dyn_srv_; + + /// \brief for setting ROS name space + private: std::string robotNamespace_; + private: ros::CallbackQueue queue_; + private: boost::thread callbackQueueThread_; +}; +} +#endif 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..e4abedfeb --- /dev/null +++ b/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 +#include +#include + +#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("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 + (*this->rosnode_); + dynamic_reconfigure::Server + ::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)); + } +} +} diff --git a/gazebo_plugins/test/test_worlds/trisphere_cycle_wheel_slip.world b/gazebo_plugins/test/test_worlds/trisphere_cycle_wheel_slip.world new file mode 100644 index 000000000..eb3fb0350 --- /dev/null +++ b/gazebo_plugins/test/test_worlds/trisphere_cycle_wheel_slip.world @@ -0,0 +1,111 @@ + + + + + -2 0 -9.8 + + + model://ground_plane + + + model://sun + + + + model://trisphere_cycle + trisphere_cycle_slip0 + 0 0 0 0 0 0 + + wheel_slip_front + + 0 + 0 + 77 + + + + wheel_slip_rear + + 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 + + wheel_slip_front + + 1 + 1 + 77 + + + + wheel_slip_rear + + 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 + + + +