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

78
mir_robot/mir_dwb_critics/CHANGELOG.rst Executable file
View File

@@ -0,0 +1,78 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package mir_dwb_critics
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
1.1.7 (2023-01-20)
------------------
* Don't set cmake_policy CMP0048
* Drop old C++ compiler flags
* Add license headers
* Contributors: Martin Günther
1.1.6 (2022-06-02)
------------------
1.1.5 (2022-02-11)
------------------
1.1.4 (2021-12-10)
------------------
1.1.3 (2021-06-11)
------------------
* Merge branch 'melodic-2.8' into noetic
* Reformat python code using black
* Contributors: Martin Günther
1.1.2 (2021-05-12)
------------------
1.1.1 (2021-02-11)
------------------
* Fix bug in path_dist_pruned
With some paths, the previous code crashed with "terminate called after throwing an instance
of 'std::bad_alloc'".
* Contributors: Martin Günther
1.1.0 (2020-06-30)
------------------
* Initial release into noetic
* Update scripts to Python3 (Noetic)
* Contributors: Martin Günther
1.0.6 (2020-06-30)
------------------
* Add missing matplotlib dependency
* Fix some catkin_lint warnings
* Set cmake_policy CMP0048 to fix warning
* Contributors: Martin Günther
1.0.5 (2020-05-01)
------------------
* mir_dwb_critics: Add plot_dwb_scores.py
* mir_dwb_critics: Improve print_dwb_scores output
* added PathDistPrunedCritic for dwb (`#42 <https://github.com/DFKI-NI/mir_robot/issues/42>`_)
which works exactly like the original PathDistCritic, except that it
searches for a local minimum in the distance from the global path to the robots
current position. It then prunes the global_path from the start up to
this point, therefore approximately cutting of a segment of the path
that the robot already followed.
* Contributors: Martin Günther, Nils Niemann
1.0.4 (2019-05-06)
------------------
1.0.3 (2019-03-04)
------------------
* PathProgressCritic: Add heading score
* Add package: mir_dwb_critics
* Contributors: Martin Günther
1.0.2 (2018-07-30)
------------------
1.0.1 (2018-07-17)
------------------
1.0.0 (2018-07-12)
------------------

55
mir_robot/mir_dwb_critics/CMakeLists.txt Executable file
View File

@@ -0,0 +1,55 @@
cmake_minimum_required(VERSION 3.5.1)
project(mir_dwb_critics)
set_directory_properties(PROPERTIES COMPILE_OPTIONS "-Wall;-Werror")
find_package(catkin REQUIRED COMPONENTS
angles
costmap_queue
dwb_critics
dwb_local_planner
geometry_msgs
nav_2d_msgs
nav_2d_utils
nav_core2
nav_grid_iterators
pluginlib
roscpp
sensor_msgs
)
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
CATKIN_DEPENDS angles costmap_queue dwb_critics dwb_local_planner geometry_msgs nav_2d_msgs nav_2d_utils nav_core2 nav_grid_iterators pluginlib roscpp sensor_msgs
)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
add_library(${PROJECT_NAME}
src/path_angle.cpp
src/path_progress.cpp
src/path_dist_pruned.cpp
)
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
install(PROGRAMS
nodes/print_dwb_scores.py
nodes/plot_dwb_scores.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(TARGETS ${PROJECT_NAME}
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}
)
install(FILES default_critics.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

13
mir_robot/mir_dwb_critics/default_critics.xml Executable file
View File

@@ -0,0 +1,13 @@
<class_libraries>
<library path="lib/libmir_dwb_critics">
<class type="mir_dwb_critics::PathProgressCritic" base_class_type="dwb_local_planner::TrajectoryCritic">
<description>Scores trajectories based on how far along the global path they end up.</description>
</class>
<class type="mir_dwb_critics::PathAngleCritic" base_class_type="dwb_local_planner::TrajectoryCritic">
<description>TODO</description>
</class>
<class type="mir_dwb_critics::PathDistPrunedCritic" base_class_type="dwb_local_planner::TrajectoryCritic">
<description>Scores trajectories based on the distance to the global path, only taking the parts into account that are still ahead of the robot.</description>
</class>
</library>
</class_libraries>

71
mir_robot/mir_dwb_critics/include/mir_dwb_critics/path_angle.h Executable file
View File

@@ -0,0 +1,71 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2019, DFKI GmbH
* 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 copyright holder 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 HOLDER 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.
*/
#ifndef MIR_DWB_CRITICS_PATH_ANGLE_H_
#define MIR_DWB_CRITICS_PATH_ANGLE_H_
#include <dwb_local_planner/trajectory_critic.h>
#include <vector>
namespace mir_dwb_critics
{
/**
* @class PathAngleCritic
* @brief Scores trajectories based on the difference between the path's current angle and the trajectory's angle
*
* This trajectory critic helps to ensure that the robot is roughly aligned with the path (i.e.,
* if the path specifies a forward motion, the robot should point forward along the path and vice versa).
* This critic is not a replacement for the PathAlignCritic: The PathAlignCritic tries to point the robot
* towards a point on the path that is in front of the trajectory's end point, so it helps guiding the
* robot back towards the path. The PathAngleCritic on the other hand uses the path point that is closest
* to the robot's current position (not the trajectory end point, or even a point in front of that) as a
* reference, so it always lags behind. Also, it tries to keep the robot parallel to the path, not towards
* it. For these reasons, the error is squared, so that the PathAngleCritic really only affects the final
* score if the error is large. The PathAlignCritic doesn't take the path orientation into account though,
* so that's why the PathAngleCritic is a useful addition.
*/
class PathAngleCritic: public dwb_local_planner::TrajectoryCritic
{
public:
virtual bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel,
const geometry_msgs::Pose2D& goal,
const nav_2d_msgs::Path2D& global_plan) override;
virtual double scoreTrajectory(const dwb_msgs::Trajectory2D& traj) override;
protected:
double desired_angle_;
};
} // namespace mir_dwb_critics
#endif // MIR_DWB_CRITICS_PATH_ANGLE_H_

