55 #include <ignition/math/Angle.hh> 56 #include <ignition/math/Pose3.hh> 57 #include <ignition/math/Quaternion.hh> 58 #include <ignition/math/Vector3.hh> 97 if (!_sdf->HasElement(
"legacyMode"))
99 ROS_ERROR_NAMED(
"diff_drive",
"GazeboRosDiffDrive Plugin missing <legacyMode>, defaults to true\n" 100 "This setting assumes you have a old package, where the right and left wheel are changed to fix a former code issue\n" 101 "To get rid of this error just set <legacyMode> to false if you just created a new package.\n" 102 "To fix an old package you have to exchange left wheel by the right wheel.\n" 103 "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" 104 "just set <legacyMode> to true.\n" 113 std::map<std::string, OdomSource> odomOptions;
114 odomOptions[
"encoder"] =
ENCODER;
115 odomOptions[
"world"] =
WORLD;
122 joints_[
LEFT]->SetParam (
"fmax", 0, wheel_torque );
128 if (!_sdf->HasElement(
"publishTf")) {
129 ROS_WARN_NAMED(
"diff_drive",
"GazeboRosDiffDrive Plugin (ns = %s) missing <publishTf>, defaults to %d",
132 this->
publish_tf_ = _sdf->GetElement(
"publishTf")->Get<
bool>();
138 #if GAZEBO_MAJOR_VERSION >= 8 169 ros::SubscribeOptions::create<geometry_msgs::Twist>(
command_topic_, 1,
194 #if GAZEBO_MAJOR_VERSION >= 8 216 for (
int i = 0; i < 2; i++ ) {
217 physics::JointPtr joint =
joints_[i];
218 #if GAZEBO_MAJOR_VERSION >= 8 219 double position = joint->Position ( 0 );
221 double position = joint->GetAngle ( 0 ).Radian();
232 for (
int i = 0; i < 2; i++ ) {
235 std::string wheel_parent_frame =
gazebo_ros_->resolveTF(
joints_[i]->GetParent()->GetName ());
237 #if GAZEBO_MAJOR_VERSION >= 8 238 ignition::math::Pose3d poseWheel =
joints_[i]->GetChild()->RelativePose();
240 ignition::math::Pose3d poseWheel =
joints_[i]->GetChild()->GetRelativePose().Ign();
243 tf::Quaternion qt ( poseWheel.Rot().X(), poseWheel.Rot().Y(), poseWheel.Rot().Z(), poseWheel.Rot().W() );
244 tf::Vector3 vt ( poseWheel.Pos().X(), poseWheel.Pos().Y(), poseWheel.Pos().Z() );
262 for (
int i = 0; i < 2; i++ ) {
270 #if GAZEBO_MAJOR_VERSION >= 8 271 common::Time current_time =
parent->GetWorld()->SimTime();
273 common::Time current_time =
parent->GetWorld()->GetSimTime();
275 double seconds_since_last_update = ( current_time -
last_update_time_ ).Double();
285 double current_speed[2];
329 boost::mutex::scoped_lock scoped_lock (
lock );
348 boost::mutex::scoped_lock scoped_lock (
lock );
349 x_ = cmd_msg->linear.x;
350 rot_ = cmd_msg->angular.z;
355 static const double timeout = 0.01;
366 #if GAZEBO_MAJOR_VERSION >= 8 367 common::Time current_time =
parent->GetWorld()->SimTime();
369 common::Time current_time =
parent->GetWorld()->GetSimTime();
371 double seconds_since_last_update = ( current_time -
last_odom_update_ ).Double();
377 double sl = vl * (
wheel_diameter_ / 2.0 ) * seconds_since_last_update;
378 double sr = vr * (
wheel_diameter_ / 2.0 ) * seconds_since_last_update;
379 double ssum = sl + sr;
392 double dx = ( ssum ) /2.0 * cos (
pose_encoder_.theta + ( sdiff ) / ( 2.0*b ) );
393 double dy = ( ssum ) /2.0 * sin (
pose_encoder_.theta + ( sdiff ) / ( 2.0*b ) );
394 double dtheta = ( sdiff ) /b;
400 double w = dtheta/seconds_since_last_update;
401 double v = sqrt ( dx*dx+dy*dy ) /seconds_since_last_update;
408 odom_.pose.pose.position.x = vt.
x();
409 odom_.pose.pose.position.y = vt.
y();
410 odom_.pose.pose.position.z = vt.
z();
412 odom_.pose.pose.orientation.x = qt.x();
413 odom_.pose.pose.orientation.y = qt.y();
414 odom_.pose.pose.orientation.z = qt.z();
415 odom_.pose.pose.orientation.w = qt.w();
417 odom_.twist.twist.angular.z = w;
418 odom_.twist.twist.linear.x = dx/seconds_since_last_update;
419 odom_.twist.twist.linear.y = dy/seconds_since_last_update;
440 #if GAZEBO_MAJOR_VERSION >= 8 441 ignition::math::Pose3d pose =
parent->WorldPose();
443 ignition::math::Pose3d pose =
parent->GetWorldPose().Ign();
445 qt =
tf::Quaternion ( pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z(), pose.Rot().W() );
446 vt =
tf::Vector3 ( pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z() );
448 odom_.pose.pose.position.x = vt.x();
449 odom_.pose.pose.position.y = vt.y();
450 odom_.pose.pose.position.z = vt.z();
452 odom_.pose.pose.orientation.x = qt.x();
453 odom_.pose.pose.orientation.y = qt.y();
454 odom_.pose.pose.orientation.z = qt.z();
455 odom_.pose.pose.orientation.w = qt.w();
458 ignition::math::Vector3d linear;
459 #if GAZEBO_MAJOR_VERSION >= 8 460 linear =
parent->WorldLinearVel();
461 odom_.twist.twist.angular.z =
parent->WorldAngularVel().Z();
463 linear =
parent->GetWorldLinearVel().Ign();
464 odom_.twist.twist.angular.z =
parent->GetWorldAngularVel().Ign().Z();
468 float yaw = pose.Rot().Yaw();
469 odom_.twist.twist.linear.x = cosf ( yaw ) * linear.X() + sinf ( yaw ) * linear.Y();
470 odom_.twist.twist.linear.y = cosf ( yaw ) * linear.Y() - sinf ( yaw ) * linear.X();
477 odom_frame, base_footprint_frame ) );
482 odom_.pose.covariance[0] = 0.00001;
483 odom_.pose.covariance[7] = 0.00001;
484 odom_.pose.covariance[14] = 1000000000000.0;
485 odom_.pose.covariance[21] = 1000000000000.0;
486 odom_.pose.covariance[28] = 1000000000000.0;
487 odom_.pose.covariance[35] = 0.001;
491 odom_.header.stamp = current_time;
492 odom_.header.frame_id = odom_frame;
493 odom_.child_frame_id = base_footprint_frame;
#define ROS_INFO_NAMED(name,...)
void publish(const boost::shared_ptr< M > &message) const
#define ROS_WARN_NAMED(name,...)
boost::shared_ptr< tf::TransformBroadcaster > transform_broadcaster_
boost::thread callback_queue_thread_
void UpdateOdometryEncoder()
void cmdVelCallback(const geometry_msgs::Twist::ConstPtr &cmd_msg)
std::string odometry_frame_
std::string robot_namespace_
ros::Publisher joint_state_publisher_
TFSIMD_FORCE_INLINE const tfScalar & x() const
GZ_REGISTER_MODEL_PLUGIN(GazeboRosF3D)
bool publishWheelJointState_
ros::Publisher odometry_publisher_
common::Time last_update_time_
event::ConnectionPtr update_connection_
TFSIMD_FORCE_INLINE const tfScalar & z() const
void setRPY(const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
void getWheelVelocities()
sensor_msgs::JointState joint_state_
ros::Subscriber cmd_vel_subscriber_
TFSIMD_FORCE_INLINE const tfScalar & y() const
ros::CallbackQueue queue_
TFSIMD_FORCE_INLINE const tfScalar & w() const
std::string odometry_topic_
double wheel_speed_instr_[2]
std::string command_topic_
std::string robot_base_frame_
virtual void UpdateChild()
void publishOdometry(double step_time)
#define ROS_ERROR_NAMED(name,...)
void publishWheelJointState()
publishes the wheel tf's
geometry_msgs::Pose2D pose_encoder_
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
boost::shared_ptr< void > VoidPtr
boost::shared_ptr< GazeboRos > GazeboRosPtr
common::Time last_odom_update_
std::vector< physics::JointPtr > joints_