carrot_planner.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 *
3 * Software License Agreement (BSD License)
4 *
5 * Copyright (c) 2008, 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 * Authors: Eitan Marder-Eppstein, Sachin Chitta
36 *********************************************************************/
39 
40 //register this planner as a BaseGlobalPlanner plugin
42 
43 namespace carrot_planner {
44 
45  CarrotPlanner::CarrotPlanner()
46  : costmap_ros_(NULL), initialized_(false){}
47 
49  : costmap_ros_(NULL), initialized_(false){
50  initialize(name, costmap_ros);
51  }
52 
53  void CarrotPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
54  if(!initialized_){
55  costmap_ros_ = costmap_ros;
57 
58  ros::NodeHandle private_nh("~/" + name);
59  private_nh.param("step_size", step_size_, costmap_->getResolution());
60  private_nh.param("min_dist_from_robot", min_dist_from_robot_, 0.10);
62 
63  initialized_ = true;
64  }
65  else
66  ROS_WARN("This planner has already been initialized... doing nothing");
67  }
68 
69  //we need to take the footprint of the robot into account when we calculate cost to obstacles
70  double CarrotPlanner::footprintCost(double x_i, double y_i, double theta_i){
71  if(!initialized_){
72  ROS_ERROR("The planner has not been initialized, please call initialize() to use the planner");
73  return -1.0;
74  }
75 
76  std::vector<geometry_msgs::Point> footprint = costmap_ros_->getRobotFootprint();
77  //if we have no footprint... do nothing
78  if(footprint.size() < 3)
79  return -1.0;
80 
81  //check if the footprint is legal
82  double footprint_cost = world_model_->footprintCost(x_i, y_i, theta_i, footprint);
83  return footprint_cost;
84  }
85 
86 
87  bool CarrotPlanner::makePlan(const geometry_msgs::PoseStamped& start,
88  const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){
89 
90  if(!initialized_){
91  ROS_ERROR("The planner has not been initialized, please call initialize() to use the planner");
92  return false;
93  }
94 
95  ROS_DEBUG("Got a start: %.2f, %.2f, and a goal: %.2f, %.2f", start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y);
96 
97  plan.clear();
99 
100  if(goal.header.frame_id != costmap_ros_->getGlobalFrameID()){
101  ROS_ERROR("This planner as configured will only accept goals in the %s frame, but a goal was sent in the %s frame.",
102  costmap_ros_->getGlobalFrameID().c_str(), goal.header.frame_id.c_str());
103  return false;
104  }
105 
106  tf::Stamped<tf::Pose> goal_tf;
107  tf::Stamped<tf::Pose> start_tf;
108 
109  poseStampedMsgToTF(goal,goal_tf);
110  poseStampedMsgToTF(start,start_tf);
111 
112  double useless_pitch, useless_roll, goal_yaw, start_yaw;
113  start_tf.getBasis().getEulerYPR(start_yaw, useless_pitch, useless_roll);
114  goal_tf.getBasis().getEulerYPR(goal_yaw, useless_pitch, useless_roll);
115 
116  //we want to step back along the vector created by the robot's position and the goal pose until we find a legal cell
117  double goal_x = goal.pose.position.x;
118  double goal_y = goal.pose.position.y;
119  double start_x = start.pose.position.x;
120  double start_y = start.pose.position.y;
121 
122  double diff_x = goal_x - start_x;
123  double diff_y = goal_y - start_y;
124  double diff_yaw = angles::normalize_angle(goal_yaw-start_yaw);
125 
126  double target_x = goal_x;
127  double target_y = goal_y;
128  double target_yaw = goal_yaw;
129 
130  bool done = false;
131  double scale = 1.0;
132  double dScale = 0.01;
133 
134  while(!done)
135  {
136  if(scale < 0)
137  {
138  target_x = start_x;
139  target_y = start_y;
140  target_yaw = start_yaw;
141  ROS_WARN("The carrot planner could not find a valid plan for this goal");
142  break;
143  }
144  target_x = start_x + scale * diff_x;
145  target_y = start_y + scale * diff_y;
146  target_yaw = angles::normalize_angle(start_yaw + scale * diff_yaw);
147 
148  double footprint_cost = footprintCost(target_x, target_y, target_yaw);
149  if(footprint_cost >= 0)
150  {
151  done = true;
152  }
153  scale -=dScale;
154  }
155 
156  plan.push_back(start);
157  geometry_msgs::PoseStamped new_goal = goal;
158  tf::Quaternion goal_quat = tf::createQuaternionFromYaw(target_yaw);
159 
160  new_goal.pose.position.x = target_x;
161  new_goal.pose.position.y = target_y;
162 
163  new_goal.pose.orientation.x = goal_quat.x();
164  new_goal.pose.orientation.y = goal_quat.y();
165  new_goal.pose.orientation.z = goal_quat.z();
166  new_goal.pose.orientation.w = goal_quat.w();
167 
168  plan.push_back(new_goal);
169  return (done);
170  }
171 
172 };
virtual double footprintCost(const geometry_msgs::Point &position, const std::vector< geometry_msgs::Point > &footprint, double inscribed_radius, double circumscribed_radius)=0
costmap_2d::Costmap2DROS * costmap_ros_
std::string getGlobalFrameID()
PLUGINLIB_EXPORT_CLASS(BAGReader, nodelet::Nodelet)
base_local_planner::WorldModel * world_model_
The world model that the controller will use.
#define ROS_WARN(...)
static Quaternion createQuaternionFromYaw(double yaw)
costmap_2d::Costmap2D * costmap_
bool param(const std::string &param_name, T &param_val, const T &default_val) const
bool makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, std::vector< geometry_msgs::PoseStamped > &plan)
Given a goal pose in the world, compute a plan.
Provides a simple global planner that will compute a valid goal point for the local planner by walkin...
def normalize_angle(angle)
void initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros)
Initialization function for the CarrotPlanner.
double footprintCost(double x_i, double y_i, double theta_i)
Checks the legality of the robot footprint at a position and orientation using the world model...
CarrotPlanner()
Constructor for the CarrotPlanner.
double getResolution() const
Costmap2D * getCostmap()
std::vector< geometry_msgs::Point > getRobotFootprint()
#define ROS_ERROR(...)
#define ROS_DEBUG(...)


carrot_planner
Author(s): Eitan Marder-Eppstein, Sachin Chitta, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:54