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

163
navigations/dwa_local_planner/CHANGELOG.rst Executable file
View File

@@ -0,0 +1,163 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package dwa_local_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)
-------------------
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)
-------------------
* Fixes gdist- and pdist_scale node paramter names (`#936 <https://github.com/cobalt-robotics/navigation/issues/936>`_)
Renames goal and path distance dynamic reconfigure parameter
names in the cfg file in order to actually make the parameters
used by the trajectory planner changeable.
Fixes `#935 <https://github.com/cobalt-robotics/navigation/issues/935>`_
* Contributors: David Leins
1.16.3 (2019-11-15)
-------------------
* Set footprint before in place rotation continuation (`#829 <https://github.com/ros-planning/navigation/issues/829>`_) (`#861 <https://github.com/ros-planning/navigation/issues/861>`_)
* Make sure to call setFootprint() before an in-place rotation
* Change to const reference
* Remove footprint from findBestPath
* Contributors: David V. Lu!!
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 for dwa_local_planner
* add tf2_geometry_msgs (due to https://github.com/ros/geometry2/issues/275)
* add missing depends on angles, sensor_msgs, tf2
* Contributors: Michael Ferguson
1.16.1 (2018-07-28)
-------------------
1.16.0 (2018-07-25)
-------------------
* Merge pull request `#765 <https://github.com/ros-planning/navigation/issues/765>`_ from ros-planning/remove_pcl
remove left over PCL depends in dwa_local_planner
* Remove PCL from local planners
* Switch to TF2 `#755 <https://github.com/ros-planning/navigation/issues/755>`_
* Make trajectory scoring scales consistent.
* unify parameter names between base_local_planner and dwa_local_planner
addresses parts of `#90 <https://github.com/ros-planning/navigation/issues/90>`_
* Contributors: David V. Lu, Michael Ferguson, Pavlo Kolomiiets, Vincent Rabaud, moriarty
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
* Add cost function to prevent unnecessary spinning
* Fix CMakeLists + package.xmls (`#548 <https://github.com/ros-planning/navigation/issues/548>`_)
* import only PCL common
* remove GCC warnings
* Fix CMake warnings
* Contributors: Martin Günther, Mikael Arguedas, Morgan Quigley, Vincent Rabaud
1.14.0 (2016-05-20)
-------------------
1.13.1 (2015-10-29)
-------------------
1.13.0 (2015-03-17)
-------------------
* link only libraries found with find_package
* Contributors: Lukas Bulwahn
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.5 (2014-01-30)
-------------------
* Fix for `#5 <https://github.com/ros-planning/navigation/issues/5>`_
* Parameter to sum scores in DWA Planner
* Keep consistent allignment_cost scale
Use the configured alignment cost instead of resetting to 1.0. This
condition (farther from goal than forward_point_distance) is probably
met at the start of navigation, it seems this cost should be obeyed
until the robot gets close, then ignored completely.
* Cheat factor for end of DWA Plans
* Change maintainer from Hersh to Lu
1.11.4 (2013-09-27)
-------------------
* Package URL Updates
* Changed new Odom-Helper::initialize() function to setOdomTopic().
* some more corrections for the pointcloud object bug

67
navigations/dwa_local_planner/CMakeLists.txt Executable file
View File

@@ -0,0 +1,67 @@
cmake_minimum_required(VERSION 3.0.2)
project(dwa_local_planner)
find_package(catkin REQUIRED
COMPONENTS
angles
base_local_planner
cmake_modules
costmap_2d
dynamic_reconfigure
nav_core
nav_msgs
pluginlib
sensor_msgs
roscpp
tf2
tf2_geometry_msgs
tf2_ros
)
find_package(Eigen3 REQUIRED)
remove_definitions(-DDISABLE_LIBUSB-1.0)
include_directories(
include
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
)
add_definitions(${EIGEN3_DEFINITIONS})
# dynamic reconfigure
generate_dynamic_reconfigure_options(
cfg/DWAPlanner.cfg
)
catkin_package(
INCLUDE_DIRS include
LIBRARIES dwa_local_planner
CATKIN_DEPENDS
base_local_planner
dynamic_reconfigure
nav_msgs
pluginlib
sensor_msgs
roscpp
tf2
tf2_ros
)
add_library(dwa_local_planner src/dwa_planner.cpp src/dwa_planner_ros.cpp)
add_dependencies(dwa_local_planner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(dwa_local_planner ${catkin_LIBRARIES})
target_compile_options(dwa_local_planner PUBLIC "-Wno-terminate")
install(TARGETS dwa_local_planner
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)
install(FILES blp_plugin.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
PATTERN ".svn" EXCLUDE
)

7
navigations/dwa_local_planner/blp_plugin.xml Executable file
View File

@@ -0,0 +1,7 @@
<library path="lib/libdwa_local_planner">
<class name="dwa_local_planner/DWAPlannerROS" type="dwa_local_planner::DWAPlannerROS" base_class_type="nav_core::BaseLocalPlanner">
<description>
A implementation of a local planner using either a DWA approach based on configuration parameters.
</description>
</class>
</library>

39
navigations/dwa_local_planner/cfg/DWAPlanner.cfg Executable file
View File

@@ -0,0 +1,39 @@
#!/usr/bin/env python
# DWA Planner configuration
from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t, int_t, bool_t
from local_planner_limits import add_generic_localplanner_params
gen = ParameterGenerator()
# This unusual line allows to reuse existing parameter definitions
# that concern all localplanners
add_generic_localplanner_params(gen)
gen.add("sim_time", double_t, 0, "The amount of time to roll trajectories out for in seconds", 1.7, 0)
gen.add("sim_granularity", double_t, 0, "The granularity with which to check for collisions along each trajectory in meters", 0.025, 0)
gen.add("angular_sim_granularity", double_t, 0, "The granularity with which to check for collisions for rotations in radians", 0.1, 0)
gen.add("path_distance_bias", double_t, 0, "The weight for the path distance part of the cost function", 0.6, 0.0)
gen.add("goal_distance_bias", double_t, 0, "The weight for the goal distance part of the cost function", 0.8, 0.0)
gen.add("occdist_scale", double_t, 0, "The weight for the obstacle distance part of the cost function", 0.01, 0.0)
gen.add("twirling_scale", double_t, 0, "The weight for penalizing any changes in robot heading", 0.0, 0.0)
gen.add("stop_time_buffer", double_t, 0, "The amount of time that the robot must stop before a collision in order for a trajectory to be considered valid in seconds", 0.2, 0)
gen.add("oscillation_reset_dist", double_t, 0, "The distance the robot must travel before oscillation flags are reset, in meters", 0.05, 0)
gen.add("oscillation_reset_angle", double_t, 0, "The angle the robot must turn before oscillation flags are reset, in radians", 0.2, 0)
gen.add("forward_point_distance", double_t, 0, "The distance from the center point of the robot to place an additional scoring point, in meters", 0.325)
gen.add("scaling_speed", double_t, 0, "The absolute value of the velocity at which to start scaling the robot's footprint, in m/s", 0.25, 0)
gen.add("max_scaling_factor", double_t, 0, "The maximum factor to scale the robot's footprint by", 0.2, 0)
gen.add("vx_samples", int_t, 0, "The number of samples to use when exploring the x velocity space", 3, 1)
gen.add("vy_samples", int_t, 0, "The number of samples to use when exploring the y velocity space", 10, 1)
gen.add("vth_samples", int_t, 0, "The number of samples to use when exploring the theta velocity space", 20, 1)
gen.add("use_dwa", bool_t, 0, "Use dynamic window approach to constrain sampling velocities to small window.", True)
gen.add("restore_defaults", bool_t, 0, "Restore to the original configuration.", False)
exit(gen.generate("dwa_local_planner", "dwa_local_planner", "DWAPlanner"))

185
navigations/dwa_local_planner/include/dwa_local_planner/dwa_planner.h Executable file
View File

@@ -0,0 +1,185 @@
/*********************************************************************
*
* 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 DWA_LOCAL_PLANNER_DWA_PLANNER_H_
#define DWA_LOCAL_PLANNER_DWA_PLANNER_H_
#include <vector>
#include <Eigen/Core>
#include <dwa_local_planner/DWAPlannerConfig.h>
//for creating a local cost grid
#include <base_local_planner/map_grid_visualizer.h>
//for obstacle data access
#include <costmap_2d/costmap_2d.h>
#include <base_local_planner/trajectory.h>
#include <base_local_planner/local_planner_limits.h>
#include <base_local_planner/local_planner_util.h>
#include <base_local_planner/simple_trajectory_generator.h>
#include <base_local_planner/oscillation_cost_function.h>
#include <base_local_planner/map_grid_cost_function.h>
#include <base_local_planner/obstacle_cost_function.h>
#include <base_local_planner/twirling_cost_function.h>
#include <base_local_planner/simple_scored_sampling_planner.h>
#include <nav_msgs/Path.h>
namespace dwa_local_planner {
/**
* @class DWAPlanner
* @brief A class implementing a local planner using the Dynamic Window Approach
*/
class DWAPlanner {
public:
/**
* @brief Constructor for the planner
* @param name The name of the planner
* @param costmap_ros A pointer to the costmap instance the planner should use
* @param global_frame the frame id of the tf frame to use
*/
DWAPlanner(std::string name, base_local_planner::LocalPlannerUtil *planner_util);
/**
* @brief Reconfigures the trajectory planner
*/
void reconfigure(DWAPlannerConfig &cfg);
/**
* @brief Check if a trajectory is legal for a position/velocity pair
* @param pos The robot's position
* @param vel The robot's velocity
* @param vel_samples The desired velocity
* @return True if the trajectory is valid, false otherwise
*/
bool checkTrajectory(
const Eigen::Vector3f pos,
const Eigen::Vector3f vel,
const Eigen::Vector3f vel_samples);
/**
* @brief Given the current position and velocity of the robot, find the best trajectory to exectue
* @param global_pose The current position of the robot
* @param global_vel The current velocity of the robot
* @param drive_velocities The velocities to send to the robot base
* @return The highest scoring trajectory. A cost >= 0 means the trajectory is legal to execute.
*/
base_local_planner::Trajectory findBestPath(
const geometry_msgs::PoseStamped& global_pose,
const geometry_msgs::PoseStamped& global_vel,
geometry_msgs::PoseStamped& drive_velocities);
/**
* @brief Update the cost functions before planning
* @param global_pose The robot's current pose
* @param new_plan The new global plan
* @param footprint_spec The robot's footprint
*
* The obstacle cost function gets the footprint.
* The path and goal cost functions get the global_plan
* The alignment cost functions get a version of the global plan
* that is modified based on the global_pose
*/
void updatePlanAndLocalCosts(const geometry_msgs::PoseStamped& global_pose,
const std::vector<geometry_msgs::PoseStamped>& new_plan,
const std::vector<geometry_msgs::Point>& footprint_spec);
/**
* @brief Get the period at which the local planner is expected to run
* @return The simulation period
*/
double getSimPeriod() { return sim_period_; }
/**
* @brief Compute the components and total cost for a map grid cell
* @param cx The x coordinate of the cell in the map grid
* @param cy The y coordinate of the cell in the map grid
* @param path_cost Will be set to the path distance component of the cost function
* @param goal_cost Will be set to the goal distance component of the cost function
* @param occ_cost Will be set to the costmap value of the cell
* @param total_cost Will be set to the value of the overall cost function, taking into account the scaling parameters
* @return True if the cell is traversible and therefore a legal location for the robot to move to
*/
bool getCellCosts(int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost);
/**
* sets new plan and resets state
*/
bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);
private:
base_local_planner::LocalPlannerUtil *planner_util_;
double stop_time_buffer_; ///< @brief How long before hitting something we're going to enforce that the robot stop
double path_distance_bias_, goal_distance_bias_, occdist_scale_;
Eigen::Vector3f vsamples_;
double sim_period_;///< @brief The number of seconds to use to compute max/min vels for dwa
base_local_planner::Trajectory result_traj_;
double forward_point_distance_;
std::vector<geometry_msgs::PoseStamped> global_plan_;
boost::mutex configuration_mutex_;
std::string frame_id_;
ros::Publisher traj_cloud_pub_;
bool publish_cost_grid_pc_; ///< @brief Whether or not to build and publish a PointCloud
bool publish_traj_pc_;
double cheat_factor_;
base_local_planner::MapGridVisualizer map_viz_; ///< @brief The map grid visualizer for outputting the potential field generated by the cost function
// see constructor body for explanations
base_local_planner::SimpleTrajectoryGenerator generator_;
base_local_planner::OscillationCostFunction oscillation_costs_;
base_local_planner::ObstacleCostFunction obstacle_costs_;
base_local_planner::MapGridCostFunction path_costs_;
base_local_planner::MapGridCostFunction goal_costs_;
base_local_planner::MapGridCostFunction goal_front_costs_;
base_local_planner::MapGridCostFunction alignment_costs_;
base_local_planner::TwirlingCostFunction twirling_costs_;
base_local_planner::SimpleScoredSamplingPlanner scored_sampling_planner_;
};
};
#endif

