mecanum_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 <tf/transform_datatypes.h>
40 
41 #include <urdf_parser/urdf_parser.h>
42 
43 #include <boost/assign.hpp>
44 
46 
48 static bool isCylinderOrSphere(const urdf::LinkConstSharedPtr& link)
49 {
50  if(!link)
51  {
52  ROS_ERROR("Link == NULL.");
53  return false;
54  }
55 
56  if(!link->collision)
57  {
58  ROS_ERROR_STREAM("Link " << link->name << " does not have collision description. Add collision description for link to urdf.");
59  return false;
60  }
61 
62  if(!link->collision->geometry)
63  {
64  ROS_ERROR_STREAM("Link " << link->name << " does not have collision geometry description. Add collision geometry description for link to urdf.");
65  return false;
66  }
67 
68  if(link->collision->geometry->type != urdf::Geometry::CYLINDER && link->collision->geometry->type != urdf::Geometry::SPHERE)
69  {
70  ROS_ERROR_STREAM("Link " << link->name << " does not have cylinder nor sphere geometry");
71  return false;
72  }
73 
74  return true;
75 }
76 
78 {
79 
82  : open_loop_(false)
83  , command_struct_()
84  , use_realigned_roller_joints_(false)
85  , wheels_k_(0.0)
86  , wheels_radius_(0.0)
87  , cmd_vel_timeout_(0.5)
88  , base_frame_id_("base_link")
89  , odom_frame_id_("odom")
90  , enable_odom_tf_(true)
91  , wheel_joints_size_(0)
92 {
93 }
94 
95 
98  ros::NodeHandle& root_nh,
99  ros::NodeHandle &controller_nh)
100 {
101  const std::string complete_ns = controller_nh.getNamespace();
102  std::size_t id = complete_ns.find_last_of("/");
103  name_ = complete_ns.substr(id + 1);
104 
105  // Option use_realigned_roller_joints
106  controller_nh.param("use_realigned_roller_joints", use_realigned_roller_joints_, use_realigned_roller_joints_);
107  ROS_INFO_STREAM_NAMED(name_, "Use the roller's radius rather than the wheel's: " << use_realigned_roller_joints_ << ".");
108 
109  // Get joint names from the parameter server
110  std::string wheel0_name;
111  controller_nh.param("front_left_wheel_joint", wheel0_name, wheel0_name);
112  ROS_INFO_STREAM_NAMED(name_, "Front left wheel joint (wheel0) is : " << wheel0_name);
113 
114  std::string wheel1_name;
115  controller_nh.param("back_left_wheel_joint", wheel1_name, wheel1_name);
116  ROS_INFO_STREAM_NAMED(name_, "Back left wheel joint (wheel1) is : " << wheel1_name);
117 
118  std::string wheel2_name;
119  controller_nh.param("back_right_wheel_joint", wheel2_name, wheel2_name);
120  ROS_INFO_STREAM_NAMED(name_, "Back right wheel joint (wheel2) is : " << wheel2_name);
121 
122  std::string wheel3_name;
123  controller_nh.param("front_right_wheel_joint", wheel3_name, wheel3_name);
124  ROS_INFO_STREAM_NAMED(name_, "Front right wheel joint (wheel3) is : " << wheel3_name);
125 
126  // Odometry related:
127  double publish_rate;
128  controller_nh.param("publish_rate", publish_rate, 50.0);
129  ROS_INFO_STREAM_NAMED(name_, "Controller state will be published at "
130  << publish_rate << "Hz.");
131  publish_period_ = ros::Duration(1.0 / publish_rate);
132 
133  controller_nh.param("open_loop", open_loop_, open_loop_);
134 
135  // Twist command related:
136  controller_nh.param("cmd_vel_timeout", cmd_vel_timeout_, cmd_vel_timeout_);
137  ROS_INFO_STREAM_NAMED(name_, "Velocity commands will be considered old if they are older than "
138  << cmd_vel_timeout_ << "s.");
139 
140  controller_nh.param("base_frame_id", base_frame_id_, base_frame_id_);
141  ROS_INFO_STREAM_NAMED(name_, "Base frame_id set to " << base_frame_id_);
142 
143  controller_nh.param("odom_frame_id", odom_frame_id_, odom_frame_id_);
144  ROS_INFO_STREAM_NAMED(name_, "Odom frame_id set to " << odom_frame_id_);
145 
146  controller_nh.param("enable_odom_tf", enable_odom_tf_, enable_odom_tf_);
147  ROS_INFO_STREAM_NAMED(name_, "Publishing to tf is " << (enable_odom_tf_?"enabled":"disabled"));
148 
149  // Velocity and acceleration limits:
150  controller_nh.param("linear/x/has_velocity_limits" , limiter_linX_.has_velocity_limits , limiter_linX_.has_velocity_limits );
151  controller_nh.param("linear/x/has_acceleration_limits", limiter_linX_.has_acceleration_limits, limiter_linX_.has_acceleration_limits);
152  controller_nh.param("linear/x/max_velocity" , limiter_linX_.max_velocity , limiter_linX_.max_velocity );
153  controller_nh.param("linear/x/min_velocity" , limiter_linX_.min_velocity , -limiter_linX_.max_velocity );
154  controller_nh.param("linear/x/max_acceleration" , limiter_linX_.max_acceleration , limiter_linX_.max_acceleration );
155  controller_nh.param("linear/x/min_acceleration" , limiter_linX_.min_acceleration , -limiter_linX_.max_acceleration );
156 
157  controller_nh.param("linear/y/has_velocity_limits" , limiter_linY_.has_velocity_limits , limiter_linY_.has_velocity_limits );
158  controller_nh.param("linear/y/has_acceleration_limits", limiter_linY_.has_acceleration_limits, limiter_linY_.has_acceleration_limits);
159  controller_nh.param("linear/y/max_velocity" , limiter_linY_.max_velocity , limiter_linY_.max_velocity );
160  controller_nh.param("linear/y/min_velocity" , limiter_linY_.min_velocity , -limiter_linY_.max_velocity );
161  controller_nh.param("linear/y/max_acceleration" , limiter_linY_.max_acceleration , limiter_linY_.max_acceleration );
162  controller_nh.param("linear/y/min_acceleration" , limiter_linY_.min_acceleration , -limiter_linY_.max_acceleration );
163 
164  controller_nh.param("angular/z/has_velocity_limits" , limiter_ang_.has_velocity_limits , limiter_ang_.has_velocity_limits );
165  controller_nh.param("angular/z/has_acceleration_limits", limiter_ang_.has_acceleration_limits, limiter_ang_.has_acceleration_limits);
166  controller_nh.param("angular/z/max_velocity" , limiter_ang_.max_velocity , limiter_ang_.max_velocity );
167  controller_nh.param("angular/z/min_velocity" , limiter_ang_.min_velocity , -limiter_ang_.max_velocity );
168  controller_nh.param("angular/z/max_acceleration" , limiter_ang_.max_acceleration , limiter_ang_.max_acceleration );
169  controller_nh.param("angular/z/min_acceleration" , limiter_ang_.min_acceleration , -limiter_ang_.max_acceleration );
170 
171  // Get the joint objects to use in the realtime loop
172  wheel0_jointHandle_ = hw->getHandle(wheel0_name); // throws on failure
173  wheel1_jointHandle_ = hw->getHandle(wheel1_name); // throws on failure
174  wheel2_jointHandle_ = hw->getHandle(wheel2_name); // throws on failure
175  wheel3_jointHandle_ = hw->getHandle(wheel3_name); // throws on failure
176 
177  // Pass params through and setup publishers and subscribers
178  if (!setWheelParamsFromUrdf(root_nh, controller_nh, wheel0_name, wheel1_name, wheel2_name, wheel3_name))
179  return false;
180 
181  setupRtPublishersMsg(root_nh, controller_nh);
182 
183  sub_command_ = controller_nh.subscribe("cmd_vel", 1, &MecanumDriveController::cmdVelCallback, this);
184 
185  return true;
186 }
187 
188 
191 {
192  // COMPUTE AND PUBLISH ODOMETRY
193  if (open_loop_)
194  {
196  }
197  else
198  {
199  double wheel0_vel = wheel0_jointHandle_.getVelocity();
200  double wheel1_vel = wheel1_jointHandle_.getVelocity();
201  double wheel2_vel = wheel2_jointHandle_.getVelocity();
202  double wheel3_vel = wheel3_jointHandle_.getVelocity();
203 
204  if (std::isnan(wheel0_vel) || std::isnan(wheel1_vel) || std::isnan(wheel2_vel) || std::isnan(wheel3_vel))
205  return;
206 
207  // Estimate twist (using joint information) and integrate
208  odometry_.update(wheel0_vel, wheel1_vel, wheel2_vel, wheel3_vel, time);
209  }
210 
211  // Publish odometry message
213  {
215  // Compute and store orientation info
216  const geometry_msgs::Quaternion orientation(
218 
219  // Populate odom message and publish
220  if(odom_pub_->trylock())
221  {
222  odom_pub_->msg_.header.stamp = time;
223  odom_pub_->msg_.pose.pose.position.x = odometry_.getX();
224  odom_pub_->msg_.pose.pose.position.y = odometry_.getY();
225  odom_pub_->msg_.pose.pose.orientation = orientation;
226  odom_pub_->msg_.twist.twist.linear.x = odometry_.getLinearX();
227  odom_pub_->msg_.twist.twist.linear.y = odometry_.getLinearY();
228  odom_pub_->msg_.twist.twist.angular.z = odometry_.getAngular();
229  odom_pub_->unlockAndPublish();
230  }
231 
232  // Publish tf /odom frame
233  if (enable_odom_tf_ && tf_odom_pub_->trylock())
234  {
235  geometry_msgs::TransformStamped& odom_frame = tf_odom_pub_->msg_.transforms[0];
236  odom_frame.header.stamp = time;
237  odom_frame.transform.translation.x = odometry_.getX();
238  odom_frame.transform.translation.y = odometry_.getY();
239  odom_frame.transform.rotation = orientation;
240  tf_odom_pub_->unlockAndPublish();
241  }
242  }
243 
244  // MOVE ROBOT
245  // Retrieve current velocity command and time step:
246  Commands curr_cmd = *(command_.readFromRT());
247  const double dt = (time - curr_cmd.stamp).toSec();
248 
249  // Brake if cmd_vel has timeout:
250  if (dt > cmd_vel_timeout_)
251  {
252  curr_cmd.linX = 0.0;
253  curr_cmd.linY = 0.0;
254  curr_cmd.ang = 0.0;
255  }
256 
257  // Limit velocities and accelerations:
258  const double cmd_dt(period.toSec());
259  limiter_linX_.limit(curr_cmd.linX, last_cmd_.linX, cmd_dt);
260  limiter_linY_.limit(curr_cmd.linY, last_cmd_.linY, cmd_dt);
261  limiter_ang_.limit(curr_cmd.ang, last_cmd_.ang, cmd_dt);
262  last_cmd_ = curr_cmd;
263 
264  // Compute wheels velocities (this is the actual ik):
265  // NOTE: the input desired twist (from topic /cmd_vel) is a body twist.
266  const double w0_vel = 1.0 / wheels_radius_ * (curr_cmd.linX - curr_cmd.linY - wheels_k_ * curr_cmd.ang);
267  const double w1_vel = 1.0 / wheels_radius_ * (curr_cmd.linX + curr_cmd.linY - wheels_k_ * curr_cmd.ang);
268  const double w2_vel = 1.0 / wheels_radius_ * (curr_cmd.linX - curr_cmd.linY + wheels_k_ * curr_cmd.ang);
269  const double w3_vel = 1.0 / wheels_radius_ * (curr_cmd.linX + curr_cmd.linY + wheels_k_ * curr_cmd.ang);
270 
271  // Set wheels velocities:
276 }
277 
280 {
281  brake();
282 
283  // Register starting time used to keep fixed rate
285 
286  odometry_.init(time);
287 }
288 
291 {
292  brake();
293 }
294 
297 {
302 }
303 
305 void MecanumDriveController::cmdVelCallback(const geometry_msgs::Twist& command)
306 {
307  if(isRunning())
308  {
309  command_struct_.ang = command.angular.z;
310  command_struct_.linX = command.linear.x;
311  command_struct_.linY = command.linear.y;
313  command_.writeFromNonRT (command_struct_);
315  "Added values to command. "
316  << "Ang: " << command_struct_.ang << ", "
317  << "Lin: " << command_struct_.linX << ", "
318  << "Lin: " << command_struct_.linY << ", "
319  << "Stamp: " << command_struct_.stamp);
320  }
321  else
322  {
323  ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running.");
324  }
325 }
326 
329  ros::NodeHandle &controller_nh,
330  const std::string& wheel0_name,
331  const std::string& wheel1_name,
332  const std::string& wheel2_name,
333  const std::string& wheel3_name)
334 {
335  bool has_wheel_separation_x = controller_nh.getParam("wheel_separation_x", wheel_separation_x_);
336  bool has_wheel_separation_y = controller_nh.getParam("wheel_separation_y", wheel_separation_y_);
337 
338  // Check to see if both X and Y separations are overrided.
339  if (has_wheel_separation_x != has_wheel_separation_y)
340  {
341  ROS_ERROR_STREAM_NAMED(name_, "Only one wheel separation overrided");
342  return false;
343  }
344 
345  bool lookup_wheel_separation = !(has_wheel_separation_x && has_wheel_separation_y);
346  bool lookup_wheel_radius = !controller_nh.getParam("wheel_radius", wheels_radius_);
347 
348  // Avoid URDF requirement if wheel separation and radius already specified
349  if (lookup_wheel_separation || lookup_wheel_radius)
350  {
351  // Parse robot description
352  const std::string model_param_name = "robot_description";
353  bool res = root_nh.hasParam(model_param_name);
354  std::string robot_model_str="";
355  if(!res || !root_nh.getParam(model_param_name,robot_model_str))
356  {
357  ROS_ERROR_NAMED(name_, "Robot descripion couldn't be retrieved from param server.");
358  return false;
359  }
360 
361  urdf::ModelInterfaceSharedPtr model(urdf::parseURDF(robot_model_str));
362 
363  // Get wheels position and compute parameter k_ (used in mecanum wheels IK).
364  urdf::JointConstSharedPtr wheel0_urdfJoint(model->getJoint(wheel0_name));
365  if(!wheel0_urdfJoint)
366  {
367  ROS_ERROR_STREAM_NAMED(name_, wheel0_name
368  << " couldn't be retrieved from model description");
369  return false;
370  }
371  urdf::JointConstSharedPtr wheel1_urdfJoint(model->getJoint(wheel1_name));
372  if(!wheel1_urdfJoint)
373  {
374  ROS_ERROR_STREAM_NAMED(name_, wheel1_name
375  << " couldn't be retrieved from model description");
376  return false;
377  }
378  urdf::JointConstSharedPtr wheel2_urdfJoint(model->getJoint(wheel2_name));
379  if(!wheel2_urdfJoint)
380  {
381  ROS_ERROR_STREAM_NAMED(name_, wheel2_name
382  << " couldn't be retrieved from model description");
383  return false;
384  }
385  urdf::JointConstSharedPtr wheel3_urdfJoint(model->getJoint(wheel3_name));
386  if(!wheel3_urdfJoint)
387  {
388  ROS_ERROR_STREAM_NAMED(name_, wheel3_name
389  << " couldn't be retrieved from model description");
390  return false;
391  }
392 
393  if (lookup_wheel_separation)
394  {
395  ROS_INFO_STREAM("wheel0 to origin: " << wheel0_urdfJoint->parent_to_joint_origin_transform.position.x << ","
396  << wheel0_urdfJoint->parent_to_joint_origin_transform.position.y << ", "
397  << wheel0_urdfJoint->parent_to_joint_origin_transform.position.z);
398  ROS_INFO_STREAM("wheel1 to origin: " << wheel1_urdfJoint->parent_to_joint_origin_transform.position.x << ","
399  << wheel1_urdfJoint->parent_to_joint_origin_transform.position.y << ", "
400  << wheel1_urdfJoint->parent_to_joint_origin_transform.position.z);
401  ROS_INFO_STREAM("wheel2 to origin: " << wheel2_urdfJoint->parent_to_joint_origin_transform.position.x << ","
402  << wheel2_urdfJoint->parent_to_joint_origin_transform.position.y << ", "
403  << wheel2_urdfJoint->parent_to_joint_origin_transform.position.z);
404  ROS_INFO_STREAM("wheel3 to origin: " << wheel3_urdfJoint->parent_to_joint_origin_transform.position.x << ","
405  << wheel3_urdfJoint->parent_to_joint_origin_transform.position.y << ", "
406  << wheel3_urdfJoint->parent_to_joint_origin_transform.position.z);
407 
408  double wheel0_x = wheel0_urdfJoint->parent_to_joint_origin_transform.position.x;
409  double wheel0_y = wheel0_urdfJoint->parent_to_joint_origin_transform.position.y;
410  double wheel1_x = wheel1_urdfJoint->parent_to_joint_origin_transform.position.x;
411  double wheel1_y = wheel1_urdfJoint->parent_to_joint_origin_transform.position.y;
412  double wheel2_x = wheel2_urdfJoint->parent_to_joint_origin_transform.position.x;
413  double wheel2_y = wheel2_urdfJoint->parent_to_joint_origin_transform.position.y;
414  double wheel3_x = wheel3_urdfJoint->parent_to_joint_origin_transform.position.x;
415  double wheel3_y = wheel3_urdfJoint->parent_to_joint_origin_transform.position.y;
416 
417  wheels_k_ = (-(-wheel0_x - wheel0_y) - (wheel1_x - wheel1_y) + (-wheel2_x - wheel2_y) + (wheel3_x - wheel3_y))
418  / 4.0;
419  }
420  else
421  {
422  ROS_INFO_STREAM("Wheel seperation in X: " << wheel_separation_x_);
423  ROS_INFO_STREAM("Wheel seperation in Y: " << wheel_separation_y_);
424 
425  // The seperation is the total distance between the wheels in X and Y.
426 
428  }
429 
430  if (lookup_wheel_radius)
431  {
432  // Get wheels radius
433  double wheel0_radius = 0.0;
434  double wheel1_radius = 0.0;
435  double wheel2_radius = 0.0;
436  double wheel3_radius = 0.0;
437 
438  if (!getWheelRadius(model, model->getLink(wheel0_urdfJoint->child_link_name), wheel0_radius) ||
439  !getWheelRadius(model, model->getLink(wheel1_urdfJoint->child_link_name), wheel1_radius) ||
440  !getWheelRadius(model, model->getLink(wheel2_urdfJoint->child_link_name), wheel2_radius) ||
441  !getWheelRadius(model, model->getLink(wheel3_urdfJoint->child_link_name), wheel3_radius))
442  {
443  ROS_ERROR_STREAM_NAMED(name_, "Couldn't retrieve wheels' radius");
444  return false;
445  }
446 
447  if (abs(wheel0_radius - wheel1_radius) > 1e-3 ||
448  abs(wheel0_radius - wheel2_radius) > 1e-3 ||
449  abs(wheel0_radius - wheel3_radius) > 1e-3)
450  {
451  ROS_ERROR_STREAM_NAMED(name_, "Wheels radius are not egual");
452  return false;
453  }
454 
455  wheels_radius_ = wheel0_radius;
456  }
457  }
458 
459  ROS_INFO_STREAM("Wheel radius: " << wheels_radius_);
460 
461  // Set wheel params for the odometry computation
463 
464  return true;
465 }
466 
468 bool MecanumDriveController::getWheelRadius(const urdf::ModelInterfaceSharedPtr model,
469  const urdf::LinkConstSharedPtr& wheel_link, double& wheel_radius)
470 {
471  urdf::LinkConstSharedPtr radius_link = wheel_link;
472 
474  {
475  // This mode is used when the mecanum wheels are simulated and we use realigned rollers to mimic mecanum wheels.
476  const urdf::JointConstSharedPtr& roller_joint = radius_link->child_joints[0];
477  if(!roller_joint)
478  {
479  ROS_ERROR_STREAM_NAMED(name_, "No roller joint could be retrieved for wheel : " << wheel_link->name <<
480  ". Are you sure mecanum wheels are simulated using realigned rollers?");
481  return false;
482  }
483 
484  radius_link = model->getLink(roller_joint->child_link_name);
485  if(!radius_link)
486  {
487  ROS_ERROR_STREAM_NAMED(name_, "No roller link could be retrieved for wheel : " << wheel_link->name <<
488  ". Are you sure mecanum wheels are simulated using realigned rollers?");
489  return false;
490  }
491  }
492 
493  if(!isCylinderOrSphere(radius_link))
494  {
495  ROS_ERROR_STREAM("Wheel link " << radius_link->name << " is NOT modeled as a cylinder!");
496  return false;
497  }
498 
499  if (radius_link->collision->geometry->type == urdf::Geometry::CYLINDER)
500  wheel_radius = (static_cast<urdf::Cylinder*>(radius_link->collision->geometry.get()))->radius;
501  else
502  wheel_radius = (static_cast<urdf::Sphere*>(radius_link->collision->geometry.get()))->radius;
503 
504  return true;
505 }
506 
509 {
510  // Get covariance parameters for odometry.
511  XmlRpc::XmlRpcValue pose_cov_list;
512  controller_nh.getParam("pose_covariance_diagonal", pose_cov_list);
514  ROS_ASSERT(pose_cov_list.size() == 6);
515  for (int i = 0; i < pose_cov_list.size(); ++i)
516  ROS_ASSERT(pose_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
517 
518  XmlRpc::XmlRpcValue twist_cov_list;
519  controller_nh.getParam("twist_covariance_diagonal", twist_cov_list);
521  ROS_ASSERT(twist_cov_list.size() == 6);
522  for (int i = 0; i < twist_cov_list.size(); ++i)
523  ROS_ASSERT(twist_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
524 
525  // Setup odometry msg.
526  odom_pub_.reset(new realtime_tools::RealtimePublisher<nav_msgs::Odometry>(controller_nh, "odom", 100));
527  odom_pub_->msg_.header.frame_id = odom_frame_id_;
528  odom_pub_->msg_.child_frame_id = base_frame_id_;
529  odom_pub_->msg_.pose.pose.position.z = 0;
530  odom_pub_->msg_.pose.covariance = boost::assign::list_of
531  (static_cast<double>(pose_cov_list[0])) (0) (0) (0) (0) (0)
532  (0) (static_cast<double>(pose_cov_list[1])) (0) (0) (0) (0)
533  (0) (0) (static_cast<double>(pose_cov_list[2])) (0) (0) (0)
534  (0) (0) (0) (static_cast<double>(pose_cov_list[3])) (0) (0)
535  (0) (0) (0) (0) (static_cast<double>(pose_cov_list[4])) (0)
536  (0) (0) (0) (0) (0) (static_cast<double>(pose_cov_list[5]));
537  odom_pub_->msg_.twist.twist.linear.y = 0;
538  odom_pub_->msg_.twist.twist.linear.z = 0;
539  odom_pub_->msg_.twist.twist.angular.x = 0;
540  odom_pub_->msg_.twist.twist.angular.y = 0;
541  odom_pub_->msg_.twist.covariance = boost::assign::list_of
542  (static_cast<double>(twist_cov_list[0])) (0) (0) (0) (0) (0)
543  (0) (static_cast<double>(twist_cov_list[1])) (0) (0) (0) (0)
544  (0) (0) (static_cast<double>(twist_cov_list[2])) (0) (0) (0)
545  (0) (0) (0) (static_cast<double>(twist_cov_list[3])) (0) (0)
546  (0) (0) (0) (0) (static_cast<double>(twist_cov_list[4])) (0)
547  (0) (0) (0) (0) (0) (static_cast<double>(twist_cov_list[5]));
548 
549  // Setup tf msg.
550  tf_odom_pub_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>(root_nh, "/tf", 100));
551  tf_odom_pub_->msg_.transforms.resize(1);
552  tf_odom_pub_->msg_.transforms[0].transform.translation.z = 0.0;
553  tf_odom_pub_->msg_.transforms[0].child_frame_id = base_frame_id_;
554  tf_odom_pub_->msg_.transforms[0].header.frame_id = odom_frame_id_;
555 }
556 
557 } // namespace mecanum_drive_controller
void setWheelsParams(double wheels_k, double wheels_radius)
Sets the wheels parameters: mecanum geometric param and radius.
Definition: odometry.cpp:124
void limit(double &v, double v0, double dt)
Limit the velocity and acceleration.
double getLinearX() const
linearX velocity getter
Definition: odometry.h:131
double cmd_vel_timeout_
Timeout to consider cmd_vel commands old:
#define ROS_DEBUG_STREAM_NAMED(name, args)
void update(const ros::Time &time, const ros::Duration &period)
Updates controller, i.e. computes the odometry and sets the new velocity commands.
#define ROS_ERROR_STREAM_NAMED(name, args)
realtime_tools::RealtimeBuffer< Commands > command_
bool getWheelRadius(const urdf::ModelInterfaceSharedPtr model, const urdf::LinkConstSharedPtr &wheel_link, double &wheel_radius)
Get the radius of a given wheel.
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
int size() const
void brake()
Brakes the wheels, i.e. sets the velocity to 0.
boost::shared_ptr< realtime_tools::RealtimePublisher< tf::tfMessage > > tf_odom_pub_
#define ROS_INFO_STREAM_NAMED(name, args)
void starting(const ros::Time &time)
Starts controller.
Type const & getType() const
double getY() const
y position getter
Definition: odometry.h:122
double getLinearY() const
linearY velocity getter
Definition: odometry.h:140
void setupRtPublishersMsg(ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
Sets the odometry publishing fields.
bool setWheelParamsFromUrdf(ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh, const std::string &wheel0_name, const std::string &wheel1_name, const std::string &wheel2_name, const std::string &wheel3_name)
Sets odometry parameters from the URDF, i.e. the wheel radius and separation.
bool param(const std::string &param_name, T &param_val, const T &default_val) const
static bool isCylinderOrSphere(const urdf::LinkConstSharedPtr &link)
void updateOpenLoop(double linearX, double linearY, double angular, const ros::Time &time)
Updates the odometry class with latest velocity command.
Definition: odometry.cpp:110
const std::string & getNamespace() const
boost::shared_ptr< realtime_tools::RealtimePublisher< nav_msgs::Odometry > > odom_pub_
Odometry related:
double getAngular() const
angular velocity getter
Definition: odometry.h:149
double getHeading() const
heading getter
Definition: odometry.h:104
void stopping(const ros::Time &time)
Stops controller.
void init(const ros::Time &time)
Initialize the odometry.
Definition: odometry.cpp:73
bool use_realigned_roller_joints_
Wheel radius (assuming it&#39;s the same for the left and right wheels):
void cmdVelCallback(const geometry_msgs::Twist &command)
Velocity command callback.
bool init(hardware_interface::VelocityJointInterface *hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
Initialize controller.
JointHandle getHandle(const std::string &name)
hardware_interface::JointHandle wheel0_jointHandle_
Hardware handles:
void setCommand(double command)
static geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)
#define ROS_INFO_STREAM(args)
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
std::string base_frame_id_
Frame to use for the robot base:
bool getParam(const std::string &key, std::string &s) const
#define ROS_ERROR_NAMED(name,...)
static Time now()
double getX() const
x position getter
Definition: odometry.h:113
bool hasParam(const std::string &key) const
bool update(double wheel0_vel, double wheel1_vel, double wheel2_vel, double wheel3_vel, const ros::Time &time)
Updates the odometry class with latest wheels position.
Definition: odometry.cpp:85
#define ROS_ERROR_STREAM(args)
#define ROS_ASSERT(cond)
#define ROS_ERROR(...)
bool enable_odom_tf_
Whether to publish odometry to tf or not:


ridgeback_control
Author(s): Mike Purvis
autogenerated on Thu Mar 5 2020 03:31:54