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

133
navigations/carrot_planner/CHANGELOG.rst Executable file
View File

@@ -0,0 +1,133 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package carrot_planner
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
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)
-------------------
* Fix carrot planner (`#1056 <https://github.com/ros-planning/navigation/issues/1056>`_)
* fix memory leak of world_model\_
* fix uninitialized raw pointers
* move the angles header into cpp
* add missing angles dependency
Co-authored-by: Dima Dorezyuk <dorezyuk@magazino.eu>
* Contributors: Dima Dorezyuk
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)
-------------------
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 depends of carrot_planner
* declare direct dependency on tf2
* alphabetize the depends in CMake
* Contributors: Michael Ferguson
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, Rein Appeldoorn, 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.
* Contributors: Aaron Hoy, 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)
-------------------
1.11.4 (2013-09-27)
-------------------
* Package URL Updates

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

@@ -0,0 +1,58 @@
cmake_minimum_required(VERSION 3.0.2)
project(carrot_planner)
find_package(catkin REQUIRED
COMPONENTS
angles
base_local_planner
costmap_2d
nav_core
pluginlib
roscpp
tf2
tf2_geometry_msgs
tf2_ros
)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
add_definitions(${EIGEN3_DEFINITIONS})
catkin_package(
INCLUDE_DIRS include
LIBRARIES carrot_planner
CATKIN_DEPENDS
angles
base_local_planner
costmap_2d
nav_core
pluginlib
roscpp
tf2
tf2_ros
)
add_library(carrot_planner src/carrot_planner.cpp)
add_dependencies(carrot_planner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(carrot_planner
${catkin_LIBRARIES}
)
install(TARGETS carrot_planner
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}
PATTERN ".svn" EXCLUDE
)
install(FILES bgp_plugin.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

7
navigations/carrot_planner/bgp_plugin.xml Executable file
View File

@@ -0,0 +1,7 @@
<library path="lib/libcarrot_planner">
<class name="carrot_planner/CarrotPlanner" type="carrot_planner::CarrotPlanner" base_class_type="nav_core::BaseGlobalPlanner">
<description>
A simple planner that seeks to place a legal carrot in-front of the robot
</description>
</class>
</library>

107
navigations/carrot_planner/include/carrot_planner/carrot_planner.h Executable file
View File

@@ -0,0 +1,107 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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 CARROT_PLANNER_H_
#define CARROT_PLANNER_H_
#include <ros/ros.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/costmap_2d.h>
#include <nav_core/base_global_planner.h>
#include <geometry_msgs/PoseStamped.h>
#include <base_local_planner/world_model.h>
#include <base_local_planner/costmap_model.h>
namespace carrot_planner{
/**
* @class CarrotPlanner
* @brief Provides a simple global planner that will compute a valid goal point for the local planner by walking back along the vector between the robot and the user-specified goal point until a valid cost is found.
*/
class CarrotPlanner : public nav_core::BaseGlobalPlanner {
public:
/**
* @brief Constructor for the CarrotPlanner
*/
CarrotPlanner();
/**
* @brief Constructor for the CarrotPlanner
* @param name The name of this planner
* @param costmap_ros A pointer to the ROS wrapper of the costmap to use for planning
*/
CarrotPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
/**
* @brief Destructor
*/
~CarrotPlanner();
/**
* @brief Initialization function for the CarrotPlanner
* @param name The name of this planner
* @param costmap_ros A pointer to the ROS wrapper of the costmap to use for planning
*/
void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
/**
* @brief Given a goal pose in the world, compute a plan
* @param start The start pose
* @param goal The goal pose
* @param plan The plan... filled by the planner
* @return True if a valid plan was found, false otherwise
*/
bool makePlan(const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan);
private:
costmap_2d::Costmap2DROS* costmap_ros_;
double step_size_, min_dist_from_robot_;
costmap_2d::Costmap2D* costmap_;
base_local_planner::WorldModel* world_model_; ///< @brief The world model that the controller will use
/**
* @brief Checks the legality of the robot footprint at a position and orientation using the world model
* @param x_i The x position of the robot
* @param y_i The y position of the robot
* @param theta_i The orientation of the robot
* @return
*/
double footprintCost(double x_i, double y_i, double theta_i);
bool initialized_;
};
};
#endif

39
navigations/carrot_planner/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>carrot_planner</name>
<version>1.17.3</version>
<description>
This planner attempts to find a legal place to put a carrot for the robot to follow. It does this by moving back along the vector between the robot and the goal point.
</description>
<author>Eitan Marder-Eppstein</author>
<author>Sachin Chitta</author>
<author>contradict@gmail.com</author>
<maintainer email="ahoy@fetchrobotics.com">Aaron Hoy</maintainer>
<maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
<maintainer email="mfergs7@gmail.com">Michael Ferguson</maintainer>
<license>BSD</license>
<url>http://wiki.ros.org/carrot_planner</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>tf2_geometry_msgs</build_depend>
<depend>angles</depend>
<depend>base_local_planner</depend>
<depend>costmap_2d</depend>
<depend>eigen</depend>
<depend>nav_core</depend>
<depend>pluginlib</depend>
<depend>roscpp</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<export>
<nav_core plugin="${prefix}/bgp_plugin.xml" />
</export>
</package>

175
navigations/carrot_planner/src/carrot_planner.cpp Executable file
View File

@@ -0,0 +1,175 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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.
*
* Authors: Eitan Marder-Eppstein, Sachin Chitta
*********************************************************************/
#include <angles/angles.h>
#include <carrot_planner/carrot_planner.h>
#include <pluginlib/class_list_macros.hpp>
#include <tf2/convert.h>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
//register this planner as a BaseGlobalPlanner plugin
PLUGINLIB_EXPORT_CLASS(carrot_planner::CarrotPlanner, nav_core::BaseGlobalPlanner)
namespace carrot_planner {
CarrotPlanner::CarrotPlanner()
: costmap_ros_(NULL), costmap_(NULL), world_model_(NULL), initialized_(false){}
CarrotPlanner::CarrotPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
: costmap_ros_(NULL), costmap_(NULL), world_model_(NULL), initialized_(false){
initialize(name, costmap_ros);
}
CarrotPlanner::~CarrotPlanner() {
// deleting a nullptr is a noop
delete world_model_;
}
void CarrotPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
if(!initialized_){
costmap_ros_ = costmap_ros;
costmap_ = costmap_ros_->getCostmap();
ros::NodeHandle private_nh("~/" + name);
private_nh.param("step_size", step_size_, costmap_->getResolution());
private_nh.param("min_dist_from_robot", min_dist_from_robot_, 0.10);
world_model_ = new base_local_planner::CostmapModel(*costmap_);
initialized_ = true;
}
else
ROS_WARN("This planner has already been initialized... doing nothing");
}
//we need to take the footprint of the robot into account when we calculate cost to obstacles
double CarrotPlanner::footprintCost(double x_i, double y_i, double theta_i){
if(!initialized_){
ROS_ERROR("The planner has not been initialized, please call initialize() to use the planner");
return -1.0;
}
std::vector<geometry_msgs::Point> footprint = costmap_ros_->getRobotFootprint();
//if we have no footprint... do nothing
if(footprint.size() < 3)
return -1.0;
//check if the footprint is legal
double footprint_cost = world_model_->footprintCost(x_i, y_i, theta_i, footprint);
return footprint_cost;
}
bool CarrotPlanner::makePlan(const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){
if(!initialized_){
ROS_ERROR("The planner has not been initialized, please call initialize() to use the planner");
return false;
}
ROS_DEBUG("Got a start: %.2f, %.2f, and a goal: %.2f, %.2f", start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y);
plan.clear();
costmap_ = costmap_ros_->getCostmap();
if(goal.header.frame_id != costmap_ros_->getGlobalFrameID()){
ROS_ERROR("This planner as configured will only accept goals in the %s frame, but a goal was sent in the %s frame.",
costmap_ros_->getGlobalFrameID().c_str(), goal.header.frame_id.c_str());
return false;
}
const double start_yaw = tf2::getYaw(start.pose.orientation);
const double goal_yaw = tf2::getYaw(goal.pose.orientation);
//we want to step back along the vector created by the robot's position and the goal pose until we find a legal cell
double goal_x = goal.pose.position.x;
double goal_y = goal.pose.position.y;
double start_x = start.pose.position.x;
double start_y = start.pose.position.y;
double diff_x = goal_x - start_x;
double diff_y = goal_y - start_y;
double diff_yaw = angles::normalize_angle(goal_yaw-start_yaw);
double target_x = goal_x;
double target_y = goal_y;
double target_yaw = goal_yaw;
bool done = false;
double scale = 1.0;
double dScale = 0.01;
while(!done)
{
if(scale < 0)
{
target_x = start_x;
target_y = start_y;
target_yaw = start_yaw;
ROS_WARN("The carrot planner could not find a valid plan for this goal");
break;
}
target_x = start_x + scale * diff_x;
target_y = start_y + scale * diff_y;
target_yaw = angles::normalize_angle(start_yaw + scale * diff_yaw);
double footprint_cost = footprintCost(target_x, target_y, target_yaw);
if(footprint_cost >= 0)
{
done = true;
}
scale -=dScale;
}
plan.push_back(start);
geometry_msgs::PoseStamped new_goal = goal;
tf2::Quaternion goal_quat;
goal_quat.setRPY(0, 0, target_yaw);
new_goal.pose.position.x = target_x;
new_goal.pose.position.y = target_y;
new_goal.pose.orientation.x = goal_quat.x();
new_goal.pose.orientation.y = goal_quat.y();
new_goal.pose.orientation.z = goal_quat.z();
new_goal.pose.orientation.w = goal_quat.w();
plan.push_back(new_goal);
return (done);
}
};