45 #include <ignition/math/Pose3.hh> 46 #include <ignition/math/Vector3.hh> 52 #include <geometry_msgs/Twist.h> 53 #include <nav_msgs/GetMap.h> 54 #include <nav_msgs/Odometry.h> 55 #include <boost/bind.hpp> 56 #include <boost/thread/mutex.hpp> 79 this->
world = _parent->GetWorld();
82 if (!_sdf->HasElement(
"robotNamespace")) {
83 ROS_INFO_NAMED(
"skid_steer_drive",
"GazeboRosSkidSteerDrive Plugin missing <robotNamespace>, defaults to \"%s\"",
87 _sdf->GetElement(
"robotNamespace")->Get<std::string>() +
"/";
91 if (!_sdf->HasElement(
"broadcastTF")) {
92 if (!this->broadcast_tf_)
93 ROS_INFO_NAMED(
"skid_steer_drive",
"GazeboRosSkidSteerDrive Plugin (ns = %s) missing <broadcastTF>, defaults to false.",this->
robot_namespace_.c_str());
94 else ROS_INFO_NAMED(
"skid_steer_drive",
"GazeboRosSkidSteerDrive Plugin (ns = %s) missing <broadcastTF>, defaults to true.",this->
robot_namespace_.c_str());
97 this->broadcast_tf_ = _sdf->GetElement(
"broadcastTF")->Get<
bool>();
102 if (!_sdf->HasElement(
"leftFrontJoint")) {
103 ROS_WARN_NAMED(
"skid_steer_drive",
"GazeboRosSkidSteerDrive Plugin (ns = %s) missing <leftFrontJoint>, defaults to \"%s\"",
106 this->left_front_joint_name_ = _sdf->GetElement(
"leftFrontJoint")->Get<std::string>();
110 if (!_sdf->HasElement(
"rightFrontJoint")) {
111 ROS_WARN_NAMED(
"skid_steer_drive",
"GazeboRosSkidSteerDrive Plugin (ns = %s) missing <rightFrontJoint>, defaults to \"%s\"",
114 this->right_front_joint_name_ = _sdf->GetElement(
"rightFrontJoint")->Get<std::string>();
118 if (!_sdf->HasElement(
"leftRearJoint")) {
119 ROS_WARN_NAMED(
"skid_steer_drive",
"GazeboRosSkidSteerDrive Plugin (ns = %s) missing <leftRearJoint>, defaults to \"%s\"",
122 this->left_rear_joint_name_ = _sdf->GetElement(
"leftRearJoint")->Get<std::string>();
126 if (!_sdf->HasElement(
"rightRearJoint")) {
127 ROS_WARN_NAMED(
"skid_steer_drive",
"GazeboRosSkidSteerDrive Plugin (ns = %s) missing <rightRearJoint>, defaults to \"%s\"",
130 this->right_rear_joint_name_ = _sdf->GetElement(
"rightRearJoint")->Get<std::string>();
140 if (!_sdf->HasElement(
"wheelSeparation")) {
141 ROS_WARN_NAMED(
"skid_steer_drive",
"GazeboRosSkidSteerDrive Plugin (ns = %s) missing <wheelSeparation>, defaults to value from robot_description: %f",
144 this->wheel_separation_ =
145 _sdf->GetElement(
"wheelSeparation")->Get<
double>();
150 if (!_sdf->HasElement(
"wheelDiameter")) {
151 ROS_WARN_NAMED(
"skid_steer_drive",
"GazeboRosSkidSteerDrive Plugin (ns = %s) missing <wheelDiameter>, defaults to %f",
154 this->wheel_diameter_ = _sdf->GetElement(
"wheelDiameter")->Get<
double>();
158 if (!_sdf->HasElement(
"torque")) {
159 ROS_WARN_NAMED(
"skid_steer_drive",
"GazeboRosSkidSteerDrive Plugin (ns = %s) missing <torque>, defaults to %f",
162 this->torque = _sdf->GetElement(
"torque")->Get<
double>();
166 if (!_sdf->HasElement(
"commandTopic")) {
167 ROS_WARN_NAMED(
"skid_steer_drive",
"GazeboRosSkidSteerDrive Plugin (ns = %s) missing <commandTopic>, defaults to \"%s\"",
170 this->command_topic_ = _sdf->GetElement(
"commandTopic")->Get<std::string>();
174 if (!_sdf->HasElement(
"odometryTopic")) {
175 ROS_WARN_NAMED(
"skid_steer_drive",
"GazeboRosSkidSteerDrive Plugin (ns = %s) missing <odometryTopic>, defaults to \"%s\"",
178 this->odometry_topic_ = _sdf->GetElement(
"odometryTopic")->Get<std::string>();
182 if (!_sdf->HasElement(
"odometryFrame")) {
183 ROS_WARN_NAMED(
"skid_steer_drive",
"GazeboRosSkidSteerDrive Plugin (ns = %s) missing <odometryFrame>, defaults to \"%s\"",
186 this->odometry_frame_ = _sdf->GetElement(
"odometryFrame")->Get<std::string>();
190 if (!_sdf->HasElement(
"robotBaseFrame")) {
191 ROS_WARN_NAMED(
"skid_steer_drive",
"GazeboRosSkidSteerDrive Plugin (ns = %s) missing <robotBaseFrame>, defaults to \"%s\"",
194 this->robot_base_frame_ = _sdf->GetElement(
"robotBaseFrame")->Get<std::string>();
198 if (!_sdf->HasElement(
"updateRate")) {
199 ROS_WARN_NAMED(
"skid_steer_drive",
"GazeboRosSkidSteerDrive Plugin (ns = %s) missing <updateRate>, defaults to %f",
202 this->update_rate_ = _sdf->GetElement(
"updateRate")->Get<
double>();
206 if (!_sdf->HasElement(
"covariance_x")) {
207 ROS_WARN_NAMED(
"skid_steer_drive",
"GazeboRosSkidSteerDrive Plugin (ns = %s) missing <covariance_x>, defaults to %f",
210 covariance_x_ = _sdf->GetElement(
"covariance_x")->Get<
double>();
214 if (!_sdf->HasElement(
"covariance_y")) {
215 ROS_WARN_NAMED(
"skid_steer_drive",
"GazeboRosSkidSteerDrive Plugin (ns = %s) missing <covariance_y>, defaults to %f",
218 covariance_y_ = _sdf->GetElement(
"covariance_y")->Get<
double>();
222 if (!_sdf->HasElement(
"covariance_yaw")) {
223 ROS_WARN_NAMED(
"skid_steer_drive",
"GazeboRosSkidSteerDrive Plugin (ns = %s) missing <covariance_yaw>, defaults to %f",
226 covariance_yaw_ = _sdf->GetElement(
"covariance_yaw")->Get<
double>();
230 if (this->update_rate_ > 0.0) {
235 #if GAZEBO_MAJOR_VERSION >= 8 259 "GazeboRosSkidSteerDrive Plugin (ns = %s) couldn't get left front hinge joint named \"%s\"",
267 "GazeboRosSkidSteerDrive Plugin (ns = %s) couldn't get right front hinge joint named \"%s\"",
275 "GazeboRosSkidSteerDrive Plugin (ns = %s) couldn't get left rear hinge joint named \"%s\"",
283 "GazeboRosSkidSteerDrive Plugin (ns = %s) couldn't get right rear hinge joint named \"%s\"",
288 #if GAZEBO_MAJOR_VERSION > 2 303 ROS_FATAL_STREAM_NAMED(
"skid_steer_drive",
"A ROS node for Gazebo has not been initialized, unable to load plugin. " 304 <<
"Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
317 ros::SubscribeOptions::create<geometry_msgs::Twist>(
command_topic_, 1,
331 event::Events::ConnectWorldUpdateBegin(
338 #if GAZEBO_MAJOR_VERSION >= 8 339 common::Time current_time = this->
world->SimTime();
341 common::Time current_time = this->
world->GetSimTime();
343 double seconds_since_last_update =
351 #if GAZEBO_MAJOR_VERSION > 2 378 boost::mutex::scoped_lock scoped_lock(
lock);
392 const geometry_msgs::Twist::ConstPtr& cmd_msg) {
394 boost::mutex::scoped_lock scoped_lock(
lock);
395 x_ = cmd_msg->linear.x;
396 rot_ = cmd_msg->angular.z;
400 static const double timeout = 0.01;
409 std::string odom_frame =
411 std::string base_footprint_frame =
416 #if GAZEBO_MAJOR_VERSION >= 8 417 ignition::math::Pose3d pose = this->
parent->WorldPose();
419 ignition::math::Pose3d pose = this->
parent->GetWorldPose().Ign();
422 tf::Quaternion qt(pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z(), pose.Rot().W());
423 tf::Vector3 vt(pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z());
430 odom_frame, base_footprint_frame));
435 odom_.pose.pose.position.x = pose.Pos().X();
436 odom_.pose.pose.position.y = pose.Pos().Y();
438 odom_.pose.pose.orientation.x = pose.Rot().X();
439 odom_.pose.pose.orientation.y = pose.Rot().Y();
440 odom_.pose.pose.orientation.z = pose.Rot().Z();
441 odom_.pose.pose.orientation.w = pose.Rot().W();
444 odom_.pose.covariance[14] = 1000000000000.0;
445 odom_.pose.covariance[21] = 1000000000000.0;
446 odom_.pose.covariance[28] = 1000000000000.0;
450 ignition::math::Vector3d linear;
451 #if GAZEBO_MAJOR_VERSION >= 8 452 linear = this->
parent->WorldLinearVel();
453 odom_.twist.twist.angular.z = this->
parent->WorldAngularVel().Z();
455 linear = this->
parent->GetWorldLinearVel().Ign();
456 odom_.twist.twist.angular.z = this->
parent->GetWorldAngularVel().Ign().Z();
460 float yaw = pose.Rot().Yaw();
461 odom_.twist.twist.linear.x = cosf(yaw) * linear.X() + sinf(yaw) * linear.Y();
462 odom_.twist.twist.linear.y = cosf(yaw) * linear.Y() - sinf(yaw) * linear.X();
465 odom_.twist.covariance[14] = 1000000000000.0;
466 odom_.twist.covariance[21] = 1000000000000.0;
467 odom_.twist.covariance[28] = 1000000000000.0;
470 odom_.header.stamp = current_time;
471 odom_.header.frame_id = odom_frame;
472 odom_.child_frame_id = base_footprint_frame;
common::Time last_update_time_
std::string robot_namespace_
physics::JointPtr joints[4]
ros::Publisher odometry_publisher_
#define ROS_INFO_NAMED(name,...)
void publish(const boost::shared_ptr< M > &message) const
std::string getPrefixParam(ros::NodeHandle &nh)
#define ROS_WARN_NAMED(name,...)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
std::string left_front_joint_name_
ROSCPP_DECL bool isInitialized()
std::string odometry_frame_
void publishOdometry(double step_time)
std::string right_rear_joint_name_
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
std::string resolve(const std::string &prefix, const std::string &frame_name)
~GazeboRosSkidSteerDrive()
std::string command_topic_
GZ_REGISTER_MODEL_PLUGIN(GazeboRosF3D)
std::string robot_base_frame_
void cmdVelCallback(const geometry_msgs::Twist::ConstPtr &cmd_msg)
#define ROS_FATAL_STREAM_NAMED(name, args)
std::string left_rear_joint_name_
event::ConnectionPtr update_connection_
tf::TransformBroadcaster * transform_broadcaster_
virtual void UpdateChild()
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void getWheelVelocities()
std::string right_front_joint_name_
boost::thread callback_queue_thread_
GazeboRosSkidSteerDrive()
boost::shared_ptr< void > VoidPtr
ros::CallbackQueue queue_
ros::Subscriber cmd_vel_subscriber_
ros::NodeHandle * rosnode_
std::string odometry_topic_