47 #include <ignition/math/Pose3.hh> 48 #include <ignition/math/Vector3.hh> 54 #include <geometry_msgs/Twist.h> 55 #include <nav_msgs/GetMap.h> 56 #include <nav_msgs/Odometry.h> 57 #include <boost/bind.hpp> 58 #include <boost/thread/mutex.hpp> 107 std::map<std::string, OdomSource> odomOptions;
108 odomOptions[
"encoder"] =
ENCODER;
109 odomOptions[
"world"] =
WORLD;
119 #if GAZEBO_MAJOR_VERSION >= 8 139 ros::SubscribeOptions::create<geometry_msgs::Twist> (
command_topic_, 1,
159 std::vector<physics::JointPtr> joints;
171 for ( std::size_t i = 0; i < joints.size(); i++ ) {
173 #if GAZEBO_MAJOR_VERSION >= 8 176 joint_state_.position[i] = joints[i]->GetAngle ( 0 ).Radian();
178 joint_state_.velocity[i] = joints[i]->GetVelocity ( 0 );
187 std::vector<physics::JointPtr> joints;
192 for ( std::size_t i = 0; i < joints.size(); i++ ) {
193 std::string frame =
gazebo_ros_->resolveTF ( joints[i]->GetName() );
194 std::string parent_frame =
gazebo_ros_->resolveTF ( joints[i]->GetParent()->GetName() );
196 #if GAZEBO_MAJOR_VERSION >= 8 197 ignition::math::Pose3d pose = joints[i]->GetChild()->RelativePose();
199 ignition::math::Pose3d pose = joints[i]->GetChild()->GetRelativePose().Ign();
202 tf::Quaternion qt ( pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z(), pose.Rot().W() );
203 tf::Vector3 vt ( pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z() );
214 #if GAZEBO_MAJOR_VERSION >= 8 215 common::Time current_time =
parent->GetWorld()->SimTime();
217 common::Time current_time =
parent->GetWorld()->GetSimTime();
227 double target_steering_angle =
cmd_.
angle;
229 motorController ( target_wheel_roation_speed, target_steering_angle, seconds_since_last_update );
240 double applied_speed = target_speed;
241 double applied_angle = target_angle;
246 double diff_speed = current_speed - target_speed;
249 applied_speed = current_speed;
265 #if GAZEBO_MAJOR_VERSION >= 8 272 if (target_angle > +M_PI / 2.0)
274 target_angle = +M_PI / 2.0;
276 else if (target_angle < -M_PI / 2.0)
278 target_angle = -M_PI / 2.0;
283 double diff_angle = current_angle - target_angle;
286 double applied_steering_speed = 0;
289 applied_steering_speed = 0;
290 }
else if ( diff_angle < target_speed ) {
318 applied_angle = target_angle;
320 #if GAZEBO_MAJOR_VERSION >= 9 342 boost::mutex::scoped_lock scoped_lock (
lock );
349 static const double timeout = 0.01;
360 #if GAZEBO_MAJOR_VERSION >= 8 361 common::Time current_time =
parent->GetWorld()->SimTime();
363 common::Time current_time =
parent->GetWorld()->GetSimTime();
365 double seconds_since_last_update = ( current_time -
last_odom_update_ ).Double();
373 double theta = ( sl - sr ) /b;
376 double dx = ( sl + sr ) /2.0 * cos (
pose_encoder_.theta + ( sl - sr ) / ( 2.0*b ) );
377 double dy = ( sl + sr ) /2.0 * sin (
pose_encoder_.theta + ( sl - sr ) / ( 2.0*b ) );
378 double dtheta = ( sl - sr ) /b;
384 double w = dtheta/seconds_since_last_update;
385 double v = sqrt ( dx*dx+dy*dy ) /seconds_since_last_update;
392 odom_.pose.pose.position.x = vt.x();
393 odom_.pose.pose.position.y = vt.y();
394 odom_.pose.pose.position.z = vt.z();
396 odom_.pose.pose.orientation.x = qt.x();
397 odom_.pose.pose.orientation.y = qt.y();
398 odom_.pose.pose.orientation.z = qt.z();
399 odom_.pose.pose.orientation.w = qt.w();
401 odom_.twist.twist.angular.z = w;
402 odom_.twist.twist.linear.x = dx/seconds_since_last_update;
403 odom_.twist.twist.linear.y = dy/seconds_since_last_update;
419 vt = tf::Vector3 (
odom_.pose.pose.position.x,
odom_.pose.pose.position.y,
odom_.pose.pose.position.z );
424 #if GAZEBO_MAJOR_VERSION >= 8 425 ignition::math::Pose3d pose =
parent->WorldPose();
427 ignition::math::Pose3d pose =
parent->GetWorldPose().Ign();
429 qt =
tf::Quaternion ( pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z(), pose.Rot().W() );
430 vt = tf::Vector3 ( pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z() );
432 odom_.pose.pose.position.x = vt.x();
433 odom_.pose.pose.position.y = vt.y();
434 odom_.pose.pose.position.z = vt.z();
436 odom_.pose.pose.orientation.x = qt.x();
437 odom_.pose.pose.orientation.y = qt.y();
438 odom_.pose.pose.orientation.z = qt.z();
439 odom_.pose.pose.orientation.w = qt.w();
442 ignition::math::Vector3d linear;
443 #if GAZEBO_MAJOR_VERSION >= 8 444 linear =
parent->WorldLinearVel();
445 odom_.twist.twist.angular.z =
parent->WorldAngularVel().Z();
447 linear =
parent->GetWorldLinearVel().Ign();
448 odom_.twist.twist.angular.z =
parent->GetWorldAngularVel().Ign().Z();
452 float yaw = pose.Rot().Yaw();
453 odom_.twist.twist.linear.x = cosf ( yaw ) * linear.X() + sinf ( yaw ) * linear.Y();
454 odom_.twist.twist.linear.y = cosf ( yaw ) * linear.Y() - sinf ( yaw ) * linear.X();
460 odom_frame, base_footprint_frame ) );
464 odom_.pose.covariance[0] = 0.00001;
465 odom_.pose.covariance[7] = 0.00001;
466 odom_.pose.covariance[14] = 1000000000000.0;
467 odom_.pose.covariance[21] = 1000000000000.0;
468 odom_.pose.covariance[28] = 1000000000000.0;
469 odom_.pose.covariance[35] = 0.001;
473 odom_.header.stamp = current_time;
474 odom_.header.frame_id = odom_frame;
475 odom_.child_frame_id = base_footprint_frame;
double separation_encoder_wheel_
#define ROS_INFO_NAMED(name,...)
bool publishWheelJointState_
void UpdateOdometryEncoder()
updates the relative robot pose based on the wheel encoders
physics::JointPtr joint_wheel_encoder_left_
std::string command_topic_
double steering_angle_tolerance_
void publishWheelJointState()
publishes the wheel tf's
~GazeboRosTricycleDrive()
boost::shared_ptr< tf::TransformBroadcaster > transform_broadcaster_
ros::CallbackQueue queue_
sensor_msgs::JointState joint_state_
physics::JointPtr joint_wheel_encoder_right_
double diameter_encoder_wheel_
GZ_REGISTER_MODEL_PLUGIN(GazeboRosF3D)
void cmdVelCallback(const geometry_msgs::Twist::ConstPtr &cmd_msg)
void publish(const boost::shared_ptr< M > &message) const
event::ConnectionPtr update_connection_
void setRPY(const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
std::string robot_base_frame_
common::Time last_actuator_update_
boost::thread callback_queue_thread_
void publishOdometry(double step_time)
double diameter_actuated_wheel_
ros::Publisher joint_state_publisher_
ros::Publisher odometry_publisher_
ros::Subscriber cmd_vel_subscriber_
std::string odometry_topic_
physics::JointPtr joint_steering_
virtual void UpdateChild()
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
double wheel_deceleration_
common::Time last_odom_update_
std::string odometry_frame_
double wheel_speed_tolerance_
physics::JointPtr joint_wheel_actuated_
void motorController(double target_speed, double target_angle, double dt)
boost::shared_ptr< void > VoidPtr
boost::shared_ptr< GazeboRos > GazeboRosPtr
double wheel_acceleration_
geometry_msgs::Pose2D pose_encoder_