Merge pull request #397 from Hacks4ROS-forks/fix_inverted_wheels_in_d…

…iff_drive gazebo_plugins fix inverted left and right wheel
ros-simulation · Apr 26, 2016 · cf95acb · cf95acb
2 parents 92c8e83 + 6237ca4
commit cf95acb
Show file tree

Hide file tree

Showing 2 changed files with 39 additions and 8 deletions.
diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h
@@ -125,6 +125,7 @@ namespace gazebo {
       std::string odometry_frame_;
       std::string robot_base_frame_;
       bool publish_tf_;
+      bool legacy_mode_;
       // Custom Callback Queue
       ros::CallbackQueue queue_;
       boost::thread callback_queue_thread_;

diff --git a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp
@@ -85,6 +85,18 @@ void GazeboRosDiffDrive::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf
     gazebo_ros_->getParameter<std::string> ( robot_base_frame_, "robotBaseFrame", "base_footprint" );
     gazebo_ros_->getParameterBoolean ( publishWheelTF_, "publishWheelTF", false );
     gazebo_ros_->getParameterBoolean ( publishWheelJointState_, "publishWheelJointState", false );
+    gazebo_ros_->getParameterBoolean ( legacy_mode_, "legacyMode", true );
+
+    if (!_sdf->HasElement("legacyMode")) 
+    {
+      ROS_ERROR("GazeboRosDiffDrive Plugin missing <legacyMode>, defaults to true\n" 
+	       "This setting assumes you have a old package, where the right and left wheel are changed to fix a former code issue\n"
+	       "To get rid of this error just set <legacyMode> to false if you just created a new package.\n"
+	       "To fix an old package you have to exchange left wheel by the right wheel.\n"
+	       "If you do not want to fix this issue in an old package or your z axis points down instead of the ROS standard defined in REP 103\n"
+	       "just set <legacyMode> to true.\n"
+      );
+    }
 
     gazebo_ros_->getParameter<double> ( wheel_separation_, "wheelSeparation", 0.34 );
     gazebo_ros_->getParameter<double> ( wheel_diameter_, "wheelDiameter", 0.15 );
@@ -317,8 +329,16 @@ void GazeboRosDiffDrive::getWheelVelocities()
     double vr = x_;
     double va = rot_;
 
-    wheel_speed_[LEFT] = vr + va * wheel_separation_ / 2.0;
-    wheel_speed_[RIGHT] = vr - va * wheel_separation_ / 2.0;
+    if(legacy_mode_)
+    {
+      wheel_speed_[LEFT] = vr + va * wheel_separation_ / 2.0;
+      wheel_speed_[RIGHT] = vr - va * wheel_separation_ / 2.0;     
+    }
+    else
+    {
+      wheel_speed_[LEFT] = vr - va * wheel_separation_ / 2.0;
+      wheel_speed_[RIGHT] = vr + va * wheel_separation_ / 2.0;      
+    }
 }
 
 void GazeboRosDiffDrive::cmdVelCallback ( const geometry_msgs::Twist::ConstPtr& cmd_msg )
@@ -350,12 +370,22 @@ void GazeboRosDiffDrive::UpdateOdometryEncoder()
     // 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 theta = ( sl - sr ) /b;
-
-
-    double dx = ( sl + sr ) /2.0 * cos ( pose_encoder_.theta + ( sl - sr ) / ( 2.0*b ) );
-    double dy = ( sl + sr ) /2.0 * sin ( pose_encoder_.theta + ( sl - sr ) / ( 2.0*b ) );
-    double dtheta = ( sl - sr ) /b;
+    double ssum = sl + sr; 
+
+    double sdiff;
+    if(legacy_mode_)
+    {
+      sdiff = sl - sr;
+    }
+    else
+    {
+
+      sdiff = sr - sl;
+    }
+
+    double dx = ( ssum ) /2.0 * cos ( pose_encoder_.theta + ( sdiff ) / ( 2.0*b ) );
+    double dy = ( ssum ) /2.0 * sin ( pose_encoder_.theta + ( sdiff ) / ( 2.0*b ) );
+    double dtheta = ( sdiff ) /b;
 
     pose_encoder_.x += dx;
     pose_encoder_.y += dy;