four_wheel_steering_controller.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2017, Irstea
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 Irstea 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 #include <cmath>
36 
37 #include <tf/transform_datatypes.h>
38 
39 #include <boost/assign.hpp>
40 
43 
45 
47  : command_struct_twist_()
48  , command_struct_four_wheel_steering_()
49  , track_(0.0)
50  , wheel_steering_y_offset_(0.0)
51  , wheel_radius_(0.0)
52  , wheel_base_(0.0)
53  , cmd_vel_timeout_(0.5)
54  , base_frame_id_("base_link")
55  , enable_odom_tf_(true)
56  , enable_twist_cmd_(false)
57  {
58  }
59 
61  ros::NodeHandle& root_nh,
62  ros::NodeHandle &controller_nh)
63  {
64  const std::string complete_ns = controller_nh.getNamespace();
65  std::size_t id = complete_ns.find_last_of("/");
66  name_ = complete_ns.substr(id + 1);
67 
68  // Get joint names from the parameter server
69  std::vector<std::string> front_wheel_names, rear_wheel_names;
70  if (!getWheelNames(controller_nh, "front_wheel", front_wheel_names) ||
71  !getWheelNames(controller_nh, "rear_wheel", rear_wheel_names))
72  {
73  return false;
74  }
75 
76  if (front_wheel_names.size() != rear_wheel_names.size())
77  {
79  "#front wheels (" << front_wheel_names.size() << ") != " <<
80  "#rear wheels (" << rear_wheel_names.size() << ").");
81  return false;
82  }
83  else if (front_wheel_names.size() != 2)
84  {
86  "#two wheels by axle (left and right) is needed; now : "<<front_wheel_names.size()<<" .");
87  return false;
88  }
89  else
90  {
91  front_wheel_joints_.resize(front_wheel_names.size());
92  rear_wheel_joints_.resize(front_wheel_names.size());
93  }
94 
95  // Get steering joint names from the parameter server
96  std::vector<std::string> front_steering_names, rear_steering_names;
97  if (!getWheelNames(controller_nh, "front_steering", front_steering_names) ||
98  !getWheelNames(controller_nh, "rear_steering", rear_steering_names))
99  {
100  return false;
101  }
102 
103  if (front_steering_names.size() != rear_steering_names.size())
104  {
106  "#left steerings (" << front_steering_names.size() << ") != " <<
107  "#right steerings (" << rear_steering_names.size() << ").");
108  return false;
109  }
110  else if (front_steering_names.size() != 2)
111  {
113  "#two steering by axle (left and right) is needed; now : "<<front_steering_names.size()<<" .");
114  return false;
115  }
116  else
117  {
118  front_steering_joints_.resize(front_steering_names.size());
119  rear_steering_joints_.resize(front_steering_names.size());
120  }
121 
122  // Odometry related:
123  double publish_rate;
124  controller_nh.param("publish_rate", publish_rate, 50.0);
125  ROS_INFO_STREAM_NAMED(name_, "Controller state will be published at "
126  << publish_rate << "Hz.");
127  publish_period_ = ros::Duration(1.0 / publish_rate);
128 
129  controller_nh.param("open_loop", open_loop_, open_loop_);
130 
131  int velocity_rolling_window_size = 10;
132  controller_nh.param("velocity_rolling_window_size", velocity_rolling_window_size, velocity_rolling_window_size);
133  ROS_INFO_STREAM_NAMED(name_, "Velocity rolling window size of "
134  << velocity_rolling_window_size << ".");
135 
136  odometry_.setVelocityRollingWindowSize(velocity_rolling_window_size);
137 
138  // Twist command related:
139  controller_nh.param("cmd_vel_timeout", cmd_vel_timeout_, cmd_vel_timeout_);
140  ROS_INFO_STREAM_NAMED(name_, "Velocity commands will be considered old if they are older than "
141  << cmd_vel_timeout_ << "s.");
142 
143  controller_nh.param("base_frame_id", base_frame_id_, base_frame_id_);
144  ROS_INFO_STREAM_NAMED(name_, "Base frame_id set to " << base_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_lin_.has_velocity_limits , limiter_lin_.has_velocity_limits );
151  controller_nh.param("linear/x/has_acceleration_limits", limiter_lin_.has_acceleration_limits, limiter_lin_.has_acceleration_limits);
152  controller_nh.param("linear/x/max_velocity" , limiter_lin_.max_velocity , limiter_lin_.max_velocity );
153  controller_nh.param("linear/x/min_velocity" , limiter_lin_.min_velocity , -limiter_lin_.max_velocity );
154  controller_nh.param("linear/x/max_acceleration" , limiter_lin_.max_acceleration , limiter_lin_.max_acceleration );
155  controller_nh.param("linear/x/min_acceleration" , limiter_lin_.min_acceleration , -limiter_lin_.max_acceleration );
156 
157  controller_nh.param("angular/z/has_velocity_limits" , limiter_ang_.has_velocity_limits , limiter_ang_.has_velocity_limits );
158  controller_nh.param("angular/z/has_acceleration_limits", limiter_ang_.has_acceleration_limits, limiter_ang_.has_acceleration_limits);
159  controller_nh.param("angular/z/max_velocity" , limiter_ang_.max_velocity , limiter_ang_.max_velocity );
160  controller_nh.param("angular/z/min_velocity" , limiter_ang_.min_velocity , -limiter_ang_.max_velocity );
161  controller_nh.param("angular/z/max_acceleration" , limiter_ang_.max_acceleration , limiter_ang_.max_acceleration );
162  controller_nh.param("angular/z/min_acceleration" , limiter_ang_.min_acceleration , -limiter_ang_.max_acceleration );
163 
164  // If either parameter is not available, we need to look up the value in the URDF
165  bool lookup_track = !controller_nh.getParam("track", track_);
166  bool lookup_wheel_radius = !controller_nh.getParam("wheel_radius", wheel_radius_);
167  bool lookup_wheel_base = !controller_nh.getParam("wheel_base", wheel_base_);
168 
170  if(lookup_track)
171  if(!uvk.getDistanceBetweenJoints(front_wheel_names[0], front_wheel_names[1], track_))
172  return false;
173  else
174  controller_nh.setParam("track",track_);
175 
176  if(!uvk.getDistanceBetweenJoints(front_steering_names[0], front_wheel_names[0], wheel_steering_y_offset_))
177  return false;
178  else
179  controller_nh.setParam("wheel_steering_y_offset",wheel_steering_y_offset_);
180 
181  if(lookup_wheel_radius)
182  if(!uvk.getJointRadius(front_wheel_names[0], wheel_radius_))
183  return false;
184  else
185  controller_nh.setParam("wheel_radius",wheel_radius_);
186 
187  if(lookup_wheel_base)
188  if(!uvk.getDistanceBetweenJoints(front_wheel_names[0], rear_wheel_names[0], wheel_base_))
189  return false;
190  else
191  controller_nh.setParam("wheel_base",wheel_base_);
192 
193  // Regardless of how we got the separation and radius, use them
194  // to set the odometry parameters
197  "Odometry params : track " << track_
198  << ", wheel radius " << wheel_radius_
199  << ", wheel base " << wheel_base_
200  << ", wheel steering offset " << wheel_steering_y_offset_);
201 
202  setOdomPubFields(root_nh, controller_nh);
203 
204 
207 
208  // Get the joint object to use in the realtime loop
209  for (size_t i = 0; i < front_wheel_joints_.size(); ++i)
210  {
212  "Adding left wheel with joint name: " << front_wheel_names[i]
213  << " and right wheel with joint name: " << rear_wheel_names[i]);
214  front_wheel_joints_[i] = vel_joint_hw->getHandle(front_wheel_names[i]); // throws on failure
215  rear_wheel_joints_[i] = vel_joint_hw->getHandle(rear_wheel_names[i]); // throws on failure
216  }
217 
218  // Get the steering joint object to use in the realtime loop
219  for (size_t i = 0; i < front_steering_joints_.size(); ++i)
220  {
222  "Adding left steering with joint name: " << front_steering_names[i]
223  << " and right steering with joint name: " << rear_steering_names[i]);
224  front_steering_joints_[i] = pos_joint_hw->getHandle(front_steering_names[i]); // throws on failure
225  rear_steering_joints_[i] = pos_joint_hw->getHandle(rear_steering_names[i]); // throws on failure
226  }
227 
228  sub_command_ = controller_nh.subscribe("cmd_vel", 1, &FourWheelSteeringController::cmdVelCallback, this);
230 
231  return true;
232  }
233 
235  {
236  updateOdometry(time);
237  updateCommand(time, period);
238  }
239 
241  {
242  brake();
243 
244  // Register starting time used to keep fixed rate
246 
247  odometry_.init(time);
248  }
249 
251  {
252  brake();
253  }
254 
256  {
257  // COMPUTE AND PUBLISH ODOMETRY
258  const double fl_speed = front_wheel_joints_[0].getVelocity();
259  const double fr_speed = front_wheel_joints_[1].getVelocity();
260  const double rl_speed = rear_wheel_joints_[0].getVelocity();
261  const double rr_speed = rear_wheel_joints_[1].getVelocity();
262  if (std::isnan(fl_speed) || std::isnan(fr_speed)
263  || std::isnan(rl_speed) || std::isnan(rr_speed))
264  return;
265 
266  const double fl_steering = front_steering_joints_[0].getPosition();
267  const double fr_steering = front_steering_joints_[1].getPosition();
268  const double rl_steering = rear_steering_joints_[0].getPosition();
269  const double rr_steering = rear_steering_joints_[1].getPosition();
270  if (std::isnan(fl_steering) || std::isnan(fr_steering)
271  || std::isnan(rl_steering) || std::isnan(rr_steering))
272  return;
273  double front_steering_pos = 0.0;
274  if(fabs(fl_steering) > 0.001 || fabs(fr_steering) > 0.001)
275  {
276  front_steering_pos = atan(2*tan(fl_steering)*tan(fr_steering)/
277  (tan(fl_steering) + tan(fr_steering)));
278  }
279  double rear_steering_pos = 0.0;
280  if(fabs(rl_steering) > 0.001 || fabs(rr_steering) > 0.001)
281  {
282  rear_steering_pos = atan(2*tan(rl_steering)*tan(rr_steering)/
283  (tan(rl_steering) + tan(rr_steering)));
284  }
285 
286  ROS_DEBUG_STREAM_THROTTLE(1, "rl_steering "<<rl_steering<<" rr_steering "<<rr_steering<<" rear_steering_pos "<<rear_steering_pos);
287  // Estimate linear and angular velocity using joint information
288  odometry_.update(fl_speed, fr_speed, rl_speed, rr_speed,
289  front_steering_pos, rear_steering_pos, time);
290 
291  // Publish odometry message
293  {
295  // Compute and store orientation info
296  const geometry_msgs::Quaternion orientation(
298 
299  // Populate odom message and publish
300  if (odom_pub_->trylock())
301  {
302  odom_pub_->msg_.header.stamp = time;
303  odom_pub_->msg_.pose.pose.position.x = odometry_.getX();
304  odom_pub_->msg_.pose.pose.position.y = odometry_.getY();
305  odom_pub_->msg_.pose.pose.orientation = orientation;
306  odom_pub_->msg_.twist.twist.linear.x = odometry_.getLinearX();
307  odom_pub_->msg_.twist.twist.linear.y = odometry_.getLinearY();
308  odom_pub_->msg_.twist.twist.angular.z = odometry_.getAngular();
309  odom_pub_->unlockAndPublish();
310  }
311  if (odom_4ws_pub_->trylock())
312  {
313  odom_4ws_pub_->msg_.header.stamp = time;
314  odom_4ws_pub_->msg_.data.speed = odometry_.getLinear();
315  odom_4ws_pub_->msg_.data.acceleration = odometry_.getLinearAcceleration();
316  odom_4ws_pub_->msg_.data.jerk = odometry_.getLinearJerk();
317  odom_4ws_pub_->msg_.data.front_steering_angle = front_steering_pos;
318  odom_4ws_pub_->msg_.data.front_steering_angle_velocity = odometry_.getFrontSteerVel();
319  odom_4ws_pub_->msg_.data.rear_steering_angle = rear_steering_pos;
320  odom_4ws_pub_->msg_.data.rear_steering_angle_velocity = odometry_.getRearSteerVel();
321  odom_4ws_pub_->unlockAndPublish();
322  }
323 
324  // Publish tf /odom frame
325  if (enable_odom_tf_ && tf_odom_pub_->trylock())
326  {
327  geometry_msgs::TransformStamped& odom_frame = tf_odom_pub_->msg_.transforms[0];
328  odom_frame.header.stamp = time;
329  odom_frame.transform.translation.x = odometry_.getX();
330  odom_frame.transform.translation.y = odometry_.getY();
331  odom_frame.transform.rotation = orientation;
332  tf_odom_pub_->unlockAndPublish();
333  }
334  }
335  }
336 
338  {
339  // Retreive current velocity command and time step:
340  Command* cmd;
341  CommandTwist curr_cmd_twist = *(command_twist_.readFromRT());
342  Command4ws curr_cmd_4ws = *(command_four_wheel_steering_.readFromRT());
343 
344  if(curr_cmd_4ws.stamp >= curr_cmd_twist.stamp)
345  {
346  cmd = &curr_cmd_4ws;
347  enable_twist_cmd_ = false;
348  }
349  else
350  {
351  cmd = &curr_cmd_twist;
352  enable_twist_cmd_ = true;
353  }
354 
355  const double dt = (time - cmd->stamp).toSec();
356  // Brake if cmd_vel has timeout:
357  if (dt > cmd_vel_timeout_)
358  {
359  curr_cmd_twist.lin_x = 0.0;
360  curr_cmd_twist.lin_y = 0.0;
361  curr_cmd_twist.ang = 0.0;
362  curr_cmd_4ws.lin = 0.0;
363  curr_cmd_4ws.front_steering = 0.0;
364  curr_cmd_4ws.rear_steering = 0.0;
365  }
366 
367  const double cmd_dt(period.toSec());
368 
369  const double angular_speed = odometry_.getAngular();
370  const double steering_track = track_-2*wheel_steering_y_offset_;
371 
372  ROS_DEBUG_STREAM("angular_speed "<<angular_speed<< " wheel_radius_ "<<wheel_radius_);
373  double vel_left_front = 0, vel_right_front = 0;
374  double vel_left_rear = 0, vel_right_rear = 0;
375  double front_left_steering = 0, front_right_steering = 0;
376  double rear_left_steering = 0, rear_right_steering = 0;
377 
378  if(enable_twist_cmd_ == true)
379  {
380  // Limit velocities and accelerations:
381  limiter_lin_.limit(curr_cmd_twist.lin_x, last0_cmd_.lin_x, last1_cmd_.lin_x, cmd_dt);
382  limiter_ang_.limit(curr_cmd_twist.ang, last0_cmd_.ang, last1_cmd_.ang, cmd_dt);
384  last0_cmd_ = curr_cmd_twist;
385 
386  // Compute wheels velocities:
387  if(fabs(curr_cmd_twist.lin_x) > 0.001)
388  {
389  double vel_steering_offset = (curr_cmd_twist.ang*wheel_steering_y_offset_)/wheel_radius_;
390  vel_left_front = copysign(1.0, curr_cmd_twist.lin_x) * sqrt((pow(curr_cmd_twist.lin_x - curr_cmd_twist.ang*steering_track/2,2)
391  +pow(wheel_base_*curr_cmd_twist.ang/2.0,2)))/wheel_radius_
392  - vel_steering_offset;
393  vel_right_front = copysign(1.0, curr_cmd_twist.lin_x) * sqrt((pow(curr_cmd_twist.lin_x + curr_cmd_twist.ang*steering_track/2,2)
394  +pow(wheel_base_*curr_cmd_twist.ang/2.0,2)))/wheel_radius_
395  + vel_steering_offset;
396  vel_left_rear = copysign(1.0, curr_cmd_twist.lin_x) * sqrt((pow(curr_cmd_twist.lin_x - curr_cmd_twist.ang*steering_track/2,2)
397  +pow(wheel_base_*curr_cmd_twist.ang/2.0,2)))/wheel_radius_
398  - vel_steering_offset;
399  vel_right_rear = copysign(1.0, curr_cmd_twist.lin_x) * sqrt((pow(curr_cmd_twist.lin_x + curr_cmd_twist.ang*steering_track/2,2)
400  +pow(wheel_base_*curr_cmd_twist.ang/2.0,2)))/wheel_radius_
401  + vel_steering_offset;
402  }
403 
404  // Compute steering angles
405  if(fabs(2.0*curr_cmd_twist.lin_x) > fabs(curr_cmd_twist.ang*steering_track))
406  {
407  front_left_steering = atan(curr_cmd_twist.ang*wheel_base_ /
408  (2.0*curr_cmd_twist.lin_x - curr_cmd_twist.ang*steering_track));
409  front_right_steering = atan(curr_cmd_twist.ang*wheel_base_ /
410  (2.0*curr_cmd_twist.lin_x + curr_cmd_twist.ang*steering_track));
411  rear_left_steering = -atan(curr_cmd_twist.ang*wheel_base_ /
412  (2.0*curr_cmd_twist.lin_x - curr_cmd_twist.ang*steering_track));
413  rear_right_steering = -atan(curr_cmd_twist.ang*wheel_base_ /
414  (2.0*curr_cmd_twist.lin_x + curr_cmd_twist.ang*steering_track));
415  }
416  else if(fabs(curr_cmd_twist.lin_x) > 0.001)
417  {
418  front_left_steering = copysign(M_PI_2, curr_cmd_twist.ang);
419  front_right_steering = copysign(M_PI_2, curr_cmd_twist.ang);
420  rear_left_steering = copysign(M_PI_2, -curr_cmd_twist.ang);
421  rear_right_steering = copysign(M_PI_2, -curr_cmd_twist.ang);
422  }
423  }
424  else
425  {
426  // Limit velocities and accelerations:
427  limiter_lin_.limit(curr_cmd_4ws.lin, last0_cmd_.lin_x, last1_cmd_.lin_x, cmd_dt);
429  last0_cmd_.lin_x = curr_cmd_4ws.lin;
430  curr_cmd_4ws.front_steering = clamp(curr_cmd_4ws.front_steering, -M_PI_2, M_PI_2);
431  curr_cmd_4ws.rear_steering = clamp(curr_cmd_4ws.rear_steering, -M_PI_2, M_PI_2);
432 
433  // Compute steering angles
434  double steering_diff = steering_track*(tan(curr_cmd_4ws.front_steering) - tan(curr_cmd_4ws.rear_steering))/2.0;
435  if(fabs(wheel_base_ - fabs(steering_diff)) > 0.001)
436  {
437  front_left_steering = atan(wheel_base_*tan(curr_cmd_4ws.front_steering)/(wheel_base_-steering_diff));
438  front_right_steering = atan(wheel_base_*tan(curr_cmd_4ws.front_steering)/(wheel_base_+steering_diff));
439  rear_left_steering = atan(wheel_base_*tan(curr_cmd_4ws.rear_steering)/(wheel_base_-steering_diff));
440  rear_right_steering = atan(wheel_base_*tan(curr_cmd_4ws.rear_steering)/(wheel_base_+steering_diff));
441  }
442 
443  // Compute wheels velocities:
444  if(fabs(curr_cmd_4ws.lin) > 0.001)
445  {
446  //Virutal front and rear wheelbase
447  // distance between the projection of the CIR on the wheelbase and the front axle
448  double l_front = 0;
449  if(fabs(tan(front_left_steering) - tan(front_right_steering)) > 0.01)
450  {
451  l_front = tan(front_right_steering) * tan(front_left_steering) * steering_track
452  / (tan(front_left_steering) - tan(front_right_steering));
453  }
454  // distance between the projection of the CIR on the wheelbase and the rear axle
455  double l_rear = 0;
456  if(fabs(tan(rear_left_steering) - tan(rear_right_steering)) > 0.01)
457  {
458  l_rear = tan(rear_right_steering) * tan(rear_left_steering) * steering_track
459  / (tan(rear_left_steering) - tan(rear_right_steering));
460  }
461 
462  double angular_speed_cmd = curr_cmd_4ws.lin * (tan(curr_cmd_4ws.front_steering)-tan(curr_cmd_4ws.rear_steering))/wheel_base_;
463  double vel_steering_offset = (angular_speed_cmd*wheel_steering_y_offset_)/wheel_radius_;
464 
465  vel_left_front = copysign(1.0, curr_cmd_4ws.lin) * sqrt((pow(curr_cmd_4ws.lin - angular_speed_cmd*steering_track/2,2)
466  +pow(l_front*angular_speed_cmd,2)))/wheel_radius_
467  - vel_steering_offset;
468  vel_right_front = copysign(1.0, curr_cmd_4ws.lin) * sqrt((pow(curr_cmd_4ws.lin + angular_speed_cmd*steering_track/2,2)
469  +pow(l_front*angular_speed_cmd,2)))/wheel_radius_
470  + vel_steering_offset;
471  vel_left_rear = copysign(1.0, curr_cmd_4ws.lin) * sqrt((pow(curr_cmd_4ws.lin - angular_speed_cmd*steering_track/2,2)
472  +pow(l_rear*angular_speed_cmd,2)))/wheel_radius_
473  - vel_steering_offset;
474  vel_right_rear = copysign(1.0, curr_cmd_4ws.lin) * sqrt((pow(curr_cmd_4ws.lin + angular_speed_cmd*steering_track/2,2)
475  +pow(l_rear*angular_speed_cmd,2)))/wheel_radius_
476  + vel_steering_offset;
477  }
478  }
479 
480  ROS_DEBUG_STREAM_THROTTLE(1, "vel_left_rear "<<vel_left_rear<<" front_right_steering "<<front_right_steering);
481  // Set wheels velocities:
482  if(front_wheel_joints_.size() == 2 && rear_wheel_joints_.size() == 2)
483  {
484  front_wheel_joints_[0].setCommand(vel_left_front);
485  front_wheel_joints_[1].setCommand(vel_right_front);
486  rear_wheel_joints_[0].setCommand(vel_left_rear);
487  rear_wheel_joints_[1].setCommand(vel_right_rear);
488  }
489 
491  if(front_steering_joints_.size() == 2 && rear_steering_joints_.size() == 2)
492  {
493  front_steering_joints_[0].setCommand(front_left_steering);
494  front_steering_joints_[1].setCommand(front_right_steering);
495  rear_steering_joints_[0].setCommand(rear_left_steering);
496  rear_steering_joints_[1].setCommand(rear_right_steering);
497  }
498  }
499 
501  {
502  const double vel = 0.0;
503  for (size_t i = 0; i < front_wheel_joints_.size(); ++i)
504  {
505  front_wheel_joints_[i].setCommand(vel);
506  rear_wheel_joints_[i].setCommand(vel);
507  }
508 
509  const double pos = 0.0;
510  for (size_t i = 0; i < front_steering_joints_.size(); ++i)
511  {
512  front_steering_joints_[i].setCommand(pos);
513  rear_steering_joints_[i].setCommand(pos);
514  }
515  }
516 
517  void FourWheelSteeringController::cmdVelCallback(const geometry_msgs::Twist& command)
518  {
519  if (isRunning())
520  {
521  if(std::isnan(command.angular.z) || std::isnan(command.linear.x))
522  {
523  ROS_WARN("Received NaN in geometry_msgs::Twist. Ignoring command.");
524  return;
525  }
526  command_struct_twist_.ang = command.angular.z;
527  command_struct_twist_.lin_x = command.linear.x;
528  command_struct_twist_.lin_y = command.linear.y;
530  command_twist_.writeFromNonRT (command_struct_twist_);
532  "Added values to command. "
533  << "Ang: " << command_struct_twist_.ang << ", "
534  << "Lin x: " << command_struct_twist_.lin_x << ", "
535  << "Lin y: " << command_struct_twist_.lin_y << ", "
536  << "Stamp: " << command_struct_twist_.stamp);
537  }
538  else
539  {
540  ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running.");
541  }
542  }
543 
544  void FourWheelSteeringController::cmdFourWheelSteeringCallback(const four_wheel_steering_msgs::FourWheelSteering& command)
545  {
546  if (isRunning())
547  {
548  if(std::isnan(command.front_steering_angle) || std::isnan(command.rear_steering_angle)
549  || std::isnan(command.speed))
550  {
551  ROS_WARN("Received NaN in four_wheel_steering_msgs::FourWheelSteering. Ignoring command.");
552  return;
553  }
554  command_struct_four_wheel_steering_.front_steering = command.front_steering_angle;
555  command_struct_four_wheel_steering_.rear_steering = command.rear_steering_angle;
560  "Added values to command. "
561  << "Steering front : " << command_struct_four_wheel_steering_.front_steering << ", "
562  << "Steering rear : " << command_struct_four_wheel_steering_.rear_steering << ", "
563  << "Lin: " << command_struct_four_wheel_steering_.lin << ", "
565  }
566  else
567  {
568  ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running.");
569  }
570  }
571 
573  const std::string& wheel_param,
574  std::vector<std::string>& wheel_names)
575  {
576  XmlRpc::XmlRpcValue wheel_list;
577  if (!controller_nh.getParam(wheel_param, wheel_list))
578  {
580  "Couldn't retrieve wheel param '" << wheel_param << "'.");
581  return false;
582  }
583 
584  if (wheel_list.getType() == XmlRpc::XmlRpcValue::TypeArray)
585  {
586  if (wheel_list.size() == 0)
587  {
589  "Wheel param '" << wheel_param << "' is an empty list");
590  return false;
591  }
592 
593  for (int i = 0; i < wheel_list.size(); ++i)
594  {
595  if (wheel_list[i].getType() != XmlRpc::XmlRpcValue::TypeString)
596  {
598  "Wheel param '" << wheel_param << "' #" << i <<
599  " isn't a string.");
600  return false;
601  }
602  }
603 
604  wheel_names.resize(wheel_list.size());
605  for (int i = 0; i < wheel_list.size(); ++i)
606  {
607  wheel_names[i] = static_cast<std::string>(wheel_list[i]);
608  //ROS_INFO_STREAM("wheel name "<<i<<" " << wheel_names[i]);
609  }
610  }
611  else if (wheel_list.getType() == XmlRpc::XmlRpcValue::TypeString)
612  {
613  wheel_names.push_back(wheel_list);
614  }
615  else
616  {
618  "Wheel param '" << wheel_param <<
619  "' is neither a list of strings nor a string.");
620  return false;
621  }
622  return true;
623  }
624 
626  {
627  // Get and check params for covariances
628  XmlRpc::XmlRpcValue pose_cov_list;
629  controller_nh.getParam("pose_covariance_diagonal", pose_cov_list);
631  ROS_ASSERT(pose_cov_list.size() == 6);
632  for (int i = 0; i < pose_cov_list.size(); ++i)
633  ROS_ASSERT(pose_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
634 
635  XmlRpc::XmlRpcValue twist_cov_list;
636  controller_nh.getParam("twist_covariance_diagonal", twist_cov_list);
638  ROS_ASSERT(twist_cov_list.size() == 6);
639  for (int i = 0; i < twist_cov_list.size(); ++i)
640  ROS_ASSERT(twist_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
641 
642  // Setup odometry realtime publisher + odom message constant fields
643  odom_pub_.reset(new realtime_tools::RealtimePublisher<nav_msgs::Odometry>(controller_nh, "odom", 100));
644  odom_pub_->msg_.header.frame_id = "odom";
645  odom_pub_->msg_.child_frame_id = base_frame_id_;
646  odom_pub_->msg_.pose.pose.position.z = 0;
647  odom_pub_->msg_.pose.covariance = boost::assign::list_of
648  (static_cast<double>(pose_cov_list[0])) (0) (0) (0) (0) (0)
649  (0) (static_cast<double>(pose_cov_list[1])) (0) (0) (0) (0)
650  (0) (0) (static_cast<double>(pose_cov_list[2])) (0) (0) (0)
651  (0) (0) (0) (static_cast<double>(pose_cov_list[3])) (0) (0)
652  (0) (0) (0) (0) (static_cast<double>(pose_cov_list[4])) (0)
653  (0) (0) (0) (0) (0) (static_cast<double>(pose_cov_list[5]));
654  odom_pub_->msg_.twist.twist.linear.y = 0;
655  odom_pub_->msg_.twist.twist.linear.z = 0;
656  odom_pub_->msg_.twist.twist.angular.x = 0;
657  odom_pub_->msg_.twist.twist.angular.y = 0;
658  odom_pub_->msg_.twist.covariance = boost::assign::list_of
659  (static_cast<double>(twist_cov_list[0])) (0) (0) (0) (0) (0)
660  (0) (static_cast<double>(twist_cov_list[1])) (0) (0) (0) (0)
661  (0) (0) (static_cast<double>(twist_cov_list[2])) (0) (0) (0)
662  (0) (0) (0) (static_cast<double>(twist_cov_list[3])) (0) (0)
663  (0) (0) (0) (0) (static_cast<double>(twist_cov_list[4])) (0)
664  (0) (0) (0) (0) (0) (static_cast<double>(twist_cov_list[5]));
665 
667  odom_4ws_pub_->msg_.header.frame_id = "odom";
668 
669  tf_odom_pub_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>(root_nh, "/tf", 100));
670  tf_odom_pub_->msg_.transforms.resize(1);
671  tf_odom_pub_->msg_.transforms[0].transform.translation.z = 0.0;
672  tf_odom_pub_->msg_.transforms[0].child_frame_id = base_frame_id_;
673  tf_odom_pub_->msg_.transforms[0].header.frame_id = "odom";
674  }
675 
676 } // namespace four_wheel_steering_controller
void setVelocityRollingWindowSize(size_t velocity_rolling_window_size)
Velocity rolling window size setter.
Definition: odometry.cpp:131
#define ROS_DEBUG_STREAM_THROTTLE(rate, args)
double cmd_vel_timeout_
Timeout to consider cmd_vel commands old:
double getX() const
x position getter
Definition: odometry.h:100
void cmdVelCallback(const geometry_msgs::Twist &command)
Velocity command callback.
#define ROS_DEBUG_STREAM_NAMED(name, args)
#define ROS_ERROR_STREAM_NAMED(name, args)
void updateCommand(const ros::Time &time, const ros::Duration &period)
Compute and publish command.
double getLinearY() const
linear velocity getter along Y on the robot base link frame
Definition: odometry.h:137
std::vector< hardware_interface::JointHandle > front_steering_joints_
std::vector< hardware_interface::JointHandle > rear_wheel_joints_
double getLinearAcceleration() const
linear acceleration getter
Definition: odometry.h:155
void update(const ros::Time &time, const ros::Duration &period)
Updates controller, i.e. computes the odometry and sets the new velocity commands.
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
string cmd
int size() const
double getLinear() const
linear velocity getter norm
Definition: odometry.h:119
void updateOdometry(const ros::Time &time)
Update and publish odometry.
bool init(hardware_interface::RobotHW *robot_hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
Initialize controller.
std::vector< hardware_interface::JointHandle > rear_steering_joints_
void cmdFourWheelSteeringCallback(const four_wheel_steering_msgs::FourWheelSteering &command)
Velocity and steering command callback.
#define ROS_INFO_STREAM_NAMED(name, args)
bool enable_twist_cmd_
Whether the control is make with four_wheel_steering msg or twist msg:
double getY() const
y position getter
Definition: odometry.h:109
Type const & getType() const
#define ROS_WARN(...)
double limit(double &v, double v0, double v1, double dt)
Limit the velocity and acceleration.
double wheel_radius_
Wheel radius (assuming it&#39;s the same for the left and right wheels):
bool update(const double &fl_speed, const double &fr_speed, const double &rl_speed, const double &rr_speed, double front_steering, double rear_steering, const ros::Time &time)
Updates the odometry class with latest wheels and steerings position.
Definition: odometry.cpp:72
std::vector< hardware_interface::JointHandle > front_wheel_joints_
Hardware handles:
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.
boost::shared_ptr< realtime_tools::RealtimePublisher< tf::tfMessage > > tf_odom_pub_
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void setWheelParams(double steering_track, double wheel_steering_y_offset, double wheel_radius, double wheel_base)
Sets the wheel parameters: radius and separation.
Definition: odometry.cpp:123
const std::string & getNamespace() const
double getLinearJerk() const
linear jerk getter
Definition: odometry.h:164
void setOdomPubFields(ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
Sets the odometry publishing fields.
#define ROS_DEBUG_STREAM(args)
void brake()
Brakes the wheels, i.e. sets the velocity to 0.
bool getJointRadius(const std::string &joint_name, double &radius)
boost::shared_ptr< realtime_tools::RealtimePublisher< nav_msgs::Odometry > > odom_pub_
Odometry related:
JointHandle getHandle(const std::string &name)
static geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)
bool getParam(const std::string &key, std::string &s) const
#define ROS_ERROR_NAMED(name,...)
static Time now()
bool getDistanceBetweenJoints(const std::string &first_joint_name, const std::string &second_joint_name, double &distance)
double getFrontSteerVel() const
front steering velocity getter
Definition: odometry.h:173
boost::shared_ptr< realtime_tools::RealtimePublisher< four_wheel_steering_msgs::FourWheelSteeringStamped > > odom_4ws_pub_
double getHeading() const
heading getter
Definition: odometry.h:91
#define ROS_ASSERT(cond)
double getRearSteerVel() const
rear steering velocity getter
Definition: odometry.h:182
void init(const ros::Time &time)
Initialize the odometry.
Definition: odometry.cpp:65
double getAngular() const
angular velocity getter
Definition: odometry.h:146
double track_
Wheel separation (or track), distance between left and right wheels (from the midpoint of the wheel w...
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
double getLinearX() const
linear velocity getter along X on the robot base link frame
Definition: odometry.h:128
double wheel_base_
Wheel base (distance between front and rear wheel):
T clamp(T x, T min, T max)
Definition: speed_limiter.h:46
realtime_tools::RealtimeBuffer< Command4ws > command_four_wheel_steering_
FourWheelSteering command related:


four_wheel_steering_controller
Author(s): Vincent Rousseau
autogenerated on Sat Apr 18 2020 03:58:13