move_slow_and_clear: move_slow_and_clear: move_slow_and

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 <move_slow_and_clear/move_slow_and_clear.h>
00038 #include <pluginlib/class_list_macros.h>
00039 
00040 PLUGINLIB_DECLARE_CLASS(move_slow_and_clear, MoveSlowAndClear, move_slow_and_clear::MoveSlowAndClear,
00041     nav_core::RecoveryBehavior)
00042 
00043 namespace move_slow_and_clear
00044 {
00045   MoveSlowAndClear::MoveSlowAndClear():global_costmap_(NULL), local_costmap_(NULL), 
00046                                        initialized_(false), remove_limit_thread_(NULL), limit_set_(false){}
00047 
00048   MoveSlowAndClear::~MoveSlowAndClear()
00049   {
00050     delete remove_limit_thread_;
00051   }
00052 
00053   void MoveSlowAndClear::initialize (std::string n, tf::TransformListener* tf,
00054       costmap_2d::Costmap2DROS* global_costmap,
00055       costmap_2d::Costmap2DROS* local_costmap)
00056   {
00057     global_costmap_ = global_costmap;
00058     local_costmap_ = local_costmap;
00059 
00060     ros::NodeHandle private_nh_("~/" + n);
00061     private_nh_.param("clearing_distance", clearing_distance_, 0.5);
00062     private_nh_.param("limited_trans_speed", limited_trans_speed_, 0.25);
00063     private_nh_.param("limited_rot_speed", limited_rot_speed_, 0.45);
00064     private_nh_.param("limited_distance", limited_distance_, 0.3);
00065 
00066     std::string planner_namespace;
00067     private_nh_.param("planner_namespace", planner_namespace, std::string("DWAPlannerROS"));
00068 
00069     planner_nh_ = ros::NodeHandle("~/" + planner_namespace);
00070 
00071     initialized_ = true;
00072   }
00073 
00074   void MoveSlowAndClear::runBehavior()
00075   {
00076     if(!initialized_)
00077     {
00078       ROS_ERROR("This recovery behavior has not been initialized, doing nothing.");
00079       return;
00080     }
00081 
00082     ROS_DEBUG("Running move slow and clear behavior");
00083     tf::Stamped<tf::Pose> global_pose, local_pose;
00084     global_costmap_->getRobotPose(global_pose);
00085     local_costmap_->getRobotPose(local_pose);
00086 
00087     std::vector<geometry_msgs::Point> global_poly, local_poly;
00088 
00089     geometry_msgs::Point pt;
00090     for(int i = -1; i <= 1; i+=2)
00091     {
00092       pt.x = global_pose.getOrigin().x() + i * clearing_distance_;
00093       pt.y = global_pose.getOrigin().y() + i * clearing_distance_;
00094       global_poly.push_back(pt);
00095 
00096       pt.x = global_pose.getOrigin().x() + i * clearing_distance_;
00097       pt.y = global_pose.getOrigin().y() + -1.0 * i * clearing_distance_;
00098       global_poly.push_back(pt);
00099 
00100       pt.x = local_pose.getOrigin().x() + i * clearing_distance_;
00101       pt.y = local_pose.getOrigin().y() + i * clearing_distance_;
00102       local_poly.push_back(pt);
00103 
00104       pt.x = local_pose.getOrigin().x() + i * clearing_distance_;
00105       pt.y = local_pose.getOrigin().y() + -1.0 * i * clearing_distance_;
00106       local_poly.push_back(pt);
00107     }
00108 
00109     //clear the desired space in both costmaps
00110     global_costmap_->setConvexPolygonCost(global_poly, costmap_2d::FREE_SPACE);
00111     local_costmap_->setConvexPolygonCost(local_poly, costmap_2d::FREE_SPACE);
00112 
00113     //lock... just in case we're already speed limited
00114     boost::mutex::scoped_lock l(mutex_);
00115 
00116     //get the old maximum speed for the robot... we'll want to set it back
00117     if(!limit_set_)
00118     {
00119       if(!planner_nh_.getParam("max_trans_vel", old_trans_speed_))
00120       {
00121         ROS_ERROR("The planner %s, does not have the parameter max_trans_vel", planner_nh_.getNamespace().c_str());
00122       }
00123 
00124       if(!planner_nh_.getParam("max_rot_vel", old_rot_speed_))
00125       {
00126         ROS_ERROR("The planner %s, does not have the parameter max_rot_vel", planner_nh_.getNamespace().c_str());
00127       }
00128     }
00129 
00130     //we also want to save our current position so that we can remove the speed limit we impose later on
00131     speed_limit_pose_ = global_pose;
00132 
00133     //limit the speed of the robot until it moves a certain distance
00134     setRobotSpeed(limited_trans_speed_, limited_rot_speed_);
00135     limit_set_ = true;
00136     distance_check_timer_ = private_nh_.createTimer(ros::Duration(0.1), &MoveSlowAndClear::distanceCheck, this);
00137   }
00138 
00139   double MoveSlowAndClear::getSqDistance()
00140   {
00141     tf::Stamped<tf::Pose> global_pose;
00142     global_costmap_->getRobotPose(global_pose);
00143     double x1 = global_pose.getOrigin().x();
00144     double y1 = global_pose.getOrigin().y();
00145 
00146     double x2 = speed_limit_pose_.getOrigin().x();
00147     double y2 = speed_limit_pose_.getOrigin().y();
00148 
00149     return (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1);
00150   }
00151 
00152   void MoveSlowAndClear::distanceCheck(const ros::TimerEvent& e)
00153   {
00154     if(limited_distance_ * limited_distance_ <= getSqDistance())
00155     {
00156       ROS_INFO("Moved far enough, removing speed limit.");
00157       //have to do this because a system call within a timer cb does not seem to play nice
00158       if(remove_limit_thread_)
00159       {
00160         remove_limit_thread_->join();
00161         delete remove_limit_thread_;
00162       }
00163       remove_limit_thread_ = new boost::thread(boost::bind(&MoveSlowAndClear::removeSpeedLimit, this));
00164 
00165       distance_check_timer_.stop();
00166     }
00167   }
00168 
00169   void MoveSlowAndClear::removeSpeedLimit()
00170   {
00171     boost::mutex::scoped_lock l(mutex_);
00172     setRobotSpeed(old_trans_speed_, old_rot_speed_);
00173     limit_set_ = false;
00174   }
00175 
00176   void MoveSlowAndClear::setRobotSpeed(double trans_speed, double rot_speed)
00177   {
00178     std::ostringstream trans_command;
00179     trans_command << "rosrun dynamic_reconfigure dynparam set " << planner_nh_.getNamespace() << " max_trans_vel " << trans_speed;
00180     ROS_INFO("Recovery setting trans vel: %s", trans_command.str().c_str());
00181     if(system(trans_command.str().c_str()) < 0)
00182     {
00183       ROS_ERROR("Something went wrong in the system call to dynparam");
00184     }
00185 
00186     std::ostringstream rot_command;
00187     rot_command << "rosrun dynamic_reconfigure dynparam set " << planner_nh_.getNamespace() << " max_rot_vel " << rot_speed;
00188     ROS_INFO("Recovery setting rot vel: %s", rot_command.str().c_str());
00189     if(system(rot_command.str().c_str()) < 0)
00190     {
00191       ROS_INFO("Something went wrong in the system call to dynparam");
00192     }
00193   }
00194 
00195 };
move_slow_and_clear.cpp