159
navigations/dwa_local_planner/include/dwa_local_planner/dwa_planner_ros.h Executable file
View File

@@ -0,0 +1,159 @@
/*********************************************************************
*
* 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 DWA_LOCAL_PLANNER_DWA_PLANNER_ROS_H_
#define DWA_LOCAL_PLANNER_DWA_PLANNER_ROS_H_
#include <boost/shared_ptr.hpp>
#include <boost/thread.hpp>
#include <tf2_ros/buffer.h>
#include <dynamic_reconfigure/server.h>
#include <dwa_local_planner/DWAPlannerConfig.h>
#include <angles/angles.h>
#include <nav_msgs/Odometry.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <nav_core/base_local_planner.h>
#include <base_local_planner/latched_stop_rotate_controller.h>
#include <base_local_planner/odometry_helper_ros.h>
#include <dwa_local_planner/dwa_planner.h>
namespace dwa_local_planner {
/**
* @class DWAPlannerROS
* @brief ROS Wrapper for the DWAPlanner that adheres to the
* BaseLocalPlanner interface and can be used as a plugin for move_base.
*/
class DWAPlannerROS : public nav_core::BaseLocalPlanner {
public:
/**
* @brief Constructor for DWAPlannerROS wrapper
*/
DWAPlannerROS();
/**
* @brief Constructs the ros wrapper
* @param name The name to give this instance of the trajectory planner
* @param tf A pointer to a transform listener
* @param costmap The cost map to use for assigning costs to trajectories
*/
void initialize(std::string name, tf2_ros::Buffer* tf,
costmap_2d::Costmap2DROS* costmap_ros);
/**
* @brief Destructor for the wrapper
*/
~DWAPlannerROS();
/**
* @brief Given the current position, orientation, and velocity of the robot,
* compute velocity commands to send to the base
* @param cmd_vel Will be filled with the velocity command to be passed to the robot base
* @return True if a valid trajectory was found, false otherwise
*/
bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);
/**
* @brief Given the current position, orientation, and velocity of the robot,
* compute velocity commands to send to the base, using dynamic window approach
* @param cmd_vel Will be filled with the velocity command to be passed to the robot base
* @return True if a valid trajectory was found, false otherwise
*/
bool dwaComputeVelocityCommands(geometry_msgs::PoseStamped& global_pose, geometry_msgs::Twist& cmd_vel);
/**
* @brief Set the plan that the controller is following
* @param orig_global_plan The plan to pass to the controller
* @return True if the plan was updated successfully, false otherwise
*/
bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);
/**
* @brief Check if the goal pose has been achieved
* @return True if achieved, false otherwise
*/
bool isGoalReached();
bool isInitialized() {
return initialized_;
}
private:
/**
* @brief Callback to update the local planner's parameters based on dynamic reconfigure
*/
void reconfigureCB(DWAPlannerConfig &config, uint32_t level);
void publishLocalPlan(std::vector<geometry_msgs::PoseStamped>& path);
void publishGlobalPlan(std::vector<geometry_msgs::PoseStamped>& path);
tf2_ros::Buffer* tf_; ///< @brief Used for transforming point clouds
// for visualisation, publishers of global and local plan
ros::Publisher g_plan_pub_, l_plan_pub_;
base_local_planner::LocalPlannerUtil planner_util_;
boost::shared_ptr<DWAPlanner> dp_; ///< @brief The trajectory controller
costmap_2d::Costmap2DROS* costmap_ros_;
dynamic_reconfigure::Server<DWAPlannerConfig> *dsrv_;
dwa_local_planner::DWAPlannerConfig default_config_;
bool setup_;
geometry_msgs::PoseStamped current_pose_;
base_local_planner::LatchedStopRotateController latchedStopRotateController_;
bool initialized_;
base_local_planner::OdometryHelperRos odom_helper_;
std::string odom_topic_;
};
};
#endif

