git commit -m "first commit"
This commit is contained in:
198
navigations/move_base/CHANGELOG.rst
Executable file
198
navigations/move_base/CHANGELOG.rst
Executable file
@@ -0,0 +1,198 @@
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
Changelog for package move_base
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
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)
|
||||
-------------------
|
||||
* Check if stamp of transformed pose is non-zero (`#1200 <https://github.com/ros-planning/navigation/issues/1200>`_)
|
||||
* Create move_base catkin library (`#1116 <https://github.com/ros-planning/navigation/issues/1116>`_)
|
||||
Co-authored-by: vkotaru <prasant.kotaru@gmail.com>
|
||||
* move_base crash when using default recovery behaviors (`#1071 <https://github.com/ros-planning/navigation/issues/1071>`_)
|
||||
* Publish recovery behavior melodic (`#1051 <https://github.com/ros-planning/navigation/issues/1051>`_) (`#1052 <https://github.com/ros-planning/navigation/issues/1052>`_)
|
||||
* First prototype of publishing recovery status
|
||||
custom message tells you where the behavior occured, the name, index,
|
||||
and total number of behaviors.
|
||||
* fix message field typos
|
||||
Co-authored-by: Peter Mitrano <mitranopeter@gmail.com>
|
||||
Co-authored-by: Peter Mitrano <mitranopeter@gmail.com>
|
||||
* Contributors: David V. Lu!!, Prasanth Kotaru, Yuki Furuta, mattp256
|
||||
|
||||
1.17.1 (2020-08-27)
|
||||
-------------------
|
||||
* Fix `#933 <https://github.com/ros-planning/navigation/issues/933>`_ (`#988 <https://github.com/ros-planning/navigation/issues/988>`_)
|
||||
* Contributors: David V. Lu!!
|
||||
|
||||
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
|
||||
* move_base: Add options for make_plan service (`#981 <https://github.com/ros-planning/navigation/issues/981>`_)
|
||||
Adds the following two parameters for the ~make_plan service:
|
||||
1. make_plan_clear_costmap
|
||||
Whether or not to clear the global costmap on make_plan service call.
|
||||
2. make_plan_add_unreachable_goal
|
||||
Whether or not to add the original goal to the path if it is unreachable in the make_plan service call.
|
||||
* Contributors: Michael Ferguson, nxdefiant
|
||||
|
||||
1.16.6 (2020-03-18)
|
||||
-------------------
|
||||
|
||||
1.16.5 (2020-03-15)
|
||||
-------------------
|
||||
|
||||
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
|
||||
* Added publishZeroVelocity() before starting planner (`#751 <https://github.com/ros-planning/navigation/issues/751>`_)
|
||||
Edit for Issue `#750 <https://github.com/ros-planning/navigation/issues/750>`_
|
||||
* Merge pull request `#831 <https://github.com/ros-planning/navigation/issues/831>`_ from ros-planning/feature/remove_slashes
|
||||
[melodic] Remove leading slashes from default frame_id parameters
|
||||
* Remove leading slashes from default frame_id parameters
|
||||
* Contributors: David V. Lu, Michael Ferguson, SUNIL SULANIA, Steven Macenski
|
||||
|
||||
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>`_
|
||||
* Merge pull request `#723 <https://github.com/ros-planning/navigation/issues/723>`_ from moriarty/melodic-buildfarm-errors
|
||||
Melodic buildfarm errors
|
||||
* Merge pull request `#719 <https://github.com/ros-planning/navigation/issues/719>`_ from ros-planning/lunar_711
|
||||
adding mutex locks to costmap clearing service
|
||||
* Contributors: Alexander Moriarty, Michael Ferguson, Vincent Rabaud, stevemacenski
|
||||
|
||||
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)
|
||||
-------------------
|
||||
* Add a max_planning_retries parameter to move_base [kinetic] (`#539 <https://github.com/ros-planning/navigation/issues/539>`_)
|
||||
* Fix for `#517 <https://github.com/ros-planning/navigation/issues/517>`_: create a getRobotPose method on move_base instead of using that on the costmaps
|
||||
* Fixed deadlock when changing global planner
|
||||
* rebase fixups
|
||||
* convert packages to format2
|
||||
* Fix CMakeLists + package.xmls (`#548 <https://github.com/ros-planning/navigation/issues/548>`_)
|
||||
* Added deps to amcl costmap_2d move_base (`#512 <https://github.com/ros-planning/navigation/issues/512>`_)
|
||||
* Fix CMake warnings
|
||||
* move_base: Add move_base_msgs to find_package.
|
||||
* Contributors: Jorge Santos, Jorge Santos Simón, Maarten de Vries, Martin Günther, Mikael Arguedas, Vincent Rabaud, mryellow, ne0
|
||||
|
||||
1.14.0 (2016-05-20)
|
||||
-------------------
|
||||
|
||||
1.13.1 (2015-10-29)
|
||||
-------------------
|
||||
* Removes installation of nonexistent directories
|
||||
* use correct size for clearing window
|
||||
* full name has been required for eons, this code just adds unneeded complexity
|
||||
* remove ancient conversion scripts from v0.2 to v0.3
|
||||
* proper locking during costmap update
|
||||
* Contributors: Michael Ferguson, Thiago de Freitas Oliveira Araujo
|
||||
|
||||
1.13.0 (2015-03-17)
|
||||
-------------------
|
||||
* Fixing various memory freeing operations
|
||||
* Contributors: Alex Bencz
|
||||
|
||||
1.12.0 (2015-02-04)
|
||||
-------------------
|
||||
* update maintainer email
|
||||
* Contributors: Michael Ferguson
|
||||
|
||||
1.11.15 (2015-02-03)
|
||||
--------------------
|
||||
* Disable global planner when resetting state.
|
||||
* Mark move_base headers for installation
|
||||
* Add ARCHIVE DESTINATION for move_base
|
||||
* Break infinite loop when tolerance 0 is used
|
||||
* remove partial usage of <tab> in the code
|
||||
* Contributors: Gary Servin, Michael Ferguson, ohendriks, v4hn
|
||||
|
||||
1.11.14 (2014-12-05)
|
||||
--------------------
|
||||
* use timer rather than rate for immediate wakeup
|
||||
* adding lock to planner makePlan fail case
|
||||
* Contributors: Michael Ferguson, phil0stine
|
||||
|
||||
1.11.13 (2014-10-02)
|
||||
--------------------
|
||||
|
||||
1.11.12 (2014-10-01)
|
||||
--------------------
|
||||
|
||||
1.11.11 (2014-07-23)
|
||||
--------------------
|
||||
* removes trailing spaces and empty lines
|
||||
* Contributors: Enrique Fernández Perdomo
|
||||
|
||||
1.11.10 (2014-06-25)
|
||||
--------------------
|
||||
* Remove unnecessary colons
|
||||
* move_base planService now searches out from desired goal
|
||||
* Contributors: David Lu!!, Kaijen Hsiao
|
||||
|
||||
1.11.9 (2014-06-10)
|
||||
-------------------
|
||||
* uses ::hypot(x, y) instead of sqrt(x*x, y*y)
|
||||
* Contributors: Enrique Fernández Perdomo
|
||||
|
||||
1.11.8 (2014-05-21)
|
||||
-------------------
|
||||
|
||||
1.11.7 (2014-05-21)
|
||||
-------------------
|
||||
* update build to find eigen using cmake_modules
|
||||
* Fix classloader warnings on exit of move_base
|
||||
* Contributors: Michael Ferguson
|
||||
|
||||
1.11.4 (2013-09-27)
|
||||
-------------------
|
||||
* Package URL Updates
|
||||
* Reintroduce ClearCostmaps Service
|
||||
* Add dependencies to recovery behaviors.
|
||||
86
navigations/move_base/CMakeLists.txt
Executable file
86
navigations/move_base/CMakeLists.txt
Executable file
@@ -0,0 +1,86 @@
|
||||
cmake_minimum_required(VERSION 3.0.2)
|
||||
project(move_base)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
actionlib
|
||||
base_local_planner
|
||||
clear_costmap_recovery
|
||||
cmake_modules
|
||||
costmap_2d
|
||||
dynamic_reconfigure
|
||||
geometry_msgs
|
||||
message_generation
|
||||
move_base_msgs
|
||||
nav_core
|
||||
nav_msgs
|
||||
navfn
|
||||
pluginlib
|
||||
roscpp
|
||||
rospy
|
||||
rotate_recovery
|
||||
std_srvs
|
||||
tf2_geometry_msgs
|
||||
tf2_ros
|
||||
)
|
||||
find_package(Eigen3 REQUIRED)
|
||||
add_definitions(${EIGEN3_DEFINITIONS})
|
||||
|
||||
# dynamic reconfigure
|
||||
generate_dynamic_reconfigure_options(
|
||||
cfg/MoveBase.cfg
|
||||
)
|
||||
|
||||
catkin_package(
|
||||
|
||||
INCLUDE_DIRS
|
||||
include
|
||||
${EIGEN3_INCLUDE_DIRS}
|
||||
LIBRARIES move_base
|
||||
CATKIN_DEPENDS
|
||||
dynamic_reconfigure
|
||||
geometry_msgs
|
||||
move_base_msgs
|
||||
nav_msgs
|
||||
roscpp
|
||||
)
|
||||
|
||||
include_directories(
|
||||
include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
${EIGEN3_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
# move_base
|
||||
add_library(move_base
|
||||
src/move_base.cpp
|
||||
)
|
||||
target_link_libraries(move_base
|
||||
${Boost_LIBRARIES}
|
||||
${catkin_LIBRARIES}
|
||||
)
|
||||
add_dependencies(move_base ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
add_executable(move_base_node
|
||||
src/move_base_node.cpp
|
||||
)
|
||||
add_dependencies(move_base_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
target_link_libraries(move_base_node move_base)
|
||||
set_target_properties(move_base_node PROPERTIES OUTPUT_NAME move_base)
|
||||
|
||||
install(
|
||||
TARGETS
|
||||
move_base_node
|
||||
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
|
||||
install(
|
||||
TARGETS
|
||||
move_base
|
||||
DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
)
|
||||
|
||||
## Mark cpp header files for installation
|
||||
install(DIRECTORY include/${PROJECT_NAME}/
|
||||
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
FILES_MATCHING PATTERN "*.h"
|
||||
)
|
||||
33
navigations/move_base/cfg/MoveBase.cfg
Executable file
33
navigations/move_base/cfg/MoveBase.cfg
Executable file
@@ -0,0 +1,33 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
PACKAGE = 'move_base'
|
||||
|
||||
from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, str_t, double_t, bool_t, int_t
|
||||
|
||||
gen = ParameterGenerator()
|
||||
|
||||
gen.add("base_global_planner", str_t, 0, "The name of the plugin for the global planner to use with move_base.", "navfn/NavfnROS")
|
||||
gen.add("base_local_planner", str_t, 0, "The name of the plugin for the local planner to use with move_base.", "base_local_planner/TrajectoryPlannerROS")
|
||||
|
||||
#gen.add("recovery_behaviors", str_t, 0, "A list of recovery behavior plugins to use with move_base.", "[{name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery}, {name: rotate_recovery, type: rotate_recovery/RotateRecovery}, {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery}]")
|
||||
|
||||
gen.add("planner_frequency", double_t, 0, "The rate in Hz at which to run the planning loop.", 0, 0, 100)
|
||||
gen.add("controller_frequency", double_t, 0, "The rate in Hz at which to run the control loop and send velocity commands to the base.", 20, 0, 100)
|
||||
gen.add("planner_patience", double_t, 0, "How long the planner will wait in seconds in an attempt to find a valid plan before space-clearing operations are performed.", 5.0, 0, 100)
|
||||
gen.add("controller_patience", double_t, 0, "How long the controller will wait in seconds without receiving a valid control before space-clearing operations are performed.", 5.0, 0, 100)
|
||||
gen.add("max_planning_retries", int_t, 0, "How many times we will recall the planner in an attempt to find a valid plan before space-clearing operations are performed", -1, -1, 1000)
|
||||
gen.add("conservative_reset_dist", double_t, 0, "The distance away from the robot in meters at which obstacles will be cleared from the costmap when attempting to clear space in the map.", 3, 0, 50)
|
||||
|
||||
gen.add("recovery_behavior_enabled", bool_t, 0, "Whether or not to enable the move_base recovery behaviors to attempt to clear out space.", True)
|
||||
# Doesnt exist
|
||||
gen.add("clearing_rotation_allowed", bool_t, 0, "Determines whether or not the robot will attempt an in-place rotation when attempting to clear out space.", True)
|
||||
gen.add("shutdown_costmaps", bool_t, 0, "Determines whether or not to shutdown the costmaps of the node when move_base is in an inactive state", False)
|
||||
|
||||
gen.add("oscillation_timeout", double_t, 0, "How long in seconds to allow for oscillation before executing recovery behaviors.", 0.0, 0, 60)
|
||||
gen.add("oscillation_distance", double_t, 0, "How far in meters the robot must move to be considered not to be oscillating.", 0.5, 0, 10)
|
||||
|
||||
gen.add("make_plan_clear_costmap", bool_t, 0, "Whether or not to clear the global costmap on make_plan service call.", True)
|
||||
gen.add("make_plan_add_unreachable_goal", bool_t, 0, "Whether or not to add the original goal to the path if it is unreachable in the make_plan service call.", True)
|
||||
|
||||
gen.add("restore_defaults", bool_t, 0, "Restore to the original configuration", False)
|
||||
exit(gen.generate(PACKAGE, "move_base_node", "MoveBase"))
|
||||
958
navigations/move_base/include/move_base/Structor.drawio
Executable file
958
navigations/move_base/include/move_base/Structor.drawio
Executable file
File diff suppressed because one or more lines are too long
243
navigations/move_base/include/move_base/move_base.h
Executable file
243
navigations/move_base/include/move_base/move_base.h
Executable file
@@ -0,0 +1,243 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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 the Willow Garage 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 NAV_MOVE_BASE_ACTION_H_
|
||||
#define NAV_MOVE_BASE_ACTION_H_
|
||||
|
||||
#include <vector>
|
||||
#include <string>
|
||||
|
||||
#include <ros/ros.h>
|
||||
|
||||
#include <actionlib/server/simple_action_server.h>
|
||||
#include <move_base_msgs/MoveBaseAction.h>
|
||||
|
||||
#include <nav_core/base_local_planner.h>
|
||||
#include <nav_core/base_global_planner.h>
|
||||
#include <nav_core/recovery_behavior.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <costmap_2d/costmap_2d_ros.h>
|
||||
#include <costmap_2d/costmap_2d.h>
|
||||
#include <costmap_2d/directional_layer.h>
|
||||
#include <nav_msgs/GetPlan.h>
|
||||
|
||||
#include <pluginlib/class_loader.hpp>
|
||||
#include <std_srvs/Empty.h>
|
||||
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
#include "move_base/MoveBaseConfig.h"
|
||||
|
||||
namespace move_base
|
||||
{
|
||||
// typedefs to help us out with the action server so that we don't hace to type so much
|
||||
typedef actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> MoveBaseActionServer;
|
||||
|
||||
enum MoveBaseState
|
||||
{
|
||||
PLANNING,
|
||||
CONTROLLING,
|
||||
CLEARING
|
||||
};
|
||||
|
||||
enum RecoveryTrigger
|
||||
{
|
||||
PLANNING_R,
|
||||
CONTROLLING_R,
|
||||
OSCILLATION_R
|
||||
};
|
||||
|
||||
/**
|
||||
* @class MoveBase
|
||||
* @brief A class that uses the actionlib::ActionServer interface that moves the robot base to a goal location.
|
||||
*/
|
||||
class MoveBase
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor for the actions
|
||||
* @param name The name of the action
|
||||
* @param tf A reference to a TransformListener
|
||||
*/
|
||||
MoveBase(tf2_ros::Buffer &tf);
|
||||
|
||||
/**
|
||||
* @brief Destructor - Cleans up
|
||||
*/
|
||||
virtual ~MoveBase();
|
||||
|
||||
/**
|
||||
* @brief Performs a control cycle
|
||||
* @param goal A reference to the goal to pursue
|
||||
* @return True if processing of the goal is done, false otherwise
|
||||
*/
|
||||
bool executeCycle(geometry_msgs::PoseStamped &goal);
|
||||
|
||||
private:
|
||||
void clearDirectionalCostmap();
|
||||
/**
|
||||
* @brief A service call that clears the costmaps of obstacles
|
||||
* @param req The service request
|
||||
* @param resp The service response
|
||||
* @return True if the service call succeeds, false otherwise
|
||||
*/
|
||||
bool clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp);
|
||||
|
||||
/**
|
||||
* @brief A service call that can be made when the action is inactive that will return a plan
|
||||
* @param req The goal request
|
||||
* @param resp The plan request
|
||||
* @return True if planning succeeded, false otherwise
|
||||
*/
|
||||
bool planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp);
|
||||
|
||||
/**
|
||||
* @brief Make a new global plan
|
||||
* @param goal The goal to plan to
|
||||
* @param plan Will be filled in with the plan made by the planner
|
||||
* @return True if planning succeeds, false otherwise
|
||||
*/
|
||||
bool makePlan(const geometry_msgs::PoseStamped &goal, std::vector<geometry_msgs::PoseStamped> &plan);
|
||||
|
||||
/**
|
||||
* @brief Load the recovery behaviors for the navigation stack from the parameter server
|
||||
* @param node The ros::NodeHandle to be used for loading parameters
|
||||
* @return True if the recovery behaviors were loaded successfully, false otherwise
|
||||
*/
|
||||
bool loadRecoveryBehaviors(ros::NodeHandle node);
|
||||
|
||||
/**
|
||||
* @brief Loads the default recovery behaviors for the navigation stack
|
||||
*/
|
||||
void loadDefaultRecoveryBehaviors();
|
||||
|
||||
/**
|
||||
* @brief Clears obstacles within a window around the robot
|
||||
* @param size_x The x size of the window
|
||||
* @param size_y The y size of the window
|
||||
*/
|
||||
void clearCostmapWindows(double size_x, double size_y);
|
||||
|
||||
/**
|
||||
* @brief Publishes a velocity command of zero to the base
|
||||
*/
|
||||
void publishZeroVelocity();
|
||||
|
||||
/**
|
||||
* @brief Reset the state of the move_base action and send a zero velocity command to the base
|
||||
*/
|
||||
void resetState();
|
||||
|
||||
void goalCB(const geometry_msgs::PoseStamped::ConstPtr &goal);
|
||||
|
||||
void planThread();
|
||||
|
||||
void executeCb(const move_base_msgs::MoveBaseGoalConstPtr &move_base_goal);
|
||||
|
||||
bool isQuaternionValid(const geometry_msgs::Quaternion &q);
|
||||
|
||||
bool getRobotPose(geometry_msgs::PoseStamped &global_pose, costmap_2d::Costmap2DROS *costmap);
|
||||
|
||||
double distance(const geometry_msgs::PoseStamped &p1, const geometry_msgs::PoseStamped &p2);
|
||||
|
||||
geometry_msgs::PoseStamped goalToGlobalFrame(const geometry_msgs::PoseStamped &goal_pose_msg);
|
||||
|
||||
/**
|
||||
* @brief This is used to wake the planner at periodic intervals.
|
||||
*/
|
||||
void wakePlanner(const ros::TimerEvent &event);
|
||||
|
||||
tf2_ros::Buffer &tf_;
|
||||
|
||||
MoveBaseActionServer *as_;
|
||||
|
||||
boost::shared_ptr<nav_core::BaseLocalPlanner> tc_;
|
||||
costmap_2d::Costmap2DROS *planner_costmap_ros_;
|
||||
|
||||
boost::shared_ptr<nav_core::BaseGlobalPlanner> planner_;
|
||||
costmap_2d::Costmap2DROS *controller_costmap_ros_;
|
||||
std::string robot_base_frame_, global_frame_;
|
||||
|
||||
std::vector<boost::shared_ptr<nav_core::RecoveryBehavior>> recovery_behaviors_;
|
||||
std::vector<std::string> recovery_behavior_names_;
|
||||
unsigned int recovery_index_;
|
||||
|
||||
geometry_msgs::PoseStamped global_pose_;
|
||||
double planner_frequency_, controller_frequency_, inscribed_radius_, circumscribed_radius_;
|
||||
double planner_patience_, controller_patience_;
|
||||
int32_t max_planning_retries_;
|
||||
uint32_t planning_retries_;
|
||||
double conservative_reset_dist_, clearing_radius_;
|
||||
ros::Publisher current_goal_pub_, vel_pub_, action_goal_pub_, recovery_status_pub_;
|
||||
ros::Subscriber goal_sub_;
|
||||
ros::ServiceServer make_plan_srv_, clear_costmaps_srv_;
|
||||
bool shutdown_costmaps_, clearing_rotation_allowed_, recovery_behavior_enabled_;
|
||||
bool make_plan_clear_costmap_, make_plan_add_unreachable_goal_;
|
||||
double oscillation_timeout_, oscillation_distance_;
|
||||
|
||||
MoveBaseState state_;
|
||||
RecoveryTrigger recovery_trigger_;
|
||||
|
||||
ros::Time last_valid_plan_, last_valid_control_, last_oscillation_reset_;
|
||||
geometry_msgs::PoseStamped oscillation_pose_;
|
||||
pluginlib::ClassLoader<nav_core::BaseGlobalPlanner> bgp_loader_;
|
||||
pluginlib::ClassLoader<nav_core::BaseLocalPlanner> blp_loader_;
|
||||
pluginlib::ClassLoader<nav_core::RecoveryBehavior> recovery_loader_;
|
||||
|
||||
// set up plan triple buffer
|
||||
std::vector<geometry_msgs::PoseStamped> *planner_plan_;
|
||||
std::vector<geometry_msgs::PoseStamped> *latest_plan_;
|
||||
std::vector<geometry_msgs::PoseStamped> *controller_plan_;
|
||||
|
||||
// set up the planner's thread
|
||||
bool runPlanner_;
|
||||
boost::recursive_mutex planner_mutex_;
|
||||
boost::condition_variable_any planner_cond_;
|
||||
geometry_msgs::PoseStamped planner_goal_;
|
||||
boost::thread *planner_thread_;
|
||||
|
||||
boost::recursive_mutex configuration_mutex_;
|
||||
dynamic_reconfigure::Server<move_base::MoveBaseConfig> *dsrv_;
|
||||
|
||||
void reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level);
|
||||
|
||||
move_base::MoveBaseConfig last_config_;
|
||||
move_base::MoveBaseConfig default_config_;
|
||||
bool setup_, p_freq_change_, c_freq_change_;
|
||||
bool new_global_plan_;
|
||||
boost::shared_ptr<costmap_2d::DirectionalLayer> directional_costmap_;
|
||||
};
|
||||
};
|
||||
#endif
|
||||
120
navigations/move_base/include/move_base/move_base.puml
Executable file
120
navigations/move_base/include/move_base/move_base.puml
Executable file
@@ -0,0 +1,120 @@
|
||||
@startuml
|
||||
|
||||
class MoveBase {
|
||||
- tf_: tf2_ros::Buffer&
|
||||
- as_: MoveBaseActionServer*
|
||||
- tc_: boost::shared_ptr<nav_core::BaseLocalPlanner>
|
||||
- planner_costmap_ros_: costmap_2d::Costmap2DROS*
|
||||
- controller_costmap_ros_: costmap_2d::Costmap2DROS*
|
||||
- planner_: boost::shared_ptr<nav_core::BaseGlobalPlanner>
|
||||
- robot_base_frame_: std::string
|
||||
- global_frame_: std::string
|
||||
- recovery_behaviors_: std::vector<boost::shared_ptr<nav_core::RecoveryBehavior> >
|
||||
- recovery_behavior_names_: std::vector<std::string>
|
||||
- recovery_index_: unsigned int
|
||||
- global_pose_: geometry_msgs::PoseStamped
|
||||
- planner_frequency_: double
|
||||
- controller_frequency_: double
|
||||
- inscribed_radius_: double
|
||||
- circumscribed_radius_: double
|
||||
- planner_patience_: double
|
||||
- controller_patience_: double
|
||||
- max_planning_retries_: int32_t
|
||||
- planning_retries_: uint32_t
|
||||
- conservative_reset_dist_: double
|
||||
- clearing_radius_: double
|
||||
- current_goal_pub_: ros::Publisher
|
||||
- vel_pub_: ros::Publisher
|
||||
- action_goal_pub_: ros::Publisher
|
||||
- recovery_status_pub_: ros::Publisher
|
||||
- goal_sub_: ros::Subscriber
|
||||
- make_plan_srv_: ros::ServiceServer
|
||||
- clear_costmaps_srv_: ros::ServiceServer
|
||||
- shutdown_costmaps_: bool
|
||||
- clearing_rotation_allowed_: bool
|
||||
- recovery_behavior_enabled_: bool
|
||||
- make_plan_clear_costmap_: bool
|
||||
- make_plan_add_unreachable_goal_: bool
|
||||
- oscillation_timeout_: double
|
||||
- oscillation_distance_: double
|
||||
- state_: MoveBaseState
|
||||
- recovery_trigger_: RecoveryTrigger
|
||||
- last_valid_plan_: ros::Time
|
||||
- last_valid_control_: ros::Time
|
||||
- last_oscillation_reset_: ros::Time
|
||||
- oscillation_pose_: geometry_msgs::PoseStamped
|
||||
- bgp_loader_: pluginlib::ClassLoader<nav_core::BaseGlobalPlanner>
|
||||
- blp_loader_: pluginlib::ClassLoader<nav_core::BaseLocalPlanner>
|
||||
- recovery_loader_: pluginlib::ClassLoader<nav_core::RecoveryBehavior>
|
||||
- planner_plan_: std::vector<geometry_msgs::PoseStamped>*
|
||||
- latest_plan_: std::vector<geometry_msgs::PoseStamped>*
|
||||
- controller_plan_: std::vector<geometry_msgs::PoseStamped>*
|
||||
- runPlanner_: bool
|
||||
- planner_mutex_: boost::recursive_mutex
|
||||
- planner_cond_: boost::condition_variable_any
|
||||
- planner_goal_: geometry_msgs::PoseStamped
|
||||
- planner_thread_: boost::thread*
|
||||
- configuration_mutex_: boost::recursive_mutex
|
||||
- dsrv_: dynamic_reconfigure::Server<move_base::MoveBaseConfig>*
|
||||
- last_config_: move_base::MoveBaseConfig
|
||||
- default_config_: move_base::MoveBaseConfig
|
||||
- setup_: bool
|
||||
- p_freq_change_: bool
|
||||
- c_freq_change_: bool
|
||||
- new_global_plan_: bool
|
||||
+ MoveBase(tf: tf2_ros::Buffer&)
|
||||
+ ~MoveBase()
|
||||
+ executeCycle(goal: geometry_msgs::PoseStamped&): bool
|
||||
- clearCostmapsService(req: std_srvs::Empty::Request&, resp: std_srvs::Empty::Response&): bool
|
||||
- planService(req: nav_msgs::GetPlan::Request&, resp: nav_msgs::GetPlan::Response&): bool
|
||||
- makePlan(goal: const geometry_msgs::PoseStamped&, plan: std::vector<geometry_msgs::PoseStamped>&): bool
|
||||
- loadRecoveryBehaviors(node: ros::NodeHandle): bool
|
||||
- loadDefaultRecoveryBehaviors(): void
|
||||
- clearCostmapWindows(size_x: double, size_y: double): void
|
||||
- publishZeroVelocity(): void
|
||||
- resetState(): void
|
||||
- goalCB(goal: const geometry_msgs::PoseStamped::ConstPtr&): void
|
||||
- planThread(): void
|
||||
- executeCb(move_base_goal: const move_base_msgs::MoveBaseGoalConstPtr&): void
|
||||
- isQuaternionValid(q: const geometry_msgs::Quaternion&): bool
|
||||
- getRobotPose(global_pose: geometry_msgs::PoseStamped&, costmap: costmap_2d::Costmap2DROS*): bool
|
||||
- distance(p1: const geometry_msgs::PoseStamped&, p2: const geometry_msgs::PoseStamped&): double
|
||||
- goalToGlobalFrame(goal_pose_msg: const geometry_msgs::PoseStamped&): geometry_msgs::PoseStamped
|
||||
- wakePlanner(event: const ros::TimerEvent&): void
|
||||
- reconfigureCB(config: move_base::MoveBaseConfig, level: uint32_t): void
|
||||
}
|
||||
|
||||
enum MoveBaseState {
|
||||
PLANNING
|
||||
CONTROLLING
|
||||
CLEARING
|
||||
}
|
||||
|
||||
enum RecoveryTrigger {
|
||||
PLANNING_R
|
||||
CONTROLLING_R
|
||||
OSCILLATION_R
|
||||
}
|
||||
|
||||
MoveBase --> tf2_ros::Buffer
|
||||
MoveBase --> MoveBaseActionServer
|
||||
MoveBase --> boost::shared_ptr<nav_core::BaseLocalPlanner>
|
||||
MoveBase --> costmap_2d::Costmap2DROS
|
||||
MoveBase --> std::vector<boost::shared_ptr<nav_core::RecoveryBehavior> >
|
||||
MoveBase --> std::vector<std::string>
|
||||
MoveBase --> geometry_msgs::PoseStamped
|
||||
MoveBase --> ros::Publisher
|
||||
MoveBase --> ros::Subscriber
|
||||
MoveBase --> ros::ServiceServer
|
||||
MoveBase --> ros::Time
|
||||
MoveBase --> pluginlib::ClassLoader<nav_core::BaseGlobalPlanner>
|
||||
MoveBase --> pluginlib::ClassLoader<nav_core::BaseLocalPlanner>
|
||||
MoveBase --> pluginlib::ClassLoader<nav_core::RecoveryBehavior>
|
||||
MoveBase --> std::vector<geometry_msgs::PoseStamped>
|
||||
MoveBase --> boost::recursive_mutex
|
||||
MoveBase --> boost::condition_variable_any
|
||||
MoveBase --> boost::thread
|
||||
MoveBase --> dynamic_reconfigure::Server<move_base::MoveBaseConfig>
|
||||
MoveBase --> move_base::MoveBaseConfig
|
||||
|
||||
@enduml
|
||||
69
navigations/move_base/package.xml
Executable file
69
navigations/move_base/package.xml
Executable file
@@ -0,0 +1,69 @@
|
||||
<?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_base</name>
|
||||
<version>1.17.3</version>
|
||||
<description>
|
||||
|
||||
The move_base package provides an implementation of an action (see the <a href="http://www.ros.org/wiki/actionlib">actionlib</a> package) that, given a goal in the world, will attempt to reach it with a mobile base. The move_base node links together a global and local planner to accomplish its global navigation task. It supports any global planner adhering to the nav_core::BaseGlobalPlanner interface specified in the <a href="http://www.ros.org/wiki/nav_core">nav_core</a> package and any local planner adhering to the nav_core::BaseLocalPlanner interface specified in the <a href="http://www.ros.org/wiki/nav_core">nav_core</a> package. The move_base node also maintains two costmaps, one for the global planner, and one for a local planner (see the <a href="http://www.ros.org/wiki/costmap_2d">costmap_2d</a> package) that are used to accomplish navigation tasks.
|
||||
|
||||
</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_base</url>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<build_depend>cmake_modules</build_depend>
|
||||
<build_depend>message_generation</build_depend>
|
||||
<build_depend>tf2_geometry_msgs</build_depend>
|
||||
|
||||
<depend>actionlib</depend>
|
||||
|
||||
<build_depend>costmap_2d</build_depend>
|
||||
<exec_depend>costmap_2d</exec_depend>
|
||||
|
||||
<depend>dynamic_reconfigure</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
|
||||
<build_depend>move_base_msgs</build_depend>
|
||||
<exec_depend>move_base_msgs</exec_depend>
|
||||
|
||||
<build_depend>nav_core</build_depend>
|
||||
<exec_depend>nav_core</exec_depend>
|
||||
|
||||
<build_depend>nav_msgs</build_depend>
|
||||
<exec_depend>nav_msgs</exec_depend>
|
||||
|
||||
<build_depend>pluginlib</build_depend>
|
||||
<exec_depend>pluginlib</exec_depend>
|
||||
|
||||
<depend>roscpp</depend>
|
||||
<depend>rospy</depend>
|
||||
<depend>std_srvs</depend>
|
||||
|
||||
<build_depend>tf2_ros</build_depend>
|
||||
<exec_depend>tf2_ros</exec_depend>
|
||||
|
||||
<depend>visualization_msgs</depend>
|
||||
|
||||
<!--These deps aren't strictly needed, but given the default parameters require them to work, we'll enforce that they build -->
|
||||
<build_depend>base_local_planner</build_depend>
|
||||
<exec_depend>base_local_planner</exec_depend>
|
||||
|
||||
<build_depend>clear_costmap_recovery</build_depend>
|
||||
<exec_depend>clear_costmap_recovery</exec_depend>
|
||||
|
||||
<build_depend>navfn</build_depend>
|
||||
<exec_depend>navfn</exec_depend>
|
||||
|
||||
<build_depend>rotate_recovery</build_depend>
|
||||
<exec_depend>rotate_recovery</exec_depend>
|
||||
|
||||
<exec_depend>message_runtime</exec_depend>
|
||||
|
||||
</package>
|
||||
12
navigations/move_base/planner_test.xml
Executable file
12
navigations/move_base/planner_test.xml
Executable file
@@ -0,0 +1,12 @@
|
||||
<launch>
|
||||
<master auto="start"/>
|
||||
<group name="wg">
|
||||
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full-0.05.pgm 0.05" respawn="false" />
|
||||
<node pkg="stage" type="stageros" name="stageros" args="$(find 2dnav_stage)/worlds/willow-pr2-5cm.world" respawn="false" >
|
||||
<param name="base_watchdog_timeout" value="0.2"/>
|
||||
</node>
|
||||
<node pkg="fake_localization" type="fake_localization" respawn="false" />
|
||||
<node pkg="nav_view" type="nav_view" respawn="false"/>
|
||||
<node pkg="nav" type="planner_test" output="screen" />
|
||||
</group>
|
||||
</launch>
|
||||
1430
navigations/move_base/src/move_base.cpp
Executable file
1430
navigations/move_base/src/move_base.cpp
Executable file
File diff suppressed because it is too large
Load Diff
44
navigations/move_base/src/move_base_node.cpp
Executable file
44
navigations/move_base/src/move_base_node.cpp
Executable file
@@ -0,0 +1,44 @@
|
||||
/*
|
||||
* Copyright (c) 2013, 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 the 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.
|
||||
*/
|
||||
|
||||
#include <move_base/move_base.h>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
|
||||
int main(int argc, char** argv){
|
||||
ros::init(argc, argv, "move_base_node");
|
||||
tf2_ros::Buffer buffer(ros::Duration(10));
|
||||
tf2_ros::TransformListener tf(buffer);
|
||||
|
||||
move_base::MoveBase move_base( buffer );
|
||||
|
||||
//ros::MultiThreadedSpinner s;
|
||||
ros::spin();
|
||||
|
||||
return(0);
|
||||
}
|
||||
Reference in New Issue
Block a user