Adding the move_slow_and_clear recovery behavior to the navigation stack

move_slow_and_clear/CMakeLists.txt

@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+rosbuild_add_library(${PROJECT_NAME} src/move_slow_and_clear.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+rosbuild_add_boost_directories()
+rosbuild_link_boost(${PROJECT_NAME} thread)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})

move_slow_and_clear/Makefile

@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk

move_slow_and_clear/include/move_slow_and_clear/move_slow_and_clear.h

@@ -0,0 +1,82 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2009, 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.
+*
+* Author: Eitan Marder-Eppstein
+*********************************************************************/
+#ifndef MOVE_SLOW_AND_CLEAR_MOVE_SLOW_AND_CLEAR_H_
+#define MOVE_SLOW_AND_CLEAR_MOVE_SLOW_AND_CLEAR_H_
+
+#include <ros/ros.h>
+#include <nav_core/recovery_behavior.h>
+#include <costmap_2d/costmap_2d_ros.h>
+#include <boost/thread.hpp>
+
+namespace move_slow_and_clear 
+{
+  class MoveSlowAndClear : public nav_core::RecoveryBehavior
+  {
+    public:
+      MoveSlowAndClear();
+      ~MoveSlowAndClear();
+
+      /// Initialize the parameters of the behavior
+      void initialize (std::string n, tf::TransformListener* tf,
+          costmap_2d::Costmap2DROS* global_costmap,
+          costmap_2d::Costmap2DROS* local_costmap);
+
+      /// Run the behavior
+      void runBehavior();
+
+    private:
+      void setRobotSpeed(double trans_speed, double rot_speed);
+      void distanceCheck(const ros::TimerEvent& e);
+      double getSqDistance();
+
+      void removeSpeedLimit();
+
+      ros::NodeHandle private_nh_, planner_nh_;
+      costmap_2d::Costmap2DROS* global_costmap_;
+      costmap_2d::Costmap2DROS* local_costmap_;
+      bool initialized_;
+      double clearing_distance_, limited_distance_;
+      double limited_trans_speed_, limited_rot_speed_, old_trans_speed_, old_rot_speed_;
+      ros::Timer distance_check_timer_;
+      tf::Stamped<tf::Pose> speed_limit_pose_;
+      boost::thread* remove_limit_thread_;
+      boost::mutex mutex_;
+      bool limit_set_;
+  };
+};
+
+#endif

move_slow_and_clear/mainpage.dox

@@ -0,0 +1,26 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b move_slow_and_clear is ... 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+
+*/

move_slow_and_clear/manifest.xml

@@ -0,0 +1,24 @@
+<package>
+  <description brief="move_slow_and_clear">
+
+     move_slow_and_clear
+
+  </description>
+  <author>Eitan Marder-Eppstein</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/move_slow_and_clear</url>
+  <depend package="roscpp"/>
+  <depend package="nav_core"/>
+  <depend package="costmap_2d"/>
+  <depend package="geometry_msgs"/>
+  <depend package="pluginlib"/>
+  <depend package="dynamic_reconfigure"/>
+
+  <export>
+    <nav_core plugin="${prefix}/recovery_plugin.xml" />
+  </export>
+
+</package>
+
+

move_slow_and_clear/recovery_plugin.xml

@@ -0,0 +1,7 @@
+<library path="lib/libmove_slow_and_clear">
+  <class name="move_slow_and_clear/MoveSlowAndClear" type="move_slow_and_clear::MoveSlowAndClear" base_class_type="nav_core::RecoveryBehavior">
+    <description>
+      A recovery behavior that clears information in the costmap within the circumscribed radius of the robot and then limits the speed of the robot. Note, this recovery behavior is not truly safe, the robot may hit things, it'll just happen at a user-specified speed. Also, this recovery behavior is only compatible with local planners that allow maximum speeds to be set via dynamic reconfigure.
+    </description>
+  </class>
+</library>

move_slow_and_clear/src/move_slow_and_clear.cpp

