$search
00001 /********************************************************************* 00002 * 00003 * Software License Agreement (BSD License) 00004 * 00005 * Copyright (c) 2009, Willow Garage, Inc. 00006 * All rights reserved. 00007 * 00008 * Redistribution and use in source and binary forms, with or without 00009 * modification, are permitted provided that the following conditions 00010 * are met: 00011 * 00012 * * Redistributions of source code must retain the above copyright 00013 * notice, this list of conditions and the following disclaimer. 00014 * * Redistributions in binary form must reproduce the above 00015 * copyright notice, this list of conditions and the following 00016 * disclaimer in the documentation and/or other materials provided 00017 * with the distribution. 00018 * * Neither the name of Willow Garage, Inc. nor the names of its 00019 * contributors may be used to endorse or promote products derived 00020 * from this software without specific prior written permission. 00021 * 00022 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00023 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00024 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00025 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00026 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00027 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00028 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00029 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00030 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00031 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00032 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00033 * POSSIBILITY OF SUCH DAMAGE. 00034 * 00035 * Author: Eitan Marder-Eppstein 00036 *********************************************************************/ 00037 #include <rotate_recovery/rotate_recovery.h> 00038 #include <pluginlib/class_list_macros.h> 00039 00040 //register this planner as a RecoveryBehavior plugin 00041 PLUGINLIB_DECLARE_CLASS(rotate_recovery, RotateRecovery, rotate_recovery::RotateRecovery, nav_core::RecoveryBehavior) 00042 00043 namespace rotate_recovery { 00044 RotateRecovery::RotateRecovery(): global_costmap_(NULL), local_costmap_(NULL), 00045 tf_(NULL), initialized_(false), world_model_(NULL) {} 00046 00047 void RotateRecovery::initialize(std::string name, tf::TransformListener* tf, 00048 costmap_2d::Costmap2DROS* global_costmap, costmap_2d::Costmap2DROS* local_costmap){ 00049 if(!initialized_){ 00050 name_ = name; 00051 tf_ = tf; 00052 global_costmap_ = global_costmap; 00053 local_costmap_ = local_costmap; 00054 00055 //get some parameters from the parameter server 00056 ros::NodeHandle private_nh("~/" + name_); 00057 ros::NodeHandle blp_nh("~/TrajectoryPlannerROS"); 00058 00059 //we'll simulate every degree by default 00060 private_nh.param("sim_granularity", sim_granularity_, 0.017); 00061 private_nh.param("frequency", frequency_, 20.0); 00062 00063 blp_nh.param("acc_lim_th", acc_lim_th_, 3.2); 00064 blp_nh.param("max_rotational_vel", max_rotational_vel_, 1.0); 00065 blp_nh.param("min_in_place_rotational_vel", min_rotational_vel_, 0.4); 00066 blp_nh.param("yaw_goal_tolerance", tolerance_, 0.10); 00067 00068 local_costmap_->getCostmapCopy(costmap_); 00069 world_model_ = new base_local_planner::CostmapModel(costmap_); 00070 00071 initialized_ = true; 00072 } 00073 else{ 00074 ROS_ERROR("You should not call initialize twice on this object, doing nothing"); 00075 } 00076 } 00077 00078 RotateRecovery::~RotateRecovery(){ 00079 delete world_model_; 00080 } 00081 00082 void RotateRecovery::runBehavior(){ 00083 if(!initialized_){ 00084 ROS_ERROR("This object must be initialized before runBehavior is called"); 00085 return; 00086 } 00087 00088 if(global_costmap_ == NULL || local_costmap_ == NULL){ 00089 ROS_ERROR("The costmaps passed to the ClearCostmapRecovery object cannot be NULL. Doing nothing."); 00090 return; 00091 } 00092 00093 ros::Rate r(frequency_); 00094 ros::NodeHandle n; 00095 ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 10); 00096 00097 tf::Stamped<tf::Pose> global_pose; 00098 local_costmap_->getRobotPose(global_pose); 00099 00100 double current_angle = -1.0 * M_PI; 00101 00102 bool got_180 = false; 00103 00104 double start_offset = 0 - angles::normalize_angle(tf::getYaw(global_pose.getRotation())); 00105 while(n.ok()){ 00106 local_costmap_->getRobotPose(global_pose); 00107 00108 double norm_angle = angles::normalize_angle(tf::getYaw(global_pose.getRotation())); 00109 current_angle = angles::normalize_angle(norm_angle + start_offset); 00110 00111 //compute the distance left to rotate 00112 double dist_left = M_PI - current_angle; 00113 00114 //update the costmap copy that the world model holds 00115 local_costmap_->getCostmapCopy(costmap_); 00116 00117 //check if that velocity is legal by forward simulating 00118 double sim_angle = 0.0; 00119 while(sim_angle < dist_left){ 00120 std::vector<geometry_msgs::Point> oriented_footprint; 00121 double theta = tf::getYaw(global_pose.getRotation()) + sim_angle; 00122 00123 geometry_msgs::Point position; 00124 position.x = global_pose.getOrigin().x(); 00125 position.y = global_pose.getOrigin().y(); 00126 00127 local_costmap_->getOrientedFootprint(position.x, position.y, theta, oriented_footprint); 00128 00129 //make sure that the point is legal, if it isn't... we'll abort 00130 double footprint_cost = world_model_->footprintCost(position, oriented_footprint, local_costmap_->getInscribedRadius(), local_costmap_->getCircumscribedRadius()); 00131 if(footprint_cost < 0.0){ 00132 ROS_ERROR("Rotate recovery can't rotate in place because there is a potential collision. Cost: %.2f", footprint_cost); 00133 return; 00134 } 00135 00136 sim_angle += sim_granularity_; 00137 } 00138 00139 //compute the velocity that will let us stop by the time we reach the goal 00140 double vel = sqrt(2 * acc_lim_th_ * dist_left); 00141 00142 //make sure that this velocity falls within the specified limits 00143 vel = std::min(std::max(vel, min_rotational_vel_), max_rotational_vel_); 00144 00145 geometry_msgs::Twist cmd_vel; 00146 cmd_vel.linear.x = 0.0; 00147 cmd_vel.linear.y = 0.0; 00148 cmd_vel.angular.z = vel; 00149 00150 vel_pub.publish(cmd_vel); 00151 00152 //makes sure that we won't decide we're done right after we start 00153 if(current_angle < 0.0) 00154 got_180 = true; 00155 00156 //if we're done with our in-place rotation... then return 00157 if(got_180 && current_angle >= (0.0 - tolerance_)) 00158 return; 00159 00160 r.sleep(); 00161 } 00162 } 00163 };