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 323 ROS_WARN_ONCE(
"The tricycle_drive plugin is using the Joint::SetPosition method without preserving the link velocity.");
324 ROS_WARN_ONCE(
"As a result, gravity will not be simulated correctly for your model.");
326 ROS_WARN_ONCE(
"For details, see https://github.com/ros-simulation/gazebo_ros_pkgs/issues/612");
346 boost::mutex::scoped_lock scoped_lock (
lock );
353 static const double timeout = 0.01;
364 #if GAZEBO_MAJOR_VERSION >= 8 365 common::Time current_time =
parent->GetWorld()->SimTime();
367 common::Time current_time =
parent->GetWorld()->GetSimTime();
369 double seconds_since_last_update = ( current_time -
last_odom_update_ ).Double();
377 double theta = ( sl - sr ) /b;
380 double dx = ( sl + sr ) /2.0 * cos (
pose_encoder_.theta + ( sl - sr ) / ( 2.0*b ) );
381 double dy = ( sl + sr ) /2.0 * sin (
pose_encoder_.theta + ( sl - sr ) / ( 2.0*b ) );
382 double dtheta = ( sl - sr ) /b;
388 double w = dtheta/seconds_since_last_update;
389 double v = sqrt ( dx*dx+dy*dy ) /seconds_since_last_update;
396 odom_.pose.pose.position.x = vt.
x();
397 odom_.pose.pose.position.y = vt.
y();
398 odom_.pose.pose.position.z = vt.
z();
400 odom_.pose.pose.orientation.x = qt.x();
401 odom_.pose.pose.orientation.y = qt.y();
402 odom_.pose.pose.orientation.z = qt.z();
403 odom_.pose.pose.orientation.w = qt.w();
405 odom_.twist.twist.angular.z = w;
406 odom_.twist.twist.linear.x = dx/seconds_since_last_update;
407 odom_.twist.twist.linear.y = dy/seconds_since_last_update;
428 #if GAZEBO_MAJOR_VERSION >= 8 429 ignition::math::Pose3d pose =
parent->WorldPose();
431 ignition::math::Pose3d pose =
parent->GetWorldPose().Ign();
433 qt =
tf::Quaternion ( pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z(), pose.Rot().W() );
434 vt =
tf::Vector3 ( pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z() );
436 odom_.pose.pose.position.x = vt.x();
437 odom_.pose.pose.position.y = vt.y();
438 odom_.pose.pose.position.z = vt.z();
440 odom_.pose.pose.orientation.x = qt.x();
441 odom_.pose.pose.orientation.y = qt.y();
442 odom_.pose.pose.orientation.z = qt.z();
443 odom_.pose.pose.orientation.w = qt.w();
446 ignition::math::Vector3d linear;
447 #if GAZEBO_MAJOR_VERSION >= 8 448 linear =
parent->WorldLinearVel();
449 odom_.twist.twist.angular.z =
parent->WorldAngularVel().Z();
451 linear =
parent->GetWorldLinearVel().Ign();
452 odom_.twist.twist.angular.z =
parent->GetWorldAngularVel().Ign().Z();
456 float yaw = pose.Rot().Yaw();
457 odom_.twist.twist.linear.x = cosf ( yaw ) * linear.X() + sinf ( yaw ) * linear.Y();
458 odom_.twist.twist.linear.y = cosf ( yaw ) * linear.Y() - sinf ( yaw ) * linear.X();
464 odom_frame, base_footprint_frame ) );
468 odom_.pose.covariance[0] = 0.00001;
469 odom_.pose.covariance[7] = 0.00001;
470 odom_.pose.covariance[14] = 1000000000000.0;
471 odom_.pose.covariance[21] = 1000000000000.0;
472 odom_.pose.covariance[28] = 1000000000000.0;
473 odom_.pose.covariance[35] = 0.001;
477 odom_.header.stamp = current_time;
478 odom_.header.frame_id = odom_frame;
479 odom_.child_frame_id = base_footprint_frame;
double separation_encoder_wheel_
#define ROS_INFO_NAMED(name,...)
void publish(const boost::shared_ptr< M > &message) const
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_
TFSIMD_FORCE_INLINE const tfScalar & x() const
GZ_REGISTER_MODEL_PLUGIN(GazeboRosF3D)
void cmdVelCallback(const geometry_msgs::Twist::ConstPtr &cmd_msg)
event::ConnectionPtr update_connection_
TFSIMD_FORCE_INLINE const tfScalar & z() const
void setRPY(const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
std::string robot_base_frame_
#define ROS_WARN_ONCE(...)
common::Time last_actuator_update_
TFSIMD_FORCE_INLINE const tfScalar & y() const
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_
TFSIMD_FORCE_INLINE const tfScalar & w() const
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_