50
navigations/dwa_local_planner/package.xml Executable file
View File

@@ -0,0 +1,50 @@
<?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>dwa_local_planner</name>
<version>1.17.3</version>
<description>
This package provides an implementation of the Dynamic Window Approach to
local robot navigation on a plane. Given a global plan to follow and a
costmap, the local planner produces velocity commands to send to a mobile
base. This package supports any robot who's footprint can be represented as
a convex polygon or cicrle, and exposes its configuration as ROS parameters
that can be set in a launch file. The parameters for this planner are also
dynamically reconfigurable. This package's ROS wrapper adheres to the
BaseLocalPlanner interface specified in the <a href="http://wiki.ros.org/nav_core">nav_core</a> package.
</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/dwa_local_planner</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>angles</build_depend>
<build_depend>cmake_modules</build_depend>
<depend>base_local_planner</depend>
<depend>costmap_2d</depend>
<depend>dynamic_reconfigure</depend>
<depend>eigen</depend>
<depend>nav_core</depend>
<depend>nav_msgs</depend>
<depend>pluginlib</depend>
<depend>sensor_msgs</depend>
<depend>roscpp</depend>
<depend>tf2</depend>
<depend>tf2_geometry_msgs</depend>
<depend>tf2_ros</depend>
<export>
<nav_core plugin="${prefix}/blp_plugin.xml" />
</export>
</package>

