gazebo_ros_planar_move.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2013 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 
18 /*
19  * Desc: Simple model controller that uses a twist message to move a robot on
20  * the xy plane.
21  * Author: Piyush Khandelwal
22  * Date: 29 July 2013
23  */
24 
26 
27 namespace gazebo
28 {
29 
31 
33 
34  // Load the controller
35  void GazeboRosPlanarMove::Load(physics::ModelPtr parent,
36  sdf::ElementPtr sdf)
37  {
38 
39  parent_ = parent;
40 
41  /* Parse parameters */
42 
43  robot_namespace_ = "";
44  if (!sdf->HasElement("robotNamespace"))
45  {
46  ROS_INFO_NAMED("planar_move", "PlanarMovePlugin missing <robotNamespace>, "
47  "defaults to \"%s\"", robot_namespace_.c_str());
48  }
49  else
50  {
52  sdf->GetElement("robotNamespace")->Get<std::string>();
53  }
54 
55  command_topic_ = "cmd_vel";
56  if (!sdf->HasElement("commandTopic"))
57  {
58  ROS_WARN_NAMED("planar_move", "PlanarMovePlugin (ns = %s) missing <commandTopic>, "
59  "defaults to \"%s\"",
60  robot_namespace_.c_str(), command_topic_.c_str());
61  }
62  else
63  {
64  command_topic_ = sdf->GetElement("commandTopic")->Get<std::string>();
65  }
66 
67  odometry_topic_ = "odom";
68  if (!sdf->HasElement("odometryTopic"))
69  {
70  ROS_WARN_NAMED("planar_move", "PlanarMovePlugin (ns = %s) missing <odometryTopic>, "
71  "defaults to \"%s\"",
72  robot_namespace_.c_str(), odometry_topic_.c_str());
73  }
74  else
75  {
76  odometry_topic_ = sdf->GetElement("odometryTopic")->Get<std::string>();
77  }
78 
79  odometry_frame_ = "odom";
80  if (!sdf->HasElement("odometryFrame"))
81  {
82  ROS_WARN_NAMED("planar_move", "PlanarMovePlugin (ns = %s) missing <odometryFrame>, "
83  "defaults to \"%s\"",
84  robot_namespace_.c_str(), odometry_frame_.c_str());
85  }
86  else
87  {
88  odometry_frame_ = sdf->GetElement("odometryFrame")->Get<std::string>();
89  }
90 
91  robot_base_frame_ = "base_footprint";
92  if (!sdf->HasElement("robotBaseFrame"))
93  {
94  ROS_WARN_NAMED("planar_move", "PlanarMovePlugin (ns = %s) missing <robotBaseFrame>, "
95  "defaults to \"%s\"",
96  robot_namespace_.c_str(), robot_base_frame_.c_str());
97  }
98  else
99  {
100  robot_base_frame_ = sdf->GetElement("robotBaseFrame")->Get<std::string>();
101  }
102 
103  odometry_rate_ = 20.0;
104  if (!sdf->HasElement("odometryRate"))
105  {
106  ROS_WARN_NAMED("planar_move", "PlanarMovePlugin (ns = %s) missing <odometryRate>, "
107  "defaults to %f",
109  }
110  else
111  {
112  odometry_rate_ = sdf->GetElement("odometryRate")->Get<double>();
113  }
114 
115 #if GAZEBO_MAJOR_VERSION >= 8
116  last_odom_publish_time_ = parent_->GetWorld()->SimTime();
117 #else
118  last_odom_publish_time_ = parent_->GetWorld()->GetSimTime();
119 #endif
120 #if GAZEBO_MAJOR_VERSION >= 8
121  last_odom_pose_ = parent_->WorldPose();
122 #else
123  last_odom_pose_ = parent_->GetWorldPose().Ign();
124 #endif
125  x_ = 0;
126  y_ = 0;
127  rot_ = 0;
128  alive_ = true;
129 
130  // Ensure that ROS has been initialized and subscribe to cmd_vel
131  if (!ros::isInitialized())
132  {
133  ROS_FATAL_STREAM_NAMED("planar_move", "PlanarMovePlugin (ns = " << robot_namespace_
134  << "). A ROS node for Gazebo has not been initialized, "
135  << "unable to load plugin. Load the Gazebo system plugin "
136  << "'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
137  return;
138  }
140 
141  ROS_DEBUG_NAMED("planar_move", "OCPlugin (%s) has started",
142  robot_namespace_.c_str());
143 
146 
147  // subscribe to the odometry topic
149  ros::SubscribeOptions::create<geometry_msgs::Twist>(command_topic_, 1,
150  boost::bind(&GazeboRosPlanarMove::cmdVelCallback, this, _1),
151  ros::VoidPtr(), &queue_);
152 
153  vel_sub_ = rosnode_->subscribe(so);
154  odometry_pub_ = rosnode_->advertise<nav_msgs::Odometry>(odometry_topic_, 1);
155 
156  // start custom queue for diff drive
158  boost::thread(boost::bind(&GazeboRosPlanarMove::QueueThread, this));
159 
160  // listen to the update event (broadcast every simulation iteration)
162  event::Events::ConnectWorldUpdateBegin(
163  boost::bind(&GazeboRosPlanarMove::UpdateChild, this));
164 
165  }
166 
167  // Update the controller
169  {
170  boost::mutex::scoped_lock scoped_lock(lock);
171 #if GAZEBO_MAJOR_VERSION >= 8
172  ignition::math::Pose3d pose = parent_->WorldPose();
173 #else
174  ignition::math::Pose3d pose = parent_->GetWorldPose().Ign();
175 #endif
176  float yaw = pose.Rot().Yaw();
177  parent_->SetLinearVel(ignition::math::Vector3d(
178  x_ * cosf(yaw) - y_ * sinf(yaw),
179  y_ * cosf(yaw) + x_ * sinf(yaw),
180  0));
181  parent_->SetAngularVel(ignition::math::Vector3d(0, 0, rot_));
182  if (odometry_rate_ > 0.0) {
183 #if GAZEBO_MAJOR_VERSION >= 8
184  common::Time current_time = parent_->GetWorld()->SimTime();
185 #else
186  common::Time current_time = parent_->GetWorld()->GetSimTime();
187 #endif
188  double seconds_since_last_update =
189  (current_time - last_odom_publish_time_).Double();
190  if (seconds_since_last_update > (1.0 / odometry_rate_)) {
191  publishOdometry(seconds_since_last_update);
192  last_odom_publish_time_ = current_time;
193  }
194  }
195  }
196 
197  // Finalize the controller
199  alive_ = false;
200  queue_.clear();
201  queue_.disable();
202  rosnode_->shutdown();
203  callback_queue_thread_.join();
204  }
205 
207  const geometry_msgs::Twist::ConstPtr& cmd_msg)
208  {
209  boost::mutex::scoped_lock scoped_lock(lock);
210  x_ = cmd_msg->linear.x;
211  y_ = cmd_msg->linear.y;
212  rot_ = cmd_msg->angular.z;
213  }
214 
216  {
217  static const double timeout = 0.01;
218  while (alive_ && rosnode_->ok())
219  {
221  }
222  }
223 
225  {
226 
227  ros::Time current_time = ros::Time::now();
228  std::string odom_frame = tf::resolve(tf_prefix_, odometry_frame_);
229  std::string base_footprint_frame =
231 
232  // getting data for base_footprint to odom transform
233 #if GAZEBO_MAJOR_VERSION >= 8
234  ignition::math::Pose3d pose = this->parent_->WorldPose();
235 #else
236  ignition::math::Pose3d pose = this->parent_->GetWorldPose().Ign();
237 #endif
238 
239  tf::Quaternion qt(pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z(), pose.Rot().W());
240  tf::Vector3 vt(pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z());
241 
242  tf::Transform base_footprint_to_odom(qt, vt);
243  transform_broadcaster_->sendTransform(
244  tf::StampedTransform(base_footprint_to_odom, current_time, odom_frame,
245  base_footprint_frame));
246 
247  // publish odom topic
248  odom_.pose.pose.position.x = pose.Pos().X();
249  odom_.pose.pose.position.y = pose.Pos().Y();
250 
251  odom_.pose.pose.orientation.x = pose.Rot().X();
252  odom_.pose.pose.orientation.y = pose.Rot().Y();
253  odom_.pose.pose.orientation.z = pose.Rot().Z();
254  odom_.pose.pose.orientation.w = pose.Rot().W();
255  odom_.pose.covariance[0] = 0.00001;
256  odom_.pose.covariance[7] = 0.00001;
257  odom_.pose.covariance[14] = 1000000000000.0;
258  odom_.pose.covariance[21] = 1000000000000.0;
259  odom_.pose.covariance[28] = 1000000000000.0;
260  odom_.pose.covariance[35] = 0.001;
261 
262  // get velocity in /odom frame
263  ignition::math::Vector3d linear;
264  linear.X() = (pose.Pos().X() - last_odom_pose_.Pos().X()) / step_time;
265  linear.Y() = (pose.Pos().Y() - last_odom_pose_.Pos().Y()) / step_time;
266  if (rot_ > M_PI / step_time)
267  {
268  // we cannot calculate the angular velocity correctly
269  odom_.twist.twist.angular.z = rot_;
270  }
271  else
272  {
273  float last_yaw = last_odom_pose_.Rot().Yaw();
274  float current_yaw = pose.Rot().Yaw();
275  while (current_yaw < last_yaw - M_PI) current_yaw += 2 * M_PI;
276  while (current_yaw > last_yaw + M_PI) current_yaw -= 2 * M_PI;
277  float angular_diff = current_yaw - last_yaw;
278  odom_.twist.twist.angular.z = angular_diff / step_time;
279  }
280  last_odom_pose_ = pose;
281 
282  // convert velocity to child_frame_id (aka base_footprint)
283  float yaw = pose.Rot().Yaw();
284  odom_.twist.twist.linear.x = cosf(yaw) * linear.X() + sinf(yaw) * linear.Y();
285  odom_.twist.twist.linear.y = cosf(yaw) * linear.Y() - sinf(yaw) * linear.X();
286 
287  odom_.header.stamp = current_time;
288  odom_.header.frame_id = odom_frame;
289  odom_.child_frame_id = base_footprint_frame;
290 
292  }
293 
295 }
boost::shared_ptr< ros::NodeHandle > rosnode_
ignition::math::Pose3d last_odom_pose_
#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,...)
void publishOdometry(double step_time)
ROSCPP_DECL bool isInitialized()
std::string resolve(const std::string &prefix, const std::string &frame_name)
#define ROS_DEBUG_NAMED(name,...)
GZ_REGISTER_MODEL_PLUGIN(GazeboRosF3D)
void Load(physics::ModelPtr parent, sdf::ElementPtr sdf)
void cmdVelCallback(const geometry_msgs::Twist::ConstPtr &cmd_msg)
#define ROS_FATAL_STREAM_NAMED(name, args)
event::ConnectionPtr update_connection_
boost::shared_ptr< tf::TransformBroadcaster > transform_broadcaster_
static Time now()
boost::shared_ptr< void > VoidPtr


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Mar 26 2019 02:31:27