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  enable_y_axis_ = true;
56  if (!sdf->HasElement ("enableYAxis"))
57  {
58  ROS_INFO_NAMED("planar_move", "PlanarMovePlugin missing <enableYAxis>, "
59  "defaults to \"%d\"", enable_y_axis_);
60  }
61  else
62  {
63  enable_y_axis_ = sdf->GetElement("enableYAxis")->Get<bool>();
64  }
65 
66  command_topic_ = "cmd_vel";
67  if (!sdf->HasElement("commandTopic"))
68  {
69  ROS_WARN_NAMED("planar_move", "PlanarMovePlugin (ns = %s) missing <commandTopic>, "
70  "defaults to \"%s\"",
71  robot_namespace_.c_str(), command_topic_.c_str());
72  }
73  else
74  {
75  command_topic_ = sdf->GetElement("commandTopic")->Get<std::string>();
76  }
77 
78  odometry_topic_ = "odom";
79  if (!sdf->HasElement("odometryTopic"))
80  {
81  ROS_WARN_NAMED("planar_move", "PlanarMovePlugin (ns = %s) missing <odometryTopic>, "
82  "defaults to \"%s\"",
83  robot_namespace_.c_str(), odometry_topic_.c_str());
84  }
85  else
86  {
87  odometry_topic_ = sdf->GetElement("odometryTopic")->Get<std::string>();
88  }
89 
90  odometry_frame_ = "odom";
91  if (!sdf->HasElement("odometryFrame"))
92  {
93  ROS_WARN_NAMED("planar_move", "PlanarMovePlugin (ns = %s) missing <odometryFrame>, "
94  "defaults to \"%s\"",
95  robot_namespace_.c_str(), odometry_frame_.c_str());
96  }
97  else
98  {
99  odometry_frame_ = sdf->GetElement("odometryFrame")->Get<std::string>();
100  }
101 
102  robot_base_frame_ = "base_footprint";
103  if (!sdf->HasElement("robotBaseFrame"))
104  {
105  ROS_WARN_NAMED("planar_move", "PlanarMovePlugin (ns = %s) missing <robotBaseFrame>, "
106  "defaults to \"%s\"",
107  robot_namespace_.c_str(), robot_base_frame_.c_str());
108  }
109  else
110  {
111  robot_base_frame_ = sdf->GetElement("robotBaseFrame")->Get<std::string>();
112  }
113 
114  odometry_rate_ = 20.0;
115  if (!sdf->HasElement("odometryRate"))
116  {
117  ROS_WARN_NAMED("planar_move", "PlanarMovePlugin (ns = %s) missing <odometryRate>, "
118  "defaults to %f",
120  }
121  else
122  {
123  odometry_rate_ = sdf->GetElement("odometryRate")->Get<double>();
124  }
125 
126 #if GAZEBO_MAJOR_VERSION >= 8
127  last_odom_publish_time_ = parent_->GetWorld()->SimTime();
128 #else
129  last_odom_publish_time_ = parent_->GetWorld()->GetSimTime();
130 #endif
131 #if GAZEBO_MAJOR_VERSION >= 8
132  last_odom_pose_ = parent_->WorldPose();
133 #else
134  last_odom_pose_ = parent_->GetWorldPose().Ign();
135 #endif
136  x_ = 0;
137  y_ = 0;
138  rot_ = 0;
139  alive_ = true;
140 
141  // Ensure that ROS has been initialized and subscribe to cmd_vel
142  if (!ros::isInitialized())
143  {
144  ROS_FATAL_STREAM_NAMED("planar_move", "PlanarMovePlugin (ns = " << robot_namespace_
145  << "). A ROS node for Gazebo has not been initialized, "
146  << "unable to load plugin. Load the Gazebo system plugin "
147  << "'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
148  return;
149  }
151 
152  ROS_DEBUG_NAMED("planar_move", "OCPlugin (%s) has started",
153  robot_namespace_.c_str());
154 
157 
158  // subscribe to the odometry topic
160  ros::SubscribeOptions::create<geometry_msgs::Twist>(command_topic_, 1,
161  boost::bind(&GazeboRosPlanarMove::cmdVelCallback, this, _1),
162  ros::VoidPtr(), &queue_);
163 
164  vel_sub_ = rosnode_->subscribe(so);
165  odometry_pub_ = rosnode_->advertise<nav_msgs::Odometry>(odometry_topic_, 1);
166 
167  // start custom queue for diff drive
169  boost::thread(boost::bind(&GazeboRosPlanarMove::QueueThread, this));
170 
171  // listen to the update event (broadcast every simulation iteration)
173  event::Events::ConnectWorldUpdateBegin(
174  boost::bind(&GazeboRosPlanarMove::UpdateChild, this));
175 
176  }
177 
178  // Update the controller
180  {
181  boost::mutex::scoped_lock scoped_lock(lock);
182 #if GAZEBO_MAJOR_VERSION >= 8
183  ignition::math::Pose3d pose = parent_->WorldPose();
184 #else
185  ignition::math::Pose3d pose = parent_->GetWorldPose().Ign();
186 #endif
187  float yaw = pose.Rot().Yaw();
188  parent_->SetLinearVel(ignition::math::Vector3d(
189  x_ * cosf(yaw) - y_ * sinf(yaw),
190  y_ * cosf(yaw) + x_ * sinf(yaw),
191  0));
192  parent_->SetAngularVel(ignition::math::Vector3d(0, 0, rot_));
193  if (odometry_rate_ > 0.0) {
194 #if GAZEBO_MAJOR_VERSION >= 8
195  common::Time current_time = parent_->GetWorld()->SimTime();
196 #else
197  common::Time current_time = parent_->GetWorld()->GetSimTime();
198 #endif
199  double seconds_since_last_update =
200  (current_time - last_odom_publish_time_).Double();
201  if (seconds_since_last_update > (1.0 / odometry_rate_)) {
202  publishOdometry(seconds_since_last_update);
203  last_odom_publish_time_ = current_time;
204  }
205  }
206  }
207 
208  // Finalize the controller
210  alive_ = false;
211  queue_.clear();
212  queue_.disable();
213  rosnode_->shutdown();
214  callback_queue_thread_.join();
215  }
216 
218  const geometry_msgs::Twist::ConstPtr& cmd_msg)
219  {
220  boost::mutex::scoped_lock scoped_lock(lock);
221  x_ = cmd_msg->linear.x;
222  if (enable_y_axis_)
223  {
224  y_ = cmd_msg->linear.y;
225  }
226  rot_ = cmd_msg->angular.z;
227  }
228 
230  {
231  static const double timeout = 0.01;
232  while (alive_ && rosnode_->ok())
233  {
235  }
236  }
237 
239  {
240 
241  ros::Time current_time = ros::Time::now();
242  std::string odom_frame = tf::resolve(tf_prefix_, odometry_frame_);
243  std::string base_footprint_frame =
245 
246  // getting data for base_footprint to odom transform
247 #if GAZEBO_MAJOR_VERSION >= 8
248  ignition::math::Pose3d pose = this->parent_->WorldPose();
249 #else
250  ignition::math::Pose3d pose = this->parent_->GetWorldPose().Ign();
251 #endif
252 
253  tf::Quaternion qt(pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z(), pose.Rot().W());
254  tf::Vector3 vt(pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z());
255 
256  tf::Transform base_footprint_to_odom(qt, vt);
257  transform_broadcaster_->sendTransform(
258  tf::StampedTransform(base_footprint_to_odom, current_time, odom_frame,
259  base_footprint_frame));
260 
261  // publish odom topic
262  odom_.pose.pose.position.x = pose.Pos().X();
263  odom_.pose.pose.position.y = pose.Pos().Y();
264 
265  odom_.pose.pose.orientation.x = pose.Rot().X();
266  odom_.pose.pose.orientation.y = pose.Rot().Y();
267  odom_.pose.pose.orientation.z = pose.Rot().Z();
268  odom_.pose.pose.orientation.w = pose.Rot().W();
269  odom_.pose.covariance[0] = 0.00001;
270  odom_.pose.covariance[7] = 0.00001;
271  odom_.pose.covariance[14] = 1000000000000.0;
272  odom_.pose.covariance[21] = 1000000000000.0;
273  odom_.pose.covariance[28] = 1000000000000.0;
274  odom_.pose.covariance[35] = 0.001;
275 
276  // get velocity in /odom frame
277  ignition::math::Vector3d linear;
278  linear.X() = (pose.Pos().X() - last_odom_pose_.Pos().X()) / step_time;
279  linear.Y() = (pose.Pos().Y() - last_odom_pose_.Pos().Y()) / step_time;
280  if (rot_ > M_PI / step_time)
281  {
282  // we cannot calculate the angular velocity correctly
283  odom_.twist.twist.angular.z = rot_;
284  }
285  else
286  {
287  float last_yaw = last_odom_pose_.Rot().Yaw();
288  float current_yaw = pose.Rot().Yaw();
289  while (current_yaw < last_yaw - M_PI) current_yaw += 2 * M_PI;
290  while (current_yaw > last_yaw + M_PI) current_yaw -= 2 * M_PI;
291  float angular_diff = current_yaw - last_yaw;
292  odom_.twist.twist.angular.z = angular_diff / step_time;
293  }
294  last_odom_pose_ = pose;
295 
296  // convert velocity to child_frame_id (aka base_footprint)
297  float yaw = pose.Rot().Yaw();
298  odom_.twist.twist.linear.x = cosf(yaw) * linear.X() + sinf(yaw) * linear.Y();
299  odom_.twist.twist.linear.y = cosf(yaw) * linear.Y() - sinf(yaw) * linear.X();
300 
301  odom_.header.stamp = current_time;
302  odom_.header.frame_id = odom_frame;
303  odom_.child_frame_id = base_footprint_frame;
304 
306  }
307 
309 }
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_
bool enable_y_axis_
Enable Y-axis movement.
static Time now()
boost::shared_ptr< void > VoidPtr


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Apr 6 2021 02:19:39