394
navigations/dwa_local_planner/src/dwa_planner.cpp Executable file
View File

@@ -0,0 +1,394 @@
/*********************************************************************
*
* 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 <dwa_local_planner/dwa_planner.h>
#include <base_local_planner/goal_functions.h>
#include <cmath>
//for computing path distance
#include <queue>
#include <angles/angles.h>
#include <ros/ros.h>
#include <tf2/utils.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>
namespace dwa_local_planner {
void DWAPlanner::reconfigure(DWAPlannerConfig &config)
{
boost::mutex::scoped_lock l(configuration_mutex_);
generator_.setParameters(
config.sim_time,
config.sim_granularity,
config.angular_sim_granularity,
config.use_dwa,
sim_period_);
double resolution = planner_util_->getCostmap()->getResolution();
path_distance_bias_ = resolution * config.path_distance_bias;
// pdistscale used for both path and alignment, set forward_point_distance to zero to discard alignment
path_costs_.setScale(path_distance_bias_);
alignment_costs_.setScale(path_distance_bias_);
goal_distance_bias_ = resolution * config.goal_distance_bias;
goal_costs_.setScale(goal_distance_bias_);
goal_front_costs_.setScale(goal_distance_bias_);
occdist_scale_ = config.occdist_scale;
obstacle_costs_.setScale(occdist_scale_);
stop_time_buffer_ = config.stop_time_buffer;
oscillation_costs_.setOscillationResetDist(config.oscillation_reset_dist, config.oscillation_reset_angle);
forward_point_distance_ = config.forward_point_distance;
goal_front_costs_.setXShift(forward_point_distance_);
alignment_costs_.setXShift(forward_point_distance_);
// obstacle costs can vary due to scaling footprint feature
obstacle_costs_.setParams(config.max_vel_trans, config.max_scaling_factor, config.scaling_speed);
twirling_costs_.setScale(config.twirling_scale);
int vx_samp, vy_samp, vth_samp;
vx_samp = config.vx_samples;
vy_samp = config.vy_samples;
vth_samp = config.vth_samples;
if (vx_samp <= 0) {
ROS_WARN("You've specified that you don't want any samples in the x dimension. We'll at least assume that you want to sample one value... so we're going to set vx_samples to 1 instead");
vx_samp = 1;
config.vx_samples = vx_samp;
}
if (vy_samp <= 0) {
ROS_WARN("You've specified that you don't want any samples in the y dimension. We'll at least assume that you want to sample one value... so we're going to set vy_samples to 1 instead");
vy_samp = 1;
config.vy_samples = vy_samp;
}
if (vth_samp <= 0) {
ROS_WARN("You've specified that you don't want any samples in the th dimension. We'll at least assume that you want to sample one value... so we're going to set vth_samples to 1 instead");
vth_samp = 1;
config.vth_samples = vth_samp;
}
vsamples_[0] = vx_samp;
vsamples_[1] = vy_samp;
vsamples_[2] = vth_samp;
}
DWAPlanner::DWAPlanner(std::string name, base_local_planner::LocalPlannerUtil *planner_util) :
planner_util_(planner_util),
obstacle_costs_(planner_util->getCostmap()),
path_costs_(planner_util->getCostmap()),
goal_costs_(planner_util->getCostmap(), 0.0, 0.0, true),
goal_front_costs_(planner_util->getCostmap(), 0.0, 0.0, true),
alignment_costs_(planner_util->getCostmap())
{
ros::NodeHandle private_nh("~/" + name);
goal_front_costs_.setStopOnFailure( false );
alignment_costs_.setStopOnFailure( false );
//Assuming this planner is being run within the navigation stack, we can
//just do an upward search for the frequency at which its being run. This
//also allows the frequency to be overwritten locally.
std::string controller_frequency_param_name;
if(!private_nh.searchParam("controller_frequency", controller_frequency_param_name)) {
sim_period_ = 0.05;
} else {
double controller_frequency = 0;
private_nh.param(controller_frequency_param_name, controller_frequency, 20.0);
if(controller_frequency > 0) {
sim_period_ = 1.0 / controller_frequency;
} else {
ROS_WARN("A controller_frequency less than 0 has been set. Ignoring the parameter, assuming a rate of 20Hz");
sim_period_ = 0.05;
}
}
ROS_INFO("Sim period is set to %.2f", sim_period_);
oscillation_costs_.resetOscillationFlags();
bool sum_scores;
private_nh.param("sum_scores", sum_scores, false);
obstacle_costs_.setSumScores(sum_scores);
private_nh.param("publish_cost_grid_pc", publish_cost_grid_pc_, false);
map_viz_.initialize(name,
planner_util->getGlobalFrame(),
[this](int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost){
return getCellCosts(cx, cy, path_cost, goal_cost, occ_cost, total_cost);
});
private_nh.param("global_frame_id", frame_id_, std::string("odom"));
traj_cloud_pub_ = private_nh.advertise<sensor_msgs::PointCloud2>("trajectory_cloud", 1);
private_nh.param("publish_traj_pc", publish_traj_pc_, false);
// set up all the cost functions that will be applied in order
// (any function returning negative values will abort scoring, so the order can improve performance)
std::vector<base_local_planner::TrajectoryCostFunction*> critics;
critics.push_back(&oscillation_costs_); // discards oscillating motions (assisgns cost -1)
critics.push_back(&obstacle_costs_); // discards trajectories that move into obstacles
critics.push_back(&goal_front_costs_); // prefers trajectories that make the nose go towards (local) nose goal
critics.push_back(&alignment_costs_); // prefers trajectories that keep the robot nose on nose path
critics.push_back(&path_costs_); // prefers trajectories on global path
critics.push_back(&goal_costs_); // prefers trajectories that go towards (local) goal, based on wave propagation
critics.push_back(&twirling_costs_); // optionally prefer trajectories that don't spin
// trajectory generators
std::vector<base_local_planner::TrajectorySampleGenerator*> generator_list;
generator_list.push_back(&generator_);
scored_sampling_planner_ = base_local_planner::SimpleScoredSamplingPlanner(generator_list, critics);
private_nh.param("cheat_factor", cheat_factor_, 1.0);
}
// used for visualization only, total_costs are not really total costs
bool DWAPlanner::getCellCosts(int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost) {
path_cost = path_costs_.getCellCosts(cx, cy);
goal_cost = goal_costs_.getCellCosts(cx, cy);
occ_cost = planner_util_->getCostmap()->getCost(cx, cy);
if (path_cost == path_costs_.obstacleCosts() ||
path_cost == path_costs_.unreachableCellCosts() ||
occ_cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE) {
return false;
}
total_cost =
path_distance_bias_ * path_cost +
goal_distance_bias_ * goal_cost +
occdist_scale_ * occ_cost;
return true;
}
bool DWAPlanner::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {
oscillation_costs_.resetOscillationFlags();
return planner_util_->setPlan(orig_global_plan);
}
/**
* This function is used when other strategies are to be applied,
* but the cost functions for obstacles are to be reused.
*/
bool DWAPlanner::checkTrajectory(
Eigen::Vector3f pos,
Eigen::Vector3f vel,
Eigen::Vector3f vel_samples){
oscillation_costs_.resetOscillationFlags();
base_local_planner::Trajectory traj;
geometry_msgs::PoseStamped goal_pose = global_plan_.back();
Eigen::Vector3f goal(goal_pose.pose.position.x, goal_pose.pose.position.y, tf2::getYaw(goal_pose.pose.orientation));
base_local_planner::LocalPlannerLimits limits = planner_util_->getCurrentLimits();
generator_.initialise(pos,
vel,
goal,
&limits,
vsamples_);
generator_.generateTrajectory(pos, vel, vel_samples, traj);
double cost = scored_sampling_planner_.scoreTrajectory(traj, -1);
//if the trajectory is a legal one... the check passes
if(cost >= 0) {
return true;
}
ROS_WARN("Invalid Trajectory %f, %f, %f, cost: %f", vel_samples[0], vel_samples[1], vel_samples[2], cost);
//otherwise the check fails
return false;
}
void DWAPlanner::updatePlanAndLocalCosts(
const geometry_msgs::PoseStamped& global_pose,
const std::vector<geometry_msgs::PoseStamped>& new_plan,
const std::vector<geometry_msgs::Point>& footprint_spec) {
global_plan_.resize(new_plan.size());
for (unsigned int i = 0; i < new_plan.size(); ++i) {
global_plan_[i] = new_plan[i];
}
obstacle_costs_.setFootprint(footprint_spec);
// costs for going away from path
path_costs_.setTargetPoses(global_plan_);
// costs for not going towards the local goal as much as possible
goal_costs_.setTargetPoses(global_plan_);
// alignment costs
geometry_msgs::PoseStamped goal_pose = global_plan_.back();
Eigen::Vector3f pos(global_pose.pose.position.x, global_pose.pose.position.y, tf2::getYaw(global_pose.pose.orientation));
double sq_dist =
(pos[0] - goal_pose.pose.position.x) * (pos[0] - goal_pose.pose.position.x) +
(pos[1] - goal_pose.pose.position.y) * (pos[1] - goal_pose.pose.position.y);
// we want the robot nose to be drawn to its final position
// (before robot turns towards goal orientation), not the end of the
// path for the robot center. Choosing the final position after
// turning towards goal orientation causes instability when the
// robot needs to make a 180 degree turn at the end
std::vector<geometry_msgs::PoseStamped> front_global_plan = global_plan_;
double angle_to_goal = atan2(goal_pose.pose.position.y - pos[1], goal_pose.pose.position.x - pos[0]);
front_global_plan.back().pose.position.x = front_global_plan.back().pose.position.x +
forward_point_distance_ * cos(angle_to_goal);
front_global_plan.back().pose.position.y = front_global_plan.back().pose.position.y + forward_point_distance_ *
sin(angle_to_goal);
goal_front_costs_.setTargetPoses(front_global_plan);
// keeping the nose on the path
if (sq_dist > forward_point_distance_ * forward_point_distance_ * cheat_factor_) {
alignment_costs_.setScale(path_distance_bias_);
// costs for robot being aligned with path (nose on path, not ju
alignment_costs_.setTargetPoses(global_plan_);
} else {
// once we are close to goal, trying to keep the nose close to anything destabilizes behavior.
alignment_costs_.setScale(0.0);
}
}
/*
* given the current state of the robot, find a good trajectory
*/
base_local_planner::Trajectory DWAPlanner::findBestPath(
const geometry_msgs::PoseStamped& global_pose,
const geometry_msgs::PoseStamped& global_vel,
geometry_msgs::PoseStamped& drive_velocities) {
//make sure that our configuration doesn't change mid-run
boost::mutex::scoped_lock l(configuration_mutex_);
Eigen::Vector3f pos(global_pose.pose.position.x, global_pose.pose.position.y, tf2::getYaw(global_pose.pose.orientation));
Eigen::Vector3f vel(global_vel.pose.position.x, global_vel.pose.position.y, tf2::getYaw(global_vel.pose.orientation));
geometry_msgs::PoseStamped goal_pose = global_plan_.back();
Eigen::Vector3f goal(goal_pose.pose.position.x, goal_pose.pose.position.y, tf2::getYaw(goal_pose.pose.orientation));
base_local_planner::LocalPlannerLimits limits = planner_util_->getCurrentLimits();
// prepare cost functions and generators for this run
generator_.initialise(pos,
vel,
goal,
&limits,
vsamples_);
result_traj_.cost_ = -7;
// find best trajectory by sampling and scoring the samples
std::vector<base_local_planner::Trajectory> all_explored;
scored_sampling_planner_.findBestTrajectory(result_traj_, &all_explored);
if(publish_traj_pc_)
{
sensor_msgs::PointCloud2 traj_cloud;
traj_cloud.header.frame_id = frame_id_;
traj_cloud.header.stamp = ros::Time::now();
sensor_msgs::PointCloud2Modifier cloud_mod(traj_cloud);
cloud_mod.setPointCloud2Fields(5, "x", 1, sensor_msgs::PointField::FLOAT32,
"y", 1, sensor_msgs::PointField::FLOAT32,
"z", 1, sensor_msgs::PointField::FLOAT32,
"theta", 1, sensor_msgs::PointField::FLOAT32,
"cost", 1, sensor_msgs::PointField::FLOAT32);
unsigned int num_points = 0;
for(std::vector<base_local_planner::Trajectory>::iterator t=all_explored.begin(); t != all_explored.end(); ++t)
{
if (t->cost_<0)
continue;
num_points += t->getPointsSize();
}
cloud_mod.resize(num_points);
sensor_msgs::PointCloud2Iterator<float> iter_x(traj_cloud, "x");
for(std::vector<base_local_planner::Trajectory>::iterator t=all_explored.begin(); t != all_explored.end(); ++t)
{
if(t->cost_<0)
continue;
// Fill out the plan
for(unsigned int i = 0; i < t->getPointsSize(); ++i) {
double p_x, p_y, p_th;
t->getPoint(i, p_x, p_y, p_th);
iter_x[0] = p_x;
iter_x[1] = p_y;
iter_x[2] = 0.0;
iter_x[3] = p_th;
iter_x[4] = t->cost_;
++iter_x;
}
}
traj_cloud_pub_.publish(traj_cloud);
}
// verbose publishing of point clouds
if (publish_cost_grid_pc_) {
//we'll publish the visualization of the costs to rviz before returning our best trajectory
map_viz_.publishCostCloud(planner_util_->getCostmap());
}
// debrief stateful scoring functions
oscillation_costs_.updateOscillationFlags(pos, &result_traj_, planner_util_->getCurrentLimits().min_vel_trans);
//if we don't have a legal trajectory, we'll just command zero
if (result_traj_.cost_ < 0) {
drive_velocities.pose.position.x = 0;
drive_velocities.pose.position.y = 0;
drive_velocities.pose.position.z = 0;
drive_velocities.pose.orientation.w = 1;
drive_velocities.pose.orientation.x = 0;
drive_velocities.pose.orientation.y = 0;
drive_velocities.pose.orientation.z = 0;
} else {
drive_velocities.pose.position.x = result_traj_.xv_;
drive_velocities.pose.position.y = result_traj_.yv_;
drive_velocities.pose.position.z = 0;
tf2::Quaternion q;
q.setRPY(0, 0, result_traj_.thetav_);
tf2::convert(q, drive_velocities.pose.orientation);
}
return result_traj_;
}
};