56
mir_robot/mir_dwb_critics/include/mir_dwb_critics/path_dist_pruned.h Executable file
View File

@@ -0,0 +1,56 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2019, DFKI GmbH
* 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 copyright holder 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 HOLDER 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.
*/
#ifndef MIR_DWB_CRITICS_PATH_DISTANCE_PRUNED_H_
#define MIR_DWB_CRITICS_PATH_DISTANCE_PRUNED_H_
#include <dwb_critics/path_dist.h>
#include <vector>
namespace mir_dwb_critics
{
/**
* @class PathDistPrunedCritic
* @brief Scores trajectories based on how far from the global path they end up,
* where the global path is pruned to only include waypoints ahead of the
* robot.
*/
class PathDistPrunedCritic : public dwb_critics::PathDistCritic {
public:
virtual bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel,
const geometry_msgs::Pose2D& goal,
const nav_2d_msgs::Path2D& global_plan) override;
};
} // namespace mir_dwb_critics
#endif // MIR_DWB_CRITICS_DISTANCE_PRUNED_H_

86
mir_robot/mir_dwb_critics/include/mir_dwb_critics/path_progress.h Executable file
View File

@@ -0,0 +1,86 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2019, DFKI GmbH
* 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 copyright holder 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 HOLDER 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.
*/
#ifndef MIR_DWB_CRITICS_PATH_PROGRESS_H_
#define MIR_DWB_CRITICS_PATH_PROGRESS_H_
#include <dwb_critics/map_grid.h>
#include <vector>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
namespace mir_dwb_critics {
/**
* @class PathProgressCritic
* @brief Calculates an intermediate goal along the global path and scores trajectories on the distance to that goal.
*
* This trajectory critic helps ensure progress along the global path. It
* calculates an intermediate goal that is as far as possible along the global
* path as long as the path continues to move in one direction (+/-
* angle_threshold).
*/
class PathProgressCritic : public dwb_critics::MapGridCritic {
public:
bool prepare(const geometry_msgs::Pose2D &pose,
const nav_2d_msgs::Twist2D &vel,
const geometry_msgs::Pose2D &goal,
const nav_2d_msgs::Path2D &global_plan) override;
void onInit() override;
void reset() override;
double scoreTrajectory(const dwb_msgs::Trajectory2D& traj) override;
protected:
bool getGoalPose(const geometry_msgs::Pose2D &robot_pose,
const nav_2d_msgs::Path2D &global_plan,
unsigned int &x,
unsigned int &y,
double &desired_angle);
unsigned int getGoalIndex(const geometry_msgs::Pose2D &robot_pose,
const std::vector<geometry_msgs::Pose2D> &plan,
unsigned int start_index,
unsigned int last_valid_index) const;
double xy_local_goal_tolerance_;
double angle_threshold_;
double heading_scale_;
std::vector<geometry_msgs::Pose2D> reached_intermediate_goals_;
double desired_angle_;
ros::Publisher reached_intermediate_goals_pub_, current_goal_pub_, closet_robot_goal_pub_;
};
} // namespace mir_dwb_critics
#endif // MIR_DWB_CRITICS_PATH_PROGRESS_H_

81
mir_robot/mir_dwb_critics/include/mir_dwb_critics/path_progress_default.h Executable file
View File

