HiepLM/mir_amr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit"

Browse Source
This commit is contained in:
HiepLM
2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

150
navigations/move_slow_and_clear/CHANGELOG.rst Executable file
View File

@@ -0,0 +1,150 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package move_slow_and_clear
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
1.17.3 (2023-01-10)
-------------------
* [ROS-O] various patches (`#1225 <https://github.com/ros-planning/navigation/issues/1225>`_)
* do not specify obsolete c++11 standard
this breaks with current versions of log4cxx.
* update pluginlib include paths
the non-hpp headers have been deprecated since kinetic
* use lambdas in favor of boost::bind
Using boost's _1 as a global system is deprecated since C++11.
The ROS packages in Debian removed the implicit support for the global symbols,
so this code fails to compile there without the patch.
* Contributors: Michael Görner
1.17.2 (2022-06-20)
-------------------
* [move_slow_and_clear] Add rosparam representing max vel rosparam name (`#1039 <https://github.com/ros-planning/navigation/issues/1039>`_)
* Contributors: Naoya Yamaguchi
1.17.1 (2020-08-27)
-------------------
1.17.0 (2020-04-02)
-------------------
* Merge pull request `#982 <https://github.com/ros-planning/navigation/issues/982>`_ from ros-planning/noetic_prep
Noetic Migration
* increase required cmake version
* Contributors: Michael Ferguson
1.16.6 (2020-03-18)
-------------------
1.16.5 (2020-03-15)
-------------------
* [melodic] updated install for better portability. (`#973 <https://github.com/ros-planning/navigation/issues/973>`_)
* Contributors: Sean Yen
1.16.4 (2020-03-04)
-------------------
* [Windows][melodic] Navigation (except for map_server and amcl) Windows build bring up (`#851 <https://github.com/cobalt-robotics/navigation/issues/851>`_)
* Merge pull request `#955 <https://github.com/cobalt-robotics/navigation/issues/955>`_ from isjfk/melodic-devel
Fix move_slow_and_clear sending obsolete params to dwa_local_planner
* Fix move_slow_and_clear sending obsolete params to dwa_local_planner
issue.
* Contributors: Jimmy F. Klarke, Michael Ferguson, Sean Yen
1.16.3 (2019-11-15)
-------------------
1.16.2 (2018-07-31)
-------------------
1.16.1 (2018-07-28)
-------------------
1.16.0 (2018-07-25)
-------------------
* Switch to TF2 `#755 <https://github.com/ros-planning/navigation/issues/755>`_
* Contributors: Michael Ferguson, Vincent Rabaud
1.15.2 (2018-03-22)
-------------------
* Merge pull request `#673 <https://github.com/ros-planning/navigation/issues/673>`_ from ros-planning/email_update_lunar
update maintainer email (lunar)
* Merge pull request `#649 <https://github.com/ros-planning/navigation/issues/649>`_ from aaronhoy/lunar_add_ahoy
Add myself as a maintainer.
* Rebase PRs from Indigo/Kinetic (`#637 <https://github.com/ros-planning/navigation/issues/637>`_)
* Respect planner_frequency intended behavior (`#622 <https://github.com/ros-planning/navigation/issues/622>`_)
* Only do a getRobotPose when no start pose is given (`#628 <https://github.com/ros-planning/navigation/issues/628>`_)
Omit the unnecessary call to getRobotPose when the start pose was
already given, so that move_base can also generate a path in
situations where getRobotPose would fail.
This is actually to work around an issue of getRobotPose randomly
failing.
* Update gradient_path.cpp (`#576 <https://github.com/ros-planning/navigation/issues/576>`_)
* Update gradient_path.cpp
* Update navfn.cpp
* update to use non deprecated pluginlib macro (`#630 <https://github.com/ros-planning/navigation/issues/630>`_)
* update to use non deprecated pluginlib macro
* multiline version as well
* Print SDL error on IMG_Load failure in server_map (`#631 <https://github.com/ros-planning/navigation/issues/631>`_)
* Contributors: Aaron Hoy, David V. Lu!!, Michael Ferguson
1.15.1 (2017-08-14)
-------------------
1.15.0 (2017-08-07)
-------------------
* convert packages to format2
* Fix CMakeLists + package.xmls (`#548 <https://github.com/ros-planning/navigation/issues/548>`_)
* import only PCL common
* address gcc6 build error
* remove GCC warnings
* Fix CMake warnings
* Contributors: Lukas Bulwahn, Martin Günther, Mikael Arguedas, Vincent Rabaud
1.14.0 (2016-05-20)
-------------------
1.13.1 (2015-10-29)
-------------------
1.13.0 (2015-03-17)
-------------------
1.12.0 (2015-02-04)
-------------------
* update maintainer email
* Contributors: Michael Ferguson
1.11.15 (2015-02-03)
--------------------
* Add ARCHIVE_DESTINATION for static builds
* Contributors: Gary Servin
1.11.14 (2014-12-05)
--------------------
1.11.13 (2014-10-02)
--------------------
1.11.12 (2014-10-01)
--------------------
1.11.11 (2014-07-23)
--------------------
* Use service call, rather than system call, to call dynamic
reconfigure when decreasing move_base speed.
* Contributors: Ryohei Ueda
1.11.10 (2014-06-25)
--------------------
1.11.9 (2014-06-10)
-------------------
1.11.8 (2014-05-21)
-------------------
1.11.7 (2014-05-21)
-------------------
* update build to find eigen using cmake_modules
* Contributors: Michael Ferguson
1.11.4 (2013-09-27)
-------------------
* Package URL Updates

