move_slow_and_clear.cpp
Go to the documentation of this file.
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 #include <costmap_2d/obstacle_layer.h>
00040 
00041 PLUGINLIB_DECLARE_CLASS(move_slow_and_clear, MoveSlowAndClear, move_slow_and_clear::MoveSlowAndClear,
00042     nav_core::RecoveryBehavior)
00043 
00044 namespace move_slow_and_clear
00045 {
00046   MoveSlowAndClear::MoveSlowAndClear():global_costmap_(NULL), local_costmap_(NULL), 
00047                                        initialized_(false), remove_limit_thread_(NULL), limit_set_(false){}
00048 
00049   MoveSlowAndClear::~MoveSlowAndClear()
00050   {
00051     delete remove_limit_thread_;
00052   }
00053 
00054   void MoveSlowAndClear::initialize (std::string n, tf::TransformListener* tf,
00055       costmap_2d::Costmap2DROS* global_costmap,
00056       costmap_2d::Costmap2DROS* local_costmap)
00057   {
00058     global_costmap_ = global_costmap;
00059     local_costmap_ = local_costmap;
00060 
00061     ros::NodeHandle private_nh_("~/" + n);
00062     private_nh_.param("clearing_distance", clearing_distance_, 0.5);
00063     private_nh_.param("limited_trans_speed", limited_trans_speed_, 0.25);
00064     private_nh_.param("limited_rot_speed", limited_rot_speed_, 0.45);
00065     private_nh_.param("limited_distance", limited_distance_, 0.3);
00066 
00067     std::string planner_namespace;
00068     private_nh_.param("planner_namespace", planner_namespace, std::string("DWAPlannerROS"));
00069     planner_nh_ = ros::NodeHandle("~/" + planner_namespace);
00070     planner_dynamic_reconfigure_service_ = planner_nh_.serviceClient<dynamic_reconfigure::Reconfigure>("set_parameters", true);
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     ROS_WARN("Move slow and clear recovery behavior started.");
00082     tf::Stamped<tf::Pose> global_pose, local_pose;
00083     global_costmap_->getRobotPose(global_pose);
00084     local_costmap_->getRobotPose(local_pose);
00085 
00086     std::vector<geometry_msgs::Point> global_poly, local_poly;
00087     geometry_msgs::Point pt;
00088 
00089     for(int i = -1; i <= 1; i+=2)
00090     {
00091       pt.x = global_pose.getOrigin().x() + i * clearing_distance_;
00092       pt.y = global_pose.getOrigin().y() + i * clearing_distance_;
00093       global_poly.push_back(pt);
00094 
00095       pt.x = global_pose.getOrigin().x() + i * clearing_distance_;
00096       pt.y = global_pose.getOrigin().y() + -1.0 * i * clearing_distance_;
00097       global_poly.push_back(pt);
00098 
00099       pt.x = local_pose.getOrigin().x() + i * clearing_distance_;
00100       pt.y = local_pose.getOrigin().y() + i * clearing_distance_;
00101       local_poly.push_back(pt);
00102 
00103       pt.x = local_pose.getOrigin().x() + i * clearing_distance_;
00104       pt.y = local_pose.getOrigin().y() + -1.0 * i * clearing_distance_;
00105       local_poly.push_back(pt);
00106     }
00107 
00108     //clear the desired space in the costmaps
00109     std::vector<boost::shared_ptr<costmap_2d::Layer> >* plugins = global_costmap_->getLayeredCostmap()->getPlugins();
00110     for (std::vector<boost::shared_ptr<costmap_2d::Layer> >::iterator pluginp = plugins->begin(); pluginp != plugins->end(); ++pluginp) {
00111             boost::shared_ptr<costmap_2d::Layer> plugin = *pluginp;
00112           if(plugin->getName().find("obstacles")!=std::string::npos){
00113             boost::shared_ptr<costmap_2d::ObstacleLayer> costmap;
00114             costmap = boost::static_pointer_cast<costmap_2d::ObstacleLayer>(plugin);
00115             costmap->setConvexPolygonCost(global_poly, costmap_2d::FREE_SPACE);
00116           }
00117     }
00118      
00119     plugins = local_costmap_->getLayeredCostmap()->getPlugins();
00120     for (std::vector<boost::shared_ptr<costmap_2d::Layer> >::iterator pluginp = plugins->begin(); pluginp != plugins->end(); ++pluginp) {
00121             boost::shared_ptr<costmap_2d::Layer> plugin = *pluginp;
00122           if(plugin->getName().find("obstacles")!=std::string::npos){
00123             boost::shared_ptr<costmap_2d::ObstacleLayer> costmap;
00124             costmap = boost::static_pointer_cast<costmap_2d::ObstacleLayer>(plugin);
00125             costmap->setConvexPolygonCost(local_poly, costmap_2d::FREE_SPACE);
00126           }
00127     } 
00128 
00129     //lock... just in case we're already speed limited
00130     boost::mutex::scoped_lock l(mutex_);
00131 
00132     //get the old maximum speed for the robot... we'll want to set it back
00133     if(!limit_set_)
00134     {
00135       if(!planner_nh_.getParam("max_trans_vel", old_trans_speed_))
00136       {
00137         ROS_ERROR("The planner %s, does not have the parameter max_trans_vel", planner_nh_.getNamespace().c_str());
00138       }
00139 
00140       if(!planner_nh_.getParam("max_rot_vel", old_rot_speed_))
00141       {
00142         ROS_ERROR("The planner %s, does not have the parameter max_rot_vel", planner_nh_.getNamespace().c_str());
00143       }
00144     }
00145 
00146     //we also want to save our current position so that we can remove the speed limit we impose later on
00147     speed_limit_pose_ = global_pose;
00148 
00149     //limit the speed of the robot until it moves a certain distance
00150     setRobotSpeed(limited_trans_speed_, limited_rot_speed_);
00151     limit_set_ = true;
00152     distance_check_timer_ = private_nh_.createTimer(ros::Duration(0.1), &MoveSlowAndClear::distanceCheck, this);
00153   }
00154 
00155   double MoveSlowAndClear::getSqDistance()
00156   {
00157     tf::Stamped<tf::Pose> global_pose;
00158     global_costmap_->getRobotPose(global_pose);
00159     double x1 = global_pose.getOrigin().x();
00160     double y1 = global_pose.getOrigin().y();
00161 
00162     double x2 = speed_limit_pose_.getOrigin().x();
00163     double y2 = speed_limit_pose_.getOrigin().y();
00164 
00165     return (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1);
00166   }
00167 
00168   void MoveSlowAndClear::distanceCheck(const ros::TimerEvent& e)
00169   {
00170     if(limited_distance_ * limited_distance_ <= getSqDistance())
00171     {
00172       ROS_INFO("Moved far enough, removing speed limit.");
00173       //have to do this because a system call within a timer cb does not seem to play nice
00174       if(remove_limit_thread_)
00175       {
00176         remove_limit_thread_->join();
00177         delete remove_limit_thread_;
00178       }
00179       remove_limit_thread_ = new boost::thread(boost::bind(&MoveSlowAndClear::removeSpeedLimit, this));
00180 
00181       distance_check_timer_.stop();
00182     }
00183   }
00184 
00185   void MoveSlowAndClear::removeSpeedLimit()
00186   {
00187     boost::mutex::scoped_lock l(mutex_);
00188     setRobotSpeed(old_trans_speed_, old_rot_speed_);
00189     limit_set_ = false;
00190   }
00191 
00192   void MoveSlowAndClear::setRobotSpeed(double trans_speed, double rot_speed)
00193   {
00194 
00195     {
00196       dynamic_reconfigure::Reconfigure vel_reconfigure;
00197       dynamic_reconfigure::DoubleParameter new_trans;
00198       new_trans.name = "max_trans_vel";
00199       new_trans.value = trans_speed;
00200       vel_reconfigure.request.config.doubles.push_back(new_trans);
00201       try {
00202         planner_dynamic_reconfigure_service_.call(vel_reconfigure);
00203         ROS_INFO_STREAM("Recovery setting trans vel: " << trans_speed);
00204       }
00205       catch(...) {
00206         ROS_ERROR("Something went wrong in the service call to dynamic_reconfigure");
00207       }
00208     }
00209     {
00210       dynamic_reconfigure::Reconfigure rot_reconfigure;
00211       dynamic_reconfigure::DoubleParameter new_rot;
00212       new_rot.name = "max_rot_vel";
00213       new_rot.value = rot_speed;
00214       rot_reconfigure.request.config.doubles.push_back(new_rot);
00215       try {
00216         planner_dynamic_reconfigure_service_.call(rot_reconfigure);
00217         ROS_INFO_STREAM("Recovery setting rot vel: " << rot_speed);
00218       }
00219       catch(...) {
00220         ROS_ERROR("Something went wrong in the service call to dynamic_reconfigure");
00221       }
00222     }
00223   }
00224 };


move_slow_and_clear
Author(s): Eitan Marder-Eppstein, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:46:48