@@ -0,0 +1,81 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2019, DFKI GmbH
* 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 copyright holder 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 HOLDER 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.
*/
#ifndef MIR_DWB_CRITICS_PATH_PROGRESS_H_
#define MIR_DWB_CRITICS_PATH_PROGRESS_H_
#include <dwb_critics/map_grid.h>
#include <vector>
namespace mir_dwb_critics {
/**
* @class PathProgressCritic
* @brief Calculates an intermediate goal along the global path and scores trajectories on the distance to that goal.
*
* This trajectory critic helps ensure progress along the global path. It
* calculates an intermediate goal that is as far as possible along the global
* path as long as the path continues to move in one direction (+/-
* angle_threshold).
*/
class PathProgressCritic : public dwb_critics::MapGridCritic {
public:
bool prepare(const geometry_msgs::Pose2D &pose,
const nav_2d_msgs::Twist2D &vel,
const geometry_msgs::Pose2D &goal,
const nav_2d_msgs::Path2D &global_plan) override;
void onInit() override;
void reset() override;
double scoreTrajectory(const dwb_msgs::Trajectory2D& traj) override;
protected:
bool getGoalPose(const geometry_msgs::Pose2D &robot_pose,
const nav_2d_msgs::Path2D &global_plan,
unsigned int &x,
unsigned int &y,
double &desired_angle);
unsigned int getGoalIndex(const std::vector<geometry_msgs::Pose2D> &plan,
unsigned int start_index,
unsigned int last_valid_index) const;
double xy_local_goal_tolerance_;
double angle_threshold_;
double heading_scale_;
std::vector<geometry_msgs::Pose2D> reached_intermediate_goals_;
double desired_angle_;
};
} // namespace mir_dwb_critics
#endif // MIR_DWB_CRITICS_PATH_PROGRESS_H_

94
mir_robot/mir_dwb_critics/nodes/plot_dwb_scores.py Executable file
View File

@@ -0,0 +1,94 @@
#!/usr/bin/env python3
# Copyright (c) 2018-2022, Martin Günther (DFKI GmbH) and contributors
#
# 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 the copyright holder 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 HOLDER 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: Martin Günther
import os
import tkinter
import matplotlib.pyplot as plt
import numpy as np
import rospy
from dwb_msgs.msg import LocalPlanEvaluation
fig = None
rects = None
max_score = 0.0
def eval_cb(msg):
global fig, rects, max_score
labels = [s.name for s in msg.twists[msg.best_index].scores]
scaled_scores = [s.scale * s.raw_score for s in msg.twists[msg.best_index].scores]
# reverse labels + scaled_scores to show the critics in the correct order top to bottom
labels.reverse()
scaled_scores.reverse()
if not fig:
# init
y = np.arange(len(labels)) # the label locations
height = 0.35 # the height of the bars
fig, ax = plt.subplots()
rects = ax.barh(y - height / 2, scaled_scores, height, label='scaled score')
ax.set_ylabel('DWB Critics')
ax.set_title('Scaled scores')
ax.set_yticks(y)
ax.set_yticklabels(labels)
fig.tight_layout()
fig.canvas.set_window_title('DWB Scores')
# update axis limit
if max_score < max(scaled_scores):
max_score = max(scaled_scores)
plt.xlim(0.0, max_score)
for rect, h in zip(rects, scaled_scores):
rect.set_width(h)
try:
fig.canvas.draw()
fig.canvas.flush_events()
except tkinter.TclError:
rospy.loginfo('Window was closed, exiting.')
os._exit(0)
def main():
rospy.init_node('plot_dwb_scores')
rospy.Subscriber('move_base_node/DWBLocalPlanner/evaluation', LocalPlanEvaluation, eval_cb)
rospy.loginfo('plot_dwb_scores ready.')
plt.ion()
rospy.spin()
if __name__ == '__main__':
main()

57
mir_robot/mir_dwb_critics/nodes/print_dwb_scores.py Executable file
View File

@@ -0,0 +1,57 @@
#!/usr/bin/env python3
# Copyright (c) 2018-2022, Martin Günther (DFKI GmbH) and contributors
#
# 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 the copyright holder 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 HOLDER 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: Martin Günther
import rospy
from dwb_msgs.msg import LocalPlanEvaluation
def eval_cb(msg):
print('\n\n=========================================================\n\n')
for heading, i in zip(['best trajectory', 'worst trajectory'], [msg.best_index, msg.worst_index]):
print('### {}\n'.format(heading))
print('Name | Raw | Scale | Scaled Score')
print('---------------------|-----------|---------|-------------')
for s in msg.twists[i].scores:
print('{:20} | {:9.4f} | {:7.4f} | {:12.4f}'.format(s.name, s.raw_score, s.scale, s.raw_score * s.scale))
print('---------------------------------------- total: {:9.4f}'.format(msg.twists[i].total))
print()
def main():
rospy.init_node('print_dwb_scores', anonymous=True)
rospy.Subscriber('move_base_node/DWBLocalPlanner/evaluation', LocalPlanEvaluation, eval_cb)
rospy.loginfo('print_dwb_scores ready.')
rospy.spin()
if __name__ == '__main__':
main()

38
mir_robot/mir_dwb_critics/package.xml Executable file
View File

