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

40
navigations/sbpl_recovery/CHANGELOG.rst Executable file
View File

@@ -0,0 +1,40 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package sbpl_recovery
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.4.1 (2022-08-24)
------------------
0.4.0 (2022-03-07)
------------------
0.3.4 (2020-06-19)
------------------
* Initial release into noetic
* Set cmake_policy CMP0048 to fix warning
* Contributors: Martin Günther
0.3.3 (2019-10-15)
------------------
* Add READMEs
* Contributors: Martin Günther
0.3.2 (2019-01-16)
------------------
* Fix some includes
* Contributors: Martin Günther
0.3.1 (2018-09-05)
------------------
* sbpl_recovery: Ignore SBPL compile warning
* Contributors: Martin Günther
0.3.0 (2018-09-04)
------------------
* Convert to TF2 + new navigation API (for melodic)
* Use non deprecated pluginlib macro + headers
* Contributors: Martin Günther
0.2.0 (2018-09-03)
------------------
* Initial release into indigo, kinetic, lunar and melodic

50
navigations/sbpl_recovery/CMakeLists.txt Executable file
View File

@@ -0,0 +1,50 @@
cmake_minimum_required(VERSION 3.5.1)
cmake_policy(SET CMP0048 NEW)
project(sbpl_recovery)
##############################################################################
# Find dependencies
##############################################################################
set(THIS_PACKAGE_ROS_DEPS roscpp nav_core sbpl_lattice_planner pose_follower
costmap_2d pluginlib base_local_planner tf2_ros)
find_package(catkin REQUIRED COMPONENTS ${THIS_PACKAGE_ROS_DEPS})
include_directories(include ${catkin_INCLUDE_DIRS})
##############################################################################
# Define package
##############################################################################
catkin_package(
INCLUDE_DIRS include
LIBRARIES sbpl_recovery
CATKIN_DEPENDS ${THIS_PACKAGE_ROS_DEPS}
)
##############################################################################
# Build
##############################################################################
add_library(sbpl_recovery src/sbpl_recovery.cpp)
target_link_libraries(sbpl_recovery ${catkin_LIBRARIES})
target_compile_options(sbpl_recovery PUBLIC "-Wno-terminate") # suppress warning from included SBPL header
##############################################################################
# Install
##############################################################################
install(TARGETS sbpl_recovery
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
)
install(FILES recovery_plugin.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

5
navigations/sbpl_recovery/README.md Executable file
View File

@@ -0,0 +1,5 @@
sbpl_recovery
=============
A recovery behavior that uses the SBPL lattice planner and the pose follower to
try to plan in full 3D to get the robot out of really tricky situations.

88
navigations/sbpl_recovery/include/sbpl_recovery/sbpl_recovery.h Executable file
View File

@@ -0,0 +1,88 @@
/*********************************************************************
*
* 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 SBPL_RECOVERY_SBPL_RECOVERY_H_
#define SBPL_RECOVERY_SBPL_RECOVERY_H_
#include <ros/ros.h>
#include <nav_core/recovery_behavior.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <pose_follower/pose_follower.h>
#include <sbpl_lattice_planner/sbpl_lattice_planner.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
#include <boost/thread.hpp>
#include <base_local_planner/goal_functions.h>
#include <tf2_ros/buffer.h>
namespace sbpl_recovery
{
class SBPLRecovery : public nav_core::RecoveryBehavior
{
public:
SBPLRecovery();
// 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 planCB(const nav_msgs::Path::ConstPtr& plan);
double sqDistance(const geometry_msgs::PoseStamped& p1,
const geometry_msgs::PoseStamped& p2);
std::vector<geometry_msgs::PoseStamped> makePlan();
costmap_2d::Costmap2DROS* global_costmap_;
costmap_2d::Costmap2DROS* local_costmap_;
tf2_ros::Buffer* tf_;
sbpl_lattice_planner::SBPLLatticePlanner global_planner_;
pose_follower::PoseFollower local_planner_;
bool initialized_;
ros::Subscriber plan_sub_;
ros::Publisher vel_pub_;
boost::mutex plan_mutex_;
nav_msgs::Path plan_;
double control_frequency_, sq_planning_distance_, controller_patience_;
int planning_attempts_, attempts_per_run_;
bool use_local_frame_;
};
};
#endif

31
navigations/sbpl_recovery/package.xml Executable file
View File

@@ -0,0 +1,31 @@
<?xml version="1.0"?>
<package format="2">
<name>sbpl_recovery</name>
<version>0.4.1</version>
<description>
A recovery behavior that uses the sbpl lattice planner and the pose
follower to try to plan in full 3D to get the robot out of really tricky
situations.
</description>
<maintainer email="martin.guenther@dfki.de">Martin Günther</maintainer>
<author>Eitan Marder-Eppstein</author>
<license>BSD</license>
<url type="website">http://wiki.ros.org/sbpl_recovery</url>
<url type="repository">https://github.com/ros-planning/navigation_experimental.git</url>
<url type="bugtracker">https://github.com/ros-planning/navigation_experimental/issues</url>
<buildtool_depend>catkin</buildtool_depend>
<depend>base_local_planner</depend>
<depend>costmap_2d</depend>
<depend>nav_core</depend>
<depend>pluginlib</depend>
<depend>pose_follower</depend>
<depend>roscpp</depend>
<depend>sbpl_lattice_planner</depend>
<depend>tf2_ros</depend>
<export>
<nav_core plugin="${prefix}/recovery_plugin.xml" />
</export>
</package>

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

@@ -0,0 +1,7 @@
<library path="lib/libsbpl_recovery">
<class name="sbpl_recovery/SBPLRecovery" type="sbpl_recovery::SBPLRecovery" base_class_type="nav_core::RecoveryBehavior">
<description>
A recovery behavior that uses the sbpl lattice planner and the pose follower to try to plan in full 3D to get the robot out of really tricky situations.
</description>
</class>
</library>

241
navigations/sbpl_recovery/src/sbpl_recovery.cpp Executable file
View File

@@ -0,0 +1,241 @@
/*********************************************************************
*
* 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 <sbpl_recovery/sbpl_recovery.h>
#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(sbpl_recovery::SBPLRecovery, nav_core::RecoveryBehavior)
namespace sbpl_recovery
{
SBPLRecovery::SBPLRecovery():
global_costmap_(NULL),
local_costmap_(NULL),
tf_(NULL),
initialized_(false)
{
}
void SBPLRecovery::initialize (std::string n, tf2_ros::Buffer* tf,
costmap_2d::Costmap2DROS* global_costmap,
costmap_2d::Costmap2DROS* local_costmap)
{
ros::NodeHandle nh = ros::NodeHandle();
ros::NodeHandle p_nh = ros::NodeHandle("~/" + n);
std::string plan_topic;
p_nh.param("9", plan_topic, std::string("NavfnROS/plan"));
p_nh.param("control_frequency", control_frequency_, 10.0);
p_nh.param("controller_patience", controller_patience_, 5.0);
p_nh.param("planning_attempts", planning_attempts_, 3);
p_nh.param("attempts_per_run", attempts_per_run_, 3);
p_nh.param("use_local_frame", use_local_frame_, true);
double planning_distance;
p_nh.param("planning_distance", planning_distance, 2.0);
sq_planning_distance_ = planning_distance * planning_distance;
//we need to initialize our costmaps
global_costmap_ = global_costmap;
local_costmap_ = local_costmap;
tf_ = tf;
//we need to initialize our local and global planners
if(use_local_frame_)
global_planner_.initialize(n , local_costmap_);
else
global_planner_.initialize(n , global_costmap_);
local_planner_.initialize(n , tf, local_costmap_);
//we'll need to subscribe to get the latest plan information
vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
ros::NodeHandle node_nh("~/");
plan_sub_ = node_nh.subscribe<nav_msgs::Path>(plan_topic, 1, boost::bind(&SBPLRecovery::planCB, this, _1));
initialized_ = true;
}
void SBPLRecovery::planCB(const nav_msgs::Path::ConstPtr& plan)
{
//just copy the plan data over
geometry_msgs::PoseStamped global_pose;
local_costmap_->getRobotPose(global_pose);
costmap_2d::Costmap2D costmap;
costmap = *(local_costmap_->getCostmap());
if(use_local_frame_)
{
std::vector<geometry_msgs::PoseStamped> transformed_plan;
if(base_local_planner::transformGlobalPlan(*tf_, plan->poses, global_pose, costmap, local_costmap_->getGlobalFrameID(), transformed_plan))
{
boost::mutex::scoped_lock l(plan_mutex_);
if(!transformed_plan.empty())
plan_.header = transformed_plan[0].header;
plan_.poses = transformed_plan;
}
else
ROS_WARN("Could not transform to frame of the local recovery");
}
else
{
boost::mutex::scoped_lock l(plan_mutex_);
plan_ = *plan;
}
}
double SBPLRecovery::sqDistance(const geometry_msgs::PoseStamped& p1,
const geometry_msgs::PoseStamped& p2)
{
return (p1.pose.position.x - p2.pose.position.x) * (p1.pose.position.x - p2.pose.position.x) +
(p1.pose.position.y - p2.pose.position.y) * (p1.pose.position.y - p2.pose.position.y);
}
std::vector<geometry_msgs::PoseStamped> SBPLRecovery::makePlan()
{
boost::mutex::scoped_lock l(plan_mutex_);
std::vector<geometry_msgs::PoseStamped> sbpl_plan;
geometry_msgs::PoseStamped start;
if(use_local_frame_)
{
if(!local_costmap_->getRobotPose(start))
{
ROS_ERROR("SBPL recovery behavior could not get the current pose of the robot. Doing nothing.");
return sbpl_plan;
}
}
else
{
if(!global_costmap_->getRobotPose(start))
{
ROS_ERROR("SBPL recovery behavior could not get the current pose of the robot. Doing nothing.");
return sbpl_plan;
}
}
//first, we want to walk far enough along the path that we get to a point
//that is within the recovery distance from the robot. Otherwise, we might
//move backwards along the plan
unsigned int index = 0;
for(index=0; index < plan_.poses.size(); ++index)
{
if(sqDistance(start, plan_.poses[index]) < sq_planning_distance_)
break;
}
//next, we want to find a goal point that is far enough away from the robot on the
//original plan and attempt to plan to it
int unsuccessful_attempts = 0;
for(unsigned int i = index; i < plan_.poses.size(); ++i)
{
ROS_DEBUG("SQ Distance: %.2f, spd: %.2f, start (%.2f, %.2f), goal (%.2f, %.2f)",
sqDistance(start, plan_.poses[i]),
sq_planning_distance_,
start.pose.position.x, start.pose.position.y,
plan_.poses[i].pose.position.x,
plan_.poses[i].pose.position.y);
if(sqDistance(start, plan_.poses[i]) >= sq_planning_distance_ || i == (plan_.poses.size() - 1))
{
ROS_INFO("Calling sbpl planner with start (%.2f, %.2f), goal (%.2f, %.2f)",
start.pose.position.x, start.pose.position.y,
plan_.poses[i].pose.position.x,
plan_.poses[i].pose.position.y);
if(global_planner_.makePlan(start, plan_.poses[i], sbpl_plan) && !sbpl_plan.empty())
{
ROS_INFO("Got a valid plan");
return sbpl_plan;
}
sbpl_plan.clear();
//make sure that we don't spend forever planning
unsuccessful_attempts++;
if(unsuccessful_attempts >= attempts_per_run_)
return sbpl_plan;
}
}
return sbpl_plan;
}
void SBPLRecovery::runBehavior()
{
if(!initialized_)
{
ROS_ERROR("Please initialize this recovery behavior before attempting to run it.");
return;
}
ROS_INFO("Starting the sbpl recovery behavior");
for(int i=0; i <= planning_attempts_; ++i)
{
std::vector<geometry_msgs::PoseStamped> sbpl_plan = makePlan();
if(sbpl_plan.empty())
{
ROS_ERROR("Unable to find a valid pose to plan to on the global plan.");
return;
}
//ok... now we've got a plan so we need to try to follow it
local_planner_.setPlan(sbpl_plan);
ros::Rate r(control_frequency_);
ros::Time last_valid_control = ros::Time::now();
while(ros::ok() &&
last_valid_control + ros::Duration(controller_patience_) >= ros::Time::now() &&
!local_planner_.isGoalReached())
{
geometry_msgs::Twist cmd_vel;
bool valid_control = local_planner_.computeVelocityCommands(cmd_vel);
if(valid_control)
last_valid_control = ros::Time::now();
vel_pub_.publish(cmd_vel);
r.sleep();
}
if(local_planner_.isGoalReached())
ROS_INFO("The sbpl recovery behavior made it to its desired goal");
else
ROS_WARN("The sbpl recovery behavior failed to make it to its desired goal");
}
}
};