ackermann_steering_bot.h
Go to the documentation of this file.
1 // Copyright (C) 2013, PAL Robotics S.L.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright notice,
7 // this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name of PAL Robotics, Inc. nor the names of its
12 // contributors may be used to endorse or promote products derived from
13 // this software without specific prior written permission.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25 // POSSIBILITY OF SUCH DAMAGE.
27 
29 
30 // NOTE: The contents of this file have been taken largely from the ros_control wiki tutorials
31 
32 // ROS
33 #include <ros/ros.h>
34 #include <std_msgs/Float64.h>
35 #include <std_srvs/Empty.h>
36 
37 // ros_control
38 #include <controller_manager/controller_manager.h>
43 
44 // NaN
45 #include <limits>
46 
47 // ostringstream
48 #include <sstream>
49 
53 };
54 
56 {
57 public:
59  : running_(true)
62  , ns_("ackermann_steering_bot_hw_sim/")
63  {
64  // Intialize raw data
65  this->cleanUp();
66  this->getJointNames(nh_);
68 
69  nh_.getParam(ns_ + "wheel_separation_w", wheel_separation_w_);
70  nh_.getParam(ns_ + "wheel_separation_h", wheel_separation_h_);
71  ROS_INFO_STREAM("wheel_separation_w in test ackermann_steering_bot= " << wheel_separation_w_);
72  ROS_INFO_STREAM("wheel_separation_h in test ackermann_steering_bot= " << wheel_separation_h_);
73  }
74 
75  ros::Time getTime() const {return ros::Time::now();}
76  ros::Duration getPeriod() const {return ros::Duration(0.01);}
77 
78  void read()
79  {
80  std::ostringstream os;
81  // directly get from controller
82  os << rear_wheel_jnt_vel_cmd_ << ", ";
83  os << front_steer_jnt_pos_cmd_ << ", ";
84 
85  // convert to each joint velocity
86  //-- differential drive
87  for (unsigned int i = 0; i < virtual_rear_wheel_jnt_vel_cmd_.size(); i++)
88  {
90  os << virtual_rear_wheel_jnt_vel_cmd_[i] << ", ";
91  }
92 
93  //-- ackerman link
94  const double h = wheel_separation_h_;
95  const double w = wheel_separation_w_;
96  virtual_front_steer_jnt_pos_cmd_[INDEX_RIGHT] = atan2(2*h*tan(front_steer_jnt_pos_cmd_), 2*h + w/2.0*tan(front_steer_jnt_pos_cmd_));
97  virtual_front_steer_jnt_pos_cmd_[INDEX_LEFT] = atan2(2*h*tan(front_steer_jnt_pos_cmd_), 2*h - w/2.0*tan(front_steer_jnt_pos_cmd_));
98 
99  for (unsigned int i = 0; i < virtual_front_steer_jnt_pos_cmd_.size(); i++)
100  {
101  os << virtual_front_steer_jnt_pos_cmd_[i] << ", ";
102  }
103 
104  if (rear_wheel_jnt_vel_cmd_ != 0.0 || front_steer_jnt_pos_cmd_ != 0.0)
105  ROS_INFO_STREAM("Commands for joints: " << os.str());
106 
107  }
108 
109  void write()
110  {
111  std::ostringstream os;
112  if (running_)
113  {
114  // wheels
117  for (unsigned int i = 0; i < virtual_rear_wheel_jnt_vel_cmd_.size(); i++)
118  {
119  // Note that pos_[i] will be NaN for one more cycle after we start(),
120  // but that is consistent with the knowledge we have about the state
121  // of the robot.
124  }
125 
126  // steers
128  for (unsigned int i = 0; i < virtual_front_steer_jnt_pos_cmd_.size(); i++)
129  {
131  }
132 
133  // directly get from controller
134  os << rear_wheel_jnt_vel_cmd_ << ", ";
135  os << front_steer_jnt_pos_cmd_ << ", ";
136 
137  // convert to each joint velocity
138  //-- differential drive
139  for (unsigned int i = 0; i < virtual_rear_wheel_jnt_pos_.size(); i++)
140  {
141  os << virtual_rear_wheel_jnt_pos_[i] << ", ";
142  }
143 
144  //-- ackerman link
145  for (unsigned int i = 0; i < virtual_front_steer_jnt_pos_.size(); i++)
146  {
147  os << virtual_front_steer_jnt_pos_[i] << ", ";
148  }
149  }
150  else
151  {
152  // wheels
153  rear_wheel_jnt_pos_= std::numeric_limits<double>::quiet_NaN();
154  rear_wheel_jnt_vel_= std::numeric_limits<double>::quiet_NaN();
155  std::fill_n(virtual_rear_wheel_jnt_pos_.begin(), virtual_rear_wheel_jnt_pos_.size(), std::numeric_limits<double>::quiet_NaN());
156  std::fill_n(virtual_rear_wheel_jnt_vel_.begin(), virtual_rear_wheel_jnt_vel_.size(), std::numeric_limits<double>::quiet_NaN());
157  // steers
158  front_steer_jnt_pos_= std::numeric_limits<double>::quiet_NaN();
159  front_steer_jnt_vel_= std::numeric_limits<double>::quiet_NaN();
160  std::fill_n(virtual_front_steer_jnt_pos_.begin(), virtual_front_steer_jnt_pos_.size(), std::numeric_limits<double>::quiet_NaN());
161  std::fill_n(virtual_front_steer_jnt_vel_.begin(), virtual_front_steer_jnt_vel_.size(), std::numeric_limits<double>::quiet_NaN());
162 
163  // wheels
164  os << rear_wheel_jnt_pos_ << ", ";
165  os << rear_wheel_jnt_vel_ << ", ";
166  for (unsigned int i = 0; i < virtual_rear_wheel_jnt_pos_.size(); i++)
167  {
168  os << virtual_rear_wheel_jnt_pos_[i] << ", ";
169  }
170 
171  // steers
172  os << front_steer_jnt_pos_ << ", ";
173  os << front_steer_jnt_vel_ << ", ";
174  for (unsigned int i = 0; i < virtual_front_steer_jnt_pos_.size(); i++)
175  {
176  os << virtual_front_steer_jnt_pos_[i] << ", ";
177  }
178  }
179  ROS_INFO_STREAM("running_ = " << running_ << ". commands are " << os.str());
180  }
181 
182  bool startCallback(std_srvs::Empty::Request& /*req*/, std_srvs::Empty::Response& /*res*/)
183  {
184  ROS_INFO_STREAM("running_ = " << running_ << ".");
185  running_ = true;
186  return true;
187  }
188 
189  bool stopCallback(std_srvs::Empty::Request& /*req*/, std_srvs::Empty::Response& /*res*/)
190  {
191  ROS_INFO_STREAM("running_ = " << running_ << ".");
192  running_ = false;
193  return true;
194  }
195 
196 private:
197 
198  void cleanUp()
199  {
200  // wheel
201  //-- wheel joint names
202  rear_wheel_jnt_name_.empty();
204  //-- actual rear wheel joint
209  //-- virtual rear wheel joint
214  //-- virtual front wheel joint
219 
220  // steer
221  //-- steer joint names
222  front_steer_jnt_name_.empty();
224  //-- front steer joint
229  //-- virtual wheel joint
234  }
235 
237  {
238  this->getWheelJointNames(_nh);
239  this->getSteerJointNames(_nh);
240  }
241 
243  {
244  // wheel joint to get linear command
245  _nh.getParam(ns_ + "rear_wheel", rear_wheel_jnt_name_);
246 
247  // virtual wheel joint for gazebo con_rol
248  _nh.getParam(ns_ + "rear_wheels", virtual_rear_wheel_jnt_names_);
249  int dof = virtual_rear_wheel_jnt_names_.size();
250  virtual_rear_wheel_jnt_pos_.resize(dof);
251  virtual_rear_wheel_jnt_vel_.resize(dof);
252  virtual_rear_wheel_jnt_eff_.resize(dof);
254 
255  _nh.getParam(ns_ + "front_wheels", virtual_front_wheel_jnt_names_);
256  dof = virtual_front_wheel_jnt_names_.size();
257  virtual_front_wheel_jnt_pos_.resize(dof);
258  virtual_front_wheel_jnt_vel_.resize(dof);
259  virtual_front_wheel_jnt_eff_.resize(dof);
261  }
262 
264  {
265  // steer joint to get angular command
266  _nh.getParam(ns_ + "front_steer", front_steer_jnt_name_);
267 
268  // virtual steer joint for gazebo control
269  _nh.getParam(ns_ + "front_steers", virtual_front_steer_jnt_names_);
270 
271  const int dof = virtual_front_steer_jnt_names_.size();
272  virtual_front_steer_jnt_pos_.resize(dof);
273  virtual_front_steer_jnt_vel_.resize(dof);
274  virtual_front_steer_jnt_eff_.resize(dof);
276  }
277 
279  {
280  this->registerSteerInterface();
281  this->registerWheelInterface();
282 
283  // register mapped interface to ros_control
287  }
288 
290  {
291  // actual wheel joints
295 
296  // virtual rear wheel joints
297  for (int i = 0; i < virtual_rear_wheel_jnt_names_.size(); i++)
298  {
302  }
303  // virtual front wheel joints
304  for (int i = 0; i < virtual_front_wheel_jnt_names_.size(); i++)
305  {
309  }
310  }
311 
313  {
314  // actual steer joints
318 
319  // virtual steer joints
320  for (int i = 0; i < virtual_front_steer_jnt_names_.size(); i++)
321  {
325  }
326  }
327 
329  hardware_interface::JointStateInterface& _jnt_state_interface,
330  hardware_interface::JointCommandInterface& _jnt_cmd_interface,
331  const std::string _jnt_name, double& _jnt_pos, double& _jnt_vel, double& _jnt_eff, double& _jnt_cmd)
332  {
333  // register joint (both JointState and CommandJoint)
334  this->registerJointStateInterfaceHandle(_jnt_state_interface, _jnt_name,
335  _jnt_pos, _jnt_vel, _jnt_eff);
336  this->registerCommandJointInterfaceHandle(_jnt_state_interface, _jnt_cmd_interface,
337  _jnt_name, _jnt_cmd);
338  }
339 
341  hardware_interface::JointStateInterface& _jnt_state_interface,
342  const std::string _jnt_name, double& _jnt_pos, double& _jnt_vel, double& _jnt_eff)
343  {
344  hardware_interface::JointStateHandle state_handle(_jnt_name,
345  &_jnt_pos,
346  &_jnt_vel,
347  &_jnt_eff);
348  _jnt_state_interface.registerHandle(state_handle);
349 
350  ROS_INFO_STREAM("Registered joint '" << _jnt_name << " ' in the JointStateInterface");
351  }
352 
354  hardware_interface::JointStateInterface& _jnt_state_interface,
355  hardware_interface::JointCommandInterface& _jnt_cmd_interface,
356  const std::string _jnt_name, double& _jnt_cmd)
357  {
358  // joint command
359  hardware_interface::JointHandle _handle(_jnt_state_interface.getHandle(_jnt_name),
360  &_jnt_cmd);
361  _jnt_cmd_interface.registerHandle(_handle);
362 
363  ROS_INFO_STREAM("Registered joint '" << _jnt_name << " ' in the CommandJointInterface");
364  }
365 
366 private:
367  // common
369  //-- actual joint(single actuator)
370  //---- joint name
371  std::string rear_wheel_jnt_name_;
372  //---- joint interface parameters
376  //---- joint interface command
378  //---- Hardware interface: joint
380  //hardware_interface::JointStateInterface wheel_jnt_state_interface_;
381  //
382  //-- virtual joints(two rear wheels)
383  //---- joint name
384  std::vector<std::string> virtual_rear_wheel_jnt_names_;
385  //---- joint interface parameters
386  std::vector<double> virtual_rear_wheel_jnt_pos_;
387  std::vector<double> virtual_rear_wheel_jnt_vel_;
388  std::vector<double> virtual_rear_wheel_jnt_eff_;
389  //---- joint interface command
390  std::vector<double> virtual_rear_wheel_jnt_vel_cmd_;
391  //-- virtual joints(two front wheels)
392  //---- joint name
393  std::vector<std::string> virtual_front_wheel_jnt_names_;
394  //---- joint interface parameters
395  std::vector<double> virtual_front_wheel_jnt_pos_;
396  std::vector<double> virtual_front_wheel_jnt_vel_;
397  std::vector<double> virtual_front_wheel_jnt_eff_;
398  //---- joint interface command
399  std::vector<double> virtual_front_wheel_jnt_vel_cmd_;
400 
401  // front steer
402  //-- actual joint(single actuator)
403  //---- joint name
405  //---- joint interface parameters
409  //---- joint interface command
411  //---- Hardware interface: joint
413  //hardware_interface::JointStateInterface steer_jnt_state_interface_;
414  //
415  //-- virtual joints(two steers)
416  //---- joint name
417  std::vector<std::string> virtual_front_steer_jnt_names_;
418  //---- joint interface parameters
419  std::vector<double> virtual_front_steer_jnt_pos_;
420  std::vector<double> virtual_front_steer_jnt_vel_;
421  std::vector<double> virtual_front_steer_jnt_eff_;
422  //---- joint interface command
423  std::vector<double> virtual_front_steer_jnt_pos_cmd_;
424 
425  // Wheel separation, wrt the midpoint of the wheel width:
427  // Wheel separation, wrt the midpoint of the wheel width:
429 
430  std::string ns_;
431  bool running_;
432 
436 };
std::vector< double > virtual_rear_wheel_jnt_eff_
std::vector< double > virtual_front_wheel_jnt_vel_cmd_
hardware_interface::JointStateInterface jnt_state_interface_
void getSteerJointNames(ros::NodeHandle &_nh)
void registerCommandJointInterfaceHandle(hardware_interface::JointStateInterface &_jnt_state_interface, hardware_interface::JointCommandInterface &_jnt_cmd_interface, const std::string _jnt_name, double &_jnt_cmd)
bool stopCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
std::vector< double > virtual_rear_wheel_jnt_pos_
std::vector< double > virtual_front_steer_jnt_pos_cmd_
ServiceServer advertiseService(ros::NodeHandle n, std::string name, boost::function< bool(const typename ActionSpec::_action_goal_type::_goal_type &, typename ActionSpec::_action_result_type::_result_type &result)> service_cb)
hardware_interface::VelocityJointInterface rear_wheel_jnt_vel_cmd_interface_
std::vector< double > virtual_front_steer_jnt_vel_
void registerInterfaceHandles(hardware_interface::JointStateInterface &_jnt_state_interface, hardware_interface::JointCommandInterface &_jnt_cmd_interface, const std::string _jnt_name, double &_jnt_pos, double &_jnt_vel, double &_jnt_eff, double &_jnt_cmd)
hardware_interface::PositionJointInterface front_steer_jnt_pos_cmd_interface_
void registerJointStateInterfaceHandle(hardware_interface::JointStateInterface &_jnt_state_interface, const std::string _jnt_name, double &_jnt_pos, double &_jnt_vel, double &_jnt_eff)
ros::Time getTime() const
std::vector< double > virtual_front_steer_jnt_pos_
void getJointNames(ros::NodeHandle &_nh)
ros::ServiceServer stop_srv_
std::vector< double > virtual_rear_wheel_jnt_vel_cmd_
std::vector< double > virtual_front_steer_jnt_eff_
std::vector< double > virtual_front_wheel_jnt_eff_
std::vector< double > virtual_rear_wheel_jnt_vel_
void registerHandle(const JointStateHandle &handle)
JointStateHandle getHandle(const std::string &name)
TFSIMD_FORCE_INLINE const tfScalar & w() const
void getWheelJointNames(ros::NodeHandle &_nh)
std::vector< std::string > virtual_front_steer_jnt_names_
#define ROS_INFO_STREAM(args)
std::vector< double > virtual_front_wheel_jnt_vel_
ros::Duration getPeriod() const
bool getParam(const std::string &key, std::string &s) const
bool startCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
static Time now()
std::vector< std::string > virtual_front_wheel_jnt_names_
std::vector< double > virtual_front_wheel_jnt_pos_
std::vector< std::string > virtual_rear_wheel_jnt_names_
ros::ServiceServer start_srv_


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