@@ -0,0 +1,38 @@
<?xml version="1.0"?>
<package format="2">
<name>mir_dwb_critics</name>
<version>1.1.7</version>
<description>
Trajectory critics for the dwb_local_planner that work well together with the SBPL global planner on the MiR robot
</description>
<maintainer email="martin.guenther@dfki.de">Martin Günther</maintainer>
<author email="martin.guenther@dfki.de">Martin Günther</author>
<license>BSD</license>
<url type="website">https://github.com/DFKI-NI/mir_robot</url>
<url type="repository">https://github.com/DFKI-NI/mir_robot</url>
<url type="bugtracker">https://github.com/DFKI-NI/mir_robot/issues</url>
<buildtool_depend>catkin</buildtool_depend>
<depend>angles</depend>
<depend>costmap_queue</depend>
<depend>dwb_critics</depend>
<depend>dwb_local_planner</depend>
<depend>geometry_msgs</depend>
<depend>nav_2d_msgs</depend>
<depend>nav_2d_utils</depend>
<depend>nav_core2</depend>
<depend>nav_grid_iterators</depend>
<depend>pluginlib</depend>
<depend>roscpp</depend>
<depend>sensor_msgs</depend>
<exec_depend>python3-matplotlib</exec_depend>
<export>
<dwb_local_planner plugin="${prefix}/default_critics.xml"/>
</export>
</package>

93
mir_robot/mir_dwb_critics/src/path_angle.cpp Executable file
View File

@@ -0,0 +1,93 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2019, DFKI GmbH
* 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 copyright holder 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 HOLDER 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 <mir_dwb_critics/path_angle.h>
#include <pluginlib/class_list_macros.h>
#include <nav_2d_utils/path_ops.h>
#include <nav_grid/coordinate_conversion.h>
#include <vector>
namespace mir_dwb_critics {
bool PathAngleCritic::prepare(const geometry_msgs::Pose2D &pose, const nav_2d_msgs::Twist2D &vel,
const geometry_msgs::Pose2D &goal, const nav_2d_msgs::Path2D &global_plan) {
const nav_core2::Costmap &costmap = *costmap_;
const nav_grid::NavGridInfo &info = costmap.getInfo();
nav_2d_msgs::Path2D adjusted_global_plan = nav_2d_utils::adjustPlanResolution(global_plan, info.resolution);
if (global_plan.poses.empty()) {
ROS_ERROR_NAMED("PathAngleCritic", "The global plan was empty.");
return false;
}
// find the angle of the plan at the pose on the plan closest to the robot
double distance_min = std::numeric_limits<double>::infinity();
bool started_path = false;
for (unsigned int i = 0; i < adjusted_global_plan.poses.size(); i++) {
double g_x = adjusted_global_plan.poses[i].x;
double g_y = adjusted_global_plan.poses[i].y;
unsigned int map_x, map_y;
if (worldToGridBounded(info, g_x, g_y, map_x, map_y) && costmap(map_x, map_y) != costmap.NO_INFORMATION) {
double distance = nav_2d_utils::poseDistance(adjusted_global_plan.poses[i], pose);
if (distance_min > distance) {
// still getting closer
desired_angle_ = adjusted_global_plan.poses[i].theta;
distance_min = distance;
started_path = true;
} else {
// plan is going away from the robot again
break;
}
} else if (started_path) {
// Off the costmap after being on the costmap.
break;
}
// else, we have not yet found a point on the costmap, so we just continue
}
if (!started_path) {
ROS_ERROR_NAMED("PathAngleCritic", "None of the points of the global plan were in the local costmap.");
return false;
}
return true;
}
double PathAngleCritic::scoreTrajectory(const dwb_msgs::Trajectory2D &traj) {
double diff = fabs(remainder(traj.poses.back().theta - desired_angle_, 2 * M_PI));
return diff * diff;
}
} // namespace mir_dwb_critics
PLUGINLIB_EXPORT_CLASS(mir_dwb_critics::PathAngleCritic, dwb_local_planner::TrajectoryCritic)

101
mir_robot/mir_dwb_critics/src/path_dist_pruned.cpp Executable file
View File

