nav_pcontroller.cc
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2009, Ingo Kresse <kresse@in.tum.de>, U. Klank
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 
31 // For min/max
32 #include <algorithm>
33 
34 #include <boost/bind.hpp>
35 
36 #include <math.h>
37 #include <stdio.h>
38 
39 #include <geometry_msgs/Twist.h>
40 
42 
43 
44 double BasePController::p_control(double x, double p, double limit)
45 {
46  return (x > 0) ? std::min(p*x, limit) : std::max(p*x, -limit);
47 }
48 
49 
50 double BasePController::limit_acc(double x, double x_old, double limit)
51 {
52  x = std::min(x, x_old + limit);
53  x = std::max(x, x_old - limit);
54  return x;
55 }
56 
58  : n_("~"),
59  move_base_actionserver_(n_, "move_base", false)
60 {
61  parseParams();
62 
63  move_base_actionserver_.registerGoalCallback(boost::bind(&BasePController::newMoveBaseGoal, this));
64  move_base_actionserver_.registerPreemptCallback(boost::bind(&BasePController::preemptMoveBaseGoal, this));
65 
66  sub_goal_ = n_.subscribe("goal", 1, &BasePController::newGoal, this);
67  pub_vel_ = n_.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
68 
69  vx_=0; vy_=0; vth_=0; // \todo: get from base driver
70 
71  goal_set_ = false;
72 
74 }
75 
77 {
78 }
79 
81 {
82  ros::Rate loop(loop_rate_);
83  ros::AsyncSpinner spinner(1);
84 
85  spinner.start();
86  while(n_.ok()) {
87  if(goal_set_)
88  cycle();
89  loop.sleep();
90  }
91 }
92 
94 {
95  n_.param<double>("xy_tolerance", xy_tolerance_, 0.005);
96  n_.param<double>("th_tolerance", th_tolerance_, 0.005);
97 
98  double tmp_fail_timeout;
99  n_.param<double>("fail_timeout", tmp_fail_timeout, 5.0);
100  fail_timeout_ = ros::Duration(tmp_fail_timeout);
101 
102  n_.param<double>("fail_velocity", fail_velocity_, 0.02);
103  n_.param<double>("vel_ang_max", vel_ang_max_, 0.2);
104  n_.param<double>("vel_lin_max", vel_lin_max_, 0.2);
105  n_.param<double>("acc_ang_max", acc_ang_max_, 0.4);
106  n_.param<double>("acc_lin_max", acc_lin_max_, 0.4);
107  n_.param<int>("loop_rate", loop_rate_, 30);
108  n_.param<double>("p", p_, 1.2);
109  n_.param<bool>("keep_distance", keep_distance_, true);
110 
111  n_.param<std::string>("global_frame", global_frame_, "map");
112  n_.param<std::string>("base_link_frame", base_link_frame_, "base_link");
113 
114  //CHANGED
115  // dist_control_.setFootprint(0.42, -0.3075, 0.3075, -0.42, 0.0);
116  /*(0.309, -0.43, 0.43, -0.309, 0.0);*/
117  double front, rear, left, right, tolerance;
118  std::string stName;
119  n_.param("speed_filter_name", stName, std::string("/speed_filter"));
120  n_.param(stName + "/footprint/left", left, 0.309);
121  n_.param(stName + "/footprint/right", right, -0.309);
122  n_.param(stName + "/footprint/front", front, 0.43);
123  n_.param(stName + "/footprint/rear", rear, -0.43);
124  n_.param(stName + "/footprint/tolerance", tolerance, 0.0);
125 
126  dist_control_.setFootprint(front, rear, left, right, tolerance);
127 }
128 
130 {
131  move_base_msgs::MoveBaseGoal::ConstPtr msg = move_base_actionserver_.acceptNewGoal();
132 
133  // To be able to reconfigure the base controller on the fly, whe read the parameters whenever we receive a goal
134  parseParams();
135 
136  newGoal(msg->target_pose);
137  ROS_INFO("received goal: %f %f %f", x_goal_, y_goal_, th_goal_);
138 }
139 
141 {
142  boost::mutex::scoped_lock curr_lock(lock);
143 
144  goal_set_ = false;
145  stopRobot();
146  move_base_actionserver_.setPreempted();
147 }
148 
149 void BasePController::newGoal(const geometry_msgs::PoseStamped &msg)
150 {
151  boost::mutex::scoped_lock curr_lock(lock);
152 
153  geometry_msgs::PoseStamped goal;
154 
155  try
156  {
157  tf_.waitForTransform(
158  global_frame_, msg.header.frame_id, msg.header.stamp, ros::Duration(1.0));
159  tf_.transformPose(global_frame_, msg, goal);
160  }
161  catch(tf::TransformException& ex)
162  {
163  ROS_WARN("no localization information yet %s",ex.what());
164  return;
165  }
166 
167  x_goal_ = goal.pose.position.x;
168  y_goal_ = goal.pose.position.y;
169 
170  // th = atan2(r21/2,r11/2)
171  const geometry_msgs::Quaternion &q = goal.pose.orientation;
172  th_goal_ = atan2(q.x*q.y + q.w*q.z, 0.5 - q.y*q.y -q.z*q.z);
173 
174  ROS_INFO("got goal: %f %f %f", x_goal_, y_goal_, th_goal_);
175 
176  low_speed_time_ = ros::Time::now();
177 
178  goal_set_ = true;
179 }
180 
181 void BasePController::newGoal(const geometry_msgs::PoseStamped::ConstPtr& msg)
182 {
183  if(move_base_actionserver_.isActive())
185 
186  newGoal(*msg);
187 }
188 
191 {
192  tf::StampedTransform global_pose;
193  try
194  {
195  tf_.lookupTransform(global_frame_, base_link_frame_, ros::Time(0), global_pose);
196  }
197  catch(tf::TransformException& ex)
198  {
199  ROS_WARN("no localization information yet %s",ex.what());
200  return false;
201  }
202 
203  // find out where we are now
204  x_now_ = global_pose.getOrigin().x();
205  y_now_ = global_pose.getOrigin().y();
206 
207  // th = atan2(r_21/2, r_11/2)
208  const tf::Quaternion &q = global_pose.getRotation();
209  th_now_ = atan2(q.x()*q.y() + q.w()*q.z(), 0.5 - q.y()*q.y() - q.z()*q.z());
210 
211  return true;
212 }
213 
215 {
216  //compute differences (world space)
217  double x_diff = x_goal_ - x_now_;
218  double y_diff = y_goal_ - y_now_;
219 
220  // \todo: clean this ugly code
221  double th_diff = fmod(th_goal_ - th_now_, 2*M_PI);
222  if(th_diff > M_PI) th_diff = th_diff - 2*M_PI;
223  if(th_diff < -M_PI) th_diff = th_diff + 2*M_PI;
224 
225  // transform to robot space
226  double dx = x_diff*cos(th_now_) + y_diff*sin(th_now_);
227  double dy = -x_diff*sin(th_now_) + y_diff*cos(th_now_);
228  double dth = th_diff;
229 
230  // do p-controller with limit (robot space)
231  double vel_x = p_control(dx, p_, vel_lin_max_);
232  double vel_y = p_control(dy, p_, vel_lin_max_);
233  double vel_th = p_control(dth, p_, vel_ang_max_);
234 
235  // limit acceleration (robot space)
236  vel_x = limit_acc(vel_x, vx_, acc_lin_max_/loop_rate_);
237  vel_y = limit_acc(vel_y, vy_, acc_lin_max_/loop_rate_);
238  vel_th = limit_acc(vel_th, vth_, acc_ang_max_/loop_rate_);
239 
240  // store resulting velocity
241  vx_ = vel_x;
242  vy_ = vel_y;
243  vth_ = vel_th;
244 }
245 
246 #ifndef max
247 #define max(A,B) (A) > (B) ? (A) : (B)
248 #endif
250 {
251  boost::mutex::scoped_lock curr_lock(lock);
252 
253  if(!retrieve_pose()) {
254  stopRobot();
255  return;
256  }
257 
259 
260  if (keep_distance_)
262 
263  sendVelCmd(vx_, vy_, vth_);
264 
266 
267  if(move_base_actionserver_.isActive())
268  move_base_actionserver_.setSucceeded();
269  goal_set_ = false;
270  stopRobot();
271  }
272  // Sort of a bad hack. It might be a bad idea to 'unify' angular
273  // and linear velocity and just take te maximum.
274  double velocity = max(sqrt(vx_ * vx_+ vy_ * vy_) , vth_);
275 
276  if( velocity < fail_velocity_ )
277  {
278  if( ros::Time::now() - low_speed_time_ > fail_timeout_ )
279  {
280  goal_set_ = false;
281  stopRobot();
282 
283  if(move_base_actionserver_.isActive())
284  move_base_actionserver_.setAborted();
285  return;
286  }
287  }
288  else
289  low_speed_time_ = ros::Time::now();
290 
291  if(move_base_actionserver_.isActive())
292  {
293  move_base_msgs::MoveBaseFeedback feedback;
294  feedback.base_position.header.stamp = ros::Time::now();
295  feedback.base_position.header.frame_id = global_frame_;
296  feedback.base_position.pose.position.x = x_now_;
297  feedback.base_position.pose.position.y = y_now_;
298  feedback.base_position.pose.position.z = 0.0;
299  feedback.base_position.pose.orientation = tf::createQuaternionMsgFromYaw(th_now_);
300  move_base_actionserver_.publishFeedback(feedback);
301  }
302 }
303 
304 #define ANG_NORM(a) atan2(sin((a)),cos((a)))
305 
307 {
308  sendVelCmd(0.0,0.0,0.0);
309 }
310 
311 void BasePController::sendVelCmd(double vx, double vy, double vth)
312 {
313  geometry_msgs::Twist cmdvel;
314 
315  cmdvel.linear.x = vx;
316  cmdvel.linear.y = vy;
317  cmdvel.angular.z = vth;
318 
319  pub_vel_.publish(cmdvel);
320 }
321 
322 
323 bool BasePController::comparePoses(double x1, double y1, double a1,
324  double x2, double y2, double a2)
325 {
326  bool res;
327  if((fabs(x2-x1) <= xy_tolerance_) &&
328  (fabs(y2-y1) <= xy_tolerance_) &&
329  (fabs(ANG_NORM(ANG_NORM(a2)-ANG_NORM(a1))) <= th_tolerance_))
330  res = true;
331  else
332  res = false;
333  return(res);
334 }
335 
336 int main(int argc, char *argv[])
337 {
338  ros::init(argc, argv, "nav_pcontroller");
339 
340  BasePController pc;
341  pc.main();
342  return 0;
343 }
ros::Duration fail_timeout_
double p_control(double x, double p, double limit)
bool retrieve_pose()
retrieves tf pose and updates (x_now_, y_now_, th_now_)
void setFootprint(double front, double rear, double left, double right, double tolerance)
Sets front_, rear_, left_, right_, tolerance_, and early_reject_distance_.
actionlib::SimpleActionServer< move_base_msgs::MoveBaseAction > move_base_actionserver_
double limit_acc(double x, double x_old, double limit)
BaseDistance dist_control_
void newGoal(const geometry_msgs::PoseStamped &msg)
ros::Subscriber sub_goal_
std::string global_frame_
ros::Publisher pub_vel_
tf::TransformListener tf_
boost::mutex lock
bool comparePoses(double x1, double y1, double a1, double x2, double y2, double a2)
void compute_distance_keeping(double *vx, double *vy, double *vth)
Sets vx_last_, vy_last and vth_last_ to input parameters, adjusting them.
ros::Time low_speed_time_
std::string base_link_frame_
ros::NodeHandle n_
void sendVelCmd(double vx, double vy, double vth)


nav_pcontroller
Author(s): Ingo Kresse
autogenerated on Mon Jul 18 2016 16:50:55