ackermann_steering_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  * Author: Masaru Morita
38  */
39 
40 #include <cmath>
41 
42 #include <boost/assign.hpp>
44 #include <tf/transform_datatypes.h>
45 #include <urdf_parser/urdf_parser.h>
46 
48 
49 static double euclideanOfVectors(const urdf::Vector3& vec1, const urdf::Vector3& vec2)
50 {
51  return std::sqrt(std::pow(vec1.x-vec2.x,2) +
52  std::pow(vec1.y-vec2.y,2) +
53  std::pow(vec1.z-vec2.z,2));
54 }
55 
56 /*
57  * \brief Check if the link is modeled as a cylinder
58  * \param link Link
59  * \return true if the link is modeled as a Cylinder; false otherwise
60  */
62 {
63  if (!link)
64  {
65  ROS_ERROR("Link == NULL.");
66  return false;
67  }
68 
69  if (!link->collision)
70  {
71  ROS_ERROR_STREAM("Link " << link->name << " does not have collision description. Add collision description for link to urdf.");
72  return false;
73  }
74 
75  if (!link->collision->geometry)
76  {
77  ROS_ERROR_STREAM("Link " << link->name << " does not have collision geometry description. Add collision geometry description for link to urdf.");
78  return false;
79  }
80 
81  if (link->collision->geometry->type != urdf::Geometry::CYLINDER)
82  {
83  ROS_ERROR_STREAM("Link " << link->name << " does not have cylinder geometry");
84  return false;
85  }
86 
87  return true;
88 }
89 
90 /*
91  * \brief Get the wheel radius
92  * \param [in] wheel_link Wheel link
93  * \param [out] wheel_radius Wheel radius [m]
94  * \return true if the wheel radius was found; false other
95 wise
96  */
97 static bool getWheelRadius(const boost::shared_ptr<const urdf::Link>& wheel_link, double& wheel_radius)
98 {
99  if (!isCylinder(wheel_link))
100  {
101  ROS_ERROR_STREAM("Wheel link " << wheel_link->name << " is NOT modeled as a cylinder!");
102  return false;
103  }
104 
105  wheel_radius = (static_cast<urdf::Cylinder*>(wheel_link->collision->geometry.get()))->radius;
106  return true;
107 }
108 
110 
112  : open_loop_(false)
113  , command_struct_()
114  , wheel_separation_h_(0.0)
115  , wheel_radius_(0.0)
116  , wheel_separation_h_multiplier_(1.0)
117  , wheel_radius_multiplier_(1.0)
118  , steer_pos_multiplier_(1.0)
119  , cmd_vel_timeout_(0.5)
120  , allow_multiple_cmd_vel_publishers_(true)
121  , base_frame_id_("base_link")
122  , odom_frame_id_("odom")
123  , enable_odom_tf_(true)
124  , wheel_joints_size_(0)
125  {
126  }
127 
129  ros::NodeHandle& root_nh,
130  ros::NodeHandle& controller_nh)
131  {
134  typedef hardware_interface::JointStateInterface StateIface;
135 
136  // get multiple types of hardware_interface
137  VelIface *vel_joint_if = robot_hw->get<VelIface>(); // vel for wheels
138  PosIface *pos_joint_if = robot_hw->get<PosIface>(); // pos for steers
139 
140  const std::string complete_ns = controller_nh.getNamespace();
141 
142  std::size_t id = complete_ns.find_last_of("/");
143  name_ = complete_ns.substr(id + 1);
144 
145  //-- single rear wheel joint
146  std::string rear_wheel_name = "rear_wheel_joint";
147  controller_nh.param("rear_wheel", rear_wheel_name, rear_wheel_name);
148 
149  //-- single front steer joint
150  std::string front_steer_name = "front_steer_joint";
151  controller_nh.param("front_steer", front_steer_name, front_steer_name);
152 
153 
154  // Odometry related:
155  double publish_rate;
156  controller_nh.param("publish_rate", publish_rate, 50.0);
157  ROS_INFO_STREAM_NAMED(name_, "Controller state will be published at "
158  << publish_rate << "Hz.");
159  publish_period_ = ros::Duration(1.0 / publish_rate);
160 
161  controller_nh.param("open_loop", open_loop_, open_loop_);
162 
163  controller_nh.param("wheel_separation_h_multiplier", wheel_separation_h_multiplier_, wheel_separation_h_multiplier_);
164  ROS_INFO_STREAM_NAMED(name_, "Wheel separation height will be multiplied by "
166 
167  controller_nh.param("wheel_radius_multiplier", wheel_radius_multiplier_, wheel_radius_multiplier_);
168  ROS_INFO_STREAM_NAMED(name_, "Wheel radius will be multiplied by "
169  << wheel_radius_multiplier_ << ".");
170 
171  controller_nh.param("steer_pos_multiplier", steer_pos_multiplier_, steer_pos_multiplier_);
172  ROS_INFO_STREAM_NAMED(name_, "Steer pos will be multiplied by "
173  << steer_pos_multiplier_ << ".");
174 
175  int velocity_rolling_window_size = 10;
176  controller_nh.param("velocity_rolling_window_size", velocity_rolling_window_size, velocity_rolling_window_size);
177  ROS_INFO_STREAM_NAMED(name_, "Velocity rolling window size of "
178  << velocity_rolling_window_size << ".");
179 
180  odometry_.setVelocityRollingWindowSize(velocity_rolling_window_size);
181 
182  // Twist command related:
183  controller_nh.param("cmd_vel_timeout", cmd_vel_timeout_, cmd_vel_timeout_);
184  ROS_INFO_STREAM_NAMED(name_, "Velocity commands will be considered old if they are older than "
185  << cmd_vel_timeout_ << "s.");
186 
187  controller_nh.param("allow_multiple_cmd_vel_publishers", allow_multiple_cmd_vel_publishers_, allow_multiple_cmd_vel_publishers_);
188  ROS_INFO_STREAM_NAMED(name_, "Allow mutiple cmd_vel publishers is "
189  << (allow_multiple_cmd_vel_publishers_?"enabled":"disabled"));
190 
191  controller_nh.param("base_frame_id", base_frame_id_, base_frame_id_);
192  ROS_INFO_STREAM_NAMED(name_, "Base frame_id set to " << base_frame_id_);
193 
194  controller_nh.param("odom_frame_id", odom_frame_id_, odom_frame_id_);
195  ROS_INFO_STREAM_NAMED(name_, "Odometry frame_id set to " << odom_frame_id_);
196 
197  controller_nh.param("enable_odom_tf", enable_odom_tf_, enable_odom_tf_);
198  ROS_INFO_STREAM_NAMED(name_, "Publishing to tf is " << (enable_odom_tf_?"enabled":"disabled"));
199 
200  // Velocity and acceleration limits:
201  controller_nh.param("linear/x/has_velocity_limits" , limiter_lin_.has_velocity_limits , limiter_lin_.has_velocity_limits );
202  controller_nh.param("linear/x/has_acceleration_limits", limiter_lin_.has_acceleration_limits, limiter_lin_.has_acceleration_limits);
203  controller_nh.param("linear/x/has_jerk_limits" , limiter_lin_.has_jerk_limits , limiter_lin_.has_jerk_limits );
204  controller_nh.param("linear/x/max_velocity" , limiter_lin_.max_velocity , limiter_lin_.max_velocity );
205  controller_nh.param("linear/x/min_velocity" , limiter_lin_.min_velocity , -limiter_lin_.max_velocity );
206  controller_nh.param("linear/x/max_acceleration" , limiter_lin_.max_acceleration , limiter_lin_.max_acceleration );
207  controller_nh.param("linear/x/min_acceleration" , limiter_lin_.min_acceleration , -limiter_lin_.max_acceleration );
208  controller_nh.param("linear/x/max_jerk" , limiter_lin_.max_jerk , limiter_lin_.max_jerk );
209  controller_nh.param("linear/x/min_jerk" , limiter_lin_.min_jerk , -limiter_lin_.max_jerk );
210 
211  controller_nh.param("angular/z/has_velocity_limits" , limiter_ang_.has_velocity_limits , limiter_ang_.has_velocity_limits );
212  controller_nh.param("angular/z/has_acceleration_limits", limiter_ang_.has_acceleration_limits, limiter_ang_.has_acceleration_limits);
213  controller_nh.param("angular/z/has_jerk_limits" , limiter_ang_.has_jerk_limits , limiter_ang_.has_jerk_limits );
214  controller_nh.param("angular/z/max_velocity" , limiter_ang_.max_velocity , limiter_ang_.max_velocity );
215  controller_nh.param("angular/z/min_velocity" , limiter_ang_.min_velocity , -limiter_ang_.max_velocity );
216  controller_nh.param("angular/z/max_acceleration" , limiter_ang_.max_acceleration , limiter_ang_.max_acceleration );
217  controller_nh.param("angular/z/min_acceleration" , limiter_ang_.min_acceleration , -limiter_ang_.max_acceleration );
218  controller_nh.param("angular/z/max_jerk" , limiter_ang_.max_jerk , limiter_ang_.max_jerk );
219  controller_nh.param("angular/z/min_jerk" , limiter_ang_.min_jerk , -limiter_ang_.max_jerk );
220 
221  // If either parameter is not available, we need to look up the value in the URDF
222  bool lookup_wheel_separation_h = !controller_nh.getParam("wheel_separation_h", wheel_separation_h_);
223  bool lookup_wheel_radius = !controller_nh.getParam("wheel_radius", wheel_radius_);
224 
225  if (!setOdomParamsFromUrdf(root_nh,
226  rear_wheel_name,
227  front_steer_name,
228  lookup_wheel_separation_h,
229  lookup_wheel_radius))
230  {
231  return false;
232  }
233 
234  // Regardless of how we got the separation and radius, use them
235  // to set the odometry parameters
237  const double wr = wheel_radius_multiplier_ * wheel_radius_;
238  odometry_.setWheelParams(ws_h, wr);
240  "Odometry params : wheel separation height " << ws_h
241  << ", wheel radius " << wr);
242 
243  setOdomPubFields(root_nh, controller_nh);
244 
245  //-- rear wheel
246  //---- handles need to be previously registerd in ackermann_steering_test.cpp
248  "Adding the rear wheel with joint name: " << rear_wheel_name);
249  rear_wheel_joint_ = vel_joint_if->getHandle(rear_wheel_name); // throws on failure
250  //-- front steer
252  "Adding the front steer with joint name: " << front_steer_name);
253  front_steer_joint_ = pos_joint_if->getHandle(front_steer_name); // throws on failure
255  "Adding the subscriber: cmd_vel");
256  sub_command_ = controller_nh.subscribe("cmd_vel", 1, &AckermannSteeringController::cmdVelCallback, this);
257  ROS_INFO_STREAM_NAMED(name_, "Finished controller initialization");
258 
259  return true;
260  }
261 
263  {
264  // COMPUTE AND PUBLISH ODOMETRY
265  if (open_loop_)
266  {
268  }
269  else
270  {
271  double wheel_pos = rear_wheel_joint_.getPosition();
272  double steer_pos = front_steer_joint_.getPosition();
273 
274  if (std::isnan(wheel_pos) || std::isnan(steer_pos))
275  return;
276 
277  // Estimate linear and angular velocity using joint information
278  steer_pos = steer_pos * steer_pos_multiplier_;
279  odometry_.update(wheel_pos, steer_pos, time);
280  }
281 
282  // Publish odometry message
284  {
286  // Compute and store orientation info
287  const geometry_msgs::Quaternion orientation(
289 
290  // Populate odom message and publish
291  if (odom_pub_->trylock())
292  {
293  odom_pub_->msg_.header.stamp = time;
294  odom_pub_->msg_.pose.pose.position.x = odometry_.getX();
295  odom_pub_->msg_.pose.pose.position.y = odometry_.getY();
296  odom_pub_->msg_.pose.pose.orientation = orientation;
297  odom_pub_->msg_.twist.twist.linear.x = odometry_.getLinear();
298  odom_pub_->msg_.twist.twist.angular.z = odometry_.getAngular();
299  odom_pub_->unlockAndPublish();
300  }
301 
302  // Publish tf /odom frame
303  if (enable_odom_tf_ && tf_odom_pub_->trylock())
304  {
305  geometry_msgs::TransformStamped& odom_frame = tf_odom_pub_->msg_.transforms[0];
306  odom_frame.header.stamp = time;
307  odom_frame.transform.translation.x = odometry_.getX();
308  odom_frame.transform.translation.y = odometry_.getY();
309  odom_frame.transform.rotation = orientation;
310  tf_odom_pub_->unlockAndPublish();
311  }
312  }
313 
314  // MOVE ROBOT
315  // Retreive current velocity command and time step:
316  Commands curr_cmd = *(command_.readFromRT());
317  const double dt = (time - curr_cmd.stamp).toSec();
318 
319  // Brake if cmd_vel has timeout:
320  if (dt > cmd_vel_timeout_)
321  {
322  curr_cmd.lin = 0.0;
323  curr_cmd.ang = 0.0;
324  }
325 
326  // Limit velocities and accelerations:
327  const double cmd_dt(period.toSec());
328 
329  limiter_lin_.limit(curr_cmd.lin, last0_cmd_.lin, last1_cmd_.lin, cmd_dt);
330  limiter_ang_.limit(curr_cmd.ang, last0_cmd_.ang, last1_cmd_.ang, cmd_dt);
331 
333  last0_cmd_ = curr_cmd;
334 
335  // Set Command
336  const double wheel_vel = curr_cmd.lin/wheel_radius_; // omega = linear_vel / radius
337  rear_wheel_joint_.setCommand(wheel_vel);
339 
340  }
341 
343  {
344  brake();
345 
346  // Register starting time used to keep fixed rate
348 
349  odometry_.init(time);
350  }
351 
353  {
354  brake();
355  }
356 
358  {
359  const double steer_pos = 0.0;
360  const double wheel_vel = 0.0;
361 
362  rear_wheel_joint_.setCommand(steer_pos);
363  front_steer_joint_.setCommand(wheel_vel);
364  }
365 
366  void AckermannSteeringController::cmdVelCallback(const geometry_msgs::Twist& command)
367  {
368  if (isRunning())
369  {
370  // check that we don't have multiple publishers on the command topic
372  {
374  << " publishers. Only 1 publisher is allowed. Going to brake.");
375  brake();
376  return;
377  }
378 
379  command_struct_.ang = command.angular.z;
380  command_struct_.lin = command.linear.x;
384  "Added values to command. "
385  << "Ang: " << command_struct_.ang << ", "
386  << "Lin: " << command_struct_.lin << ", "
387  << "Stamp: " << command_struct_.stamp);
388  }
389  else
390  {
391  ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running.");
392  }
393  }
394 
395 
397  const std::string rear_wheel_name,
398  const std::string front_steer_name,
399  bool lookup_wheel_separation_h,
400  bool lookup_wheel_radius)
401  {
402  if (!(lookup_wheel_separation_h || lookup_wheel_radius))
403  {
404  // Short-circuit in case we don't need to look up anything, so we don't have to parse the URDF
405  return true;
406  }
407 
408  // Parse robot description
409  const std::string model_param_name = "robot_description";
410  bool res = root_nh.hasParam(model_param_name);
411  std::string robot_model_str="";
412  if (!res || !root_nh.getParam(model_param_name,robot_model_str))
413  {
414  ROS_ERROR_NAMED(name_, "Robot descripion couldn't be retrieved from param server.");
415  return false;
416  }
417 
418  boost::shared_ptr<urdf::ModelInterface> model(urdf::parseURDF(robot_model_str));
419 
420  boost::shared_ptr<const urdf::Joint> rear_wheel_joint(model->getJoint(rear_wheel_name));
421  boost::shared_ptr<const urdf::Joint> front_steer_joint(model->getJoint(front_steer_name));
422 
423  if (lookup_wheel_separation_h)
424  {
425  // Get wheel separation
426  if (!rear_wheel_joint)
427  {
428  ROS_ERROR_STREAM_NAMED(name_, rear_wheel_name
429  << " couldn't be retrieved from model description");
430  return false;
431  }
432 
433  if (!front_steer_joint)
434  {
435  ROS_ERROR_STREAM_NAMED(name_, front_steer_name
436  << " couldn't be retrieved from model description");
437  return false;
438  }
439 
440  ROS_INFO_STREAM("rear wheel to origin: "
441  << rear_wheel_joint->parent_to_joint_origin_transform.position.x << ","
442  << rear_wheel_joint->parent_to_joint_origin_transform.position.y << ", "
443  << rear_wheel_joint->parent_to_joint_origin_transform.position.z);
444 
445  ROS_INFO_STREAM("front steer to origin: "
446  << front_steer_joint->parent_to_joint_origin_transform.position.x << ","
447  << front_steer_joint->parent_to_joint_origin_transform.position.y << ", "
448  << front_steer_joint->parent_to_joint_origin_transform.position.z);
449 
450  wheel_separation_h_ = fabs(
451  rear_wheel_joint->parent_to_joint_origin_transform.position.x
452  - front_steer_joint->parent_to_joint_origin_transform.position.x);
453 
454  ROS_INFO_STREAM("Calculated wheel_separation_h: " << wheel_separation_h_);
455  }
456 
457  if (lookup_wheel_radius)
458  {
459  // Get wheel radius
460  if (!getWheelRadius(model->getLink(rear_wheel_joint->child_link_name), wheel_radius_))
461  {
462  ROS_ERROR_STREAM_NAMED(name_, "Couldn't retrieve " << rear_wheel_name << " wheel radius");
463  return false;
464  }
465  ROS_INFO_STREAM("Retrieved wheel_radius: " << wheel_radius_);
466  }
467 
468  return true;
469  }
470 
472  {
473  // Get and check params for covariances
474  XmlRpc::XmlRpcValue pose_cov_list;
475  controller_nh.getParam("pose_covariance_diagonal", pose_cov_list);
477  ROS_ASSERT(pose_cov_list.size() == 6);
478  for (int i = 0; i < pose_cov_list.size(); ++i)
479  ROS_ASSERT(pose_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
480 
481  XmlRpc::XmlRpcValue twist_cov_list;
482  controller_nh.getParam("twist_covariance_diagonal", twist_cov_list);
484  ROS_ASSERT(twist_cov_list.size() == 6);
485  for (int i = 0; i < twist_cov_list.size(); ++i)
486  ROS_ASSERT(twist_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
487 
488  // Setup odometry realtime publisher + odom message constant fields
489  odom_pub_.reset(new realtime_tools::RealtimePublisher<nav_msgs::Odometry>(controller_nh, "odom", 100));
490  odom_pub_->msg_.header.frame_id = odom_frame_id_;
491  odom_pub_->msg_.child_frame_id = base_frame_id_;
492  odom_pub_->msg_.pose.pose.position.z = 0;
493  odom_pub_->msg_.pose.covariance = boost::assign::list_of
494  (static_cast<double>(pose_cov_list[0])) (0) (0) (0) (0) (0)
495  (0) (static_cast<double>(pose_cov_list[1])) (0) (0) (0) (0)
496  (0) (0) (static_cast<double>(pose_cov_list[2])) (0) (0) (0)
497  (0) (0) (0) (static_cast<double>(pose_cov_list[3])) (0) (0)
498  (0) (0) (0) (0) (static_cast<double>(pose_cov_list[4])) (0)
499  (0) (0) (0) (0) (0) (static_cast<double>(pose_cov_list[5]));
500  odom_pub_->msg_.twist.twist.linear.y = 0;
501  odom_pub_->msg_.twist.twist.linear.z = 0;
502  odom_pub_->msg_.twist.twist.angular.x = 0;
503  odom_pub_->msg_.twist.twist.angular.y = 0;
504  odom_pub_->msg_.twist.covariance = boost::assign::list_of
505  (static_cast<double>(twist_cov_list[0])) (0) (0) (0) (0) (0)
506  (0) (static_cast<double>(twist_cov_list[1])) (0) (0) (0) (0)
507  (0) (0) (static_cast<double>(twist_cov_list[2])) (0) (0) (0)
508  (0) (0) (0) (static_cast<double>(twist_cov_list[3])) (0) (0)
509  (0) (0) (0) (0) (static_cast<double>(twist_cov_list[4])) (0)
510  (0) (0) (0) (0) (0) (static_cast<double>(twist_cov_list[5]));
511  tf_odom_pub_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>(root_nh, "/tf", 100));
512  tf_odom_pub_->msg_.transforms.resize(1);
513  tf_odom_pub_->msg_.transforms[0].transform.translation.z = 0.0;
514  tf_odom_pub_->msg_.transforms[0].child_frame_id = base_frame_id_;
515  tf_odom_pub_->msg_.transforms[0].header.frame_id = odom_frame_id_;
516  }
517 
519 } // namespace ackermann_steering_controller
void init(const ros::Time &time)
Initialize the odometry.
Definition: odometry.cpp:68
static bool getWheelRadius(const boost::shared_ptr< const urdf::Link > &wheel_link, double &wheel_radius)
#define ROS_DEBUG_STREAM_NAMED(name, args)
#define ROS_ERROR_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.
bool enable_odom_tf_
Whether to publish odometry to tf or not:
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void brake()
Brakes the wheels, i.e. sets the velocity to 0.
int size() const
boost::shared_ptr< realtime_tools::RealtimePublisher< nav_msgs::Odometry > > odom_pub_
Odometry related:
static bool isCylinder(const boost::shared_ptr< const urdf::Link > &link)
#define ROS_INFO_STREAM_NAMED(name, args)
Type const & getType() const
double getAngular() const
angular velocity getter
Definition: odometry.h:138
#define ROS_ERROR_STREAM_THROTTLE_NAMED(period, name, args)
PLUGINLIB_EXPORT_CLASS(diff_drive_controller::DiffDriveController, controller_interface::ControllerBase)
void setWheelParams(double wheel_reparation_h, double wheel_radius)
Sets the wheel parameters: radius and separation.
Definition: odometry.cpp:126
double getHeading() const
heading getter
Definition: odometry.h:102
void writeFromNonRT(const T &data)
bool init(hardware_interface::RobotHW *robot_hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
Initialize controller.
double getLinear() const
linear velocity getter
Definition: odometry.h:129
bool setOdomParamsFromUrdf(ros::NodeHandle &root_nh, const std::string rear_wheel_name, const std::string front_steer_name, bool lookup_wheel_separation_h, bool lookup_wheel_radius)
Sets odometry parameters from the URDF, i.e. the wheel radius and separation.
double wheel_separation_h_multiplier_
Wheel separation and radius calibration multipliers:
static double euclideanOfVectors(const urdf::Vector3 &vec1, const urdf::Vector3 &vec2)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
uint32_t getNumPublishers() const
const std::string & getNamespace() const
std::string odom_frame_id_
Frame to use for odometry and odom tf:
void updateOpenLoop(double linear, double angular, const ros::Time &time)
Updates the odometry class with latest velocity command.
Definition: odometry.cpp:114
bool allow_multiple_cmd_vel_publishers_
Whether to allow multiple publishers on cmd_vel topic or not:
double cmd_vel_timeout_
Timeout to consider cmd_vel commands old:
double wheel_separation_h_
Wheel separation, wrt the midpoint of the wheel width:
hardware_interface::JointHandle rear_wheel_joint_
Hardware handles:
double getX() const
x position getter
Definition: odometry.h:111
void setCommand(double command)
static geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)
#define ROS_INFO_STREAM(args)
boost::shared_ptr< realtime_tools::RealtimePublisher< tf::tfMessage > > tf_odom_pub_
bool getParam(const std::string &key, std::string &s) const
double getY() const
y position getter
Definition: odometry.h:120
#define ROS_ERROR_NAMED(name,...)
static Time now()
void setOdomPubFields(ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
Sets the odometry publishing fields.
bool hasParam(const std::string &key) const
#define ROS_ERROR_STREAM(args)
#define ROS_ASSERT(cond)
#define ROS_ERROR(...)
double limit(double &v, double v0, double v1, double dt)
double wheel_radius_
Wheel radius (assuming it&#39;s the same for the left and right wheels):
void cmdVelCallback(const geometry_msgs::Twist &command)
Velocity command callback.
void setVelocityRollingWindowSize(size_t velocity_rolling_window_size)
Velocity rolling window size setter.
Definition: odometry.cpp:132
bool update(double rear_wheel_pos, double front_steer_pos, const ros::Time &time)
Updates the odometry class with latest wheels position.
Definition: odometry.cpp:75


ackermann_steering_controller
Author(s): Masaru Morita
autogenerated on Thu Apr 11 2019 03:08:13