gazebo_ros_force_based_move.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2015 Stefan Kohlbrecher, TU Darmstadt
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 exert
20  * forces on a robot, resulting in motion. Based on the
21  * planar_move plugin by Piyush Khandelwal.
22  * Author: Stefan Kohlbrecher
23  * Date: 06 August 2015
24  */
25 
27 
28 namespace gazebo
29 {
30 
32 
34 
35  // Load the controller
36  void GazeboRosForceBasedMove::Load(physics::ModelPtr parent,
37  sdf::ElementPtr sdf)
38  {
39 
40  parent_ = parent;
41 
42  /* Parse parameters */
43 
44  robot_namespace_ = "";
45  if (!sdf->HasElement("robotNamespace"))
46  {
47  ROS_INFO("PlanarMovePlugin missing <robotNamespace>, "
48  "defaults to \"%s\"", robot_namespace_.c_str());
49  }
50  else
51  {
53  sdf->GetElement("robotNamespace")->Get<std::string>();
54  }
55 
56  command_topic_ = "cmd_vel";
57  if (!sdf->HasElement("commandTopic"))
58  {
59  ROS_WARN("PlanarMovePlugin (ns = %s) missing <commandTopic>, "
60  "defaults to \"%s\"",
61  robot_namespace_.c_str(), command_topic_.c_str());
62  }
63  else
64  {
65  command_topic_ = sdf->GetElement("commandTopic")->Get<std::string>();
66  }
67 
68  odometry_topic_ = "odom";
69  if (!sdf->HasElement("odometryTopic"))
70  {
71  ROS_WARN("PlanarMovePlugin (ns = %s) missing <odometryTopic>, "
72  "defaults to \"%s\"",
73  robot_namespace_.c_str(), odometry_topic_.c_str());
74  }
75  else
76  {
77  odometry_topic_ = sdf->GetElement("odometryTopic")->Get<std::string>();
78  }
79 
80  odometry_frame_ = "odom";
81  if (!sdf->HasElement("odometryFrame"))
82  {
83  ROS_WARN("PlanarMovePlugin (ns = %s) missing <odometryFrame>, "
84  "defaults to \"%s\"",
85  robot_namespace_.c_str(), odometry_frame_.c_str());
86  }
87  else
88  {
89  odometry_frame_ = sdf->GetElement("odometryFrame")->Get<std::string>();
90  }
91 
92 
94  force_x_velocity_p_gain_ = 10000.0;
95  force_y_velocity_p_gain_ = 10000.0;
96 
97  if (sdf->HasElement("yaw_velocity_p_gain"))
98  (sdf->GetElement("yaw_velocity_p_gain")->GetValue()->Get(torque_yaw_velocity_p_gain_));
99 
100  if (sdf->HasElement("x_velocity_p_gain"))
101  (sdf->GetElement("x_velocity_p_gain")->GetValue()->Get(force_x_velocity_p_gain_));
102 
103  if (sdf->HasElement("y_velocity_p_gain"))
104  (sdf->GetElement("y_velocity_p_gain")->GetValue()->Get(force_y_velocity_p_gain_));
105 
106  ROS_INFO_STREAM("ForceBasedMove using gains: yaw: " << torque_yaw_velocity_p_gain_ <<
107  " x: " << force_x_velocity_p_gain_ <<
108  " y: " << force_y_velocity_p_gain_ << "\n");
109 
110  robot_base_frame_ = "base_footprint";
111  if (!sdf->HasElement("robotBaseFrame"))
112  {
113  ROS_WARN("PlanarMovePlugin (ns = %s) missing <robotBaseFrame>, "
114  "defaults to \"%s\"",
115  robot_namespace_.c_str(), robot_base_frame_.c_str());
116  }
117  else
118  {
119  robot_base_frame_ = sdf->GetElement("robotBaseFrame")->Get<std::string>();
120  }
121 
122  ROS_INFO_STREAM("robotBaseFrame for force based move plugin: " << robot_base_frame_ << "\n");
123 
124  this->link_ = parent->GetLink(robot_base_frame_);
125 
126  odometry_rate_ = 20.0;
127  if (!sdf->HasElement("odometryRate"))
128  {
129  ROS_WARN("PlanarMovePlugin (ns = %s) missing <odometryRate>, "
130  "defaults to %f",
132  }
133  else
134  {
135  odometry_rate_ = sdf->GetElement("odometryRate")->Get<double>();
136  }
137 
138  this->publish_odometry_tf_ = true;
139  if (!sdf->HasElement("publishOdometryTf")) {
140  ROS_WARN("PlanarMovePlugin Plugin (ns = %s) missing <publishOdometryTf>, defaults to %s",
141  this->robot_namespace_.c_str(), this->publish_odometry_tf_ ? "true" : "false");
142  } else {
143  this->publish_odometry_tf_ = sdf->GetElement("publishOdometryTf")->Get<bool>();
144  }
145 
146 #if (GAZEBO_MAJOR_VERSION >= 8)
147  last_odom_publish_time_ = parent_->GetWorld()->SimTime();
148  last_odom_pose_ = parent_->WorldPose();
149 #else
150  last_odom_publish_time_ = parent_->GetWorld()->GetSimTime();
151  last_odom_pose_ = parent_->GetWorldPose();
152 #endif
153  x_ = 0;
154  y_ = 0;
155  rot_ = 0;
156  alive_ = true;
157 
159 
160  // Ensure that ROS has been initialized and subscribe to cmd_vel
161  if (!ros::isInitialized())
162  {
163  ROS_FATAL_STREAM("PlanarMovePlugin (ns = " << robot_namespace_
164  << "). A ROS node for Gazebo has not been initialized, "
165  << "unable to load plugin. Load the Gazebo system plugin "
166  << "'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
167  return;
168  }
170 
171  ROS_DEBUG("OCPlugin (%s) has started!",
172  robot_namespace_.c_str());
173 
175 
176  if (publish_odometry_tf_)
178 
179  // subscribe to the odometry topic
181  ros::SubscribeOptions::create<geometry_msgs::Twist>(command_topic_, 1,
182  boost::bind(&GazeboRosForceBasedMove::cmdVelCallback, this, _1),
183  ros::VoidPtr(), &queue_);
184 
185  vel_sub_ = rosnode_->subscribe(so);
186  odometry_pub_ = rosnode_->advertise<nav_msgs::Odometry>(odometry_topic_, 1);
187 
188  // start custom queue for diff drive
190  boost::thread(boost::bind(&GazeboRosForceBasedMove::QueueThread, this));
191 
192  // listen to the update event (broadcast every simulation iteration)
194  event::Events::ConnectWorldUpdateBegin(
195  boost::bind(&GazeboRosForceBasedMove::UpdateChild, this));
196 
197  }
198 
199  // Update the controller
201  {
202  boost::mutex::scoped_lock scoped_lock(lock);
203 #if (GAZEBO_MAJOR_VERSION >= 8)
204  ignition::math::Pose3d pose = parent_->WorldPose();
205 
206  ignition::math::Vector3d angular_vel = parent_->WorldAngularVel();
207 
208  double error = angular_vel.Z() - rot_;
209 
210  link_->AddTorque(ignition::math::Vector3d(0.0, 0.0, -error * torque_yaw_velocity_p_gain_));
211 
212  float yaw = pose.Rot().Yaw();
213 
214  ignition::math::Vector3d linear_vel = parent_->RelativeLinearVel();
215 
216  link_->AddRelativeForce(ignition::math::Vector3d((x_ - linear_vel.X())* force_x_velocity_p_gain_,
217  (y_ - linear_vel.Y())* force_y_velocity_p_gain_,
218  0.0));
219 #else
220  math::Pose pose = parent_->GetWorldPose();
221 
222  math::Vector3 angular_vel = parent_->GetWorldAngularVel();
223 
224  double error = angular_vel.z - rot_;
225 
226  link_->AddTorque(math::Vector3(0.0, 0.0, -error * torque_yaw_velocity_p_gain_));
227 
228  float yaw = pose.rot.GetYaw();
229 
230  math::Vector3 linear_vel = parent_->GetRelativeLinearVel();
231 
232  link_->AddRelativeForce(math::Vector3((x_ - linear_vel.x)* force_x_velocity_p_gain_,
233  (y_ - linear_vel.y)* force_y_velocity_p_gain_,
234  0.0));
235 #endif
236  //parent_->PlaceOnNearestEntityBelow();
237  //parent_->SetLinearVel(math::Vector3(
238  // x_ * cosf(yaw) - y_ * sinf(yaw),
239  // y_ * cosf(yaw) + x_ * sinf(yaw),
240  // 0));
241  //parent_->SetAngularVel(math::Vector3(0, 0, rot_));
242 
243  if (odometry_rate_ > 0.0) {
244 #if (GAZEBO_MAJOR_VERSION >= 8)
245  common::Time current_time = parent_->GetWorld()->SimTime();
246 #else
247  common::Time current_time = parent_->GetWorld()->GetSimTime();
248 #endif
249  double seconds_since_last_update =
250  (current_time - last_odom_publish_time_).Double();
251  if (seconds_since_last_update > (1.0 / odometry_rate_)) {
252  publishOdometry(seconds_since_last_update);
253  last_odom_publish_time_ = current_time;
254  }
255  }
256  }
257 
258  // Finalize the controller
260  alive_ = false;
261  queue_.clear();
262  queue_.disable();
263  rosnode_->shutdown();
264  callback_queue_thread_.join();
265  }
266 
268  const geometry_msgs::Twist::ConstPtr& cmd_msg)
269  {
270  boost::mutex::scoped_lock scoped_lock(lock);
271  x_ = cmd_msg->linear.x;
272  y_ = cmd_msg->linear.y;
273  rot_ = cmd_msg->angular.z;
274  }
275 
277  {
278  static const double timeout = 0.01;
279  while (alive_ && rosnode_->ok())
280  {
282  }
283  }
284 
286  {
287 
288  ros::Time current_time = ros::Time::now();
289  std::string odom_frame = tf::resolve(tf_prefix_, odometry_frame_);
290  std::string base_footprint_frame =
292 
293 #if (GAZEBO_MAJOR_VERSION >= 8)
294  ignition::math::Vector3d angular_vel = parent_->RelativeAngularVel();
295  ignition::math::Vector3d linear_vel = parent_->RelativeLinearVel();
296 
297  odom_transform_= odom_transform_ * this->getTransformForMotion(linear_vel.X(), angular_vel.Z(), step_time);
298 
300  odom_.twist.twist.angular.z = angular_vel.Z();
301  odom_.twist.twist.linear.x = linear_vel.X();
302 #else
303  math::Vector3 angular_vel = parent_->GetRelativeAngularVel();
304  math::Vector3 linear_vel = parent_->GetRelativeLinearVel();
305 
306  odom_transform_= odom_transform_ * this->getTransformForMotion(linear_vel.x, angular_vel.z, step_time);
307 
309  odom_.twist.twist.angular.z = angular_vel.z;
310  odom_.twist.twist.linear.x = linear_vel.x;
311 #endif
312 
313  odom_.header.stamp = current_time;
314  odom_.header.frame_id = odom_frame;
315  odom_.child_frame_id = base_footprint_frame;
316 
317  if (transform_broadcaster_.get()){
318  transform_broadcaster_->sendTransform(
319  tf::StampedTransform(odom_transform_, current_time, odom_frame,
320  base_footprint_frame));
321  }
322 
323  odom_.pose.covariance[0] = 0.001;
324  odom_.pose.covariance[7] = 0.001;
325  odom_.pose.covariance[14] = 1000000000000.0;
326  odom_.pose.covariance[21] = 1000000000000.0;
327  odom_.pose.covariance[28] = 1000000000000.0;
328 
329 #if (GAZEBO_MAJOR_VERSION >= 8)
330  if (std::abs(angular_vel.Z()) < 0.0001) {
331 #else
332  if (std::abs(angular_vel.z) < 0.0001) {
333 #endif
334  odom_.pose.covariance[35] = 0.01;
335  }else{
336  odom_.pose.covariance[35] = 100.0;
337  }
338 
339  odom_.twist.covariance[0] = 0.001;
340  odom_.twist.covariance[7] = 0.001;
341  odom_.twist.covariance[14] = 0.001;
342  odom_.twist.covariance[21] = 1000000000000.0;
343  odom_.twist.covariance[28] = 1000000000000.0;
344 
345 #if (GAZEBO_MAJOR_VERSION >= 8)
346  if (std::abs(angular_vel.Z()) < 0.0001) {
347 #else
348  if (std::abs(angular_vel.z) < 0.0001) {
349 #endif
350  odom_.twist.covariance[35] = 0.01;
351  }else{
352  odom_.twist.covariance[35] = 100.0;
353  }
354 
355 
356 
358  }
359 
360 
361  tf::Transform GazeboRosForceBasedMove::getTransformForMotion(double linear_vel_x, double angular_vel, double timeSeconds) const
362  {
363  tf::Transform tmp;
364  tmp.setIdentity();
365 
366 
367  if (std::abs(angular_vel) < 0.0001) {
368  //Drive straight
369  tmp.setOrigin(tf::Vector3(static_cast<double>(linear_vel_x*timeSeconds), 0.0, 0.0));
370  } else {
371  //Follow circular arc
372  double distChange = linear_vel_x * timeSeconds;
373  double angleChange = angular_vel * timeSeconds;
374 
375  double arcRadius = distChange / angleChange;
376 
377  tmp.setOrigin(tf::Vector3(std::sin(angleChange) * arcRadius,
378  arcRadius - std::cos(angleChange) * arcRadius,
379  0.0));
380  tmp.setRotation(tf::createQuaternionFromYaw(angleChange));
381  }
382 
383  return tmp;
384  }
385 
386  GZ_REGISTER_MODEL_PLUGIN(GazeboRosForceBasedMove)
387 }
388 
TFSIMD_FORCE_INLINE void setRotation(const Quaternion &q)
boost::shared_ptr< ros::NodeHandle > rosnode_
void publish(const boost::shared_ptr< M > &message) const
std::string getPrefixParam(ros::NodeHandle &nh)
physics::LinkPtr link_
A pointer to the Link, where force is applied.
ROSCPP_DECL bool isInitialized()
tf::Transform getTransformForMotion(double linear_vel_x, double angular_vel, double timeSeconds) const
#define ROS_WARN(...)
void setIdentity()
std::string resolve(const std::string &prefix, const std::string &frame_name)
static Quaternion createQuaternionFromYaw(double yaw)
#define ROS_FATAL_STREAM(args)
#define ROS_INFO(...)
void Load(physics::ModelPtr parent, sdf::ElementPtr sdf)
boost::shared_ptr< tf::TransformBroadcaster > transform_broadcaster_
static void poseTFToMsg(const Pose &bt, geometry_msgs::Pose &msg)
#define ROS_INFO_STREAM(args)
static Time now()
void cmdVelCallback(const geometry_msgs::Twist::ConstPtr &cmd_msg)
boost::shared_ptr< void > VoidPtr
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
#define ROS_DEBUG(...)


hector_gazebo_plugins
Author(s): Stefan Kohlbrecher , Johannes Meyer
autogenerated on Wed Jun 5 2019 22:40:23