314
navigations/dwa_local_planner/src/dwa_planner_ros.cpp Executable file
View File

@@ -0,0 +1,314 @@
/*********************************************************************
*
* 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 <dwa_local_planner/dwa_planner_ros.h>
#include <Eigen/Core>
#include <cmath>
#include <ros/console.h>
#include <pluginlib/class_list_macros.hpp>
#include <base_local_planner/goal_functions.h>
#include <nav_msgs/Path.h>
#include <tf2/utils.h>
#include <nav_core/parameter_magic.h>
//register this planner as a BaseLocalPlanner plugin
PLUGINLIB_EXPORT_CLASS(dwa_local_planner::DWAPlannerROS, nav_core::BaseLocalPlanner)
namespace dwa_local_planner {
void DWAPlannerROS::reconfigureCB(DWAPlannerConfig &config, uint32_t level) {
if (setup_ && config.restore_defaults) {
config = default_config_;
config.restore_defaults = false;
}
if ( ! setup_) {
default_config_ = config;
setup_ = true;
}
// update generic local planner params
base_local_planner::LocalPlannerLimits limits;
limits.max_vel_trans = config.max_vel_trans;
limits.min_vel_trans = config.min_vel_trans;
limits.max_vel_x = config.max_vel_x;
limits.min_vel_x = config.min_vel_x;
limits.max_vel_y = config.max_vel_y;
limits.min_vel_y = config.min_vel_y;
limits.max_vel_theta = config.max_vel_theta;
limits.min_vel_theta = config.min_vel_theta;
limits.acc_lim_x = config.acc_lim_x;
limits.acc_lim_y = config.acc_lim_y;
limits.acc_lim_theta = config.acc_lim_theta;
limits.acc_lim_trans = config.acc_lim_trans;
limits.xy_goal_tolerance = config.xy_goal_tolerance;
limits.yaw_goal_tolerance = config.yaw_goal_tolerance;
limits.prune_plan = config.prune_plan;
limits.trans_stopped_vel = config.trans_stopped_vel;
limits.theta_stopped_vel = config.theta_stopped_vel;
planner_util_.reconfigureCB(limits, config.restore_defaults);
// update dwa specific configuration
dp_->reconfigure(config);
}
DWAPlannerROS::DWAPlannerROS() : initialized_(false),
odom_helper_("odom"), setup_(false) {
}
void DWAPlannerROS::initialize(
std::string name,
tf2_ros::Buffer* tf,
costmap_2d::Costmap2DROS* costmap_ros) {
if (! isInitialized()) {
ros::NodeHandle private_nh("~/" + name);
g_plan_pub_ = private_nh.advertise<nav_msgs::Path>("global_plan", 1);
l_plan_pub_ = private_nh.advertise<nav_msgs::Path>("local_plan", 1);
tf_ = tf;
costmap_ros_ = costmap_ros;
costmap_ros_->getRobotPose(current_pose_);
// make sure to update the costmap we'll use for this cycle
costmap_2d::Costmap2D* costmap = costmap_ros_->getCostmap();
planner_util_.initialize(tf, costmap, costmap_ros_->getGlobalFrameID());
//create the actual planner that we'll use.. it'll configure itself from the parameter server
dp_ = boost::shared_ptr<DWAPlanner>(new DWAPlanner(name, &planner_util_));
if( private_nh.getParam( "odom_topic", odom_topic_ ))
{
odom_helper_.setOdomTopic( odom_topic_ );
}
initialized_ = true;
// Warn about deprecated parameters -- remove this block in N-turtle
nav_core::warnRenamedParameter(private_nh, "max_vel_trans", "max_trans_vel");
nav_core::warnRenamedParameter(private_nh, "min_vel_trans", "min_trans_vel");
nav_core::warnRenamedParameter(private_nh, "max_vel_theta", "max_rot_vel");
nav_core::warnRenamedParameter(private_nh, "min_vel_theta", "min_rot_vel");
nav_core::warnRenamedParameter(private_nh, "acc_lim_trans", "acc_limit_trans");
nav_core::warnRenamedParameter(private_nh, "theta_stopped_vel", "rot_stopped_vel");
dsrv_ = new dynamic_reconfigure::Server<DWAPlannerConfig>(private_nh);
dynamic_reconfigure::Server<DWAPlannerConfig>::CallbackType cb = [this](auto& config, auto level){ reconfigureCB(config, level); };
dsrv_->setCallback(cb);
}
else{
ROS_WARN("This planner has already been initialized, doing nothing.");
}
}
bool DWAPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {
if (! isInitialized()) {
ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
return false;
}
//when we get a new plan, we also want to clear any latch we may have on goal tolerances
latchedStopRotateController_.resetLatching();
ROS_INFO("Got new plan");
return dp_->setPlan(orig_global_plan);
}
bool DWAPlannerROS::isGoalReached() {
if (! isInitialized()) {
ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
return false;
}
if ( ! costmap_ros_->getRobotPose(current_pose_)) {
ROS_ERROR("Could not get robot pose");
return false;
}
if(latchedStopRotateController_.isGoalReached(&planner_util_, odom_helper_, current_pose_)) {
ROS_INFO("Goal reached");
return true;
} else {
return false;
}
}
void DWAPlannerROS::publishLocalPlan(std::vector<geometry_msgs::PoseStamped>& path) {
base_local_planner::publishPlan(path, l_plan_pub_);
}
void DWAPlannerROS::publishGlobalPlan(std::vector<geometry_msgs::PoseStamped>& path) {
base_local_planner::publishPlan(path, g_plan_pub_);
}
DWAPlannerROS::~DWAPlannerROS(){
//make sure to clean things up
delete dsrv_;
}
bool DWAPlannerROS::dwaComputeVelocityCommands(geometry_msgs::PoseStamped &global_pose, geometry_msgs::Twist& cmd_vel) {
// dynamic window sampling approach to get useful velocity commands
if(! isInitialized()){
ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
return false;
}
geometry_msgs::PoseStamped robot_vel;
odom_helper_.getRobotVel(robot_vel);
/* For timing uncomment
struct timeval start, end;
double start_t, end_t, t_diff;
gettimeofday(&start, NULL);
*/
//compute what trajectory to drive along
geometry_msgs::PoseStamped drive_cmds;
drive_cmds.header.frame_id = costmap_ros_->getBaseFrameID();
// call with updated footprint
base_local_planner::Trajectory path = dp_->findBestPath(global_pose, robot_vel, drive_cmds);
//ROS_ERROR("Best: %.2f, %.2f, %.2f, %.2f", path.xv_, path.yv_, path.thetav_, path.cost_);
/* For timing uncomment
gettimeofday(&end, NULL);
start_t = start.tv_sec + double(start.tv_usec) / 1e6;
end_t = end.tv_sec + double(end.tv_usec) / 1e6;
t_diff = end_t - start_t;
ROS_INFO("Cycle time: %.9f", t_diff);
*/
//pass along drive commands
cmd_vel.linear.x = drive_cmds.pose.position.x;
cmd_vel.linear.y = drive_cmds.pose.position.y;
cmd_vel.angular.z = tf2::getYaw(drive_cmds.pose.orientation);
//if we cannot move... tell someone
std::vector<geometry_msgs::PoseStamped> local_plan;
if(path.cost_ < 0) {
ROS_DEBUG_NAMED("dwa_local_planner",
"The dwa local planner failed to find a valid plan, cost functions discarded all candidates. This can mean there is an obstacle too close to the robot.");
local_plan.clear();
publishLocalPlan(local_plan);
return false;
}
ROS_DEBUG_NAMED("dwa_local_planner", "A valid velocity command of (%.2f, %.2f, %.2f) was found for this cycle.",
cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);
// Fill out the local plan
for(unsigned int i = 0; i < path.getPointsSize(); ++i) {
double p_x, p_y, p_th;
path.getPoint(i, p_x, p_y, p_th);
geometry_msgs::PoseStamped p;
p.header.frame_id = costmap_ros_->getGlobalFrameID();
p.header.stamp = ros::Time::now();
p.pose.position.x = p_x;
p.pose.position.y = p_y;
p.pose.position.z = 0.0;
tf2::Quaternion q;
q.setRPY(0, 0, p_th);
tf2::convert(q, p.pose.orientation);
local_plan.push_back(p);
}
//publish information to the visualizer
publishLocalPlan(local_plan);
return true;
}
bool DWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) {
// dispatches to either dwa sampling control or stop and rotate control, depending on whether we have been close enough to goal
if ( ! costmap_ros_->getRobotPose(current_pose_)) {
ROS_ERROR("Could not get robot pose");
return false;
}
std::vector<geometry_msgs::PoseStamped> transformed_plan;
if ( ! planner_util_.getLocalPlan(current_pose_, transformed_plan)) {
ROS_ERROR("Could not get local plan");
return false;
}
//if the global plan passed in is empty... we won't do anything
if(transformed_plan.empty()) {
ROS_WARN_NAMED("dwa_local_planner", "Received an empty transformed plan.");
return false;
}
ROS_DEBUG_NAMED("dwa_local_planner", "Received a transformed plan with %zu points.", transformed_plan.size());
// update plan in dwa_planner even if we just stop and rotate, to allow checkTrajectory
dp_->updatePlanAndLocalCosts(current_pose_, transformed_plan, costmap_ros_->getRobotFootprint());
if (latchedStopRotateController_.isPositionReached(&planner_util_, current_pose_)) {
//publish an empty plan because we've reached our goal position
std::vector<geometry_msgs::PoseStamped> local_plan;
std::vector<geometry_msgs::PoseStamped> transformed_plan;
publishGlobalPlan(transformed_plan);
publishLocalPlan(local_plan);
base_local_planner::LocalPlannerLimits limits = planner_util_.getCurrentLimits();
return latchedStopRotateController_.computeVelocityCommandsStopRotate(
cmd_vel,
limits.getAccLimits(),
dp_->getSimPeriod(),
&planner_util_,
odom_helper_,
current_pose_,
[this](auto pos, auto vel, auto vel_samples){ return dp_->checkTrajectory(pos, vel, vel_samples); });
} else {
bool isOk = dwaComputeVelocityCommands(current_pose_, cmd_vel);
if (isOk) {
publishGlobalPlan(transformed_plan);
} else {
ROS_WARN_NAMED("dwa_local_planner", "DWA planner failed to produce path.");
std::vector<geometry_msgs::PoseStamped> empty_plan;
publishGlobalPlan(empty_plan);
}
return isOk;
}
}
};