gazebo_ros_planar_move.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2013 Open Source Robotics Foundation
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  *
00016 */
00017 
00018 /*
00019  * Desc: Simple model controller that uses a twist message to move a robot on
00020  *       the xy plane.
00021  * Author: Piyush Khandelwal
00022  * Date: 29 July 2013
00023  */
00024 
00025 #include <gazebo_plugins/gazebo_ros_planar_move.h>
00026 
00027 namespace gazebo 
00028 {
00029 
00030   GazeboRosPlanarMove::GazeboRosPlanarMove() {}
00031 
00032   GazeboRosPlanarMove::~GazeboRosPlanarMove() {}
00033 
00034   // Load the controller
00035   void GazeboRosPlanarMove::Load(physics::ModelPtr parent, 
00036       sdf::ElementPtr sdf) 
00037   {
00038 
00039     parent_ = parent;
00040 
00041     /* Parse parameters */
00042 
00043     robot_namespace_ = "";
00044     if (!sdf->HasElement("robotNamespace")) 
00045     {
00046       ROS_INFO("PlanarMovePlugin missing <robotNamespace>, "
00047           "defaults to \"%s\"", robot_namespace_.c_str());
00048     }
00049     else 
00050     {
00051       robot_namespace_ = 
00052         sdf->GetElement("robotNamespace")->Get<std::string>();
00053     }
00054 
00055     command_topic_ = "cmd_vel";
00056     if (!sdf->HasElement("commandTopic")) 
00057     {
00058       ROS_WARN("PlanarMovePlugin (ns = %s) missing <commandTopic>, "
00059           "defaults to \"%s\"", 
00060           robot_namespace_.c_str(), command_topic_.c_str());
00061     } 
00062     else 
00063     {
00064       command_topic_ = sdf->GetElement("commandTopic")->Get<std::string>();
00065     }
00066 
00067     odometry_topic_ = "odom";
00068     if (!sdf->HasElement("odometryTopic")) 
00069     {
00070       ROS_WARN("PlanarMovePlugin (ns = %s) missing <odometryTopic>, "
00071           "defaults to \"%s\"", 
00072           robot_namespace_.c_str(), odometry_topic_.c_str());
00073     } 
00074     else 
00075     {
00076       odometry_topic_ = sdf->GetElement("odometryTopic")->Get<std::string>();
00077     }
00078 
00079     odometry_frame_ = "odom";
00080     if (!sdf->HasElement("odometryFrame")) 
00081     {
00082       ROS_WARN("PlanarMovePlugin (ns = %s) missing <odometryFrame>, "
00083           "defaults to \"%s\"",
00084           robot_namespace_.c_str(), odometry_frame_.c_str());
00085     }
00086     else 
00087     {
00088       odometry_frame_ = sdf->GetElement("odometryFrame")->Get<std::string>();
00089     }
00090 
00091     robot_base_frame_ = "base_footprint";
00092     if (!sdf->HasElement("robotBaseFrame")) 
00093     {
00094       ROS_WARN("PlanarMovePlugin (ns = %s) missing <robotBaseFrame>, "
00095           "defaults to \"%s\"",
00096           robot_namespace_.c_str(), robot_base_frame_.c_str());
00097     } 
00098     else 
00099     {
00100       robot_base_frame_ = sdf->GetElement("robotBaseFrame")->Get<std::string>();
00101     } 
00102 
00103     odometry_rate_ = 20.0;
00104     if (!sdf->HasElement("odometryRate")) 
00105     {
00106       ROS_WARN("PlanarMovePlugin (ns = %s) missing <odometryRate>, "
00107           "defaults to %f",
00108           robot_namespace_.c_str(), odometry_rate_);
00109     } 
00110     else 
00111     {
00112       odometry_rate_ = sdf->GetElement("odometryRate")->Get<double>();
00113     } 
00114  
00115     last_odom_publish_time_ = parent_->GetWorld()->GetSimTime();
00116     last_odom_pose_ = parent_->GetWorldPose();
00117     x_ = 0;
00118     y_ = 0;
00119     rot_ = 0;
00120     alive_ = true;
00121 
00122     // Ensure that ROS has been initialized and subscribe to cmd_vel
00123     if (!ros::isInitialized()) 
00124     {
00125       ROS_FATAL_STREAM("PlanarMovePlugin (ns = " << robot_namespace_
00126         << "). A ROS node for Gazebo has not been initialized, "
00127         << "unable to load plugin. Load the Gazebo system plugin "
00128         << "'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
00129       return;
00130     }
00131     rosnode_.reset(new ros::NodeHandle(robot_namespace_));
00132 
00133     ROS_DEBUG("OCPlugin (%s) has started!", 
00134         robot_namespace_.c_str());
00135 
00136     tf_prefix_ = tf::getPrefixParam(*rosnode_);
00137     transform_broadcaster_.reset(new tf::TransformBroadcaster());
00138 
00139     // subscribe to the odometry topic
00140     ros::SubscribeOptions so =
00141       ros::SubscribeOptions::create<geometry_msgs::Twist>(command_topic_, 1,
00142           boost::bind(&GazeboRosPlanarMove::cmdVelCallback, this, _1),
00143           ros::VoidPtr(), &queue_);
00144 
00145     vel_sub_ = rosnode_->subscribe(so);
00146     odometry_pub_ = rosnode_->advertise<nav_msgs::Odometry>(odometry_topic_, 1);
00147 
00148     // start custom queue for diff drive
00149     callback_queue_thread_ = 
00150       boost::thread(boost::bind(&GazeboRosPlanarMove::QueueThread, this));
00151 
00152     // listen to the update event (broadcast every simulation iteration)
00153     update_connection_ = 
00154       event::Events::ConnectWorldUpdateBegin(
00155           boost::bind(&GazeboRosPlanarMove::UpdateChild, this));
00156 
00157   }
00158 
00159   // Update the controller
00160   void GazeboRosPlanarMove::UpdateChild() 
00161   {
00162     boost::mutex::scoped_lock scoped_lock(lock);
00163     math::Pose pose = parent_->GetWorldPose();
00164     float yaw = pose.rot.GetYaw();
00165     parent_->SetLinearVel(math::Vector3(
00166           x_ * cosf(yaw) - y_ * sinf(yaw), 
00167           y_ * cosf(yaw) + x_ * sinf(yaw), 
00168           0));
00169     parent_->SetAngularVel(math::Vector3(0, 0, rot_));
00170     if (odometry_rate_ > 0.0) {
00171       common::Time current_time = parent_->GetWorld()->GetSimTime();
00172       double seconds_since_last_update = 
00173         (current_time - last_odom_publish_time_).Double();
00174       if (seconds_since_last_update > (1.0 / odometry_rate_)) {
00175         publishOdometry(seconds_since_last_update);
00176         last_odom_publish_time_ = current_time;
00177       }
00178     }
00179   }
00180 
00181   // Finalize the controller
00182   void GazeboRosPlanarMove::FiniChild() {
00183     alive_ = false;
00184     queue_.clear();
00185     queue_.disable();
00186     rosnode_->shutdown();
00187     callback_queue_thread_.join();
00188   }
00189 
00190   void GazeboRosPlanarMove::cmdVelCallback(
00191       const geometry_msgs::Twist::ConstPtr& cmd_msg) 
00192   {
00193     boost::mutex::scoped_lock scoped_lock(lock);
00194     x_ = cmd_msg->linear.x;
00195     y_ = cmd_msg->linear.y;
00196     rot_ = cmd_msg->angular.z;
00197   }
00198 
00199   void GazeboRosPlanarMove::QueueThread() 
00200   {
00201     static const double timeout = 0.01;
00202     while (alive_ && rosnode_->ok()) 
00203     {
00204       queue_.callAvailable(ros::WallDuration(timeout));
00205     }
00206   }
00207 
00208   void GazeboRosPlanarMove::publishOdometry(double step_time) 
00209   {
00210 
00211     ros::Time current_time = ros::Time::now();
00212     std::string odom_frame = tf::resolve(tf_prefix_, odometry_frame_);
00213     std::string base_footprint_frame = 
00214       tf::resolve(tf_prefix_, robot_base_frame_);
00215 
00216     // getting data for base_footprint to odom transform
00217     math::Pose pose = this->parent_->GetWorldPose();
00218 
00219     tf::Quaternion qt(pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w);
00220     tf::Vector3 vt(pose.pos.x, pose.pos.y, pose.pos.z);
00221 
00222     tf::Transform base_footprint_to_odom(qt, vt);
00223     transform_broadcaster_->sendTransform(
00224         tf::StampedTransform(base_footprint_to_odom, current_time, odom_frame,
00225             base_footprint_frame));
00226 
00227     // publish odom topic
00228     odom_.pose.pose.position.x = pose.pos.x;
00229     odom_.pose.pose.position.y = pose.pos.y;
00230 
00231     odom_.pose.pose.orientation.x = pose.rot.x;
00232     odom_.pose.pose.orientation.y = pose.rot.y;
00233     odom_.pose.pose.orientation.z = pose.rot.z;
00234     odom_.pose.pose.orientation.w = pose.rot.w;
00235     odom_.pose.covariance[0] = 0.00001;
00236     odom_.pose.covariance[7] = 0.00001;
00237     odom_.pose.covariance[14] = 1000000000000.0;
00238     odom_.pose.covariance[21] = 1000000000000.0;
00239     odom_.pose.covariance[28] = 1000000000000.0;
00240     odom_.pose.covariance[35] = 0.001;
00241 
00242     // get velocity in /odom frame
00243     math::Vector3 linear;
00244     linear.x = (pose.pos.x - last_odom_pose_.pos.x) / step_time;
00245     linear.y = (pose.pos.y - last_odom_pose_.pos.y) / step_time;
00246     if (rot_ > M_PI / step_time) 
00247     { 
00248       // we cannot calculate the angular velocity correctly
00249       odom_.twist.twist.angular.z = rot_;
00250     } 
00251     else 
00252     {
00253       float last_yaw = last_odom_pose_.rot.GetYaw();
00254       float current_yaw = pose.rot.GetYaw();
00255       while (current_yaw < last_yaw - M_PI) current_yaw += 2 * M_PI;
00256       while (current_yaw > last_yaw + M_PI) current_yaw -= 2 * M_PI;
00257       float angular_diff = current_yaw - last_yaw;
00258       odom_.twist.twist.angular.z = angular_diff / step_time;
00259     }
00260     last_odom_pose_ = pose;
00261 
00262     // convert velocity to child_frame_id (aka base_footprint)
00263     float yaw = pose.rot.GetYaw();
00264     odom_.twist.twist.linear.x = cosf(yaw) * linear.x + sinf(yaw) * linear.y;
00265     odom_.twist.twist.linear.y = cosf(yaw) * linear.y - sinf(yaw) * linear.x;
00266 
00267     odom_.header.stamp = current_time;
00268     odom_.header.frame_id = odom_frame;
00269     odom_.child_frame_id = base_footprint_frame;
00270 
00271     odometry_pub_.publish(odom_);
00272   }
00273 
00274   GZ_REGISTER_MODEL_PLUGIN(GazeboRosPlanarMove)
00275 }
00276 


gazebo_plugins
Author(s): John Hsu
autogenerated on Thu Jun 6 2019 18:41:09