@@ -0,0 +1,101 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2019, DFKI GmbH
* 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 copyright holder 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 HOLDER 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 <nav_2d_utils/path_ops.h>
#include <pluginlib/class_list_macros.h>
#include "mir_dwb_critics/path_dist_pruned.h"
namespace mir_dwb_critics {
bool PathDistPrunedCritic::prepare(
const geometry_msgs::Pose2D &pose,
const nav_2d_msgs::Twist2D &vel,
const geometry_msgs::Pose2D &goal,
const nav_2d_msgs::Path2D &global_plan) {
const nav_core2::Costmap &costmap = *costmap_;
const nav_grid::NavGridInfo &info = costmap.getInfo();
auto plan = nav_2d_utils::adjustPlanResolution(global_plan, info.resolution).poses;
// --- stolen from PathProgressCritic::getGoalPose ---
// find the "start pose", i.e. the pose on the plan closest to the robot that is also on the local map
unsigned int start_index = 0;
double distance_to_start = std::numeric_limits<double>::infinity();
bool started_path = false;
for (unsigned int i = 0; i < plan.size(); i++) {
double g_x = plan[i].x;
double g_y = plan[i].y;
unsigned int map_x, map_y;
if (worldToGridBounded(info, g_x, g_y, map_x, map_y)
&& costmap(map_x, map_y) != nav_core2::Costmap::NO_INFORMATION) {
// Still on the costmap. Continue.
double distance = nav_2d_utils::poseDistance(plan[i], pose);
if (distance_to_start > distance) {
start_index = i;
distance_to_start = distance;
started_path = true;
} else {
// Plan is going away from the robot again. It's possible that it comes back and we would find a pose that's
// even closer to the robot, but then we would skip over parts of the plan.
break;
}
} else if (started_path) {
// Off the costmap after being on the costmap.
break;
}
// else, we have not yet found a point on the costmap, so we just continue
}
if (!started_path) {
ROS_ERROR_NAMED("PathDistPrunedCritic", "None of the points of the global plan were in the local costmap.");
return false;
}
// ---------------------------------------------------
// remove the first part of the path, everything before start_index, to
// disregard that part in the PathDistCritic implementation.
nav_2d_msgs::Path2D global_plan_pruned;
global_plan_pruned.header = global_plan.header;
global_plan_pruned.poses = std::vector<geometry_msgs::Pose2D>(
plan.begin() + start_index,
plan.end());
return dwb_critics::PathDistCritic::prepare(pose, vel, goal, global_plan_pruned);
}
} // namespace mir_dwb_critics
PLUGINLIB_EXPORT_CLASS(mir_dwb_critics::PathDistPrunedCritic, dwb_local_planner::TrajectoryCritic)

302
mir_robot/mir_dwb_critics/src/path_progress.cpp Executable file
View File

