pose_base_controller.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 *
3 * Software License Agreement (BSD License)
4 *
5 * Copyright (c) 2009, Willow Garage, Inc.
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Willow Garage, Inc. nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *
35 * Author: Eitan Marder-Eppstein
36 *********************************************************************/
38 
39 namespace pose_base_controller {
41  action_server_(ros::NodeHandle(),
42  "pose_base_controller",
43  boost::bind(&PoseBaseController::execute, this, _1),
44  false) {
45  ros::NodeHandle node_private("~");
46  node_private.param("k_trans", K_trans_, 1.0);
47  node_private.param("k_rot", K_rot_, 1.0);
48 
49  node_private.param("tolerance_trans", tolerance_trans_, 0.02);
50  node_private.param("tolerance_rot", tolerance_rot_, 0.04);
51  node_private.param("tolerance_timeout", tolerance_timeout_, 0.5);
52 
53  node_private.param("fixed_frame", fixed_frame_, std::string("odom_combined"));
54  node_private.param("base_frame", base_frame_, std::string("base_link"));
55 
56  node_private.param("holonomic", holonomic_, true);
57 
58  node_private.param("max_vel_lin", max_vel_lin_, 0.9);
59  node_private.param("max_vel_th", max_vel_th_, 1.4);
60 
61  node_private.param("min_vel_lin", min_vel_lin_, 0.0);
62  node_private.param("min_vel_th", min_vel_th_, 0.0);
63  node_private.param("min_in_place_vel_th", min_in_place_vel_th_, 0.0);
64  node_private.param("in_place_trans_vel", in_place_trans_vel_, 0.0);
65  node_private.param("frequency", freq_, 100.0);
66  node_private.param("transform_tolerance", transform_tolerance_, 0.5);
67 
68  node_private.param("trans_stopped_velocity", trans_stopped_velocity_, 1e-4);
69  node_private.param("rot_stopped_velocity", rot_stopped_velocity_, 1e-4);
70 
71  ros::NodeHandle node;
72  odom_sub_ = node.subscribe<nav_msgs::Odometry>("odom", 1, boost::bind(&PoseBaseController::odomCallback, this, _1));
73  vel_pub_ = node.advertise<geometry_msgs::Twist>("base_controller/command", 10);
74 
76  ROS_DEBUG("Started server");
77  }
78 
79  void PoseBaseController::odomCallback(const nav_msgs::Odometry::ConstPtr& msg){
80  //we assume that the odometry is published in the frame of the base
81  boost::mutex::scoped_lock lock(odom_lock_);
82  base_odom_.twist.twist.linear.x = msg->twist.twist.linear.x;
83  base_odom_.twist.twist.linear.y = msg->twist.twist.linear.y;
84  base_odom_.twist.twist.angular.z = msg->twist.twist.angular.z;
85  ROS_DEBUG("In the odometry callback with velocity values: (%.2f, %.2f, %.2f)",
86  base_odom_.twist.twist.linear.x, base_odom_.twist.twist.linear.y, base_odom_.twist.twist.angular.z);
87  }
88 
89  double PoseBaseController::headingDiff(double x, double y, double pt_x, double pt_y, double heading)
90  {
91  double v1_x = x - pt_x;
92  double v1_y = y - pt_y;
93  double v2_x = cos(heading);
94  double v2_y = sin(heading);
95 
96  double perp_dot = v1_x * v2_y - v1_y * v2_x;
97  double dot = v1_x * v2_x + v1_y * v2_y;
98 
99  //get the signed angle
100  double vector_angle = atan2(perp_dot, dot);
101 
102  return -1.0 * vector_angle;
103  }
104 
106  geometry_msgs::PoseStamped global_pose, robot_pose;
107  robot_pose.pose.orientation.w = 1.0;
108  robot_pose.header.frame_id = base_frame_;
109 
110  tf_.transform(robot_pose, global_pose, fixed_frame_);
111  //ROS_INFO("Delay: %f", (global_pose.header.stamp - ros::Time::now()).toSec());
112 
113  tf2::Stamped<tf2::Transform> global_pose_tf;
114  tf2::fromMsg(global_pose, global_pose_tf);
115  return global_pose_tf;
116  }
117 
118  move_base_msgs::MoveBaseGoal PoseBaseController::goalToFixedFrame(const move_base_msgs::MoveBaseGoal& goal){
119  move_base_msgs::MoveBaseGoal fixed_goal;
120  geometry_msgs::PoseStamped pose;
121  pose = goal.target_pose;
122 
123  //just get the latest available transform... for accuracy they should send
124  //goals in the frame of the planner
125  pose.header.stamp = ros::Time();
126 
127  try{
128  tf_.transform(pose, fixed_goal.target_pose, fixed_frame_);
129  }
130  catch(tf2::TransformException& ex){
131  ROS_WARN("Failed to transform the goal pose from %s into the %s frame: %s",
132  pose.header.frame_id.c_str(), fixed_frame_.c_str(), ex.what());
133  return goal;
134  }
135 
136  return fixed_goal;
137  }
138 
139  void PoseBaseController::execute(const move_base_msgs::MoveBaseGoalConstPtr& user_goal){
140  bool success = false;
141 
142  move_base_msgs::MoveBaseGoal goal = goalToFixedFrame(*user_goal);
143 
144  success = controlLoop(goal);
145 
146  //based on the control loop's exit status... we'll set our goal status
147  if(success){
148  //wait until we're stopped before returning success
149  ros::Rate r(10.0);
150  while(!stopped()){
151  geometry_msgs::Twist empty_twist;
152  vel_pub_.publish(empty_twist);
153  r.sleep();
154  }
156  }
157  else{
158  //if a preempt was requested... the control loop exits for that reason
161  else
163  }
164  }
165 
167  //copy over the odometry information
168  nav_msgs::Odometry base_odom;
169  {
170  boost::mutex::scoped_lock lock(odom_lock_);
171  base_odom = base_odom_;
172  }
173 
174  return fabs(base_odom.twist.twist.angular.z) <= rot_stopped_velocity_
175  && fabs(base_odom.twist.twist.linear.x) <= trans_stopped_velocity_
176  && fabs(base_odom.twist.twist.linear.y) <= trans_stopped_velocity_;
177  }
178 
179  bool PoseBaseController::controlLoop(const move_base_msgs::MoveBaseGoal& current_goal){
180  if(freq_ == 0.0)
181  return false;
182 
183  ros::Rate r(freq_);
184  ros::Time goal_reached_time = ros::Time::now();
185  while(ros::ok()){
187  return false;
188  }
189  ROS_DEBUG("At least looping");
190  tf2::Stamped<tf2::Transform> target_pose;
191  tf2::convert(current_goal.target_pose, target_pose);
192 
193  //get the current pose of the robot in the fixed frame
194  tf2::Stamped<tf2::Transform> robot_pose;
195  try {
196  robot_pose = getRobotPose();
197  //make sure that the transform to the dance frame isn't too out of date
199  ROS_WARN("The %s frame to %s frame transform is more than %.2f seconds old, will not move",
200  fixed_frame_.c_str(), base_frame_.c_str(), transform_tolerance_);
201  geometry_msgs::Twist empty_twist;
202  vel_pub_.publish(empty_twist);
203  return false;
204  }
205  }
206  catch(tf2::TransformException &ex){
207  ROS_ERROR("Can't transform: %s\n", ex.what());
208  geometry_msgs::Twist empty_twist;
209  vel_pub_.publish(empty_twist);
210  return false;
211  }
212  ROS_DEBUG("PoseBaseController: current robot pose %f %f ==> %f", robot_pose.getOrigin().x(), robot_pose.getOrigin().y(), tf2::getYaw(robot_pose.getRotation()));
213  ROS_DEBUG("PoseBaseController: target robot pose %f %f ==> %f", target_pose.getOrigin().x(), target_pose.getOrigin().y(), tf2::getYaw(target_pose.getRotation()));
214 
215  //get the difference between the two poses
216  geometry_msgs::Twist diff = diff2D(target_pose, robot_pose);
217  ROS_DEBUG("PoseBaseController: diff %f %f ==> %f", diff.linear.x, diff.linear.y, diff.angular.z);
218 
219  //publish the desired velocity command to the base
220  vel_pub_.publish(limitTwist(diff));
221 
222  //if we haven't reached our goal... we'll update time
223  if (fabs(diff.linear.x) > tolerance_trans_ || fabs(diff.linear.y) > tolerance_trans_ || fabs(diff.angular.z) > tolerance_rot_)
224  goal_reached_time = ros::Time::now();
225 
226  //check if we've reached our goal for long enough to succeed
227  if(goal_reached_time + ros::Duration(tolerance_timeout_) < ros::Time::now()){
228  geometry_msgs::Twist empty_twist;
229  vel_pub_.publish(empty_twist);
230  return true;
231  }
232 
233  r.sleep();
234  }
235  return false;
236  }
237 
238  geometry_msgs::Twist PoseBaseController::diff2D(const tf2::Transform& pose1, const tf2::Transform& pose2)
239  {
240  geometry_msgs::Twist res;
241  tf2::Transform diff = pose2.inverse() * pose1;
242  res.linear.x = diff.getOrigin().x();
243  res.linear.y = diff.getOrigin().y();
244  res.angular.z = tf2::getYaw(diff.getRotation());
245 
246  if(holonomic_ || (fabs(res.linear.x) <= tolerance_trans_ && fabs(res.linear.y) <= tolerance_trans_))
247  return res;
248 
249  //in the case that we're not rotating to our goal position and we have a non-holonomic robot
250  //we'll need to command a rotational velocity that will help us reach our desired heading
251 
252  //we want to compute a goal based on the heading difference between our pose and the target
253  double yaw_diff = headingDiff(pose1.getOrigin().x(), pose1.getOrigin().y(),
254  pose2.getOrigin().x(), pose2.getOrigin().y(), tf2::getYaw(pose2.getRotation()));
255 
256  //we'll also check if we can move more effectively backwards
257  double neg_yaw_diff = headingDiff(pose1.getOrigin().x(), pose1.getOrigin().y(),
258  pose2.getOrigin().x(), pose2.getOrigin().y(), M_PI + tf2::getYaw(pose2.getRotation()));
259 
260  //check if its faster to just back up
261  if(fabs(neg_yaw_diff) < fabs(yaw_diff)){
262  ROS_DEBUG("Negative is better: %.2f", neg_yaw_diff);
263  yaw_diff = neg_yaw_diff;
264  }
265 
266  //compute the desired quaterion
267  tf2::Quaternion rot_diff;
268  rot_diff.setRPY(0.0, 0.0, yaw_diff);
269  tf2::Quaternion rot = pose2.getRotation() * rot_diff;
270  tf2::Transform new_pose = pose1;
271  new_pose.setRotation(rot);
272 
273  diff = pose2.inverse() * new_pose;
274  res.linear.x = diff.getOrigin().x();
275  res.linear.y = diff.getOrigin().y();
276  res.angular.z = tf2::getYaw(diff.getRotation());
277  return res;
278  }
279 
280 
281  geometry_msgs::Twist PoseBaseController::limitTwist(const geometry_msgs::Twist& twist)
282  {
283  geometry_msgs::Twist res = twist;
284  res.linear.x *= K_trans_;
285  if(!holonomic_)
286  res.linear.y = 0.0;
287  else
288  res.linear.y *= K_trans_;
289  res.angular.z *= K_rot_;
290 
291  //make sure to bound things by our velocity limits
292  double lin_overshoot = sqrt(res.linear.x * res.linear.x + res.linear.y * res.linear.y) / max_vel_lin_;
293  if (lin_overshoot > 1.0)
294  {
295  res.linear.x /= lin_overshoot;
296  res.linear.y /= lin_overshoot;
297  }
298  if (fabs(res.angular.z) > max_vel_th_) res.angular.z = max_vel_th_ * sign(res.angular.z);
299  if (fabs(res.angular.z) < min_vel_th_) res.angular.z = min_vel_th_ * sign(res.angular.z);
300 
301  if(fabs(res.linear.x) < in_place_trans_vel_ && fabs(res.linear.y) < in_place_trans_vel_){
302  if (fabs(res.angular.z) < min_in_place_vel_th_) res.angular.z = min_in_place_vel_th_ * sign(res.angular.z);
303  }
304 
305  ROS_DEBUG("Angular command %f", res.angular.z);
306  return res;
307  }
308 };
309 
310 int main(int argc, char** argv)
311 {
312  ros::init(argc, argv, "pose_base_controller");
313 
315 
316  ros::spin();
317  return 0;
318 }
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
T & transform(const T &in, T &out, const std::string &target_frame, ros::Duration timeout=ros::Duration(0.0)) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
TF2SIMD_FORCE_INLINE void setRotation(const Quaternion &q)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
geometry_msgs::Twist diff2D(const tf2::Transform &pose1, const tf2::Transform &pose2)
#define ROS_WARN(...)
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
geometry_msgs::Twist limitTwist(const geometry_msgs::Twist &twist)
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
Transform inverse() const
tf2::Stamped< tf2::Transform > getRobotPose()
void odomCallback(const nav_msgs::Odometry::ConstPtr &msg)
void execute(const move_base_msgs::MoveBaseGoalConstPtr &user_goal)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void publish(const boost::shared_ptr< M > &message) const
void setPreempted(const Result &result=Result(), const std::string &text=std::string(""))
TF2SIMD_FORCE_INLINE Vector3 & getOrigin()
ros::Time stamp_
void fromMsg(const A &, B &b)
ROSCPP_DECL bool ok()
bool controlLoop(const move_base_msgs::MoveBaseGoal &current_goal)
ROSCPP_DECL bool execute(const std::string &method, const XmlRpc::XmlRpcValue &request, XmlRpc::XmlRpcValue &response, XmlRpc::XmlRpcValue &payload, bool wait_for_master)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ROSCPP_DECL void spin()
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
bool sleep()
double getYaw(const A &a)
int main(int argc, char **argv)
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
Quaternion getRotation() const
static Time now()
void convert(const A &a, B &b)
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
move_base_msgs::MoveBaseGoal goalToFixedFrame(const move_base_msgs::MoveBaseGoal &goal)
#define ROS_ERROR(...)
double headingDiff(double pt_x, double pt_y, double x, double y, double heading)
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
#define ROS_DEBUG(...)


pose_base_controller
Author(s): Eitan Marder-Eppstein
autogenerated on Sat Aug 27 2022 02:54:14