From fa09083dc854f275e60884d50b230fd57f4b0db5 Mon Sep 17 00:00:00 2001 From: Shivesh Khaitan Date: Wed, 19 Jun 2019 23:48:02 +0530 Subject: [PATCH] [ros2] Port skid_steer_drive to ROS2 (#927) * [ros2] Port skid_steer_drive to ROS2 Integrate skid steer drive into diff drive * Reverted to original diff drive * Delete skid steer from .ros1_unported * Fix for diff drive changed api * Add support to specify odom covariance --- .../gazebo_ros_skid_steer_drive.h | 142 ------ .../src/gazebo_ros_skid_steer_drive.cpp | 478 ------------------ gazebo_plugins/src/gazebo_ros_diff_drive.cpp | 245 ++++++--- .../test/test_gazebo_ros_diff_drive.cpp | 27 +- .../worlds/gazebo_ros_skid_steer_drive.world | 352 +++++++++++++ .../gazebo_ros_skid_steer_drive_demo.world | 391 ++++++++++++++ 6 files changed, 934 insertions(+), 701 deletions(-) delete mode 100644 .ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_skid_steer_drive.h delete mode 100644 .ros1_unported/gazebo_plugins/src/gazebo_ros_skid_steer_drive.cpp create mode 100644 gazebo_plugins/test/worlds/gazebo_ros_skid_steer_drive.world create mode 100644 gazebo_plugins/worlds/gazebo_ros_skid_steer_drive_demo.world diff --git a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_skid_steer_drive.h b/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_skid_steer_drive.h deleted file mode 100644 index 451c5d9a8..000000000 --- a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_skid_steer_drive.h +++ /dev/null @@ -1,142 +0,0 @@ -/* - * Copyright (c) 2010, Daniel Hewlett, Antons Rebguns - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the nor the - * names of its contributors may be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY Antons Rebguns ''AS IS'' AND ANY - * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL Antons Rebguns BE LIABLE FOR ANY - * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - **/ - -/* - * \file gazebo_ros_skid_steer_drive.h - * - * \brief A skid steering drive plugin. Inspired by gazebo_ros_diff_drive and SkidSteerDrivePlugin - * - * \author Zdenek Materna (imaterna@fit.vutbr.cz) - * - * $ Id: 06/25/2013 11:23:40 AM materna $ - */ - -#ifndef GAZEBO_ROS_SKID_STEER_DRIVE_H_ -#define GAZEBO_ROS_SKID_STEER_DRIVE_H_ - -#include - -#include -#include - -// ROS -#include -#include -#include -#include -#include -#include - -// Custom Callback Queue -#include -#include - -// Boost -#include -#include - -namespace gazebo { - - class Joint; - class Entity; - - class GazeboRosSkidSteerDrive : public ModelPlugin { - - public: - GazeboRosSkidSteerDrive(); - ~GazeboRosSkidSteerDrive(); - void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf); - - protected: - virtual void UpdateChild(); - virtual void FiniChild(); - - private: - void publishOdometry(double step_time); - void getWheelVelocities(); - - physics::WorldPtr world; - physics::ModelPtr parent; - event::ConnectionPtr update_connection_; - - std::string left_front_joint_name_; - std::string right_front_joint_name_; - std::string left_rear_joint_name_; - std::string right_rear_joint_name_; - - double wheel_separation_; - double wheel_diameter_; - double torque; - double wheel_speed_[4]; - - physics::JointPtr joints[4]; - - // ROS STUFF - ros::NodeHandle* rosnode_; - ros::Publisher odometry_publisher_; - ros::Subscriber cmd_vel_subscriber_; - tf::TransformBroadcaster *transform_broadcaster_; - nav_msgs::Odometry odom_; - std::string tf_prefix_; - bool broadcast_tf_; - - boost::mutex lock; - - std::string robot_namespace_; - std::string command_topic_; - std::string odometry_topic_; - std::string odometry_frame_; - std::string robot_base_frame_; - - // Custom Callback Queue - ros::CallbackQueue queue_; - boost::thread callback_queue_thread_; - void QueueThread(); - - // DiffDrive stuff - void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg); - - double x_; - double rot_; - bool alive_; - - // Update Rate - double update_rate_; - double update_period_; - common::Time last_update_time_; - - double covariance_x_; - double covariance_y_; - double covariance_yaw_; - - }; - -} - - -#endif /* GAZEBO_ROS_SKID_STEER_DRIVE_H_ */ diff --git a/.ros1_unported/gazebo_plugins/src/gazebo_ros_skid_steer_drive.cpp b/.ros1_unported/gazebo_plugins/src/gazebo_ros_skid_steer_drive.cpp deleted file mode 100644 index 61d23abc6..000000000 --- a/.ros1_unported/gazebo_plugins/src/gazebo_ros_skid_steer_drive.cpp +++ /dev/null @@ -1,478 +0,0 @@ -/* - * Copyright (c) 2010, Daniel Hewlett, Antons Rebguns - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the nor the - * names of its contributors may be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY Antons Rebguns ''AS IS'' AND ANY - * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL Antons Rebguns BE LIABLE FOR ANY - * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - **/ - -/* - * \file gazebo_ros_skid_steer_drive.cpp - * - * \brief A skid steering drive plugin. Inspired by gazebo_ros_diff_drive and SkidSteerDrivePlugin - * - * \author Zdenek Materna (imaterna@fit.vutbr.cz) - * - * $ Id: 06/25/2013 11:23:40 AM materna $ - */ - - -#include -#include - -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace gazebo { - - enum { - RIGHT_FRONT=0, - LEFT_FRONT=1, - RIGHT_REAR=2, - LEFT_REAR=3, - }; - - GazeboRosSkidSteerDrive::GazeboRosSkidSteerDrive() {} - - // Destructor - GazeboRosSkidSteerDrive::~GazeboRosSkidSteerDrive() { - delete rosnode_; - delete transform_broadcaster_; - } - - // Load the controller - void GazeboRosSkidSteerDrive::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) { - - this->parent = _parent; - this->world = _parent->GetWorld(); - - this->robot_namespace_ = ""; - if (!_sdf->HasElement("robotNamespace")) { - ROS_INFO_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin missing , defaults to \"%s\"", - this->robot_namespace_.c_str()); - } else { - this->robot_namespace_ = - _sdf->GetElement("robotNamespace")->Get() + "/"; - } - - this->broadcast_tf_ = false; - if (!_sdf->HasElement("broadcastTF")) { - if (!this->broadcast_tf_) - ROS_INFO_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to false.",this->robot_namespace_.c_str()); - else ROS_INFO_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to true.",this->robot_namespace_.c_str()); - - } else { - this->broadcast_tf_ = _sdf->GetElement("broadcastTF")->Get(); - } - - // TODO write error if joint doesn't exist! - this->left_front_joint_name_ = "left_front_joint"; - if (!_sdf->HasElement("leftFrontJoint")) { - ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to \"%s\"", - this->robot_namespace_.c_str(), this->left_front_joint_name_.c_str()); - } else { - this->left_front_joint_name_ = _sdf->GetElement("leftFrontJoint")->Get(); - } - - this->right_front_joint_name_ = "right_front_joint"; - if (!_sdf->HasElement("rightFrontJoint")) { - ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to \"%s\"", - this->robot_namespace_.c_str(), this->right_front_joint_name_.c_str()); - } else { - this->right_front_joint_name_ = _sdf->GetElement("rightFrontJoint")->Get(); - } - - this->left_rear_joint_name_ = "left_rear_joint"; - if (!_sdf->HasElement("leftRearJoint")) { - ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to \"%s\"", - this->robot_namespace_.c_str(), this->left_rear_joint_name_.c_str()); - } else { - this->left_rear_joint_name_ = _sdf->GetElement("leftRearJoint")->Get(); - } - - this->right_rear_joint_name_ = "right_rear_joint"; - if (!_sdf->HasElement("rightRearJoint")) { - ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to \"%s\"", - this->robot_namespace_.c_str(), this->right_rear_joint_name_.c_str()); - } else { - this->right_rear_joint_name_ = _sdf->GetElement("rightRearJoint")->Get(); - } - - - // This assumes that front and rear wheel spacing is identical - /*this->wheel_separation_ = this->parent->GetJoint(left_front_joint_name_)->GetAnchor(0).Distance( - this->parent->GetJoint(right_front_joint_name_)->GetAnchor(0));*/ - - this->wheel_separation_ = 0.4; - - if (!_sdf->HasElement("wheelSeparation")) { - ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to value from robot_description: %f", - this->robot_namespace_.c_str(), this->wheel_separation_); - } else { - this->wheel_separation_ = - _sdf->GetElement("wheelSeparation")->Get(); - } - - // TODO get this from robot_description - this->wheel_diameter_ = 0.15; - if (!_sdf->HasElement("wheelDiameter")) { - ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to %f", - this->robot_namespace_.c_str(), this->wheel_diameter_); - } else { - this->wheel_diameter_ = _sdf->GetElement("wheelDiameter")->Get(); - } - - this->torque = 5.0; - if (!_sdf->HasElement("torque")) { - ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to %f", - this->robot_namespace_.c_str(), this->torque); - } else { - this->torque = _sdf->GetElement("torque")->Get(); - } - - this->command_topic_ = "cmd_vel"; - if (!_sdf->HasElement("commandTopic")) { - ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to \"%s\"", - this->robot_namespace_.c_str(), this->command_topic_.c_str()); - } else { - this->command_topic_ = _sdf->GetElement("commandTopic")->Get(); - } - - this->odometry_topic_ = "odom"; - if (!_sdf->HasElement("odometryTopic")) { - ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to \"%s\"", - this->robot_namespace_.c_str(), this->odometry_topic_.c_str()); - } else { - this->odometry_topic_ = _sdf->GetElement("odometryTopic")->Get(); - } - - this->odometry_frame_ = "odom"; - if (!_sdf->HasElement("odometryFrame")) { - ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to \"%s\"", - this->robot_namespace_.c_str(), this->odometry_frame_.c_str()); - } else { - this->odometry_frame_ = _sdf->GetElement("odometryFrame")->Get(); - } - - this->robot_base_frame_ = "base_footprint"; - if (!_sdf->HasElement("robotBaseFrame")) { - ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to \"%s\"", - this->robot_namespace_.c_str(), this->robot_base_frame_.c_str()); - } else { - this->robot_base_frame_ = _sdf->GetElement("robotBaseFrame")->Get(); - } - - this->update_rate_ = 100.0; - if (!_sdf->HasElement("updateRate")) { - ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to %f", - this->robot_namespace_.c_str(), this->update_rate_); - } else { - this->update_rate_ = _sdf->GetElement("updateRate")->Get(); - } - - this->covariance_x_ = 0.0001; - if (!_sdf->HasElement("covariance_x")) { - ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to %f", - this->robot_namespace_.c_str(), covariance_x_); - } else { - covariance_x_ = _sdf->GetElement("covariance_x")->Get(); - } - - this->covariance_y_ = 0.0001; - if (!_sdf->HasElement("covariance_y")) { - ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to %f", - this->robot_namespace_.c_str(), covariance_y_); - } else { - covariance_y_ = _sdf->GetElement("covariance_y")->Get(); - } - - this->covariance_yaw_ = 0.01; - if (!_sdf->HasElement("covariance_yaw")) { - ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to %f", - this->robot_namespace_.c_str(), covariance_yaw_); - } else { - covariance_yaw_ = _sdf->GetElement("covariance_yaw")->Get(); - } - - // Initialize update rate stuff - if (this->update_rate_ > 0.0) { - this->update_period_ = 1.0 / this->update_rate_; - } else { - this->update_period_ = 0.0; - } -#if GAZEBO_MAJOR_VERSION >= 8 - last_update_time_ = this->world->SimTime(); -#else - last_update_time_ = this->world->GetSimTime(); -#endif - - // Initialize velocity stuff - wheel_speed_[RIGHT_FRONT] = 0; - wheel_speed_[LEFT_FRONT] = 0; - wheel_speed_[RIGHT_REAR] = 0; - wheel_speed_[LEFT_REAR] = 0; - - x_ = 0; - rot_ = 0; - alive_ = true; - - joints[LEFT_FRONT] = this->parent->GetJoint(left_front_joint_name_); - joints[RIGHT_FRONT] = this->parent->GetJoint(right_front_joint_name_); - joints[LEFT_REAR] = this->parent->GetJoint(left_rear_joint_name_); - joints[RIGHT_REAR] = this->parent->GetJoint(right_rear_joint_name_); - - if (!joints[LEFT_FRONT]) { - char error[200]; - snprintf(error, 200, - "GazeboRosSkidSteerDrive Plugin (ns = %s) couldn't get left front hinge joint named \"%s\"", - this->robot_namespace_.c_str(), this->left_front_joint_name_.c_str()); - gzthrow(error); - } - - if (!joints[RIGHT_FRONT]) { - char error[200]; - snprintf(error, 200, - "GazeboRosSkidSteerDrive Plugin (ns = %s) couldn't get right front hinge joint named \"%s\"", - this->robot_namespace_.c_str(), this->right_front_joint_name_.c_str()); - gzthrow(error); - } - - if (!joints[LEFT_REAR]) { - char error[200]; - snprintf(error, 200, - "GazeboRosSkidSteerDrive Plugin (ns = %s) couldn't get left rear hinge joint named \"%s\"", - this->robot_namespace_.c_str(), this->left_rear_joint_name_.c_str()); - gzthrow(error); - } - - if (!joints[RIGHT_REAR]) { - char error[200]; - snprintf(error, 200, - "GazeboRosSkidSteerDrive Plugin (ns = %s) couldn't get right rear hinge joint named \"%s\"", - this->robot_namespace_.c_str(), this->right_rear_joint_name_.c_str()); - gzthrow(error); - } - -#if GAZEBO_MAJOR_VERSION > 2 - joints[LEFT_FRONT]->SetParam("fmax", 0, torque); - joints[RIGHT_FRONT]->SetParam("fmax", 0, torque); - joints[LEFT_REAR]->SetParam("fmax", 0, torque); - joints[RIGHT_REAR]->SetParam("fmax", 0, torque); -#else - joints[LEFT_FRONT]->SetMaxForce(0, torque); - joints[RIGHT_FRONT]->SetMaxForce(0, torque); - joints[LEFT_REAR]->SetMaxForce(0, torque); - joints[RIGHT_REAR]->SetMaxForce(0, torque); -#endif - - // Make sure the ROS node for Gazebo has already been initialized - if (!ros::isInitialized()) - { - ROS_FATAL_STREAM_NAMED("skid_steer_drive", "A ROS node for Gazebo has not been initialized, unable to load plugin. " - << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); - return; - } - - rosnode_ = new ros::NodeHandle(this->robot_namespace_); - - ROS_INFO_NAMED("skid_steer_drive", "Starting GazeboRosSkidSteerDrive Plugin (ns = %s)", this->robot_namespace_.c_str()); - - tf_prefix_ = tf::getPrefixParam(*rosnode_); - transform_broadcaster_ = new tf::TransformBroadcaster(); - - // ROS: Subscribe to the velocity command topic (usually "cmd_vel") - ros::SubscribeOptions so = - ros::SubscribeOptions::create(command_topic_, 1, - boost::bind(&GazeboRosSkidSteerDrive::cmdVelCallback, this, _1), - ros::VoidPtr(), &queue_); - - cmd_vel_subscriber_ = rosnode_->subscribe(so); - - odometry_publisher_ = rosnode_->advertise(odometry_topic_, 1); - - // start custom queue for diff drive - this->callback_queue_thread_ = - boost::thread(boost::bind(&GazeboRosSkidSteerDrive::QueueThread, this)); - - // listen to the update event (broadcast every simulation iteration) - this->update_connection_ = - event::Events::ConnectWorldUpdateBegin( - boost::bind(&GazeboRosSkidSteerDrive::UpdateChild, this)); - - } - - // Update the controller - void GazeboRosSkidSteerDrive::UpdateChild() { -#if GAZEBO_MAJOR_VERSION >= 8 - common::Time current_time = this->world->SimTime(); -#else - common::Time current_time = this->world->GetSimTime(); -#endif - double seconds_since_last_update = - (current_time - last_update_time_).Double(); - if (seconds_since_last_update > update_period_) { - - publishOdometry(seconds_since_last_update); - - // Update robot in case new velocities have been requested - getWheelVelocities(); -#if GAZEBO_MAJOR_VERSION > 2 - joints[LEFT_FRONT]->SetParam("vel", 0, wheel_speed_[LEFT_FRONT] / (wheel_diameter_ / 2.0)); - joints[RIGHT_FRONT]->SetParam("vel", 0, wheel_speed_[RIGHT_FRONT] / (wheel_diameter_ / 2.0)); - joints[LEFT_REAR]->SetParam("vel", 0, wheel_speed_[LEFT_REAR] / (wheel_diameter_ / 2.0)); - joints[RIGHT_REAR]->SetParam("vel", 0, wheel_speed_[RIGHT_REAR] / (wheel_diameter_ / 2.0)); -#else - joints[LEFT_FRONT]->SetVelocity(0, wheel_speed_[LEFT_FRONT] / (wheel_diameter_ / 2.0)); - joints[RIGHT_FRONT]->SetVelocity(0, wheel_speed_[RIGHT_FRONT] / (wheel_diameter_ / 2.0)); - joints[LEFT_REAR]->SetVelocity(0, wheel_speed_[LEFT_REAR] / (wheel_diameter_ / 2.0)); - joints[RIGHT_REAR]->SetVelocity(0, wheel_speed_[RIGHT_REAR] / (wheel_diameter_ / 2.0)); -#endif - - last_update_time_+= common::Time(update_period_); - - } - } - - // Finalize the controller - void GazeboRosSkidSteerDrive::FiniChild() { - alive_ = false; - queue_.clear(); - queue_.disable(); - rosnode_->shutdown(); - callback_queue_thread_.join(); - } - - void GazeboRosSkidSteerDrive::getWheelVelocities() { - boost::mutex::scoped_lock scoped_lock(lock); - - double vr = x_; - double va = rot_; - - wheel_speed_[RIGHT_FRONT] = vr + va * wheel_separation_ / 2.0; - wheel_speed_[RIGHT_REAR] = vr + va * wheel_separation_ / 2.0; - - wheel_speed_[LEFT_FRONT] = vr - va * wheel_separation_ / 2.0; - wheel_speed_[LEFT_REAR] = vr - va * wheel_separation_ / 2.0; - - } - - void GazeboRosSkidSteerDrive::cmdVelCallback( - const geometry_msgs::Twist::ConstPtr& cmd_msg) { - - boost::mutex::scoped_lock scoped_lock(lock); - x_ = cmd_msg->linear.x; - rot_ = cmd_msg->angular.z; - } - - void GazeboRosSkidSteerDrive::QueueThread() { - static const double timeout = 0.01; - - while (alive_ && rosnode_->ok()) { - queue_.callAvailable(ros::WallDuration(timeout)); - } - } - - void GazeboRosSkidSteerDrive::publishOdometry(double step_time) { - ros::Time current_time = ros::Time::now(); - std::string odom_frame = - tf::resolve(tf_prefix_, odometry_frame_); - std::string base_footprint_frame = - tf::resolve(tf_prefix_, robot_base_frame_); - - // TODO create some non-perfect odometry! - // getting data for base_footprint to odom transform -#if GAZEBO_MAJOR_VERSION >= 8 - ignition::math::Pose3d pose = this->parent->WorldPose(); -#else - ignition::math::Pose3d pose = this->parent->GetWorldPose().Ign(); -#endif - - tf::Quaternion qt(pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z(), pose.Rot().W()); - tf::Vector3 vt(pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z()); - - tf::Transform base_footprint_to_odom(qt, vt); - if (this->broadcast_tf_) { - - transform_broadcaster_->sendTransform( - tf::StampedTransform(base_footprint_to_odom, current_time, - odom_frame, base_footprint_frame)); - - } - - // publish odom topic - odom_.pose.pose.position.x = pose.Pos().X(); - odom_.pose.pose.position.y = pose.Pos().Y(); - - odom_.pose.pose.orientation.x = pose.Rot().X(); - odom_.pose.pose.orientation.y = pose.Rot().Y(); - odom_.pose.pose.orientation.z = pose.Rot().Z(); - odom_.pose.pose.orientation.w = pose.Rot().W(); - odom_.pose.covariance[0] = this->covariance_x_; - odom_.pose.covariance[7] = this->covariance_y_; - odom_.pose.covariance[14] = 1000000000000.0; - odom_.pose.covariance[21] = 1000000000000.0; - odom_.pose.covariance[28] = 1000000000000.0; - odom_.pose.covariance[35] = this->covariance_yaw_; - - // get velocity in /odom frame - ignition::math::Vector3d linear; -#if GAZEBO_MAJOR_VERSION >= 8 - linear = this->parent->WorldLinearVel(); - odom_.twist.twist.angular.z = this->parent->WorldAngularVel().Z(); -#else - linear = this->parent->GetWorldLinearVel().Ign(); - odom_.twist.twist.angular.z = this->parent->GetWorldAngularVel().Ign().Z(); -#endif - - // convert velocity to child_frame_id (aka base_footprint) - float yaw = pose.Rot().Yaw(); - odom_.twist.twist.linear.x = cosf(yaw) * linear.X() + sinf(yaw) * linear.Y(); - odom_.twist.twist.linear.y = cosf(yaw) * linear.Y() - sinf(yaw) * linear.X(); - odom_.twist.covariance[0] = this->covariance_x_; - odom_.twist.covariance[7] = this->covariance_y_; - odom_.twist.covariance[14] = 1000000000000.0; - odom_.twist.covariance[21] = 1000000000000.0; - odom_.twist.covariance[28] = 1000000000000.0; - odom_.twist.covariance[35] = this->covariance_yaw_; - - odom_.header.stamp = current_time; - odom_.header.frame_id = odom_frame; - odom_.child_frame_id = base_footprint_frame; - - odometry_publisher_.publish(odom_); - } - - GZ_REGISTER_MODEL_PLUGIN(GazeboRosSkidSteerDrive) -} diff --git a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp index 6b583ba6c..1c70c5502 100644 --- a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp +++ b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp @@ -145,10 +145,10 @@ class GazeboRosDiffDrivePrivate gazebo::event::ConnectionPtr update_connection_; /// Distance between the wheels, in meters. - double wheel_separation_; + std::vector wheel_separation_; /// Diameter of wheels, in meters. - double wheel_diameter_; + std::vector wheel_diameter_; /// Maximum wheel torque, in Nm. double max_wheel_torque_; @@ -157,10 +157,10 @@ class GazeboRosDiffDrivePrivate double max_wheel_accel_; /// Desired wheel speed. - double desired_wheel_speed_[2]; + std::vector desired_wheel_speed_; /// Speed sent to wheel. - double wheel_speed_instr_[2]; + std::vector wheel_speed_instr_; /// Pointers to wheel joints. std::vector joints_; @@ -212,6 +212,12 @@ class GazeboRosDiffDrivePrivate /// True to publish odom-to-world transforms. bool publish_odom_tf_; + + /// Store number of wheel pairs + unsigned int num_wheel_pairs_; + + /// Covariance in odometry + double covariance_[3]; }; GazeboRosDiffDrive::GazeboRosDiffDrive() @@ -230,41 +236,97 @@ void GazeboRosDiffDrive::Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr // Initialize ROS node impl_->ros_node_ = gazebo_ros::Node::Get(_sdf); - // Get joints - impl_->joints_.resize(2); + // Get number of wheel pairs in the model + impl_->num_wheel_pairs_ = static_cast(_sdf->Get("num_wheel_pairs", 1).first); - auto left_joint = _sdf->Get("left_joint", "left_joint").first; - impl_->joints_[GazeboRosDiffDrivePrivate::LEFT] = _model->GetJoint(left_joint); + if (impl_->num_wheel_pairs_ < 1) { + impl_->num_wheel_pairs_ = 1; + RCLCPP_WARN(impl_->ros_node_->get_logger(), + "Drive requires at least one pair of wheels. Setting [num_wheel_pairs] to 1"); + } + + // Dynamic properties + impl_->max_wheel_accel_ = _sdf->Get("max_wheel_acceleration", 0.0).first; + impl_->max_wheel_torque_ = _sdf->Get("max_wheel_torque", 5.0).first; - auto right_joint = _sdf->Get("right_joint", "right_joint").first; - impl_->joints_[GazeboRosDiffDrivePrivate::RIGHT] = _model->GetJoint(right_joint); + // Get joints and Kinematic properties + std::vector left_joints, right_joints; - if (!impl_->joints_[GazeboRosDiffDrivePrivate::LEFT] || - !impl_->joints_[GazeboRosDiffDrivePrivate::RIGHT]) + for (auto left_joint_elem = _sdf->GetElement("left_joint"); left_joint_elem != nullptr; + left_joint_elem = left_joint_elem->GetNextElement("left_joint")) { - RCLCPP_ERROR(impl_->ros_node_->get_logger(), - "Joint [%s] or [%s] not found, plugin will not work.", left_joint.c_str(), - right_joint.c_str()); + auto left_joint_name = left_joint_elem->Get(); + auto left_joint = _model->GetJoint(left_joint_name); + if (!left_joint) { + RCLCPP_ERROR(impl_->ros_node_->get_logger(), + "Joint [%s] not found, plugin will not work.", left_joint_name.c_str()); + impl_->ros_node_.reset(); + return; + } + left_joint->SetParam("fmax", 0, impl_->max_wheel_torque_); + left_joints.push_back(left_joint); + } + for (auto right_joint_elem = _sdf->GetElement("right_joint"); right_joint_elem != nullptr; + right_joint_elem = right_joint_elem->GetNextElement("right_joint")) + { + auto right_joint_name = right_joint_elem->Get(); + auto right_joint = _model->GetJoint(right_joint_name); + if (!right_joint) { + RCLCPP_ERROR(impl_->ros_node_->get_logger(), + "Joint [%s] not found, plugin will not work.", right_joint_name.c_str()); + impl_->ros_node_.reset(); + return; + } + right_joint->SetParam("fmax", 0, impl_->max_wheel_torque_); + right_joints.push_back(right_joint); + } + + if (left_joints.size() != right_joints.size() || left_joints.size() != impl_->num_wheel_pairs_) { + RCLCPP_ERROR(impl_->ros_node_->get_logger(), + "Inconsistent number of joints specified. Plugin will not work."); impl_->ros_node_.reset(); return; } - // Kinematic properties - impl_->wheel_separation_ = _sdf->Get("wheel_separation", 0.34).first; - impl_->wheel_diameter_ = _sdf->Get("wheel_diameter", 0.15).first; + unsigned int index; + for (index = 0; index < impl_->num_wheel_pairs_; ++index) { + impl_->joints_.push_back(right_joints[index]); + impl_->joints_.push_back(left_joints[index]); + } - impl_->desired_wheel_speed_[GazeboRosDiffDrivePrivate::RIGHT] = 0; - impl_->desired_wheel_speed_[GazeboRosDiffDrivePrivate::LEFT] = 0; - impl_->wheel_speed_instr_[GazeboRosDiffDrivePrivate::RIGHT] = 0; - impl_->wheel_speed_instr_[GazeboRosDiffDrivePrivate::LEFT] = 0; + index = 0; + impl_->wheel_separation_.assign(impl_->num_wheel_pairs_, 0.34); + for (auto wheel_separation = _sdf->GetElement("wheel_separation"); wheel_separation != nullptr; + wheel_separation = wheel_separation->GetNextElement("wheel_separation")) + { + if (index >= impl_->num_wheel_pairs_) { + RCLCPP_WARN(impl_->ros_node_->get_logger(), "Ignoring rest of specified "); + break; + } + impl_->wheel_separation_[index] = wheel_separation->Get(); + RCLCPP_INFO(impl_->ros_node_->get_logger(), + "Wheel pair %i separation set to [%fm]", index + 1, impl_->wheel_separation_[index]); + index++; + } - // Dynamic properties - impl_->max_wheel_accel_ = _sdf->Get("max_wheel_acceleration", 0.0).first; - impl_->max_wheel_torque_ = _sdf->Get("max_wheel_torque", 5.0).first; + index = 0; + impl_->wheel_diameter_.assign(impl_->num_wheel_pairs_, 0.15); + for (auto wheel_diameter = _sdf->GetElement("wheel_diameter"); wheel_diameter != nullptr; + wheel_diameter = wheel_diameter->GetNextElement("wheel_diameter")) + { + if (index >= impl_->num_wheel_pairs_) { + RCLCPP_WARN(impl_->ros_node_->get_logger(), "Ignoring rest of specified "); + break; + } + impl_->wheel_diameter_[index] = wheel_diameter->Get(); + RCLCPP_INFO(impl_->ros_node_->get_logger(), + "Wheel pair %i diameter set to [%fm]", index + 1, impl_->wheel_diameter_[index]); + index++; + } - impl_->joints_[GazeboRosDiffDrivePrivate::LEFT]->SetParam("fmax", 0, impl_->max_wheel_torque_); - impl_->joints_[GazeboRosDiffDrivePrivate::RIGHT]->SetParam("fmax", 0, impl_->max_wheel_torque_); + impl_->wheel_speed_instr_.assign(2 * impl_->num_wheel_pairs_, 0); + impl_->desired_wheel_speed_.assign(2 * impl_->num_wheel_pairs_, 0); // Update rate auto update_rate = _sdf->Get("update_rate", 100.0).first; @@ -278,6 +340,7 @@ void GazeboRosDiffDrive::Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr impl_->cmd_vel_sub_ = impl_->ros_node_->create_subscription( "cmd_vel", rclcpp::QoS(rclcpp::KeepLast(1)), std::bind(&GazeboRosDiffDrivePrivate::OnCmdVel, impl_.get(), std::placeholders::_1)); + RCLCPP_INFO(impl_->ros_node_->get_logger(), "Subscribed to [%s]", impl_->cmd_vel_sub_->get_topic_name()); @@ -301,8 +364,8 @@ void GazeboRosDiffDrive::Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr impl_->publish_wheel_tf_ = _sdf->Get("publish_wheel_tf", false).first; impl_->publish_odom_tf_ = _sdf->Get("publish_odom_tf", false).first; if (impl_->publish_wheel_tf_ || impl_->publish_odom_tf_) { - impl_->transform_broadcaster_ = std::shared_ptr( - new tf2_ros::TransformBroadcaster(impl_->ros_node_)); + impl_->transform_broadcaster_ = + std::make_shared(impl_->ros_node_); if (impl_->publish_odom_tf_) { RCLCPP_INFO(impl_->ros_node_->get_logger(), @@ -310,14 +373,21 @@ void GazeboRosDiffDrive::Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr impl_->robot_base_frame_.c_str()); } - if (impl_->publish_wheel_tf_) { - RCLCPP_INFO(impl_->ros_node_->get_logger(), - "Publishing wheel transforms between [%s], [%s] and [%s]", impl_->robot_base_frame_.c_str(), - impl_->joints_[GazeboRosDiffDrivePrivate::LEFT]->GetName().c_str(), - impl_->joints_[GazeboRosDiffDrivePrivate::RIGHT]->GetName().c_str()); + for (index = 0; index < impl_->num_wheel_pairs_; ++index) { + if (impl_->publish_wheel_tf_) { + RCLCPP_INFO(impl_->ros_node_->get_logger(), + "Publishing wheel transforms between [%s], [%s] and [%s]", + impl_->robot_base_frame_.c_str(), + impl_->joints_[2 * index + GazeboRosDiffDrivePrivate::LEFT]->GetName().c_str(), + impl_->joints_[2 * index + GazeboRosDiffDrivePrivate::RIGHT]->GetName().c_str()); + } } } + impl_->covariance_[0] = _sdf->Get("covariance_x", 0.00001).first; + impl_->covariance_[1] = _sdf->Get("covariance_y", 0.00001).first; + impl_->covariance_[2] = _sdf->Get("covariance_yaw", 0.001).first; + // Listen to the update event (broadcast every simulation iteration) impl_->update_connection_ = gazebo::event::Events::ConnectWorldUpdateBegin( std::bind(&GazeboRosDiffDrivePrivate::OnUpdate, impl_.get(), std::placeholders::_1)); @@ -325,13 +395,17 @@ void GazeboRosDiffDrive::Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr void GazeboRosDiffDrive::Reset() { - if (impl_->joints_[GazeboRosDiffDrivePrivate::LEFT] && - impl_->joints_[GazeboRosDiffDrivePrivate::RIGHT]) - { - impl_->last_update_time_ = - impl_->joints_[GazeboRosDiffDrivePrivate::LEFT]->GetWorld()->SimTime(); - impl_->joints_[GazeboRosDiffDrivePrivate::LEFT]->SetParam("fmax", 0, impl_->max_wheel_torque_); - impl_->joints_[GazeboRosDiffDrivePrivate::RIGHT]->SetParam("fmax", 0, impl_->max_wheel_torque_); + impl_->last_update_time_ = + impl_->joints_[GazeboRosDiffDrivePrivate::LEFT]->GetWorld()->SimTime(); + for (unsigned int i = 0; i < impl_->num_wheel_pairs_; ++i) { + if (impl_->joints_[2 * i + GazeboRosDiffDrivePrivate::LEFT] && + impl_->joints_[2 * i + GazeboRosDiffDrivePrivate::RIGHT]) + { + impl_->joints_[2 * i + GazeboRosDiffDrivePrivate::LEFT]->SetParam( + "fmax", 0, impl_->max_wheel_torque_); + impl_->joints_[2 * i + GazeboRosDiffDrivePrivate::RIGHT]->SetParam( + "fmax", 0, impl_->max_wheel_torque_); + } } impl_->pose_encoder_.x = 0; impl_->pose_encoder_.y = 0; @@ -374,36 +448,46 @@ void GazeboRosDiffDrivePrivate::OnUpdate(const gazebo::common::UpdateInfo & _inf UpdateWheelVelocities(); // Current speed - double current_speed[2]; - current_speed[LEFT] = joints_[LEFT]->GetVelocity(0) * (wheel_diameter_ / 2.0); - current_speed[RIGHT] = joints_[RIGHT]->GetVelocity(0) * (wheel_diameter_ / 2.0); + std::vector current_speed(2 * num_wheel_pairs_); + for (unsigned int i = 0; i < num_wheel_pairs_; ++i) { + current_speed[2 * i + LEFT] = + joints_[2 * i + LEFT]->GetVelocity(0) * (wheel_diameter_[i] / 2.0); + current_speed[2 * i + RIGHT] = + joints_[2 * i + RIGHT]->GetVelocity(0) * (wheel_diameter_[i] / 2.0); + } // If max_accel == 0, or target speed is reached - if (max_wheel_accel_ == 0 || - (fabs(desired_wheel_speed_[LEFT] - current_speed[LEFT]) < 0.01) || - (fabs(desired_wheel_speed_[RIGHT] - current_speed[RIGHT]) < 0.01)) - { - joints_[LEFT]->SetParam("vel", 0, desired_wheel_speed_[LEFT] / (wheel_diameter_ / 2.0)); - joints_[RIGHT]->SetParam("vel", 0, desired_wheel_speed_[RIGHT] / (wheel_diameter_ / 2.0)); - } else { - if (desired_wheel_speed_[LEFT] >= current_speed[LEFT]) { - wheel_speed_instr_[LEFT] += fmin(desired_wheel_speed_[LEFT] - current_speed[LEFT], - max_wheel_accel_ * seconds_since_last_update); + for (unsigned int i = 0; i < num_wheel_pairs_; ++i) { + if (max_wheel_accel_ == 0 || + (fabs(desired_wheel_speed_[2 * i + LEFT] - current_speed[2 * i + LEFT]) < 0.01) || + (fabs(desired_wheel_speed_[2 * i + RIGHT] - current_speed[2 * i + RIGHT]) < 0.01)) + { + joints_[2 * i + LEFT]->SetParam( + "vel", 0, desired_wheel_speed_[2 * i + LEFT] / (wheel_diameter_[i] / 2.0)); + joints_[2 * i + RIGHT]->SetParam( + "vel", 0, desired_wheel_speed_[2 * i + RIGHT] / (wheel_diameter_[i] / 2.0)); } else { - wheel_speed_instr_[LEFT] += fmax(desired_wheel_speed_[LEFT] - current_speed[LEFT], - -max_wheel_accel_ * seconds_since_last_update); + if (desired_wheel_speed_[2 * i + LEFT] >= current_speed[2 * i + LEFT]) { + wheel_speed_instr_[2 * i + LEFT] += fmin(desired_wheel_speed_[2 * i + LEFT] - + current_speed[2 * i + LEFT], max_wheel_accel_ * seconds_since_last_update); + } else { + wheel_speed_instr_[2 * i + LEFT] += fmax(desired_wheel_speed_[2 * i + LEFT] - + current_speed[2 * i + LEFT], -max_wheel_accel_ * seconds_since_last_update); + } + + if (desired_wheel_speed_[2 * i + RIGHT] > current_speed[2 * i + RIGHT]) { + wheel_speed_instr_[2 * i + RIGHT] += fmin(desired_wheel_speed_[2 * i + RIGHT] - + current_speed[2 * i + RIGHT], max_wheel_accel_ * seconds_since_last_update); + } else { + wheel_speed_instr_[2 * i + RIGHT] += fmax(desired_wheel_speed_[2 * i + RIGHT] - + current_speed[2 * i + RIGHT], -max_wheel_accel_ * seconds_since_last_update); + } + + joints_[2 * i + LEFT]->SetParam( + "vel", 0, wheel_speed_instr_[2 * i + LEFT] / (wheel_diameter_[i] / 2.0)); + joints_[2 * i + RIGHT]->SetParam( + "vel", 0, wheel_speed_instr_[2 * i + RIGHT] / (wheel_diameter_[i] / 2.0)); } - - if (desired_wheel_speed_[RIGHT] > current_speed[RIGHT]) { - wheel_speed_instr_[RIGHT] += fmin(desired_wheel_speed_[RIGHT] - current_speed[RIGHT], - max_wheel_accel_ * seconds_since_last_update); - } else { - wheel_speed_instr_[RIGHT] += fmax(desired_wheel_speed_[RIGHT] - current_speed[RIGHT], - -max_wheel_accel_ * seconds_since_last_update); - } - - joints_[LEFT]->SetParam("vel", 0, wheel_speed_instr_[LEFT] / (wheel_diameter_ / 2.0)); - joints_[RIGHT]->SetParam("vel", 0, wheel_speed_instr_[RIGHT] / (wheel_diameter_ / 2.0)); } last_update_time_ = _info.simTime; @@ -416,8 +500,10 @@ void GazeboRosDiffDrivePrivate::UpdateWheelVelocities() double vr = target_x_; double va = target_rot_; - desired_wheel_speed_[LEFT] = vr - va * wheel_separation_ / 2.0; - desired_wheel_speed_[RIGHT] = vr + va * wheel_separation_ / 2.0; + for (unsigned int i = 0; i < num_wheel_pairs_; ++i) { + desired_wheel_speed_[2 * i + LEFT] = vr - va * wheel_separation_[i] / 2.0; + desired_wheel_speed_[2 * i + RIGHT] = vr + va * wheel_separation_[i] / 2.0; + } } void GazeboRosDiffDrivePrivate::OnCmdVel(const geometry_msgs::msg::Twist::SharedPtr _msg) @@ -435,11 +521,11 @@ void GazeboRosDiffDrivePrivate::UpdateOdometryEncoder(const gazebo::common::Time double seconds_since_last_update = (_current_time - last_encoder_update_).Double(); last_encoder_update_ = _current_time; - double b = wheel_separation_; + double b = wheel_separation_[0]; // Book: Sigwart 2011 Autonompus Mobile Robots page:337 - double sl = vl * (wheel_diameter_ / 2.0) * seconds_since_last_update; - double sr = vr * (wheel_diameter_ / 2.0) * seconds_since_last_update; + double sl = vl * (wheel_diameter_[0] / 2.0) * seconds_since_last_update; + double sr = vr * (wheel_diameter_[0] / 2.0) * seconds_since_last_update; double ssum = sl + sr; double sdiff = sr - sl; @@ -505,7 +591,7 @@ void GazeboRosDiffDrivePrivate::PublishOdometryTf(const gazebo::common::Time & _ void GazeboRosDiffDrivePrivate::PublishWheelsTf(const gazebo::common::Time & _current_time) { - for (auto i : {LEFT, RIGHT}) { + for (unsigned int i = 0; i < 2 * num_wheel_pairs_; ++i) { auto pose_wheel = joints_[i]->GetChild()->RelativePose(); geometry_msgs::msg::TransformStamped msg; @@ -522,12 +608,19 @@ void GazeboRosDiffDrivePrivate::PublishWheelsTf(const gazebo::common::Time & _cu void GazeboRosDiffDrivePrivate::PublishOdometryMsg(const gazebo::common::Time & _current_time) { // Set covariance - odom_.pose.covariance[0] = 0.00001; - odom_.pose.covariance[7] = 0.00001; + odom_.pose.covariance[0] = covariance_[0]; + odom_.pose.covariance[7] = covariance_[1]; odom_.pose.covariance[14] = 1000000000000.0; odom_.pose.covariance[21] = 1000000000000.0; odom_.pose.covariance[28] = 1000000000000.0; - odom_.pose.covariance[35] = 0.001; + odom_.pose.covariance[35] = covariance_[2]; + + odom_.twist.covariance[0] = covariance_[0]; + odom_.twist.covariance[7] = covariance_[1]; + odom_.twist.covariance[14] = 1000000000000.0; + odom_.twist.covariance[21] = 1000000000000.0; + odom_.twist.covariance[28] = 1000000000000.0; + odom_.twist.covariance[35] = covariance_[2]; // Set header odom_.header.frame_id = odometry_frame_; diff --git a/gazebo_plugins/test/test_gazebo_ros_diff_drive.cpp b/gazebo_plugins/test/test_gazebo_ros_diff_drive.cpp index 12084f36a..068d40f1f 100644 --- a/gazebo_plugins/test/test_gazebo_ros_diff_drive.cpp +++ b/gazebo_plugins/test/test_gazebo_ros_diff_drive.cpp @@ -19,19 +19,29 @@ #include #include +#include #define tol 10e-2 using namespace std::literals::chrono_literals; // NOLINT -class GazeboRosDiffDriveTest : public gazebo::ServerFixture +/// Test parameters +struct TestParams { + /// Path to world file + std::string world; }; -TEST_F(GazeboRosDiffDriveTest, Publishing) + +class GazeboRosDiffDriveTest + : public gazebo::ServerFixture, public ::testing::WithParamInterface +{ +}; + +TEST_P(GazeboRosDiffDriveTest, Publishing) { // Load test world and start paused - this->Load("worlds/gazebo_ros_diff_drive.world", true); + this->Load(GetParam().world, true); // World auto world = gazebo::physics::get_world(); @@ -57,7 +67,7 @@ TEST_F(GazeboRosDiffDriveTest, Publishing) }); // Step a bit for model to settle - world->Step(100); + world->Step(200); executor.spin_once(100ms); // Check model state @@ -101,9 +111,16 @@ TEST_F(GazeboRosDiffDriveTest, Publishing) EXPECT_NEAR(0.1, vehicle->WorldAngularVel().Z(), tol); } +INSTANTIATE_TEST_CASE_P(GazeboRosDiffDrive, GazeboRosDiffDriveTest, ::testing::Values( + TestParams({"worlds/gazebo_ros_diff_drive.world"}), + TestParams({"worlds/gazebo_ros_skid_steer_drive.world"}) + ), ); + int main(int argc, char ** argv) { rclcpp::init(argc, argv); ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return ret; } diff --git a/gazebo_plugins/test/worlds/gazebo_ros_skid_steer_drive.world b/gazebo_plugins/test/worlds/gazebo_ros_skid_steer_drive.world new file mode 100644 index 000000000..a41609bcf --- /dev/null +++ b/gazebo_plugins/test/worlds/gazebo_ros_skid_steer_drive.world @@ -0,0 +1,352 @@ + + + + + + model://ground_plane + + + + model://sun + + + + 0 0 0.325 0 -0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + + + + 0.3 + + + + + + 1 + 1 + 0 + 0 + + + + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0.01 + + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + + + + 0.3 + + + + + + 1 + 1 + 0 + 0 + + + + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0.01 + + + + + + + + -0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.5 + + + + + + + 0.5 + + + + + + 1 + 1 + 0 + 0 + + + + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0.01 + + + + + + + + -0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.5 + + + + + + + 0.5 + + + + + + 1 + 1 + 0 + 0 + + + + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0.01 + + + + + + + + -0.957138 -0 -0.125 0 -0 0 + + 1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + + 0.2 + + + + + + + 0.2 + + + + + + + chassis + left_wheel0 + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + right_wheel0 + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + left_wheel1 + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + right_wheel1 + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + caster + + + + + /test + cmd_vel:=cmd_test + odom:=odom_test + + + 2 + left_wheel_joint0 + right_wheel_joint0 + left_wheel_joint1 + right_wheel_joint1 + 1.25 + 1.25 + 0.6 + 1.0 + 20 + 1.0 + true + true + true + odom_frame_test + chassis + + + + + + + diff --git a/gazebo_plugins/worlds/gazebo_ros_skid_steer_drive_demo.world b/gazebo_plugins/worlds/gazebo_ros_skid_steer_drive_demo.world new file mode 100644 index 000000000..5789afaeb --- /dev/null +++ b/gazebo_plugins/worlds/gazebo_ros_skid_steer_drive_demo.world @@ -0,0 +1,391 @@ + + + + + + + model://ground_plane + + + + model://sun + + + + 0 0 0.325 0 -0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.5 + + + + + + + 0.5 + + + + + + 1 + 1 + 0 + 0 + + + + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0.01 + + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.5 + + + + + + + 0.5 + + + + + + 1 + 1 + 0 + 0 + + + + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0.01 + + + + + + + + -0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + + + + 0.3 + + + + + + 1 + 1 + 0 + 0 + + + + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0.01 + + + + + + + + -0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + + + + 0.3 + + + + + + 1 + 1 + 0 + 0 + + + + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0.01 + + + + + + + + -0.957138 -0 -0.125 0 -0 0 + + 1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + + 0.2 + + + + + + + 0.2 + + + + + + + chassis + left_wheel0 + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + right_wheel0 + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + left_wheel1 + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + right_wheel1 + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + caster + + + + + + /demo + cmd_vel:=cmd_demo + odom:=odom_demo + + + + 2 + + + left_wheel_joint0 + right_wheel_joint0 + + + left_wheel_joint1 + right_wheel_joint1 + + + 1.25 + 1.25 + + 1.0 + 0.6 + + + 20 + 1.0 + + + true + true + true + + odom_demo + chassis + + + + + + +