@@ -0,0 +1,302 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2019, DFKI GmbH
* 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 copyright holder 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 HOLDER 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 <mir_dwb_critics/path_progress.h>
#include <nav_grid/coordinate_conversion.h>
#include <pluginlib/class_list_macros.h>
#include <nav_2d_utils/path_ops.h>
#include <vector>
namespace mir_dwb_critics
{
bool PathProgressCritic::prepare(const geometry_msgs::Pose2D &pose, const nav_2d_msgs::Twist2D &vel,
const geometry_msgs::Pose2D &goal,
const nav_2d_msgs::Path2D &global_plan)
{
dwb_critics::MapGridCritic::reset();
unsigned int local_goal_x, local_goal_y;
if (!getGoalPose(pose, global_plan, local_goal_x, local_goal_y, desired_angle_))
{
return false;
}
// Enqueue just the last pose
cell_values_.setValue(local_goal_x, local_goal_y, 0.0);
queue_->enqueueCell(local_goal_x, local_goal_y);
// MapGridCritic::propogateManhattanDistances
propogateManhattanDistances();
return true;
}
void PathProgressCritic::onInit()
{
dwb_critics::MapGridCritic::onInit();
critic_nh_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.20);
critic_nh_.param("angle_threshold", angle_threshold_, M_PI_4);
critic_nh_.param("heading_scale", heading_scale_, 1.0);
reached_intermediate_goals_pub_ = critic_nh_.advertise<geometry_msgs::PoseArray>("reached_intermediate_goals", 1);
current_goal_pub_ = critic_nh_.advertise<geometry_msgs::PoseStamped>("current_goal", 1);
closet_robot_goal_pub_ = critic_nh_.advertise<geometry_msgs::PoseStamped>("closet_robot_goal", 1);
// divide heading scale by position scale because the sum will be multiplied by scale again
heading_scale_ /= getScale();
}
void PathProgressCritic::reset()
{
reached_intermediate_goals_.clear();
}
double PathProgressCritic::scoreTrajectory(const dwb_msgs::Trajectory2D &traj)
{
double position_score = MapGridCritic::scoreTrajectory(traj);
double heading_diff = fabs(remainder(traj.poses.back().theta - desired_angle_, 2 * M_PI));
double heading_score = heading_diff * heading_diff;
return position_score + heading_scale_ * heading_score;
}
bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_msgs::Path2D &global_plan,
unsigned int &x, unsigned int &y, double &desired_angle)
{
const nav_core2::Costmap &costmap = *costmap_;
const nav_grid::NavGridInfo &info = costmap.getInfo();
if (global_plan.poses.empty())
{
ROS_ERROR_NAMED("PathProgressCritic", "The global plan was empty.");
return false;
}
// Chuyển đổi dữ liệu kế hoạch đường đi 'nav_2d_msgs::Path2D' sang 'std::vector<geometry_msgs::Pose2D>'
std::vector<geometry_msgs::Pose2D> plan = nav_2d_utils::adjustPlanResolution(global_plan, info.resolution).poses;
// find the "start pose", i.e. the pose on the plan closest to the robot that is also on the local map
// Tìm điểm pose bắt đầu trên kế hoạch đường đi gần nhất với robot đồng thời có tồn tại trong local map
unsigned int start_index = 0; // số vị trí pose trong kế hoạch đường đi
double distance_to_start = std::numeric_limits<double>::infinity(); // Xét giá trị là vô cùng oo
bool started_path = false;
for (unsigned int i = 0; i < plan.size(); i++)
{
double g_x = plan[i].x;
double g_y = plan[i].y;
unsigned int map_x, map_y;
if (worldToGridBounded(info, g_x, g_y, map_x, map_y) // chuyển tọa độ từ đơn vị mét sang pixel
&& costmap(map_x, map_y) != nav_core2::Costmap::NO_INFORMATION)
{ // chi phí phải khác -1
// Still on the costmap. Continue.
double distance = nav_2d_utils::poseDistance(plan[i], robot_pose); // Tính khoảng cách từ vị trí của robot đến pose đang xét
if (distance_to_start > distance)
{
start_index = i;
distance_to_start = distance;
started_path = true;
}
// else
// {
// // Plan is going away from the robot again. It's possible that it comes back and we would find a pose that's
// // even closer to the robot, but then we would skip over parts of the plan.
// ss <<" index_max_ " << index_max_;
// break;
// }
}
else if (started_path)
{
// Off the costmap after being on the costmap.
break;
}
// else, we have not yet found a point on the costmap, so we just continue
}
geometry_msgs::PoseStamped pose;
pose.header.stamp = ros::Time::now();
pose.header.frame_id = global_plan.header.frame_id;
pose.pose.position.x = plan[start_index].x;
pose.pose.position.y = plan[start_index].y;
tf2::Quaternion temp;
temp.setRPY(0, 0, plan[start_index].theta);
pose.pose.orientation.x = temp.getX();
pose.pose.orientation.y = temp.getY();
pose.pose.orientation.z = temp.getZ();
pose.pose.orientation.w = temp.getW();
closet_robot_goal_pub_.publish(pose);
// Nếu khồng tìm được điểm gần với robot nhất thì trả về false
if (!started_path)
{
ROS_ERROR_NAMED("PathProgressCritic", "None of the points of the global plan were in the local costmap.");
return false;
}
// current_index_ = start_index;
// find the "last valid pose", i.e. the last pose on the plan after the start pose that is still on the local map
// Tìm điểm pose cuôi cùng sau điểm pose bắt đầu và có tồn tại trong local map
unsigned int last_valid_index = start_index;
for (unsigned int i = start_index + 1; i < plan.size(); i++)
{
double g_x = plan[i].x;
double g_y = plan[i].y;
unsigned int map_x, map_y;
if (worldToGridBounded(info, g_x, g_y, map_x, map_y) && costmap(map_x, map_y) != nav_core2::Costmap::NO_INFORMATION)
{
// Still on the costmap. Continue.
last_valid_index = i;
}
else
{
// Off the costmap after being on the costmap.
break;
}
}
// find the "goal pose" by walking along the plan as long as each step leads far enough away from the starting point,
// i.e. is within angle_threshold_ of the starting direction.
// Tìm điểm mục tiêu chạy từ điểm start pose đến last valid pose dọc thep plan
unsigned int goal_index = start_index;
while (goal_index < last_valid_index && critic_nh_.ok())
{
// Tìm kiếm vị trí điểm mục tiêu phù hợp
goal_index = getGoalIndex(robot_pose, plan, start_index, last_valid_index);
// check if goal already reached
bool goal_already_reached = false;
geometry_msgs::PoseArray poseArrayMsg;
poseArrayMsg.header.stamp = ros::Time::now();
poseArrayMsg.header.frame_id = global_plan.header.frame_id;
for (const auto &pose2D : reached_intermediate_goals_)
{
geometry_msgs::Pose pose;
pose.position.x = pose2D.x;
pose.position.y = pose2D.y;
tf2::Quaternion temp;
temp.setRPY(0, 0, pose2D.theta);
pose.orientation.x = temp.getX();
pose.orientation.y = temp.getY();
pose.orientation.z = temp.getZ();
pose.orientation.w = temp.getW();
poseArrayMsg.poses.push_back(pose);
}
reached_intermediate_goals_pub_.publish(poseArrayMsg);
for (auto &reached_intermediate_goal : reached_intermediate_goals_)
{
double distance = nav_2d_utils::poseDistance(reached_intermediate_goal, plan[goal_index]);
if (distance < xy_local_goal_tolerance_)
{
goal_already_reached = true;
// choose a new start_index by walking along the plan until we're outside xy_local_goal_tolerance_ and try again
// (start_index is now > goal_index)
for (start_index = goal_index; start_index <= last_valid_index; ++start_index)
{
distance = nav_2d_utils::poseDistance(reached_intermediate_goal, plan[start_index]);
if (distance >= xy_local_goal_tolerance_)
{
break;
}
}
break;
}
}
pose.header.stamp = ros::Time::now();
pose.header.frame_id = global_plan.header.frame_id;
pose.pose.position.x = plan[goal_index].x;
pose.pose.position.y = plan[goal_index].y;
tf2::Quaternion temp;
temp.setRPY(0, 0, plan[goal_index].theta);
pose.pose.orientation.x = temp.getX();
pose.pose.orientation.y = temp.getY();
pose.pose.orientation.z = temp.getZ();
pose.pose.orientation.w = temp.getW();
current_goal_pub_.publish(pose);
if (!goal_already_reached)
{
// new goal not in reached_intermediate_goals_
double distance = nav_2d_utils::poseDistance(plan[goal_index], robot_pose);
if (distance < xy_local_goal_tolerance_)
{
// the robot has currently reached the goal
reached_intermediate_goals_.push_back(plan[goal_index]);
ROS_DEBUG_NAMED("PathProgressCritic", "Number of reached_intermediate goals: %zu", reached_intermediate_goals_.size());
}
else
{
// we've found a new goal!
break;
}
}
}
if (start_index > goal_index)
start_index = goal_index;
ROS_ASSERT(goal_index <= last_valid_index);
// save goal in x, y
worldToGridBounded(info, plan[goal_index].x, plan[goal_index].y, x, y);
desired_angle = plan[start_index].theta;
return true;
}
unsigned int PathProgressCritic::getGoalIndex(const geometry_msgs::Pose2D &robot_pose,
const std::vector<geometry_msgs::Pose2D> &plan,
unsigned int start_index,
unsigned int last_valid_index) const
{
if (start_index >= last_valid_index)
return last_valid_index;
unsigned int goal_index = start_index;
for (unsigned int i = start_index + 1; i <= last_valid_index; ++i)
{
const double current_direction_x = plan[i].x - plan[i - 1].x;
const double current_direction_y = plan[i].y - plan[i - 1].y;
if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9)
{
double current_angle = atan2(current_direction_y, current_direction_x);
if (fabs(remainder(robot_pose.theta - current_angle, 2 * M_PI)) > angle_threshold_)
break;
goal_index = i;
}
}
if (nav_2d_utils::poseDistance(plan[goal_index], plan[last_valid_index]) <= xy_local_goal_tolerance_)
goal_index = last_valid_index;
return goal_index;
}
} // namespace mir_dwb_critics
PLUGINLIB_EXPORT_CLASS(mir_dwb_critics::PathProgressCritic, dwb_local_planner::TrajectoryCritic)