58
navigations/move_slow_and_clear/CMakeLists.txt Executable file
View File

@@ -0,0 +1,58 @@
cmake_minimum_required(VERSION 3.0.2)
project(move_slow_and_clear)
find_package(catkin REQUIRED
COMPONENTS
cmake_modules
costmap_2d
geometry_msgs
nav_core
pluginlib
roscpp
)
find_package(Eigen3 REQUIRED)
remove_definitions(-DDISABLE_LIBUSB-1.0)
find_package(Boost REQUIRED COMPONENTS thread)
include_directories(
include
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
)
add_definitions(${EIGEN3_DEFINITIONS})
catkin_package(
INCLUDE_DIRS include
LIBRARIES move_slow_and_clear
CATKIN_DEPENDS
geometry_msgs
nav_core
pluginlib
roscpp
)
add_library(${PROJECT_NAME} src/move_slow_and_clear.cpp)
add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(${PROJECT_NAME}
${Boost_LIBRARIES}
${catkin_LIBRARIES}
)
## Install project namespaced headers
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
PATTERN ".svn" EXCLUDE)
install(TARGETS move_slow_and_clear
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)
install(FILES recovery_plugin.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

85
navigations/move_slow_and_clear/include/move_slow_and_clear/move_slow_and_clear.h Executable file
View File

@@ -0,0 +1,85 @@
/*********************************************************************
*
* 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>
#include <dynamic_reconfigure/Reconfigure.h>
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, tf2_ros::Buffer* 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_;
std::string max_trans_param_name_, max_rot_param_name_;
ros::Timer distance_check_timer_;
geometry_msgs::PoseStamped speed_limit_pose_;
boost::thread* remove_limit_thread_;
boost::mutex mutex_;
bool limit_set_;
ros::ServiceClient planner_dynamic_reconfigure_service_;
};
};
#endif

35
navigations/move_slow_and_clear/package.xml Executable file
View File

@@ -0,0 +1,35 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="2">
<name>move_slow_and_clear</name>
<version>1.17.3</version>
<description>
move_slow_and_clear
</description>
<author>Eitan Marder-Eppstein</author>
<author>contradict@gmail.com</author>
<maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
<maintainer email="mfergs7@gmail.com">Michael Ferguson</maintainer>
<maintainer email="ahoy@fetchrobotics.com">Aaron Hoy</maintainer>
<license>BSD</license>
<url>http://wiki.ros.org/move_slow_and_clear</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>cmake_modules</build_depend>
<depend>costmap_2d</depend>
<depend>geometry_msgs</depend>
<depend>nav_core</depend>
<depend>pluginlib</depend>
<depend>roscpp</depend>
<export>
<nav_core plugin="${prefix}/recovery_plugin.xml" />
</export>
</package>

7
navigations/move_slow_and_clear/recovery_plugin.xml Executable file
View File

@@ -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>

225
navigations/move_slow_and_clear/src/move_slow_and_clear.cpp Executable file
View File

@@ -0,0 +1,225 @@
/*********************************************************************
*
* 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.hpp>
#include <costmap_2d/obstacle_layer.h>
PLUGINLIB_EXPORT_CLASS(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, tf2_ros::Buffer* 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);
private_nh_.param("max_trans_param_name", max_trans_param_name_, std::string("max_trans_vel"));
private_nh_.param("max_rot_param_name", max_rot_param_name_, std::string("max_rot_vel"));
std::string planner_namespace;
private_nh_.param("planner_namespace", planner_namespace, std::string("DWAPlannerROS"));
planner_nh_ = ros::NodeHandle("~/" + planner_namespace);
planner_dynamic_reconfigure_service_ = planner_nh_.serviceClient<dynamic_reconfigure::Reconfigure>("set_parameters", true);
initialized_ = true;
}
void MoveSlowAndClear::runBehavior()
{
if(!initialized_)
{
ROS_ERROR("This recovery behavior has not been initialized, doing nothing.");
return;
}
ROS_WARN("Move slow and clear recovery behavior started.");
geometry_msgs::PoseStamped 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.pose.position.x + i * clearing_distance_;
pt.y = global_pose.pose.position.y + i * clearing_distance_;
global_poly.push_back(pt);
pt.x = global_pose.pose.position.x + i * clearing_distance_;
pt.y = global_pose.pose.position.y + -1.0 * i * clearing_distance_;
global_poly.push_back(pt);
pt.x = local_pose.pose.position.x + i * clearing_distance_;
pt.y = local_pose.pose.position.y + i * clearing_distance_;
local_poly.push_back(pt);
pt.x = local_pose.pose.position.x + i * clearing_distance_;
pt.y = local_pose.pose.position.y + -1.0 * i * clearing_distance_;
local_poly.push_back(pt);
}
//clear the desired space in the costmaps
std::vector<boost::shared_ptr<costmap_2d::Layer> >* plugins = global_costmap_->getLayeredCostmap()->getPlugins();
for (std::vector<boost::shared_ptr<costmap_2d::Layer> >::iterator pluginp = plugins->begin(); pluginp != plugins->end(); ++pluginp) {
boost::shared_ptr<costmap_2d::Layer> plugin = *pluginp;
if(plugin->getName().find("obstacles")!=std::string::npos){
boost::shared_ptr<costmap_2d::ObstacleLayer> costmap;
costmap = boost::static_pointer_cast<costmap_2d::ObstacleLayer>(plugin);
costmap->setConvexPolygonCost(global_poly, costmap_2d::FREE_SPACE);
}
}
plugins = local_costmap_->getLayeredCostmap()->getPlugins();
for (std::vector<boost::shared_ptr<costmap_2d::Layer> >::iterator pluginp = plugins->begin(); pluginp != plugins->end(); ++pluginp) {
boost::shared_ptr<costmap_2d::Layer> plugin = *pluginp;
if(plugin->getName().find("obstacles")!=std::string::npos){
boost::shared_ptr<costmap_2d::ObstacleLayer> costmap;
costmap = boost::static_pointer_cast<costmap_2d::ObstacleLayer>(plugin);
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_param_name_, old_trans_speed_))
{
ROS_ERROR("The planner %s, does not have the parameter %s", planner_nh_.getNamespace().c_str(), max_trans_param_name_.c_str());
}
if(!planner_nh_.getParam(max_rot_param_name_, old_rot_speed_))
{
ROS_ERROR("The planner %s, does not have the parameter %s", planner_nh_.getNamespace().c_str(), max_rot_param_name_.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()
{
geometry_msgs::PoseStamped global_pose;
global_costmap_->getRobotPose(global_pose);
double x1 = global_pose.pose.position.x;
double y1 = global_pose.pose.position.y;
double x2 = speed_limit_pose_.pose.position.x;
double y2 = speed_limit_pose_.pose.position.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)
{
{
dynamic_reconfigure::Reconfigure vel_reconfigure;
dynamic_reconfigure::DoubleParameter new_trans;
new_trans.name = max_trans_param_name_;
new_trans.value = trans_speed;
vel_reconfigure.request.config.doubles.push_back(new_trans);
try {
planner_dynamic_reconfigure_service_.call(vel_reconfigure);
ROS_INFO_STREAM("Recovery setting trans vel: " << trans_speed);
}
catch(...) {
ROS_ERROR("Something went wrong in the service call to dynamic_reconfigure");
}
}
{
dynamic_reconfigure::Reconfigure rot_reconfigure;
dynamic_reconfigure::DoubleParameter new_rot;
new_rot.name = max_rot_param_name_;
new_rot.value = rot_speed;
rot_reconfigure.request.config.doubles.push_back(new_rot);
try {
planner_dynamic_reconfigure_service_.call(rot_reconfigure);
ROS_INFO_STREAM("Recovery setting rot vel: " << rot_speed);
}
catch(...) {
ROS_ERROR("Something went wrong in the service call to dynamic_reconfigure");
}
}
}
};