Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

End of legacy for diff drive plugin #707

Merged
merged 1 commit into from
May 29, 2018
Merged

End of legacy for diff drive plugin #707

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
1 change: 0 additions & 1 deletion gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -125,7 +125,6 @@ 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_;
Expand Down
36 changes: 3 additions & 33 deletions gazebo_plugins/src/gazebo_ros_diff_drive.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -88,19 +88,6 @@ 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_NAMED("diff_drive", "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 );
gazebo_ros_->getParameter<double> ( wheel_accel, "wheelAcceleration", 0.0 );
Expand Down Expand Up @@ -327,16 +314,8 @@ void GazeboRosDiffDrive::getWheelVelocities()
double vr = x_;
double va = rot_;

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;
}
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 )
Expand Down Expand Up @@ -374,16 +353,7 @@ void GazeboRosDiffDrive::UpdateOdometryEncoder()
double sr = vr * ( wheel_diameter_ / 2.0 ) * seconds_since_last_update;
double ssum = sl + sr;

double sdiff;
if(legacy_mode_)
{
sdiff = sl - sr;
}
else
{

sdiff = sr - sl;
}
double 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 ) );
Expand Down