221
mir_robot/mir_dwb_critics/src/path_progress_default.cpp Executable file
View File

@@ -0,0 +1,221 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2019, DFKI GmbH
* 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 copyright holder 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 HOLDER 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 <mir_dwb_critics/path_progress_default.h>
#include <nav_grid/coordinate_conversion.h>
#include <pluginlib/class_list_macros.h>
#include <nav_2d_utils/path_ops.h>
#include <vector>
namespace mir_dwb_critics {
bool PathProgressCritic::prepare(const geometry_msgs::Pose2D &pose, const nav_2d_msgs::Twist2D &vel,
const geometry_msgs::Pose2D &goal,
const nav_2d_msgs::Path2D &global_plan) {
dwb_critics::MapGridCritic::reset();
unsigned int local_goal_x, local_goal_y;
if (!getGoalPose(pose, global_plan, local_goal_x, local_goal_y, desired_angle_)) {
return false;
}
// Enqueue just the last pose
cell_values_.setValue(local_goal_x, local_goal_y, 0.0);
queue_->enqueueCell(local_goal_x, local_goal_y);
propogateManhattanDistances();
return true;
}
void PathProgressCritic::onInit() {
dwb_critics::MapGridCritic::onInit();
critic_nh_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.20);
critic_nh_.param("angle_threshold", angle_threshold_, M_PI_4);
critic_nh_.param("heading_scale", heading_scale_, 1.0);
// divide heading scale by position scale because the sum will be multiplied by scale again
heading_scale_ /= getScale();
}
void PathProgressCritic::reset() {
reached_intermediate_goals_.clear();
}
double PathProgressCritic::scoreTrajectory(const dwb_msgs::Trajectory2D &traj) {
double position_score = MapGridCritic::scoreTrajectory(traj);
double heading_diff = fabs(remainder(traj.poses.back().theta - desired_angle_, 2 * M_PI));
double heading_score = heading_diff * heading_diff;
return position_score + heading_scale_ * heading_score;
}
bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_msgs::Path2D &global_plan,
unsigned int &x, unsigned int &y, double &desired_angle) {
const nav_core2::Costmap &costmap = *costmap_;
const nav_grid::NavGridInfo &info = costmap.getInfo();
if (global_plan.poses.empty()) {
ROS_ERROR_NAMED("PathProgressCritic", "The global plan was empty.");
return false;
}
std::vector<geometry_msgs::Pose2D> plan = nav_2d_utils::adjustPlanResolution(global_plan, info.resolution).poses;
// find the "start pose", i.e. the pose on the plan closest to the robot that is also on the local map
unsigned int start_index = 0;
double distance_to_start = std::numeric_limits<double>::infinity();
bool started_path = false;
for (unsigned int i = 0; i < plan.size(); i++) {
double g_x = plan[i].x;
double g_y = plan[i].y;
unsigned int map_x, map_y;
if (worldToGridBounded(info, g_x, g_y, map_x, map_y)
&& costmap(map_x, map_y) != nav_core2::Costmap::NO_INFORMATION) {
// Still on the costmap. Continue.
double distance = nav_2d_utils::poseDistance(plan[i], robot_pose);
if (distance_to_start > distance) {
start_index = i;
distance_to_start = distance;
started_path = true;
} else {
// Plan is going away from the robot again. It's possible that it comes back and we would find a pose that's
// even closer to the robot, but then we would skip over parts of the plan.
break;
}
} else if (started_path) {
// Off the costmap after being on the costmap.
break;
}
// else, we have not yet found a point on the costmap, so we just continue
}
if (!started_path) {
ROS_ERROR_NAMED("PathProgressCritic", "None of the points of the global plan were in the local costmap.");
return false;
}
// find the "last valid pose", i.e. the last pose on the plan after the start pose that is still on the local map
unsigned int last_valid_index = start_index;
for (unsigned int i = start_index + 1; i < plan.size(); i++) {
double g_x = plan[i].x;
double g_y = plan[i].y;
unsigned int map_x, map_y;
if (worldToGridBounded(info, g_x, g_y, map_x, map_y)
&& costmap(map_x, map_y) != nav_core2::Costmap::NO_INFORMATION) {
// Still on the costmap. Continue.
last_valid_index = i;
} else {
// Off the costmap after being on the costmap.
break;
}
}
// find the "goal pose" by walking along the plan as long as each step leads far enough away from the starting point,
// i.e. is within angle_threshold_ of the starting direction.
unsigned int goal_index = start_index;
while (goal_index < last_valid_index) {
goal_index = getGoalIndex(plan, start_index, last_valid_index);
// check if goal already reached
bool goal_already_reached = false;
for (auto &reached_intermediate_goal : reached_intermediate_goals_) {
double distance = nav_2d_utils::poseDistance(reached_intermediate_goal, plan[goal_index]);
if (distance < xy_local_goal_tolerance_) {
goal_already_reached = true;
// choose a new start_index by walking along the plan until we're outside xy_local_goal_tolerance_ and try again
// (start_index is now > goal_index)
for (start_index = goal_index; start_index <= last_valid_index; ++start_index) {
distance = nav_2d_utils::poseDistance(reached_intermediate_goal, plan[start_index]);
if (distance >= xy_local_goal_tolerance_) {
break;
}
}
break;
}
}
if (!goal_already_reached) {
// new goal not in reached_intermediate_goals_
double distance = nav_2d_utils::poseDistance(plan[goal_index], robot_pose);
if (distance < xy_local_goal_tolerance_) {
// the robot has currently reached the goal
reached_intermediate_goals_.push_back(plan[goal_index]);
ROS_DEBUG_NAMED("PathProgressCritic", "Number of reached_intermediate goals: %zu", reached_intermediate_goals_.size());
} else {
// we've found a new goal!
break;
}
}
}
if (start_index > goal_index)
start_index = goal_index;
ROS_ASSERT(goal_index <= last_valid_index);
// save goal in x, y
worldToGridBounded(info, plan[goal_index].x, plan[goal_index].y, x, y);
desired_angle = plan[start_index].theta;
return true;
}
unsigned int PathProgressCritic::getGoalIndex(const std::vector<geometry_msgs::Pose2D> &plan,
unsigned int start_index,
unsigned int last_valid_index) const {
if (start_index >= last_valid_index)
return last_valid_index;
unsigned int goal_index = start_index;
const double start_direction_x = plan[start_index + 1].x - plan[start_index].x;
const double start_direction_y = plan[start_index + 1].y - plan[start_index].y;
if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9) { // make sure that atan2 is defined
double start_angle = atan2(start_direction_y, start_direction_x);
for (unsigned int i = start_index + 1; i <= last_valid_index; ++i) {
const double current_direction_x = plan[i].x - plan[i - 1].x;
const double current_direction_y = plan[i].y - plan[i - 1].y;
if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9) {
double current_angle = atan2(current_direction_y, current_direction_x);
// goal pose is found if plan doesn't point far enough away from the starting point
if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) > angle_threshold_)
break;
goal_index = i;
}
}
}
return goal_index;
}
} // namespace mir_dwb_critics
PLUGINLIB_EXPORT_CLASS(mir_dwb_critics::PathProgressCritic, dwb_local_planner::TrajectoryCritic)