diff_drive_controller.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, PAL Robotics, S.L.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the PAL Robotics nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /*
36  * Author: Bence Magyar
37  */
38 
39 #include <cmath>
40 
41 #include <tf/transform_datatypes.h>
42 
43 #include <urdf_parser/urdf_parser.h>
44 
45 #include <urdf/urdfdom_compatibility.h>
46 
47 #include <boost/assign.hpp>
48 
50 
51 static double euclideanOfVectors(const urdf::Vector3& vec1, const urdf::Vector3& vec2)
52 {
53  return std::sqrt(std::pow(vec1.x-vec2.x,2) +
54  std::pow(vec1.y-vec2.y,2) +
55  std::pow(vec1.z-vec2.z,2));
56 }
57 
58 /*
59 * \brief Check that a link exists and has a geometry collision.
60 * \param link The link
61 * \return true if the link has a collision element with geometry
62 */
63 static bool hasCollisionGeometry(const urdf::LinkConstSharedPtr& link)
64 {
65  if (!link)
66  {
67  ROS_ERROR("Link == NULL.");
68  return false;
69  }
70 
71  if (!link->collision)
72  {
73  ROS_ERROR_STREAM("Link " << link->name << " does not have collision description. Add collision description for link to urdf.");
74  return false;
75  }
76 
77  if (!link->collision->geometry)
78  {
79  ROS_ERROR_STREAM("Link " << link->name << " does not have collision geometry description. Add collision geometry description for link to urdf.");
80  return false;
81  }
82  return true;
83 }
84 
85 /*
86  * \brief Check if the link is modeled as a cylinder
87  * \param link Link
88  * \return true if the link is modeled as a Cylinder; false otherwise
89  */
90 static bool isCylinder(const urdf::LinkConstSharedPtr& link)
91 {
92  if (!hasCollisionGeometry(link))
93  {
94  return false;
95  }
96 
97  if (link->collision->geometry->type != urdf::Geometry::CYLINDER)
98  {
99  ROS_DEBUG_STREAM("Link " << link->name << " does not have cylinder geometry");
100  return false;
101  }
102 
103  return true;
104 }
105 
106 /*
107  * \brief Check if the link is modeled as a sphere
108  * \param link Link
109  * \return true if the link is modeled as a Sphere; false otherwise
110  */
111 static bool isSphere(const urdf::LinkConstSharedPtr& link)
112 {
113  if (!hasCollisionGeometry(link))
114  {
115  return false;
116  }
117 
118  if (link->collision->geometry->type != urdf::Geometry::SPHERE)
119  {
120  ROS_DEBUG_STREAM("Link " << link->name << " does not have sphere geometry");
121  return false;
122  }
123 
124  return true;
125 }
126 
127 /*
128  * \brief Get the wheel radius
129  * \param [in] wheel_link Wheel link
130  * \param [out] wheel_radius Wheel radius [m]
131  * \return true if the wheel radius was found; false otherwise
132  */
133 static bool getWheelRadius(const urdf::LinkConstSharedPtr& wheel_link, double& wheel_radius)
134 {
135  if (isCylinder(wheel_link))
136  {
137  wheel_radius = (static_cast<urdf::Cylinder*>(wheel_link->collision->geometry.get()))->radius;
138  return true;
139  }
140  else if (isSphere(wheel_link))
141  {
142  wheel_radius = (static_cast<urdf::Sphere*>(wheel_link->collision->geometry.get()))->radius;
143  return true;
144  }
145 
146  ROS_ERROR_STREAM("Wheel link " << wheel_link->name << " is NOT modeled as a cylinder or sphere!");
147  return false;
148 }
149 
150 namespace diff_drive_controller{
151 
153  : open_loop_(false)
154  , command_struct_()
155  , wheel_separation_(0.0)
156  , wheel_radius_(0.0)
157  , wheel_separation_multiplier_(1.0)
158  , left_wheel_radius_multiplier_(1.0)
159  , right_wheel_radius_multiplier_(1.0)
160  , cmd_vel_timeout_(0.5)
161  , allow_multiple_cmd_vel_publishers_(true)
162  , base_frame_id_("base_link")
163  , odom_frame_id_("odom")
164  , enable_odom_tf_(true)
165  , wheel_joints_size_(0)
166  , publish_cmd_(false)
167  {
168  }
169 
171  ros::NodeHandle& root_nh,
172  ros::NodeHandle &controller_nh)
173  {
174  const std::string complete_ns = controller_nh.getNamespace();
175  std::size_t id = complete_ns.find_last_of("/");
176  name_ = complete_ns.substr(id + 1);
177 
178  // Get joint names from the parameter server
179  std::vector<std::string> left_wheel_names, right_wheel_names;
180  if (!getWheelNames(controller_nh, "left_wheel", left_wheel_names) or
181  !getWheelNames(controller_nh, "right_wheel", right_wheel_names))
182  {
183  return false;
184  }
185 
186  if (left_wheel_names.size() != right_wheel_names.size())
187  {
189  "#left wheels (" << left_wheel_names.size() << ") != " <<
190  "#right wheels (" << right_wheel_names.size() << ").");
191  return false;
192  }
193  else
194  {
195  wheel_joints_size_ = left_wheel_names.size();
196 
199  }
200 
201  // Odometry related:
202  double publish_rate;
203  controller_nh.param("publish_rate", publish_rate, 50.0);
204  ROS_INFO_STREAM_NAMED(name_, "Controller state will be published at "
205  << publish_rate << "Hz.");
206  publish_period_ = ros::Duration(1.0 / publish_rate);
207 
208  controller_nh.param("open_loop", open_loop_, open_loop_);
209 
210  controller_nh.param("wheel_separation_multiplier", wheel_separation_multiplier_, wheel_separation_multiplier_);
211  ROS_INFO_STREAM_NAMED(name_, "Wheel separation will be multiplied by "
212  << wheel_separation_multiplier_ << ".");
213 
214  if (controller_nh.hasParam("wheel_radius_multiplier"))
215  {
216  double wheel_radius_multiplier;
217  controller_nh.getParam("wheel_radius_multiplier", wheel_radius_multiplier);
218 
219  left_wheel_radius_multiplier_ = wheel_radius_multiplier;
220  right_wheel_radius_multiplier_ = wheel_radius_multiplier;
221  }
222  else
223  {
224  controller_nh.param("left_wheel_radius_multiplier", left_wheel_radius_multiplier_, left_wheel_radius_multiplier_);
225  controller_nh.param("right_wheel_radius_multiplier", right_wheel_radius_multiplier_, right_wheel_radius_multiplier_);
226  }
227 
228  ROS_INFO_STREAM_NAMED(name_, "Left wheel radius will be multiplied by "
230  ROS_INFO_STREAM_NAMED(name_, "Right wheel radius will be multiplied by "
232 
233  int velocity_rolling_window_size = 10;
234  controller_nh.param("velocity_rolling_window_size", velocity_rolling_window_size, velocity_rolling_window_size);
235  ROS_INFO_STREAM_NAMED(name_, "Velocity rolling window size of "
236  << velocity_rolling_window_size << ".");
237 
238  odometry_.setVelocityRollingWindowSize(velocity_rolling_window_size);
239 
240  // Twist command related:
241  controller_nh.param("cmd_vel_timeout", cmd_vel_timeout_, cmd_vel_timeout_);
242  ROS_INFO_STREAM_NAMED(name_, "Velocity commands will be considered old if they are older than "
243  << cmd_vel_timeout_ << "s.");
244 
245  controller_nh.param("allow_multiple_cmd_vel_publishers", allow_multiple_cmd_vel_publishers_, allow_multiple_cmd_vel_publishers_);
246  ROS_INFO_STREAM_NAMED(name_, "Allow mutiple cmd_vel publishers is "
247  << (allow_multiple_cmd_vel_publishers_?"enabled":"disabled"));
248 
249  controller_nh.param("base_frame_id", base_frame_id_, base_frame_id_);
250  ROS_INFO_STREAM_NAMED(name_, "Base frame_id set to " << base_frame_id_);
251 
252  controller_nh.param("odom_frame_id", odom_frame_id_, odom_frame_id_);
253  ROS_INFO_STREAM_NAMED(name_, "Odometry frame_id set to " << odom_frame_id_);
254 
255  controller_nh.param("enable_odom_tf", enable_odom_tf_, enable_odom_tf_);
256  ROS_INFO_STREAM_NAMED(name_, "Publishing to tf is " << (enable_odom_tf_?"enabled":"disabled"));
257 
258  // Velocity and acceleration limits:
259  controller_nh.param("linear/x/has_velocity_limits" , limiter_lin_.has_velocity_limits , limiter_lin_.has_velocity_limits );
260  controller_nh.param("linear/x/has_acceleration_limits", limiter_lin_.has_acceleration_limits, limiter_lin_.has_acceleration_limits);
261  controller_nh.param("linear/x/has_jerk_limits" , limiter_lin_.has_jerk_limits , limiter_lin_.has_jerk_limits );
262  controller_nh.param("linear/x/max_velocity" , limiter_lin_.max_velocity , limiter_lin_.max_velocity );
263  controller_nh.param("linear/x/min_velocity" , limiter_lin_.min_velocity , -limiter_lin_.max_velocity );
264  controller_nh.param("linear/x/max_acceleration" , limiter_lin_.max_acceleration , limiter_lin_.max_acceleration );
265  controller_nh.param("linear/x/min_acceleration" , limiter_lin_.min_acceleration , -limiter_lin_.max_acceleration );
266  controller_nh.param("linear/x/max_jerk" , limiter_lin_.max_jerk , limiter_lin_.max_jerk );
267  controller_nh.param("linear/x/min_jerk" , limiter_lin_.min_jerk , -limiter_lin_.max_jerk );
268 
269  controller_nh.param("angular/z/has_velocity_limits" , limiter_ang_.has_velocity_limits , limiter_ang_.has_velocity_limits );
270  controller_nh.param("angular/z/has_acceleration_limits", limiter_ang_.has_acceleration_limits, limiter_ang_.has_acceleration_limits);
271  controller_nh.param("angular/z/has_jerk_limits" , limiter_ang_.has_jerk_limits , limiter_ang_.has_jerk_limits );
272  controller_nh.param("angular/z/max_velocity" , limiter_ang_.max_velocity , limiter_ang_.max_velocity );
273  controller_nh.param("angular/z/min_velocity" , limiter_ang_.min_velocity , -limiter_ang_.max_velocity );
274  controller_nh.param("angular/z/max_acceleration" , limiter_ang_.max_acceleration , limiter_ang_.max_acceleration );
275  controller_nh.param("angular/z/min_acceleration" , limiter_ang_.min_acceleration , -limiter_ang_.max_acceleration );
276  controller_nh.param("angular/z/max_jerk" , limiter_ang_.max_jerk , limiter_ang_.max_jerk );
277  controller_nh.param("angular/z/min_jerk" , limiter_ang_.min_jerk , -limiter_ang_.max_jerk );
278 
279  // Publish limited velocity:
280  controller_nh.param("publish_cmd", publish_cmd_, publish_cmd_);
281 
282  // If either parameter is not available, we need to look up the value in the URDF
283  bool lookup_wheel_separation = !controller_nh.getParam("wheel_separation", wheel_separation_);
284  bool lookup_wheel_radius = !controller_nh.getParam("wheel_radius", wheel_radius_);
285 
286  if (!setOdomParamsFromUrdf(root_nh,
287  left_wheel_names[0],
288  right_wheel_names[0],
289  lookup_wheel_separation,
290  lookup_wheel_radius))
291  {
292  return false;
293  }
294 
295  // Regardless of how we got the separation and radius, use them
296  // to set the odometry parameters
298  const double lwr = left_wheel_radius_multiplier_ * wheel_radius_;
299  const double rwr = right_wheel_radius_multiplier_ * wheel_radius_;
300  odometry_.setWheelParams(ws, lwr, rwr);
302  "Odometry params : wheel separation " << ws
303  << ", left wheel radius " << lwr
304  << ", right wheel radius " << rwr);
305 
306  setOdomPubFields(root_nh, controller_nh);
307 
308  if (publish_cmd_)
309  {
310  cmd_vel_pub_.reset(new realtime_tools::RealtimePublisher<geometry_msgs::TwistStamped>(controller_nh, "cmd_vel_out", 100));
311  }
312 
313  // Get the joint object to use in the realtime loop
314  for (size_t i = 0; i < wheel_joints_size_; ++i)
315  {
317  "Adding left wheel with joint name: " << left_wheel_names[i]
318  << " and right wheel with joint name: " << right_wheel_names[i]);
319  left_wheel_joints_[i] = hw->getHandle(left_wheel_names[i]); // throws on failure
320  right_wheel_joints_[i] = hw->getHandle(right_wheel_names[i]); // throws on failure
321  }
322 
323  sub_command_ = controller_nh.subscribe("cmd_vel", 1, &DiffDriveController::cmdVelCallback, this);
324 
325  // Initialize dynamic parameters
326  DynamicParams dynamic_params;
330 
331  dynamic_params.publish_rate = publish_rate;
332  dynamic_params.enable_odom_tf = enable_odom_tf_;
333 
334  dynamic_params_.writeFromNonRT(dynamic_params);
335 
336  // Initialize dynamic_reconfigure server
337  DiffDriveControllerConfig config;
338  config.left_wheel_radius_multiplier = left_wheel_radius_multiplier_;
339  config.right_wheel_radius_multiplier = right_wheel_radius_multiplier_;
340  config.wheel_separation_multiplier = wheel_separation_multiplier_;
341 
342  config.publish_rate = publish_rate;
343  config.enable_odom_tf = enable_odom_tf_;
344 
345  dyn_reconf_server_ = boost::make_shared<ReconfigureServer>(controller_nh);
346  dyn_reconf_server_->updateConfig(config);
347  dyn_reconf_server_->setCallback(boost::bind(&DiffDriveController::reconfCallback, this, _1, _2));
348 
349  return true;
350  }
351 
352  void DiffDriveController::update(const ros::Time& time, const ros::Duration& period)
353  {
354  // update parameter from dynamic reconf
356 
357  // Apply (possibly new) multipliers:
359  const double lwr = left_wheel_radius_multiplier_ * wheel_radius_;
360  const double rwr = right_wheel_radius_multiplier_ * wheel_radius_;
361 
362  odometry_.setWheelParams(ws, lwr, rwr);
363 
364  // COMPUTE AND PUBLISH ODOMETRY
365  if (open_loop_)
366  {
368  }
369  else
370  {
371  double left_pos = 0.0;
372  double right_pos = 0.0;
373  for (size_t i = 0; i < wheel_joints_size_; ++i)
374  {
375  const double lp = left_wheel_joints_[i].getPosition();
376  const double rp = right_wheel_joints_[i].getPosition();
377  if (std::isnan(lp) || std::isnan(rp))
378  return;
379 
380  left_pos += lp;
381  right_pos += rp;
382  }
383  left_pos /= wheel_joints_size_;
384  right_pos /= wheel_joints_size_;
385 
386  // Estimate linear and angular velocity using joint information
387  odometry_.update(left_pos, right_pos, time);
388  }
389 
390  // Publish odometry message
392  {
394  // Compute and store orientation info
395  const geometry_msgs::Quaternion orientation(
397 
398  // Populate odom message and publish
399  if (odom_pub_->trylock())
400  {
401  odom_pub_->msg_.header.stamp = time;
402  odom_pub_->msg_.pose.pose.position.x = odometry_.getX();
403  odom_pub_->msg_.pose.pose.position.y = odometry_.getY();
404  odom_pub_->msg_.pose.pose.orientation = orientation;
405  odom_pub_->msg_.twist.twist.linear.x = odometry_.getLinear();
406  odom_pub_->msg_.twist.twist.angular.z = odometry_.getAngular();
407  odom_pub_->unlockAndPublish();
408  }
409 
410  // Publish tf /odom frame
411  if (enable_odom_tf_ && tf_odom_pub_->trylock())
412  {
413  geometry_msgs::TransformStamped& odom_frame = tf_odom_pub_->msg_.transforms[0];
414  odom_frame.header.stamp = time;
415  odom_frame.transform.translation.x = odometry_.getX();
416  odom_frame.transform.translation.y = odometry_.getY();
417  odom_frame.transform.rotation = orientation;
418  tf_odom_pub_->unlockAndPublish();
419  }
420  }
421 
422  // MOVE ROBOT
423  // Retreive current velocity command and time step:
424  Commands curr_cmd = *(command_.readFromRT());
425  const double dt = (time - curr_cmd.stamp).toSec();
426 
427  // Brake if cmd_vel has timeout:
428  if (dt > cmd_vel_timeout_)
429  {
430  curr_cmd.lin = 0.0;
431  curr_cmd.ang = 0.0;
432  }
433 
434  // Limit velocities and accelerations:
435  const double cmd_dt(period.toSec());
436 
437  limiter_lin_.limit(curr_cmd.lin, last0_cmd_.lin, last1_cmd_.lin, cmd_dt);
438  limiter_ang_.limit(curr_cmd.ang, last0_cmd_.ang, last1_cmd_.ang, cmd_dt);
439 
441  last0_cmd_ = curr_cmd;
442 
443  // Publish limited velocity:
444  if (publish_cmd_ && cmd_vel_pub_ && cmd_vel_pub_->trylock())
445  {
446  cmd_vel_pub_->msg_.header.stamp = time;
447  cmd_vel_pub_->msg_.twist.linear.x = curr_cmd.lin;
448  cmd_vel_pub_->msg_.twist.angular.z = curr_cmd.ang;
449  cmd_vel_pub_->unlockAndPublish();
450  }
451 
452  // Compute wheels velocities:
453  const double vel_left = (curr_cmd.lin - curr_cmd.ang * ws / 2.0)/lwr;
454  const double vel_right = (curr_cmd.lin + curr_cmd.ang * ws / 2.0)/rwr;
455 
456  // Set wheels velocities:
457  for (size_t i = 0; i < wheel_joints_size_; ++i)
458  {
459  left_wheel_joints_[i].setCommand(vel_left);
460  right_wheel_joints_[i].setCommand(vel_right);
461  }
462  }
463 
465  {
466  brake();
467 
468  // Register starting time used to keep fixed rate
470 
471  odometry_.init(time);
472  }
473 
475  {
476  brake();
477  }
478 
480  {
481  const double vel = 0.0;
482  for (size_t i = 0; i < wheel_joints_size_; ++i)
483  {
484  left_wheel_joints_[i].setCommand(vel);
485  right_wheel_joints_[i].setCommand(vel);
486  }
487  }
488 
489  void DiffDriveController::cmdVelCallback(const geometry_msgs::Twist& command)
490  {
491  if (isRunning())
492  {
493  // check that we don't have multiple publishers on the command topic
495  {
497  << " publishers. Only 1 publisher is allowed. Going to brake.");
498  brake();
499  return;
500  }
501 
502  command_struct_.ang = command.angular.z;
503  command_struct_.lin = command.linear.x;
505  command_.writeFromNonRT (command_struct_);
507  "Added values to command. "
508  << "Ang: " << command_struct_.ang << ", "
509  << "Lin: " << command_struct_.lin << ", "
510  << "Stamp: " << command_struct_.stamp);
511  }
512  else
513  {
514  ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running.");
515  }
516  }
517 
519  const std::string& wheel_param,
520  std::vector<std::string>& wheel_names)
521  {
522  XmlRpc::XmlRpcValue wheel_list;
523  if (!controller_nh.getParam(wheel_param, wheel_list))
524  {
526  "Couldn't retrieve wheel param '" << wheel_param << "'.");
527  return false;
528  }
529 
530  if (wheel_list.getType() == XmlRpc::XmlRpcValue::TypeArray)
531  {
532  if (wheel_list.size() == 0)
533  {
535  "Wheel param '" << wheel_param << "' is an empty list");
536  return false;
537  }
538 
539  for (int i = 0; i < wheel_list.size(); ++i)
540  {
541  if (wheel_list[i].getType() != XmlRpc::XmlRpcValue::TypeString)
542  {
544  "Wheel param '" << wheel_param << "' #" << i <<
545  " isn't a string.");
546  return false;
547  }
548  }
549 
550  wheel_names.resize(wheel_list.size());
551  for (int i = 0; i < wheel_list.size(); ++i)
552  {
553  wheel_names[i] = static_cast<std::string>(wheel_list[i]);
554  }
555  }
556  else if (wheel_list.getType() == XmlRpc::XmlRpcValue::TypeString)
557  {
558  wheel_names.push_back(wheel_list);
559  }
560  else
561  {
563  "Wheel param '" << wheel_param <<
564  "' is neither a list of strings nor a string.");
565  return false;
566  }
567 
568  return true;
569  }
570 
572  const std::string& left_wheel_name,
573  const std::string& right_wheel_name,
574  bool lookup_wheel_separation,
575  bool lookup_wheel_radius)
576  {
577  if (!(lookup_wheel_separation || lookup_wheel_radius))
578  {
579  // Short-circuit in case we don't need to look up anything, so we don't have to parse the URDF
580  return true;
581  }
582 
583  // Parse robot description
584  const std::string model_param_name = "robot_description";
585  bool res = root_nh.hasParam(model_param_name);
586  std::string robot_model_str="";
587  if (!res || !root_nh.getParam(model_param_name,robot_model_str))
588  {
589  ROS_ERROR_NAMED(name_, "Robot descripion couldn't be retrieved from param server.");
590  return false;
591  }
592 
593  urdf::ModelInterfaceSharedPtr model(urdf::parseURDF(robot_model_str));
594 
595  urdf::JointConstSharedPtr left_wheel_joint(model->getJoint(left_wheel_name));
596  urdf::JointConstSharedPtr right_wheel_joint(model->getJoint(right_wheel_name));
597 
598  if (lookup_wheel_separation)
599  {
600  // Get wheel separation
601  if (!left_wheel_joint)
602  {
603  ROS_ERROR_STREAM_NAMED(name_, left_wheel_name
604  << " couldn't be retrieved from model description");
605  return false;
606  }
607 
608  if (!right_wheel_joint)
609  {
610  ROS_ERROR_STREAM_NAMED(name_, right_wheel_name
611  << " couldn't be retrieved from model description");
612  return false;
613  }
614 
615  ROS_INFO_STREAM("left wheel to origin: " << left_wheel_joint->parent_to_joint_origin_transform.position.x << ","
616  << left_wheel_joint->parent_to_joint_origin_transform.position.y << ", "
617  << left_wheel_joint->parent_to_joint_origin_transform.position.z);
618  ROS_INFO_STREAM("right wheel to origin: " << right_wheel_joint->parent_to_joint_origin_transform.position.x << ","
619  << right_wheel_joint->parent_to_joint_origin_transform.position.y << ", "
620  << right_wheel_joint->parent_to_joint_origin_transform.position.z);
621 
622  wheel_separation_ = euclideanOfVectors(left_wheel_joint->parent_to_joint_origin_transform.position,
623  right_wheel_joint->parent_to_joint_origin_transform.position);
624 
625  }
626 
627  if (lookup_wheel_radius)
628  {
629  // Get wheel radius
630  if (!getWheelRadius(model->getLink(left_wheel_joint->child_link_name), wheel_radius_))
631  {
632  ROS_ERROR_STREAM_NAMED(name_, "Couldn't retrieve " << left_wheel_name << " wheel radius");
633  return false;
634  }
635  }
636 
637  return true;
638  }
639 
641  {
642  // Get and check params for covariances
643  XmlRpc::XmlRpcValue pose_cov_list;
644  controller_nh.getParam("pose_covariance_diagonal", pose_cov_list);
646  ROS_ASSERT(pose_cov_list.size() == 6);
647  for (int i = 0; i < pose_cov_list.size(); ++i)
648  ROS_ASSERT(pose_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
649 
650  XmlRpc::XmlRpcValue twist_cov_list;
651  controller_nh.getParam("twist_covariance_diagonal", twist_cov_list);
653  ROS_ASSERT(twist_cov_list.size() == 6);
654  for (int i = 0; i < twist_cov_list.size(); ++i)
655  ROS_ASSERT(twist_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
656 
657  // Setup odometry realtime publisher + odom message constant fields
658  odom_pub_.reset(new realtime_tools::RealtimePublisher<nav_msgs::Odometry>(controller_nh, "odom", 100));
659  odom_pub_->msg_.header.frame_id = odom_frame_id_;
660  odom_pub_->msg_.child_frame_id = base_frame_id_;
661  odom_pub_->msg_.pose.pose.position.z = 0;
662  odom_pub_->msg_.pose.covariance = boost::assign::list_of
663  (static_cast<double>(pose_cov_list[0])) (0) (0) (0) (0) (0)
664  (0) (static_cast<double>(pose_cov_list[1])) (0) (0) (0) (0)
665  (0) (0) (static_cast<double>(pose_cov_list[2])) (0) (0) (0)
666  (0) (0) (0) (static_cast<double>(pose_cov_list[3])) (0) (0)
667  (0) (0) (0) (0) (static_cast<double>(pose_cov_list[4])) (0)
668  (0) (0) (0) (0) (0) (static_cast<double>(pose_cov_list[5]));
669  odom_pub_->msg_.twist.twist.linear.y = 0;
670  odom_pub_->msg_.twist.twist.linear.z = 0;
671  odom_pub_->msg_.twist.twist.angular.x = 0;
672  odom_pub_->msg_.twist.twist.angular.y = 0;
673  odom_pub_->msg_.twist.covariance = boost::assign::list_of
674  (static_cast<double>(twist_cov_list[0])) (0) (0) (0) (0) (0)
675  (0) (static_cast<double>(twist_cov_list[1])) (0) (0) (0) (0)
676  (0) (0) (static_cast<double>(twist_cov_list[2])) (0) (0) (0)
677  (0) (0) (0) (static_cast<double>(twist_cov_list[3])) (0) (0)
678  (0) (0) (0) (0) (static_cast<double>(twist_cov_list[4])) (0)
679  (0) (0) (0) (0) (0) (static_cast<double>(twist_cov_list[5]));
680  tf_odom_pub_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>(root_nh, "/tf", 100));
681  tf_odom_pub_->msg_.transforms.resize(1);
682  tf_odom_pub_->msg_.transforms[0].transform.translation.z = 0.0;
683  tf_odom_pub_->msg_.transforms[0].child_frame_id = base_frame_id_;
684  tf_odom_pub_->msg_.transforms[0].header.frame_id = odom_frame_id_;
685  }
686 
687  void DiffDriveController::reconfCallback(DiffDriveControllerConfig& config, uint32_t /*level*/)
688  {
689  DynamicParams dynamic_params;
690  dynamic_params.left_wheel_radius_multiplier = config.left_wheel_radius_multiplier;
691  dynamic_params.right_wheel_radius_multiplier = config.right_wheel_radius_multiplier;
692  dynamic_params.wheel_separation_multiplier = config.wheel_separation_multiplier;
693 
694  dynamic_params.publish_rate = config.publish_rate;
695 
696  dynamic_params.enable_odom_tf = config.enable_odom_tf;
697 
698  dynamic_params_.writeFromNonRT(dynamic_params);
699 
700  ROS_INFO_STREAM_NAMED(name_, "Dynamic Reconfigure:\n" << dynamic_params);
701  }
702 
704  {
705  // Retreive dynamic params:
706  const DynamicParams dynamic_params = *(dynamic_params_.readFromRT());
707 
711 
712  publish_period_ = ros::Duration(1.0 / dynamic_params.publish_rate);
713  enable_odom_tf_ = dynamic_params.enable_odom_tf;
714  }
715 
716 } // namespace diff_drive_controller
realtime_tools::RealtimeBuffer< Commands > command_
bool allow_multiple_cmd_vel_publishers_
Whether to allow multiple publishers on cmd_vel topic or not:
bool init(hardware_interface::VelocityJointInterface *hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
Initialize controller.
double wheel_separation_
Wheel separation, wrt the midpoint of the wheel width:
void updateDynamicParams()
Update the dynamic parameters in the RT loop.
std::vector< hardware_interface::JointHandle > left_wheel_joints_
Hardware handles:
static bool hasCollisionGeometry(const urdf::LinkConstSharedPtr &link)
bool update(double left_pos, double right_pos, const ros::Time &time)
Updates the odometry class with latest wheels position.
Definition: odometry.cpp:76
#define ROS_DEBUG_STREAM_NAMED(name, args)
double wheel_radius_
Wheel radius (assuming it&#39;s the same for the left and right wheels):
#define ROS_ERROR_STREAM_NAMED(name, args)
static bool getWheelRadius(const urdf::LinkConstSharedPtr &wheel_link, double &wheel_radius)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
int size() const
std::string odom_frame_id_
Frame to use for odometry and odom tf:
static bool isCylinder(const urdf::LinkConstSharedPtr &link)
void brake()
Brakes the wheels, i.e. sets the velocity to 0.
size_t wheel_joints_size_
Number of wheel joints:
#define ROS_INFO_STREAM_NAMED(name, args)
bool getWheelNames(ros::NodeHandle &controller_nh, const std::string &wheel_param, std::vector< std::string > &wheel_names)
Get the wheel names from a wheel param.
double getHeading() const
heading getter
Definition: odometry.h:101
Type const & getType() const
double getY() const
y position getter
Definition: odometry.h:119
static bool isSphere(const urdf::LinkConstSharedPtr &link)
std::vector< hardware_interface::JointHandle > right_wheel_joints_
void starting(const ros::Time &time)
Starts controller.
double getAngular() const
angular velocity getter
Definition: odometry.h:137
bool param(const std::string &param_name, T &param_val, const T &default_val) const
double getX() const
x position getter
Definition: odometry.h:110
boost::shared_ptr< ReconfigureServer > dyn_reconf_server_
void init(const ros::Time &time)
Initialize the odometry.
Definition: odometry.cpp:69
void cmdVelCallback(const geometry_msgs::Twist &command)
Velocity command callback.
uint32_t getNumPublishers() const
void setWheelParams(double wheel_separation, double left_wheel_radius, double right_wheel_radius)
Sets the wheel parameters: radius and separation.
Definition: odometry.cpp:126
const std::string & getNamespace() const
void update(const ros::Time &time, const ros::Duration &period)
Updates controller, i.e. computes the odometry and sets the new velocity commands.
realtime_tools::RealtimeBuffer< DynamicParams > dynamic_params_
#define ROS_DEBUG_STREAM(args)
void setVelocityRollingWindowSize(size_t velocity_rolling_window_size)
Velocity rolling window size setter.
Definition: odometry.cpp:133
ros::Duration publish_period_
Odometry related:
JointHandle getHandle(const std::string &name)
boost::shared_ptr< realtime_tools::RealtimePublisher< nav_msgs::Odometry > > odom_pub_
Odometry related:
static geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)
#define ROS_INFO_STREAM(args)
static double euclideanOfVectors(const urdf::Vector3 &vec1, const urdf::Vector3 &vec2)
bool getParam(const std::string &key, std::string &s) const
#define ROS_ERROR_STREAM_THROTTLE_NAMED(rate, name, args)
double getLinear() const
linear velocity getter
Definition: odometry.h:128
void updateOpenLoop(double linear, double angular, const ros::Time &time)
Updates the odometry class with latest velocity command.
Definition: odometry.cpp:114
double cmd_vel_timeout_
Timeout to consider cmd_vel commands old:
boost::shared_ptr< realtime_tools::RealtimePublisher< tf::tfMessage > > tf_odom_pub_
#define ROS_ERROR_NAMED(name,...)
bool setOdomParamsFromUrdf(ros::NodeHandle &root_nh, const std::string &left_wheel_name, const std::string &right_wheel_name, bool lookup_wheel_separation, bool lookup_wheel_radius)
Sets odometry parameters from the URDF, i.e. the wheel radius and separation.
static Time now()
bool enable_odom_tf_
Whether to publish odometry to tf or not:
bool hasParam(const std::string &key) const
#define ROS_ERROR_STREAM(args)
#define ROS_ASSERT(cond)
void reconfCallback(DiffDriveControllerConfig &config, uint32_t)
Callback for dynamic_reconfigure server.
boost::shared_ptr< realtime_tools::RealtimePublisher< geometry_msgs::TwistStamped > > cmd_vel_pub_
Publish executed commands.
void setOdomPubFields(ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
Sets the odometry publishing fields.
#define ROS_ERROR(...)
double limit(double &v, double v0, double v1, double dt)
Limit the velocity and acceleration.
void stopping(const ros::Time &)
Stops controller.
double wheel_separation_multiplier_
Wheel separation and radius calibration multipliers:
std::string base_frame_id_
Frame to use for the robot base:


diff_drive_controller
Author(s): Bence Magyar
autogenerated on Sat Apr 18 2020 03:58:05