Revision 6db11ec352eb44ff6ac4813a2c9955871df73ddd authored by eitan on 06 April 2010, 19:41:58 UTC, committed by eitan on 06 April 2010, 19:41:58 UTC
1 parent 674e4d3
carrot_planner.cpp
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Authors: Eitan Marder-Eppstein, Sachin Chitta
*********************************************************************/
#include <carrot_planner/carrot_planner.h>
#include <pluginlib/class_list_macros.h>
//register this planner as a BaseGlobalPlanner plugin
PLUGINLIB_REGISTER_CLASS(CarrotPlanner, carrot_planner::CarrotPlanner, nav_core::BaseGlobalPlanner)
namespace carrot_planner {
CarrotPlanner::CarrotPlanner()
: costmap_ros_(NULL), initialized_(false){}
CarrotPlanner::CarrotPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
: costmap_ros_(NULL), initialized_(false){
initialize(name, costmap_ros);
}
void CarrotPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
if(!initialized_){
costmap_ros_ = costmap_ros;
ros::NodeHandle private_nh("~/" + name);
private_nh.param("step_size", step_size_, costmap_ros_->getResolution());
private_nh.param("min_dist_from_robot", min_dist_from_robot_, 0.10);
costmap_ros_->getCostmapCopy(costmap_);
world_model_ = new base_local_planner::CostmapModel(costmap_);
//we'll get the parameters for the robot radius from the costmap we're associated with
inscribed_radius_ = costmap_ros_->getInscribedRadius();
circumscribed_radius_ = costmap_ros_->getCircumscribedRadius();
footprint_spec_ = costmap_ros_->getRobotFootprint();
initialized_ = true;
}
else
ROS_WARN("This planner has already been initialized... doing nothing");
}
//we need to take the footprint of the robot into account when we calculate cost to obstacles
double CarrotPlanner::footprintCost(double x_i, double y_i, double theta_i){
if(!initialized_){
ROS_ERROR("The planner has not been initialized, please call initialize() to use the planner");
return -1.0;
}
//if we have no footprint... do nothing
if(footprint_spec_.size() < 3)
return -1.0;
//build the oriented footprint
double cos_th = cos(theta_i);
double sin_th = sin(theta_i);
std::vector<geometry_msgs::Point> oriented_footprint;
for(unsigned int i = 0; i < footprint_spec_.size(); ++i){
geometry_msgs::Point new_pt;
new_pt.x = x_i + (footprint_spec_[i].x * cos_th - footprint_spec_[i].y * sin_th);
new_pt.y = y_i + (footprint_spec_[i].x * sin_th + footprint_spec_[i].y * cos_th);
oriented_footprint.push_back(new_pt);
}
geometry_msgs::Point robot_position;
robot_position.x = x_i;
robot_position.y = y_i;
//check if the footprint is legal
double footprint_cost = world_model_->footprintCost(robot_position, oriented_footprint, inscribed_radius_, circumscribed_radius_);
return footprint_cost;
}
bool CarrotPlanner::makePlan(const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){
if(!initialized_){
ROS_ERROR("The planner has not been initialized, please call initialize() to use the planner");
return false;
}
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);
plan.clear();
costmap_ros_->getCostmapCopy(costmap_);
if(goal.header.frame_id != costmap_ros_->getGlobalFrameID()){
ROS_ERROR("This planner as configured will only accept goals in the %s frame, but a goal was sent in the %s frame.",
costmap_ros_->getGlobalFrameID().c_str(), goal.header.frame_id.c_str());
return false;
}
tf::Stamped<tf::Pose> goal_tf;
tf::Stamped<tf::Pose> start_tf;
poseStampedMsgToTF(goal,goal_tf);
poseStampedMsgToTF(start,start_tf);
double useless_pitch, useless_roll, goal_yaw, start_yaw;
start_tf.getBasis().getEulerYPR(start_yaw, useless_pitch, useless_roll);
goal_tf.getBasis().getEulerYPR(goal_yaw, useless_pitch, useless_roll);
//we want to step back along the vector created by the robot's position and the goal pose until we find a legal cell
double goal_x = goal.pose.position.x;
double goal_y = goal.pose.position.y;
double start_x = start.pose.position.x;
double start_y = start.pose.position.y;
double diff_x = goal_x - start_x;
double diff_y = goal_y - start_y;
double diff_yaw = angles::normalize_angle(goal_yaw-start_yaw);
double target_x = goal_x;
double target_y = goal_y;
double target_yaw = goal_yaw;
bool done = false;
double scale = 1.0;
double dScale = 0.01;
while(!done)
{
if(scale < 0)
{
target_x = start_x;
target_y = start_y;
target_yaw = start_yaw;
ROS_WARN("The carrot planner could not find a valid plan for this goal");
break;
}
target_x = start_x + scale * diff_x;
target_y = start_y + scale * diff_y;
target_yaw = angles::normalize_angle(start_yaw + scale * diff_yaw);
double footprint_cost = footprintCost(target_x, target_y, target_yaw);
if(footprint_cost >= 0)
{
done = true;
}
scale -=dScale;
}
plan.push_back(start);
geometry_msgs::PoseStamped new_goal = goal;
tf::Quaternion goal_quat = tf::createQuaternionFromYaw(target_yaw);
new_goal.pose.position.x = target_x;
new_goal.pose.position.y = target_y;
new_goal.pose.orientation.x = goal_quat.x();
new_goal.pose.orientation.y = goal_quat.y();
new_goal.pose.orientation.z = goal_quat.z();
new_goal.pose.orientation.w = goal_quat.w();
plan.push_back(new_goal);
return (done);
}
};
![swh spinner](/static/img/swh-spinner.gif)
Computing file changes ...