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

148
navigations/rotate_recovery/CHANGELOG.rst Executable file
View File

@@ -0,0 +1,148 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package rotate_recovery
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
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)
-------------------
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>`_)
* Contributors: Sean Yen
1.16.3 (2019-11-15)
-------------------
* Merge branch 'melodic-devel' into layer_clear_area-melodic
* Cherry pick `#914 <https://github.com/ros-planning/navigation/issues/914>`_ (`#919 <https://github.com/ros-planning/navigation/issues/919>`_)
* Contributors: David V. Lu!!, Steven Macenski
1.16.2 (2018-07-31)
-------------------
* Merge pull request `#773 <https://github.com/ros-planning/navigation/issues/773>`_ from ros-planning/packaging_fixes
packaging fixes
* fix rotate_recovery debian build
* add depend on tf2_geometry_msgs (due to https://github.com/ros/geometry2/issues/275)
* add other hidden depends: angles, geometry_msgs, tf2
* Contributors: Michael Ferguson
1.16.1 (2018-07-28)
-------------------
1.16.0 (2018-07-25)
-------------------
* Remove dependency on PCL
* Switch to TF2 `#755 <https://github.com/ros-planning/navigation/issues/755>`_
* fix param names of RotateRecovery, closes `#706 <https://github.com/ros-planning/navigation/issues/706>`_
* Contributors: David V. Lu, 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>`_)
* Contributors: 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)
--------------------
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/rotate_recovery/CMakeLists.txt Executable file
View File

@@ -0,0 +1,58 @@
cmake_minimum_required(VERSION 3.0.2)
project(rotate_recovery)
find_package(catkin REQUIRED
COMPONENTS
angles
base_local_planner
cmake_modules
costmap_2d
geometry_msgs
nav_core
pluginlib
roscpp
tf2
tf2_geometry_msgs
tf2_ros
)
find_package(Eigen3 REQUIRED)
include_directories(
include
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
)
add_definitions(${EIGEN3_DEFINITIONS})
catkin_package(
INCLUDE_DIRS include
LIBRARIES rotate_recovery
CATKIN_DEPENDS
costmap_2d
geometry_msgs
nav_core
pluginlib
roscpp
tf2
tf2_ros
)
add_library(rotate_recovery src/rotate_recovery.cpp)
add_dependencies(rotate_recovery ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(rotate_recovery ${catkin_LIBRARIES})
install(TARGETS rotate_recovery
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
)
install(FILES rotate_plugin.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

86
navigations/rotate_recovery/include/rotate_recovery/rotate_recovery.h Executable file
View File

@@ -0,0 +1,86 @@
/*********************************************************************
*
* 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 ROTATE_RECOVERY_ROTATE_RECOVERY_H
#define ROTATE_RECOVERY_ROTATE_RECOVERY_H
#include <nav_core/recovery_behavior.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <tf2_ros/buffer.h>
#include <base_local_planner/costmap_model.h>
#include <string>
namespace rotate_recovery
{
/**
* @class RotateRecovery
* @brief A recovery behavior that rotates the robot in-place to attempt to clear out space
*/
class RotateRecovery : public nav_core::RecoveryBehavior
{
public:
/**
* @brief Constructor, make sure to call initialize in addition to actually initialize the object
*/
RotateRecovery();
/**
* @brief Initialization function for the RotateRecovery recovery behavior
* @param name Namespace used in initialization
* @param tf (unused)
* @param global_costmap (unused)
* @param local_costmap A pointer to the local_costmap used by the navigation stack
*/
void initialize(std::string name, tf2_ros::Buffer*,
costmap_2d::Costmap2DROS*, costmap_2d::Costmap2DROS* local_costmap);
/**
* @brief Run the RotateRecovery recovery behavior.
*/
void runBehavior();
/**
* @brief Destructor for the rotate recovery behavior
*/
~RotateRecovery();
private:
costmap_2d::Costmap2DROS* local_costmap_;
bool initialized_;
double sim_granularity_, min_rotational_vel_, max_rotational_vel_, acc_lim_th_, tolerance_, frequency_;
base_local_planner::CostmapModel* world_model_;
};
}; // namespace rotate_recovery
#endif // ROTATE_RECOVERY_ROTATE_RECOVERY_H

39
navigations/rotate_recovery/package.xml Executable file
View File

@@ -0,0 +1,39 @@
<?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>rotate_recovery</name>
<version>1.17.3</version>
<description>
This package provides a recovery behavior for the navigation stack that attempts to clear space by performing a 360 degree rotation of the robot.
</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/rotate_recovery</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>angles</build_depend>
<build_depend>base_local_planner</build_depend>
<build_depend>cmake_modules</build_depend>
<depend>costmap_2d</depend>
<depend>eigen</depend>
<depend>geometry_msgs</depend>
<depend>nav_core</depend>
<depend>pluginlib</depend>
<depend>roscpp</depend>
<depend>tf2</depend>
<depend>tf2_geometry_msgs</depend>
<depend>tf2_ros</depend>
<export>
<nav_core plugin="${prefix}/rotate_plugin.xml" />
</export>
</package>

7
navigations/rotate_recovery/rotate_plugin.xml Executable file
View File

@@ -0,0 +1,7 @@
<library path="lib/librotate_recovery">
<class name="rotate_recovery/RotateRecovery" type="rotate_recovery::RotateRecovery" base_class_type="nav_core::RecoveryBehavior">
<description>
A recovery behavior that performs a 360 degree in-place rotation to attempt to clear out space.
</description>
</class>
</library>

183
navigations/rotate_recovery/src/rotate_recovery.cpp Executable file
View File

@@ -0,0 +1,183 @@
/*********************************************************************
*
* 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 <rotate_recovery/rotate_recovery.h>
#include <pluginlib/class_list_macros.hpp>
#include <nav_core/parameter_magic.h>
#include <tf2/utils.h>
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Point.h>
#include <angles/angles.h>
#include <algorithm>
#include <string>
// register this planner as a RecoveryBehavior plugin
PLUGINLIB_EXPORT_CLASS(rotate_recovery::RotateRecovery, nav_core::RecoveryBehavior)
namespace rotate_recovery
{
RotateRecovery::RotateRecovery(): local_costmap_(NULL), initialized_(false), world_model_(NULL)
{
}
void RotateRecovery::initialize(std::string name, tf2_ros::Buffer*,
costmap_2d::Costmap2DROS*, costmap_2d::Costmap2DROS* local_costmap)
{
if (!initialized_)
{
local_costmap_ = local_costmap;
// get some parameters from the parameter server
ros::NodeHandle private_nh("~/" + name);
ros::NodeHandle blp_nh("~/TrajectoryPlannerROS");
// we'll simulate every degree by default
private_nh.param("sim_granularity", sim_granularity_, 0.017);
private_nh.param("frequency", frequency_, 20.0);
acc_lim_th_ = nav_core::loadParameterWithDeprecation(blp_nh, "acc_lim_theta", "acc_lim_th", 3.2);
max_rotational_vel_ = nav_core::loadParameterWithDeprecation(blp_nh, "max_vel_theta", "max_rotational_vel", 1.0);
min_rotational_vel_ = nav_core::loadParameterWithDeprecation(blp_nh, "min_in_place_vel_theta", "min_in_place_rotational_vel", 0.4);
blp_nh.param("yaw_goal_tolerance", tolerance_, 0.10);
world_model_ = new base_local_planner::CostmapModel(*local_costmap_->getCostmap());
initialized_ = true;
}
else
{
ROS_ERROR("You should not call initialize twice on this object, doing nothing");
}
}
RotateRecovery::~RotateRecovery()
{
delete world_model_;
}
void RotateRecovery::runBehavior()
{
if (!initialized_)
{
ROS_ERROR("This object must be initialized before runBehavior is called");
return;
}
if (local_costmap_ == NULL)
{
ROS_ERROR("The costmap passed to the RotateRecovery object cannot be NULL. Doing nothing.");
return;
}
ROS_WARN("Rotate recovery behavior started.");
ros::Rate r(frequency_);
ros::NodeHandle n;
ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 10);
geometry_msgs::PoseStamped global_pose;
local_costmap_->getRobotPose(global_pose);
double current_angle = tf2::getYaw(global_pose.pose.orientation);
double start_angle = current_angle;
bool got_180 = false;
while (n.ok() &&
(!got_180 ||
std::fabs(angles::shortest_angular_distance(current_angle, start_angle)) > tolerance_))
{
// Update Current Angle
local_costmap_->getRobotPose(global_pose);
current_angle = tf2::getYaw(global_pose.pose.orientation);
// compute the distance left to rotate
double dist_left;
if (!got_180)
{
// If we haven't hit 180 yet, we need to rotate a half circle plus the distance to the 180 point
double distance_to_180 = std::fabs(angles::shortest_angular_distance(current_angle, start_angle + M_PI));
dist_left = M_PI + distance_to_180;
if (distance_to_180 < tolerance_)
{
got_180 = true;
}
}
else
{
// If we have hit the 180, we just have the distance back to the start
dist_left = std::fabs(angles::shortest_angular_distance(current_angle, start_angle));
}
double x = global_pose.pose.position.x, y = global_pose.pose.position.y;
// check if that velocity is legal by forward simulating
double sim_angle = 0.0;
while (sim_angle < dist_left)
{
double theta = current_angle + sim_angle;
// make sure that the point is legal, if it isn't... we'll abort
double footprint_cost = world_model_->footprintCost(x, y, theta, local_costmap_->getRobotFootprint(), 0.0, 0.0);
if (footprint_cost < 0.0)
{
ROS_ERROR("Rotate recovery can't rotate in place because there is a potential collision. Cost: %.2f",
footprint_cost);
return;
}
sim_angle += sim_granularity_;
}
// compute the velocity that will let us stop by the time we reach the goal
double vel = sqrt(2 * acc_lim_th_ * dist_left);
// make sure that this velocity falls within the specified limits
vel = std::min(std::max(vel, min_rotational_vel_), max_rotational_vel_);
geometry_msgs::Twist cmd_vel;
cmd_vel.linear.x = 0.0;
cmd_vel.linear.y = 0.0;
cmd_vel.angular.z = vel;
vel_pub.publish(cmd_vel);
r.sleep();
}
}
}; // namespace rotate_recovery