goal_functions.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 base_local_planner {
40 
41  double getGoalPositionDistance(const tf::Stamped<tf::Pose>& global_pose, double goal_x, double goal_y) {
42  return hypot(goal_x - global_pose.getOrigin().x(), goal_y - global_pose.getOrigin().y());
43  }
44 
45  double getGoalOrientationAngleDifference(const tf::Stamped<tf::Pose>& global_pose, double goal_th) {
46  double yaw = tf::getYaw(global_pose.getRotation());
47  return angles::shortest_angular_distance(yaw, goal_th);
48  }
49 
50  void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path, const ros::Publisher& pub) {
51  //given an empty path we won't do anything
52  if(path.empty())
53  return;
54 
55  //create a path message
56  nav_msgs::Path gui_path;
57  gui_path.poses.resize(path.size());
58  gui_path.header.frame_id = path[0].header.frame_id;
59  gui_path.header.stamp = path[0].header.stamp;
60 
61  // Extract the plan in world co-ordinates, we assume the path is all in the same frame
62  for(unsigned int i=0; i < path.size(); i++){
63  gui_path.poses[i] = path[i];
64  }
65 
66  pub.publish(gui_path);
67  }
68 
69  void prunePlan(const tf::Stamped<tf::Pose>& global_pose, std::vector<geometry_msgs::PoseStamped>& plan, std::vector<geometry_msgs::PoseStamped>& global_plan){
70  ROS_ASSERT(global_plan.size() >= plan.size());
71  std::vector<geometry_msgs::PoseStamped>::iterator it = plan.begin();
72  std::vector<geometry_msgs::PoseStamped>::iterator global_it = global_plan.begin();
73  while(it != plan.end()){
74  const geometry_msgs::PoseStamped& w = *it;
75  // Fixed error bound of 2 meters for now. Can reduce to a portion of the map size or based on the resolution
76  double x_diff = global_pose.getOrigin().x() - w.pose.position.x;
77  double y_diff = global_pose.getOrigin().y() - w.pose.position.y;
78  double distance_sq = x_diff * x_diff + y_diff * y_diff;
79  if(distance_sq < 1){
80  ROS_DEBUG("Nearest waypoint to <%f, %f> is <%f, %f>\n", global_pose.getOrigin().x(), global_pose.getOrigin().y(), w.pose.position.x, w.pose.position.y);
81  break;
82  }
83  it = plan.erase(it);
84  global_it = global_plan.erase(global_it);
85  }
86  }
87 
90  const std::vector<geometry_msgs::PoseStamped>& global_plan,
91  const tf::Stamped<tf::Pose>& global_pose,
92  const costmap_2d::Costmap2D& costmap,
93  const std::string& global_frame,
94  std::vector<geometry_msgs::PoseStamped>& transformed_plan){
95  transformed_plan.clear();
96 
97  if (global_plan.empty()) {
98  ROS_ERROR("Received plan with zero length");
99  return false;
100  }
101 
102  const geometry_msgs::PoseStamped& plan_pose = global_plan[0];
103  try {
104  // get plan_to_global_transform from plan frame to global_frame
105  tf::StampedTransform plan_to_global_transform;
106  tf.waitForTransform(global_frame, ros::Time::now(),
107  plan_pose.header.frame_id, plan_pose.header.stamp,
108  plan_pose.header.frame_id, ros::Duration(0.5));
109  tf.lookupTransform(global_frame, ros::Time(),
110  plan_pose.header.frame_id, plan_pose.header.stamp,
111  plan_pose.header.frame_id, plan_to_global_transform);
112 
113  //let's get the pose of the robot in the frame of the plan
114  tf::Stamped<tf::Pose> robot_pose;
115  tf.transformPose(plan_pose.header.frame_id, global_pose, robot_pose);
116 
117  //we'll discard points on the plan that are outside the local costmap
118  double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0,
119  costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);
120 
121  unsigned int i = 0;
122  double sq_dist_threshold = dist_threshold * dist_threshold;
123  double sq_dist = 0;
124 
125  //we need to loop to a point on the plan that is within a certain distance of the robot
126  while(i < (unsigned int)global_plan.size()) {
127  double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
128  double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
129  sq_dist = x_diff * x_diff + y_diff * y_diff;
130  if (sq_dist <= sq_dist_threshold) {
131  break;
132  }
133  ++i;
134  }
135 
136  tf::Stamped<tf::Pose> tf_pose;
137  geometry_msgs::PoseStamped newer_pose;
138 
139  //now we'll transform until points are outside of our distance threshold
140  while(i < (unsigned int)global_plan.size() && sq_dist <= sq_dist_threshold) {
141  const geometry_msgs::PoseStamped& pose = global_plan[i];
142  poseStampedMsgToTF(pose, tf_pose);
143  tf_pose.setData(plan_to_global_transform * tf_pose);
144  tf_pose.stamp_ = plan_to_global_transform.stamp_;
145  tf_pose.frame_id_ = global_frame;
146  poseStampedTFToMsg(tf_pose, newer_pose);
147 
148  transformed_plan.push_back(newer_pose);
149 
150  double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
151  double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
152  sq_dist = x_diff * x_diff + y_diff * y_diff;
153 
154  ++i;
155  }
156  }
157  catch(tf::LookupException& ex) {
158  ROS_ERROR("No Transform available Error: %s\n", ex.what());
159  return false;
160  }
161  catch(tf::ConnectivityException& ex) {
162  ROS_ERROR("Connectivity Error: %s\n", ex.what());
163  return false;
164  }
165  catch(tf::ExtrapolationException& ex) {
166  ROS_ERROR("Extrapolation Error: %s\n", ex.what());
167  if (!global_plan.empty())
168  ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());
169 
170  return false;
171  }
172 
173  return true;
174  }
175 
177  const std::vector<geometry_msgs::PoseStamped>& global_plan,
178  const std::string& global_frame, tf::Stamped<tf::Pose>& goal_pose) {
179  if (global_plan.empty())
180  {
181  ROS_ERROR("Received plan with zero length");
182  return false;
183  }
184 
185  const geometry_msgs::PoseStamped& plan_goal_pose = global_plan.back();
186  try{
187  tf::StampedTransform transform;
188  tf.waitForTransform(global_frame, ros::Time::now(),
189  plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
190  plan_goal_pose.header.frame_id, ros::Duration(0.5));
191  tf.lookupTransform(global_frame, ros::Time(),
192  plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
193  plan_goal_pose.header.frame_id, transform);
194 
195  poseStampedMsgToTF(plan_goal_pose, goal_pose);
196  goal_pose.setData(transform * goal_pose);
197  goal_pose.stamp_ = transform.stamp_;
198  goal_pose.frame_id_ = global_frame;
199 
200  }
201  catch(tf::LookupException& ex) {
202  ROS_ERROR("No Transform available Error: %s\n", ex.what());
203  return false;
204  }
205  catch(tf::ConnectivityException& ex) {
206  ROS_ERROR("Connectivity Error: %s\n", ex.what());
207  return false;
208  }
209  catch(tf::ExtrapolationException& ex) {
210  ROS_ERROR("Extrapolation Error: %s\n", ex.what());
211  if (global_plan.size() > 0)
212  ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());
213 
214  return false;
215  }
216  return true;
217  }
218 
220  const std::vector<geometry_msgs::PoseStamped>& global_plan,
221  const costmap_2d::Costmap2D& costmap __attribute__((unused)),
222  const std::string& global_frame,
223  tf::Stamped<tf::Pose>& global_pose,
224  const nav_msgs::Odometry& base_odom,
225  double rot_stopped_vel, double trans_stopped_vel,
226  double xy_goal_tolerance, double yaw_goal_tolerance){
227 
228  //we assume the global goal is the last point in the global plan
229  tf::Stamped<tf::Pose> goal_pose;
230  getGoalPose(tf, global_plan, global_frame, goal_pose);
231 
232  double goal_x = goal_pose.getOrigin().getX();
233  double goal_y = goal_pose.getOrigin().getY();
234  double goal_th = tf::getYaw(goal_pose.getRotation());
235 
236  //check to see if we've reached the goal position
237  if(getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance) {
238  //check to see if the goal orientation has been reached
239  if(fabs(getGoalOrientationAngleDifference(global_pose, goal_th)) <= yaw_goal_tolerance) {
240  //make sure that we're actually stopped before returning success
241  if(stopped(base_odom, rot_stopped_vel, trans_stopped_vel))
242  return true;
243  }
244  }
245 
246  return false;
247  }
248 
249  bool stopped(const nav_msgs::Odometry& base_odom,
250  const double& rot_stopped_velocity, const double& trans_stopped_velocity){
251  return fabs(base_odom.twist.twist.angular.z) <= rot_stopped_velocity
252  && fabs(base_odom.twist.twist.linear.x) <= trans_stopped_velocity
253  && fabs(base_odom.twist.twist.linear.y) <= trans_stopped_velocity;
254  }
255 };
bool getGoalPose(const tf::TransformListener &tf, const std::vector< geometry_msgs::PoseStamped > &global_plan, const std::string &global_frame, tf::Stamped< tf::Pose > &goal_pose)
Returns last pose in plan.
bool isGoalReached(const tf::TransformListener &tf, const std::vector< geometry_msgs::PoseStamped > &global_plan, const costmap_2d::Costmap2D &costmap, const std::string &global_frame, tf::Stamped< tf::Pose > &global_pose, const nav_msgs::Odometry &base_odom, double rot_stopped_vel, double trans_stopped_vel, double xy_goal_tolerance, double yaw_goal_tolerance)
Check if the goal pose has been achieved.
void publish(const boost::shared_ptr< M > &message) const
void setData(const T &input)
double getGoalPositionDistance(const tf::Stamped< tf::Pose > &global_pose, double goal_x, double goal_y)
return squared distance to check if the goal position has been achieved
static double getYaw(const Quaternion &bt_q)
INLINE Rall1d< T, V, S > hypot(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
void prunePlan(const tf::Stamped< tf::Pose > &global_pose, std::vector< geometry_msgs::PoseStamped > &plan, std::vector< geometry_msgs::PoseStamped > &global_plan)
Trim off parts of the global plan that are far enough behind the robot.
bool stopped(const nav_msgs::Odometry &base_odom, const double &rot_stopped_velocity, const double &trans_stopped_velocity)
Check whether the robot is stopped or not.
bool waitForTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const
bool transformGlobalPlan(const tf::TransformListener &tf, const std::vector< geometry_msgs::PoseStamped > &global_plan, const tf::Stamped< tf::Pose > &global_robot_pose, const costmap_2d::Costmap2D &costmap, const std::string &global_frame, std::vector< geometry_msgs::PoseStamped > &transformed_plan)
Transforms the global plan of the robot from the planner frame to the frame of the costmap...
unsigned int getSizeInCellsY() const
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
TFSIMD_FORCE_INLINE const tfScalar & w() const
void publishPlan(const std::vector< geometry_msgs::PoseStamped > &path, const ros::Publisher &pub)
Publish a plan for visualization purposes.
unsigned int getSizeInCellsX() const
void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const
static Time now()
double getResolution() const
#define ROS_ASSERT(cond)
#define ROS_ERROR(...)
def shortest_angular_distance(from_angle, to_angle)
double getGoalOrientationAngleDifference(const tf::Stamped< tf::Pose > &global_pose, double goal_th)
return angle difference to goal to check if the goal orientation has been achieved ...
tf::tfVector4 __attribute__
#define ROS_DEBUG(...)


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:49