@@ -0,0 +1,195 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2009, 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.
+*
+* Author: Eitan Marder-Eppstein
+*********************************************************************/
+#include <move_slow_and_clear/move_slow_and_clear.h>
+#include <pluginlib/class_list_macros.h>
+
+PLUGINLIB_DECLARE_CLASS(move_slow_and_clear, MoveSlowAndClear, move_slow_and_clear::MoveSlowAndClear,
+    nav_core::RecoveryBehavior)
+
+namespace move_slow_and_clear
+{
+  MoveSlowAndClear::MoveSlowAndClear():global_costmap_(NULL), local_costmap_(NULL), 
+                                       initialized_(false), remove_limit_thread_(NULL), limit_set_(false){}
+
+  MoveSlowAndClear::~MoveSlowAndClear()
+  {
+    delete remove_limit_thread_;
+  }
+
+  void MoveSlowAndClear::initialize (std::string n, tf::TransformListener* tf,
+      costmap_2d::Costmap2DROS* global_costmap,
+      costmap_2d::Costmap2DROS* local_costmap)
+  {
+    global_costmap_ = global_costmap;
+    local_costmap_ = local_costmap;
+
+    ros::NodeHandle private_nh_("~/" + n);
+    private_nh_.param("clearing_distance", clearing_distance_, 0.5);
+    private_nh_.param("limited_trans_speed", limited_trans_speed_, 0.25);
+    private_nh_.param("limited_rot_speed", limited_rot_speed_, 0.45);
+    private_nh_.param("limited_distance", limited_distance_, 0.3);
+
+    std::string planner_namespace;
+    private_nh_.param("planner_namespace", planner_namespace, std::string("DWAPlannerROS"));
+
+    planner_nh_ = ros::NodeHandle("~/" + planner_namespace);
+
+    initialized_ = true;
+  }
+
+  void MoveSlowAndClear::runBehavior()
+  {
+    if(!initialized_)
+    {
+      ROS_ERROR("This recovery behavior has not been initialized, doing nothing.");
+      return;
+    }
+
+    ROS_DEBUG("Running move slow and clear behavior");
+    tf::Stamped<tf::Pose> global_pose, local_pose;
+    global_costmap_->getRobotPose(global_pose);
+    local_costmap_->getRobotPose(local_pose);
+
+    std::vector<geometry_msgs::Point> global_poly, local_poly;
+
+    geometry_msgs::Point pt;
+    for(int i = -1; i <= 1; i+=2)
+    {
+      pt.x = global_pose.getOrigin().x() + i * clearing_distance_;
+      pt.y = global_pose.getOrigin().y() + i * clearing_distance_;
+      global_poly.push_back(pt);
+
+      pt.x = global_pose.getOrigin().x() + i * clearing_distance_;
+      pt.y = global_pose.getOrigin().y() + -1.0 * i * clearing_distance_;
+      global_poly.push_back(pt);
+
+      pt.x = local_pose.getOrigin().x() + i * clearing_distance_;
+      pt.y = local_pose.getOrigin().y() + i * clearing_distance_;
+      local_poly.push_back(pt);
+
+      pt.x = local_pose.getOrigin().x() + i * clearing_distance_;
+      pt.y = local_pose.getOrigin().y() + -1.0 * i * clearing_distance_;
+      local_poly.push_back(pt);
+    }
+
+    //clear the desired space in both costmaps
+    global_costmap_->setConvexPolygonCost(global_poly, costmap_2d::FREE_SPACE);
+    local_costmap_->setConvexPolygonCost(local_poly, costmap_2d::FREE_SPACE);
+
+    //lock... just in case we're already speed limited
+    boost::mutex::scoped_lock l(mutex_);
+
+    //get the old maximum speed for the robot... we'll want to set it back
+    if(!limit_set_)
+    {
+      if(!planner_nh_.getParam("max_trans_vel", old_trans_speed_))
+      {
+        ROS_ERROR("The planner %s, does not have the parameter max_trans_vel", planner_nh_.getNamespace().c_str());
+      }
+
+      if(!planner_nh_.getParam("max_rot_vel", old_rot_speed_))
+      {
+        ROS_ERROR("The planner %s, does not have the parameter max_rot_vel", planner_nh_.getNamespace().c_str());
+      }
+    }
+
+    //we also want to save our current position so that we can remove the speed limit we impose later on
+    speed_limit_pose_ = global_pose;
+
+    //limit the speed of the robot until it moves a certain distance
+    setRobotSpeed(limited_trans_speed_, limited_rot_speed_);
+    limit_set_ = true;
+    distance_check_timer_ = private_nh_.createTimer(ros::Duration(0.1), &MoveSlowAndClear::distanceCheck, this);
+  }
+
+  double MoveSlowAndClear::getSqDistance()
+  {
+    tf::Stamped<tf::Pose> global_pose;
+    global_costmap_->getRobotPose(global_pose);
+    double x1 = global_pose.getOrigin().x();
+    double y1 = global_pose.getOrigin().y();
+
+    double x2 = speed_limit_pose_.getOrigin().x();
+    double y2 = speed_limit_pose_.getOrigin().y();
+
+    return (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1);
+  }
+
+  void MoveSlowAndClear::distanceCheck(const ros::TimerEvent& e)
+  {
+    if(limited_distance_ * limited_distance_ <= getSqDistance())
+    {
+      ROS_INFO("Moved far enough, removing speed limit.");
+      //have to do this because a system call within a timer cb does not seem to play nice
+      if(remove_limit_thread_)
+      {
+        remove_limit_thread_->join();
+        delete remove_limit_thread_;
+      }
+      remove_limit_thread_ = new boost::thread(boost::bind(&MoveSlowAndClear::removeSpeedLimit, this));
+
+      distance_check_timer_.stop();
+    }
+  }
+
+  void MoveSlowAndClear::removeSpeedLimit()
+  {
+    boost::mutex::scoped_lock l(mutex_);
+    setRobotSpeed(old_trans_speed_, old_rot_speed_);
+    limit_set_ = false;
+  }
+
+  void MoveSlowAndClear::setRobotSpeed(double trans_speed, double rot_speed)
+  {
+    std::ostringstream trans_command;
+    trans_command << "rosrun dynamic_reconfigure dynparam set " << planner_nh_.getNamespace() << " max_trans_vel " << trans_speed;
+    ROS_INFO("Recovery setting trans vel: %s", trans_command.str().c_str());
+    if(system(trans_command.str().c_str()) < 0)
+    {
+      ROS_ERROR("Something went wrong in the system call to dynparam");
+    }
+
+    std::ostringstream rot_command;
+    rot_command << "rosrun dynamic_reconfigure dynparam set " << planner_nh_.getNamespace() << " max_rot_vel " << rot_speed;
+    ROS_INFO("Recovery setting rot vel: %s", rot_command.str().c_str());
+    if(system(rot_command.str().c_str()) < 0)
+    {
+      ROS_INFO("Something went wrong in the system call to dynparam");
+    }
+  }
+
+};