add humble-navigation2
This commit is contained in:
X-lanni
@@ -0,0 +1,87 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(dwb_critics)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(nav2_common REQUIRED)
|
||||
find_package(angles REQUIRED)
|
||||
find_package(nav2_costmap_2d REQUIRED)
|
||||
find_package(costmap_queue REQUIRED)
|
||||
find_package(dwb_core REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(nav_2d_msgs REQUIRED)
|
||||
find_package(nav_2d_utils REQUIRED)
|
||||
find_package(pluginlib REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(nav2_util REQUIRED)
|
||||
|
||||
nav2_package()
|
||||
|
||||
include_directories(
|
||||
include
|
||||
)
|
||||
|
||||
add_library(${PROJECT_NAME} SHARED
|
||||
src/alignment_util.cpp
|
||||
src/map_grid.cpp
|
||||
src/goal_dist.cpp
|
||||
src/path_dist.cpp
|
||||
src/goal_align.cpp
|
||||
src/path_align.cpp
|
||||
src/base_obstacle.cpp
|
||||
src/obstacle_footprint.cpp
|
||||
src/oscillation.cpp
|
||||
src/prefer_forward.cpp
|
||||
src/rotate_to_goal.cpp
|
||||
src/twirling.cpp
|
||||
)
|
||||
|
||||
set(dependencies
|
||||
angles
|
||||
nav2_costmap_2d
|
||||
costmap_queue
|
||||
dwb_core
|
||||
geometry_msgs
|
||||
nav_2d_msgs
|
||||
nav_2d_utils
|
||||
pluginlib
|
||||
rclcpp
|
||||
sensor_msgs
|
||||
nav2_util
|
||||
)
|
||||
|
||||
ament_target_dependencies(${PROJECT_NAME}
|
||||
${dependencies}
|
||||
)
|
||||
|
||||
install(TARGETS ${PROJECT_NAME}
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION bin
|
||||
)
|
||||
install(DIRECTORY include/
|
||||
DESTINATION include/
|
||||
)
|
||||
install(FILES default_critics.xml
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
find_package(ament_cmake_gtest REQUIRED)
|
||||
|
||||
add_subdirectory(test)
|
||||
endif()
|
||||
|
||||
ament_export_include_directories(include)
|
||||
ament_export_libraries(${PROJECT_NAME})
|
||||
ament_export_dependencies(
|
||||
${dependencies}
|
||||
)
|
||||
|
||||
pluginlib_export_plugin_description_file(dwb_core default_critics.xml)
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,42 @@
|
||||
<class_libraries>
|
||||
<library path="dwb_critics">
|
||||
<class type="dwb_critics::PreferForwardCritic" base_class_type="dwb_core::TrajectoryCritic">
|
||||
<description>Penalize trajectories with move backwards and/or turn too much</description>
|
||||
</class>
|
||||
<class type="dwb_critics::GoalDistCritic" base_class_type="dwb_core::TrajectoryCritic">
|
||||
<description>Scores trajectories based on how far along the global path they end up.</description>
|
||||
</class>
|
||||
<class type="dwb_critics::PathAlignCritic" base_class_type="dwb_core::TrajectoryCritic">
|
||||
<description>Scores trajectories based on how far from the global path the front of the robot ends up.
|
||||
</description>
|
||||
</class>
|
||||
<class type="dwb_critics::GoalAlignCritic" base_class_type="dwb_core::TrajectoryCritic">
|
||||
<description>Scores trajectories based on whether the robot ends up pointing toward the eventual goal
|
||||
</description>
|
||||
</class>
|
||||
<class type="dwb_critics::PathDistCritic" base_class_type="dwb_core::TrajectoryCritic">
|
||||
<description>Scores trajectories based on how far from the global path they end up.</description>
|
||||
</class>
|
||||
<class type="dwb_critics::OscillationCritic" base_class_type="dwb_core::TrajectoryCritic">
|
||||
<description>Checks to see whether the sign of the commanded velocity flips frequently</description>
|
||||
</class>
|
||||
<class type="dwb_critics::RotateToGoalCritic" base_class_type="dwb_core::TrajectoryCritic">
|
||||
<description>Forces the commanded trajectories to only be rotations if within a certain distance window
|
||||
</description>
|
||||
</class>
|
||||
<class type="dwb_critics::BaseObstacleCritic" base_class_type="dwb_core::TrajectoryCritic">
|
||||
<description>Uses costmap 2d to assign negative costs if a circular robot
|
||||
would collide at any point of the trajectory.
|
||||
</description>
|
||||
</class>
|
||||
<class type="dwb_critics::ObstacleFootprintCritic" base_class_type="dwb_core::TrajectoryCritic">
|
||||
<description>Uses costmap 2d to assign negative costs if robot footprint is in obstacle
|
||||
on any point of the trajectory.
|
||||
</description>
|
||||
</class>
|
||||
<class type="dwb_critics::TwirlingCritic" base_class_type="dwb_core::TrajectoryCritic">
|
||||
<description>Penalize trajectories with rotational velocities
|
||||
</description>
|
||||
</class>
|
||||
</library>
|
||||
</class_libraries>
|
||||
@@ -0,0 +1,54 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 DWB_CRITICS__ALIGNMENT_UTIL_HPP_
|
||||
#define DWB_CRITICS__ALIGNMENT_UTIL_HPP_
|
||||
|
||||
#include "geometry_msgs/msg/pose2_d.hpp"
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
/**
|
||||
* @brief Projects the given pose forward the specified distance in the x direction.
|
||||
* @param pose Input pose
|
||||
* @param distance distance to move (in meters)
|
||||
* @return Pose distance meters in front of input pose.
|
||||
*
|
||||
* (used in both path_align and dist_align)
|
||||
*/
|
||||
geometry_msgs::msg::Pose2D getForwardPose(const geometry_msgs::msg::Pose2D & pose, double distance);
|
||||
|
||||
} // namespace dwb_critics
|
||||
|
||||
#endif // DWB_CRITICS__ALIGNMENT_UTIL_HPP_
|
||||
@@ -0,0 +1,84 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 DWB_CRITICS__BASE_OBSTACLE_HPP_
|
||||
#define DWB_CRITICS__BASE_OBSTACLE_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <utility>
|
||||
|
||||
#include "dwb_core/trajectory_critic.hpp"
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
/**
|
||||
* @class BaseObstacleCritic
|
||||
* @brief Uses costmap 2d to assign negative costs if a circular robot would collide at any point of the trajectory.
|
||||
*
|
||||
* This class can only be used to figure out if a circular robot is in collision. If the cell corresponding
|
||||
* with any of the poses in the Trajectory is an obstacle, inscribed obstacle or unknown, it will return a
|
||||
* negative cost. Otherwise it will return either the final pose's cost, or the sum of all poses, depending
|
||||
* on the sum_scores parameter.
|
||||
*
|
||||
* Other classes (like ObstacleFootprintCritic) can do more advanced checking for collisions.
|
||||
*/
|
||||
class BaseObstacleCritic : public dwb_core::TrajectoryCritic
|
||||
{
|
||||
public:
|
||||
void onInit() override;
|
||||
double scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) override;
|
||||
void addCriticVisualization(
|
||||
std::vector<std::pair<std::string, std::vector<float>>> & cost_channels) override;
|
||||
|
||||
/**
|
||||
* @brief Return the obstacle score for a particular pose
|
||||
* @param pose Pose to check
|
||||
*/
|
||||
virtual double scorePose(const geometry_msgs::msg::Pose2D & pose);
|
||||
|
||||
/**
|
||||
* @brief Check to see whether a given cell cost is valid for driving through.
|
||||
* @param cost Cost of the cell
|
||||
* @return Return true if valid cell
|
||||
*/
|
||||
virtual bool isValidCost(const unsigned char cost);
|
||||
|
||||
protected:
|
||||
nav2_costmap_2d::Costmap2D * costmap_;
|
||||
bool sum_scores_;
|
||||
};
|
||||
} // namespace dwb_critics
|
||||
|
||||
#endif // DWB_CRITICS__BASE_OBSTACLE_HPP_
|
||||
@@ -0,0 +1,68 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 DWB_CRITICS__GOAL_ALIGN_HPP_
|
||||
#define DWB_CRITICS__GOAL_ALIGN_HPP_
|
||||
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include "dwb_critics/goal_dist.hpp"
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
|
||||
/**
|
||||
* @class GoalAlignCritic
|
||||
* @brief Scores trajectories based on whether the robot ends up pointing toward the eventual goal
|
||||
*
|
||||
* Similar to GoalDistCritic, this critic finds the pose from the global path farthest from the robot
|
||||
* that is still on the costmap and then evaluates how far the front of the robot is from that point.
|
||||
* This works as a proxy to calculating which way the robot should be pointing.
|
||||
*/
|
||||
class GoalAlignCritic : public GoalDistCritic
|
||||
{
|
||||
public:
|
||||
GoalAlignCritic()
|
||||
: forward_point_distance_(0.0) {}
|
||||
void onInit() override;
|
||||
bool prepare(
|
||||
const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
|
||||
const geometry_msgs::msg::Pose2D & goal, const nav_2d_msgs::msg::Path2D & global_plan) override;
|
||||
double scorePose(const geometry_msgs::msg::Pose2D & pose) override;
|
||||
|
||||
protected:
|
||||
double forward_point_distance_;
|
||||
};
|
||||
|
||||
} // namespace dwb_critics
|
||||
#endif // DWB_CRITICS__GOAL_ALIGN_HPP_
|
||||
@@ -0,0 +1,64 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 DWB_CRITICS__GOAL_DIST_HPP_
|
||||
#define DWB_CRITICS__GOAL_DIST_HPP_
|
||||
|
||||
#include <vector>
|
||||
#include "dwb_critics/map_grid.hpp"
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
/**
|
||||
* @class GoalDistCritic
|
||||
* @brief Scores trajectories based on how far along the global path they end up.
|
||||
*
|
||||
* This trajectory critic helps ensure progress along the global path. It finds the pose from the
|
||||
* global path farthest from the robot that is still on the costmap, and aims for that point by
|
||||
* assigning the lowest cost to the cell corresponding with that farthest pose.
|
||||
*/
|
||||
class GoalDistCritic : public MapGridCritic
|
||||
{
|
||||
public:
|
||||
bool prepare(
|
||||
const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
|
||||
const geometry_msgs::msg::Pose2D & goal, const nav_2d_msgs::msg::Path2D & global_plan) override;
|
||||
|
||||
protected:
|
||||
bool getLastPoseOnCostmap(
|
||||
const nav_2d_msgs::msg::Path2D & global_plan, unsigned int & x,
|
||||
unsigned int & y);
|
||||
};
|
||||
|
||||
} // namespace dwb_critics
|
||||
#endif // DWB_CRITICS__GOAL_DIST_HPP_
|
||||
@@ -0,0 +1,151 @@
|
||||
/*
|
||||
* Copyright (c) 2012, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
* * Neither the name of the Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived from
|
||||
* this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
#ifndef DWB_CRITICS__LINE_ITERATOR_HPP_
|
||||
#define DWB_CRITICS__LINE_ITERATOR_HPP_
|
||||
|
||||
#include <stdlib.h>
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
|
||||
/** An iterator implementing Bresenham Ray-Tracing. */
|
||||
class LineIterator
|
||||
{
|
||||
public:
|
||||
LineIterator(int x0, int y0, int x1, int y1)
|
||||
: x0_(x0),
|
||||
y0_(y0),
|
||||
x1_(x1),
|
||||
y1_(y1),
|
||||
x_(x0), // X and Y start of at first endpoint.
|
||||
y_(y0),
|
||||
deltax_(abs(x1 - x0)),
|
||||
deltay_(abs(y1 - y0)),
|
||||
curpixel_(0)
|
||||
{
|
||||
if (x1_ >= x0_) { // The x-values are increasing
|
||||
xinc1_ = 1;
|
||||
xinc2_ = 1;
|
||||
} else { // The x-values are decreasing
|
||||
xinc1_ = -1;
|
||||
xinc2_ = -1;
|
||||
}
|
||||
|
||||
if (y1_ >= y0_) { // The y-values are increasing
|
||||
yinc1_ = 1;
|
||||
yinc2_ = 1;
|
||||
} else { // The y-values are decreasing
|
||||
yinc1_ = -1;
|
||||
yinc2_ = -1;
|
||||
}
|
||||
|
||||
if (deltax_ >= deltay_) { // There is at least one x-value for every y-value
|
||||
xinc1_ = 0; // Don't change the x when numerator >= denominator
|
||||
yinc2_ = 0; // Don't change the y for every iteration
|
||||
den_ = deltax_;
|
||||
num_ = deltax_ / 2;
|
||||
numadd_ = deltay_;
|
||||
numpixels_ = deltax_; // There are more x-values than y-values
|
||||
} else { // There is at least one y-value for every x-value
|
||||
xinc2_ = 0; // Don't change the x for every iteration
|
||||
yinc1_ = 0; // Don't change the y when numerator >= denominator
|
||||
den_ = deltay_;
|
||||
num_ = deltay_ / 2;
|
||||
numadd_ = deltax_;
|
||||
numpixels_ = deltay_; // There are more y-values than x-values
|
||||
}
|
||||
}
|
||||
|
||||
bool isValid() const
|
||||
{
|
||||
return curpixel_ <= numpixels_;
|
||||
}
|
||||
|
||||
void advance()
|
||||
{
|
||||
num_ += numadd_; // Increase the numerator by the top of the fraction
|
||||
if (num_ >= den_) { // Check if numerator >= denominator
|
||||
num_ -= den_; // Calculate the new numerator value
|
||||
x_ += xinc1_; // Change the x as appropriate
|
||||
y_ += yinc1_; // Change the y as appropriate
|
||||
}
|
||||
x_ += xinc2_; // Change the x as appropriate
|
||||
y_ += yinc2_; // Change the y as appropriate
|
||||
|
||||
curpixel_++;
|
||||
}
|
||||
|
||||
int getX() const
|
||||
{
|
||||
return x_;
|
||||
}
|
||||
int getY() const
|
||||
{
|
||||
return y_;
|
||||
}
|
||||
|
||||
int getX0() const
|
||||
{
|
||||
return x0_;
|
||||
}
|
||||
int getY0() const
|
||||
{
|
||||
return y0_;
|
||||
}
|
||||
|
||||
int getX1() const
|
||||
{
|
||||
return x1_;
|
||||
}
|
||||
int getY1() const
|
||||
{
|
||||
return y1_;
|
||||
}
|
||||
|
||||
private:
|
||||
int x0_; ///< X coordinate of first end point.
|
||||
int y0_; ///< Y coordinate of first end point.
|
||||
int x1_; ///< X coordinate of second end point.
|
||||
int y1_; ///< Y coordinate of second end point.
|
||||
|
||||
int x_; ///< X coordinate of current point.
|
||||
int y_; ///< Y coordinate of current point.
|
||||
|
||||
int deltax_; ///< Difference between Xs of endpoints.
|
||||
int deltay_; ///< Difference between Ys of endpoints.
|
||||
|
||||
int curpixel_; ///< index of current point in line loop.
|
||||
|
||||
int xinc1_, xinc2_, yinc1_, yinc2_;
|
||||
int den_, num_, numadd_, numpixels_;
|
||||
};
|
||||
|
||||
} // end namespace dwb_critics
|
||||
|
||||
#endif // DWB_CRITICS__LINE_ITERATOR_HPP_
|
||||
@@ -0,0 +1,142 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 DWB_CRITICS__MAP_GRID_HPP_
|
||||
#define DWB_CRITICS__MAP_GRID_HPP_
|
||||
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
|
||||
#include "dwb_core/trajectory_critic.hpp"
|
||||
#include "costmap_queue/costmap_queue.hpp"
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
/**
|
||||
* @class MapGridCritic
|
||||
* @brief breadth-first scoring of all the cells in the costmap
|
||||
*
|
||||
* This TrajectoryCritic assigns a score to every cell in the costmap based on
|
||||
* the distance to the cell from some set of source points. The cells corresponding
|
||||
* with the source points are marked with some initial score, and then every other cell
|
||||
* is updated with a score based on its relation to the closest source cell, based on a
|
||||
* breadth-first exploration of the cells of the costmap.
|
||||
*
|
||||
* This approach was chosen for computational efficiency, such that each trajectory
|
||||
* need not be compared to the list of source points.
|
||||
*/
|
||||
class MapGridCritic : public dwb_core::TrajectoryCritic
|
||||
{
|
||||
public:
|
||||
// Standard TrajectoryCritic Interface
|
||||
void onInit() override;
|
||||
double scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) override;
|
||||
void addCriticVisualization(
|
||||
std::vector<std::pair<std::string, std::vector<float>>> & cost_channels) override;
|
||||
double getScale() const override {return costmap_->getResolution() * 0.5 * scale_;}
|
||||
|
||||
// Helper Functions
|
||||
/**
|
||||
* @brief Retrieve the score for a single pose
|
||||
* @param pose The pose to score, assumed to be in the same frame as the costmap
|
||||
* @return The score associated with the cell of the costmap where the pose lies
|
||||
*/
|
||||
virtual double scorePose(const geometry_msgs::msg::Pose2D & pose);
|
||||
|
||||
/**
|
||||
* @brief Retrieve the score for a particular cell of the costmap
|
||||
* @param x x-coordinate within the costmap
|
||||
* @param y y-coordinate within the costmap
|
||||
* @return the score associated with that cell.
|
||||
*/
|
||||
inline double getScore(unsigned int x, unsigned int y)
|
||||
{
|
||||
return cell_values_[costmap_->getIndex(x, y)];
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Sets the score of a particular cell to the obstacle cost
|
||||
* @param index Index of the cell to mark
|
||||
*/
|
||||
void setAsObstacle(unsigned int index);
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Separate modes for aggregating scores across the multiple poses in a trajectory.
|
||||
*
|
||||
* Last returns the score associated with the last pose in the trajectory
|
||||
* Sum returns the sum of all the scores
|
||||
* Product returns the product of all the (non-zero) scores
|
||||
*/
|
||||
// cppcheck-suppress syntaxError
|
||||
enum class ScoreAggregationType {Last, Sum, Product};
|
||||
|
||||
/**
|
||||
* @class MapGridQueue
|
||||
* @brief Subclass of CostmapQueue that avoids Obstacles and Unknown Values
|
||||
*/
|
||||
class MapGridQueue : public costmap_queue::CostmapQueue
|
||||
{
|
||||
public:
|
||||
MapGridQueue(nav2_costmap_2d::Costmap2D & costmap, MapGridCritic & parent)
|
||||
: costmap_queue::CostmapQueue(costmap, true), parent_(parent) {}
|
||||
virtual ~MapGridQueue() = default;
|
||||
bool validCellToQueue(const costmap_queue::CellData & cell) override;
|
||||
|
||||
protected:
|
||||
MapGridCritic & parent_;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Clear the queuDWB_CRITICS_MAP_GRID_He and set cell_values_ to the appropriate number of unreachableCellScore
|
||||
*/
|
||||
void reset() override;
|
||||
|
||||
/**
|
||||
* @brief Go through the queue and set the cells to the Manhattan distance from their parents
|
||||
*/
|
||||
void propogateManhattanDistances();
|
||||
|
||||
std::shared_ptr<MapGridQueue> queue_;
|
||||
nav2_costmap_2d::Costmap2D * costmap_;
|
||||
std::vector<double> cell_values_;
|
||||
double obstacle_score_, unreachable_score_; ///< Special cell_values
|
||||
bool stop_on_failure_;
|
||||
ScoreAggregationType aggregationType_;
|
||||
};
|
||||
} // namespace dwb_critics
|
||||
|
||||
#endif // DWB_CRITICS__MAP_GRID_HPP_
|
||||
+100
@@ -0,0 +1,100 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 DWB_CRITICS__OBSTACLE_FOOTPRINT_HPP_
|
||||
#define DWB_CRITICS__OBSTACLE_FOOTPRINT_HPP_
|
||||
|
||||
#include <vector>
|
||||
#include "dwb_critics/base_obstacle.hpp"
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
typedef std::vector<geometry_msgs::msg::Point> Footprint;
|
||||
|
||||
/**
|
||||
* @brief Transform the footprint spec to be centered at the given pose
|
||||
* @param pose Robot pose
|
||||
* @param footprint_spec List of points that make up the footprint spec, centered at 0,0
|
||||
* @return oriented footprint
|
||||
*/
|
||||
Footprint getOrientedFootprint(
|
||||
const geometry_msgs::msg::Pose2D & pose,
|
||||
const Footprint & footprint_spec);
|
||||
|
||||
/**
|
||||
* @class ObstacleFootprintCritic
|
||||
* @brief Uses costmap 2d to assign negative costs if robot footprint is in obstacle on any point of the trajectory.
|
||||
*
|
||||
* Internally, this technically only checks if the border of the footprint collides with anything for computational
|
||||
* efficiency. This is valid if the obstacles in the local costmap are inflated.
|
||||
*
|
||||
* A more robust class could check every cell within the robot's footprint without inflating the obstacles,
|
||||
* at some computational cost. That is left as an excercise to the reader.
|
||||
*/
|
||||
class ObstacleFootprintCritic : public BaseObstacleCritic
|
||||
{
|
||||
public:
|
||||
bool prepare(
|
||||
const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
|
||||
const geometry_msgs::msg::Pose2D & goal, const nav_2d_msgs::msg::Path2D & global_plan) override;
|
||||
double scorePose(const geometry_msgs::msg::Pose2D & pose) override;
|
||||
virtual double scorePose(
|
||||
const geometry_msgs::msg::Pose2D & pose,
|
||||
const Footprint & oriented_footprint);
|
||||
double getScale() const override {return costmap_->getResolution() * scale_;}
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Rasterizes a line in the costmap grid and checks for collisions
|
||||
* @param x0 The x position of the first cell in grid coordinates
|
||||
* @param y0 The y position of the first cell in grid coordinates
|
||||
* @param x1 The x position of the second cell in grid coordinates
|
||||
* @param y1 The y position of the second cell in grid coordinates
|
||||
* @return A positive cost for a legal line... negative otherwise
|
||||
*/
|
||||
double lineCost(int x0, int x1, int y0, int y1);
|
||||
|
||||
/**
|
||||
* @brief Checks the cost of a point in the costmap
|
||||
* @param x The x position of the point in cell coordinates
|
||||
* @param y The y position of the point in cell coordinates
|
||||
* @return A positive cost for a legal point... negative otherwise
|
||||
*/
|
||||
double pointCost(int x, int y);
|
||||
|
||||
Footprint footprint_spec_;
|
||||
};
|
||||
} // namespace dwb_critics
|
||||
|
||||
#endif // DWB_CRITICS__OBSTACLE_FOOTPRINT_HPP_
|
||||
@@ -0,0 +1,162 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 DWB_CRITICS__OSCILLATION_HPP_
|
||||
#define DWB_CRITICS__OSCILLATION_HPP_
|
||||
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <chrono>
|
||||
#include "dwb_core/trajectory_critic.hpp"
|
||||
|
||||
using namespace std::chrono_literals; // NOLINT
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
|
||||
/**
|
||||
* @class OscillationCritic
|
||||
* @brief Checks to see whether the sign of the commanded velocity flips frequently
|
||||
*
|
||||
* This critic figures out if the commanded trajectories are oscillating by seeing
|
||||
* if one of the dimensions (x,y,theta) flips from positive to negative and then back
|
||||
* (or vice versa) without moving sufficiently far or waiting sufficiently long.
|
||||
*
|
||||
* Scenario 1: Robot moves one meter forward, and then two millimeters backward.
|
||||
* Another forward motion would be considered oscillating, since the x dimension would then
|
||||
* flip from positive to negative and then back to negative. Hence, when scoring different
|
||||
* trajectories, positive velocity commands will get the oscillation_score (-5.0, or invalid)
|
||||
* and only negative velocity commands will be considered valid.
|
||||
|
||||
* Scenario 2: Robot moves one meter forward, and then one meter backward.
|
||||
* The robot has thus moved one meter since flipping the sign of the x direction, which
|
||||
* is greater than our oscillation_reset_dist, so its not considered oscillating, so all
|
||||
* trajectories are considered valid.
|
||||
*
|
||||
* Note: The critic will only check oscillations in the x dimension while it exceeds
|
||||
* a particular value (x_only_threshold_). If it dips below that magnitude, it will
|
||||
* also check for oscillations in the y and theta dimensions. If x_only_threshold_ is
|
||||
* negative, then the critic will always check all dimensions.
|
||||
*
|
||||
* Implementation Details:
|
||||
* The critic saves the robot's current position when it prepares, and what the actual
|
||||
* commanded velocity was during the debrief step. Upon debriefing, if the sign of any of
|
||||
* dimensions has flipped since the last command, the position is saved as prev_stationary_pose_.
|
||||
*
|
||||
* If the linear or angular distance from prev_stationary_pose_ to the current pose exceeds
|
||||
* the limits, the oscillation flags are reset so the previous sign change is no longer remembered.
|
||||
* This assumes that oscillation_reset_dist_ or oscillation_reset_angle_ are positive. Otherwise,
|
||||
* it uses a time based delay reset function.
|
||||
*/
|
||||
class OscillationCritic : public dwb_core::TrajectoryCritic
|
||||
{
|
||||
public:
|
||||
OscillationCritic()
|
||||
: oscillation_reset_time_(0s) {}
|
||||
void onInit() override;
|
||||
bool prepare(
|
||||
const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
|
||||
const geometry_msgs::msg::Pose2D & goal, const nav_2d_msgs::msg::Path2D & global_plan) override;
|
||||
double scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) override;
|
||||
void reset() override;
|
||||
void debrief(const nav_2d_msgs::msg::Twist2D & cmd_vel) override;
|
||||
|
||||
private:
|
||||
/**
|
||||
* @class CommandTrend
|
||||
* @brief Helper class for performing the same logic on the x,y and theta dimensions
|
||||
*/
|
||||
class CommandTrend
|
||||
{
|
||||
public:
|
||||
CommandTrend();
|
||||
void reset();
|
||||
|
||||
/**
|
||||
* @brief update internal flags based on the commanded velocity
|
||||
* @param velocity commanded velocity for the dimension this trend is tracking
|
||||
* @return true if the sign has flipped
|
||||
*/
|
||||
bool update(double velocity);
|
||||
|
||||
/**
|
||||
* @brief Check to see whether the proposed velocity would be considered oscillating
|
||||
* @param velocity the velocity to evaluate
|
||||
* @return true if the sign has flipped more than once
|
||||
*/
|
||||
bool isOscillating(double velocity);
|
||||
|
||||
/**
|
||||
* @brief Check whether we are currently tracking a flipped sign
|
||||
* @return True if the sign has flipped
|
||||
*/
|
||||
bool hasSignFlipped();
|
||||
|
||||
private:
|
||||
// Simple Enum for Tracking
|
||||
// cppcheck-suppress syntaxError
|
||||
enum class Sign { ZERO, POSITIVE, NEGATIVE };
|
||||
|
||||
Sign sign_;
|
||||
bool positive_only_, negative_only_;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Given a command that has been selected, track each component's sign for oscillations
|
||||
* @param cmd_vel The command velocity selected by the algorithm
|
||||
* @return True if the sign on any of the components flipped
|
||||
*/
|
||||
bool setOscillationFlags(const nav_2d_msgs::msg::Twist2D & cmd_vel);
|
||||
|
||||
/**
|
||||
* @brief Return true if the robot has travelled far enough or waited long enough
|
||||
*/
|
||||
bool resetAvailable();
|
||||
|
||||
CommandTrend x_trend_, y_trend_, theta_trend_;
|
||||
double oscillation_reset_dist_, oscillation_reset_angle_, x_only_threshold_;
|
||||
rclcpp::Duration oscillation_reset_time_;
|
||||
|
||||
// Cached square parameter
|
||||
double oscillation_reset_dist_sq_;
|
||||
|
||||
// Saved positions
|
||||
geometry_msgs::msg::Pose2D pose_, prev_stationary_pose_;
|
||||
// Saved timestamp
|
||||
rclcpp::Time prev_reset_time_;
|
||||
rclcpp::Clock::SharedPtr clock_;
|
||||
};
|
||||
|
||||
} // namespace dwb_critics
|
||||
#endif // DWB_CRITICS__OSCILLATION_HPP_
|
||||
@@ -0,0 +1,74 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 DWB_CRITICS__PATH_ALIGN_HPP_
|
||||
#define DWB_CRITICS__PATH_ALIGN_HPP_
|
||||
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include "dwb_critics/path_dist.hpp"
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
/**
|
||||
* @class PathAlignCritic
|
||||
* @brief Scores trajectories based on how far from the global path the front of the robot ends up.
|
||||
*
|
||||
* This uses the costmap grid as a proxy for calculating which way the robot should be facing relative
|
||||
* to the global path. Instead of scoring how far the center of the robot is away from the global path,
|
||||
* this critic calculates how far a point forward_point_distance in front of the robot is from the global
|
||||
* path. This biases the planner toward trajectories that line up with the global plan.
|
||||
*
|
||||
* When the robot is near the end of the path, the scale of this critic is set to zero. When the projected
|
||||
* point is past the global goal, we no longer want this critic to try to align to a part of the global path
|
||||
* that isn't there.
|
||||
*/
|
||||
class PathAlignCritic : public PathDistCritic
|
||||
{
|
||||
public:
|
||||
PathAlignCritic()
|
||||
: zero_scale_(false), forward_point_distance_(0.0) {}
|
||||
void onInit() override;
|
||||
bool prepare(
|
||||
const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
|
||||
const geometry_msgs::msg::Pose2D & goal, const nav_2d_msgs::msg::Path2D & global_plan) override;
|
||||
double getScale() const override;
|
||||
double scorePose(const geometry_msgs::msg::Pose2D & pose) override;
|
||||
|
||||
protected:
|
||||
bool zero_scale_;
|
||||
double forward_point_distance_;
|
||||
};
|
||||
|
||||
} // namespace dwb_critics
|
||||
#endif // DWB_CRITICS__PATH_ALIGN_HPP_
|
||||
@@ -0,0 +1,54 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 DWB_CRITICS__PATH_DIST_HPP_
|
||||
#define DWB_CRITICS__PATH_DIST_HPP_
|
||||
|
||||
#include "dwb_critics/map_grid.hpp"
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
/**
|
||||
* @class PathDistCritic
|
||||
* @brief Scores trajectories based on how far from the global path they end up.
|
||||
*/
|
||||
class PathDistCritic : public MapGridCritic
|
||||
{
|
||||
public:
|
||||
bool prepare(
|
||||
const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
|
||||
const geometry_msgs::msg::Pose2D & goal, const nav_2d_msgs::msg::Path2D & global_plan) override;
|
||||
};
|
||||
|
||||
} // namespace dwb_critics
|
||||
#endif // DWB_CRITICS__PATH_DIST_HPP_
|
||||
@@ -0,0 +1,66 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 DWB_CRITICS__PREFER_FORWARD_HPP_
|
||||
#define DWB_CRITICS__PREFER_FORWARD_HPP_
|
||||
|
||||
#include <string>
|
||||
#include "dwb_core/trajectory_critic.hpp"
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
|
||||
/**
|
||||
* @class PreferForwardCritic
|
||||
* @brief Penalize trajectories with move backwards and/or turn too much
|
||||
*
|
||||
* Has three different scoring conditions:
|
||||
* 1) If the trajectory's x velocity is negative, return the penalty
|
||||
* 2) If the trajectory's x is low and the theta is also low, return the penalty.
|
||||
* 3) Otherwise, return a scaled version of the trajectory's theta.
|
||||
*/
|
||||
class PreferForwardCritic : public dwb_core::TrajectoryCritic
|
||||
{
|
||||
public:
|
||||
PreferForwardCritic()
|
||||
: penalty_(1.0), strafe_x_(0.1), strafe_theta_(0.2), theta_scale_(10.0) {}
|
||||
void onInit() override;
|
||||
double scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) override;
|
||||
|
||||
private:
|
||||
double penalty_, strafe_x_, strafe_theta_, theta_scale_;
|
||||
};
|
||||
|
||||
} // namespace dwb_critics
|
||||
#endif // DWB_CRITICS__PREFER_FORWARD_HPP_
|
||||
@@ -0,0 +1,101 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 DWB_CRITICS__ROTATE_TO_GOAL_HPP_
|
||||
#define DWB_CRITICS__ROTATE_TO_GOAL_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include "dwb_core/trajectory_critic.hpp"
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
|
||||
/**
|
||||
* @class RotateToGoalCritic
|
||||
* @brief Forces the commanded trajectories to only be rotations if within a certain distance window
|
||||
*
|
||||
* This used to be built in to the DWA Local Planner as the LatchedStopRotate controller,
|
||||
* but has been moved to a critic for consistency.
|
||||
*
|
||||
* The critic has three distinct phases.
|
||||
* 1) If the current pose is outside xy_goal_tolerance LINEAR distance from the goal pose, this critic
|
||||
* will just return score 0.0.
|
||||
* 2) If within the xy_goal_tolerance and the robot is still moving with non-zero linear motion, this critic
|
||||
* will only allow trajectories that are slower than the current speed in order to stop the robot (within
|
||||
* the robot's acceleration limits). The returned score will be the robot's linear speed squared, multiplied
|
||||
* by the slowing_factor parameter (default 5.0) added to the result of scoreRotation.
|
||||
* 3) If within the xy_goal_tolerance and the robot has sufficiently small linear motion, this critic will
|
||||
* score trajectories that have linear movement as invalid and score the rest based on the result of the
|
||||
* scoreRotation method
|
||||
*
|
||||
* The scoreRotation method can be overriden, but the default behavior is to return the shortest angular distance
|
||||
* between the goal pose and a pose from the trajectory. Which pose depends on the lookahead_time parameter.
|
||||
* * If the lookahead_time parameter is negative, the pose evaluated will be the last pose in the trajectory,
|
||||
* which is the same as DWA's behavior. This is the default.
|
||||
* * Otherwise, a new pose will be projected using the dwb_local_planner::projectPose. By using a lookahead
|
||||
* time shorter than sim_time, the critic will be less concerned about overshooting the goal yaw and thus will
|
||||
* continue to turn faster for longer.
|
||||
*/
|
||||
class RotateToGoalCritic : public dwb_core::TrajectoryCritic
|
||||
{
|
||||
public:
|
||||
void onInit() override;
|
||||
void reset() override;
|
||||
bool prepare(
|
||||
const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
|
||||
const geometry_msgs::msg::Pose2D & goal, const nav_2d_msgs::msg::Path2D & global_plan) override;
|
||||
double scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) override;
|
||||
/**
|
||||
* @brief Assuming that this is an actual rotation when near the goal, score the trajectory.
|
||||
*
|
||||
* This (easily overridden) method assumes that the critic is in the third phase (as described above)
|
||||
* and returns a numeric score for the trajectory relative to the goal yaw.
|
||||
* @param traj Trajectory to score
|
||||
* @return numeric score
|
||||
*/
|
||||
virtual double scoreRotation(const dwb_msgs::msg::Trajectory2D & traj);
|
||||
|
||||
private:
|
||||
bool in_window_;
|
||||
bool rotating_;
|
||||
double goal_yaw_;
|
||||
double xy_goal_tolerance_;
|
||||
double xy_goal_tolerance_sq_; ///< Cached squared tolerance
|
||||
double current_xy_speed_sq_, stopped_xy_velocity_sq_;
|
||||
double slowing_factor_;
|
||||
double lookahead_time_;
|
||||
};
|
||||
|
||||
} // namespace dwb_critics
|
||||
#endif // DWB_CRITICS__ROTATE_TO_GOAL_HPP_
|
||||
@@ -0,0 +1,59 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 DWB_CRITICS__TWIRLING_HPP_
|
||||
#define DWB_CRITICS__TWIRLING_HPP_
|
||||
|
||||
#include "dwb_core/trajectory_critic.hpp"
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
/**
|
||||
* @class TwirlingCritic
|
||||
* @brief Penalize trajectories with rotational velocities
|
||||
*
|
||||
* This class provides a cost based on how much a robot "twirls" on its way to the goal. With
|
||||
* differential-drive robots, there isn't a choice, but with holonomic or near-holonomic robots,
|
||||
* sometimes a robot spins more than you'd like on its way to a goal. This class provides a way
|
||||
* to assign a penalty purely to rotational velocities.
|
||||
*/
|
||||
class TwirlingCritic : public dwb_core::TrajectoryCritic
|
||||
{
|
||||
public:
|
||||
void onInit() override;
|
||||
double scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) override;
|
||||
};
|
||||
} // namespace dwb_critics
|
||||
|
||||
#endif // DWB_CRITICS__TWIRLING_HPP_
|
||||
@@ -0,0 +1,31 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>dwb_critics</name>
|
||||
<version>1.1.18</version>
|
||||
<description>The dwb_critics package</description>
|
||||
<maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
|
||||
<license>BSD-3-Clause</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<build_depend>nav2_common</build_depend>
|
||||
|
||||
<depend>angles</depend>
|
||||
<depend>nav2_costmap_2d</depend>
|
||||
<depend>nav2_util</depend>
|
||||
<depend>costmap_queue</depend>
|
||||
<depend>dwb_core</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>nav_2d_msgs</depend>
|
||||
<depend>nav_2d_utils</depend>
|
||||
<depend>pluginlib</depend>
|
||||
<depend>rclcpp</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_cmake_gtest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,51 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 "dwb_critics/alignment_util.hpp"
|
||||
#include <cmath>
|
||||
|
||||
using std::cos;
|
||||
using std::sin;
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
geometry_msgs::msg::Pose2D getForwardPose(const geometry_msgs::msg::Pose2D & pose, double distance)
|
||||
{
|
||||
geometry_msgs::msg::Pose2D forward_pose;
|
||||
forward_pose.x = pose.x + distance * cos(pose.theta);
|
||||
forward_pose.y = pose.y + distance * sin(pose.theta);
|
||||
forward_pose.theta = pose.theta;
|
||||
return forward_pose;
|
||||
}
|
||||
} // namespace dwb_critics
|
||||
@@ -0,0 +1,117 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 <vector>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
|
||||
#include "dwb_critics/base_obstacle.hpp"
|
||||
#include "dwb_core/exceptions.hpp"
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
#include "nav2_costmap_2d/cost_values.hpp"
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(dwb_critics::BaseObstacleCritic, dwb_core::TrajectoryCritic)
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
|
||||
void BaseObstacleCritic::onInit()
|
||||
{
|
||||
costmap_ = costmap_ros_->getCostmap();
|
||||
|
||||
auto node = node_.lock();
|
||||
if (!node) {
|
||||
throw std::runtime_error{"Failed to lock node"};
|
||||
}
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node,
|
||||
dwb_plugin_name_ + "." + name_ + ".sum_scores", rclcpp::ParameterValue(false));
|
||||
node->get_parameter(dwb_plugin_name_ + "." + name_ + ".sum_scores", sum_scores_);
|
||||
}
|
||||
|
||||
double BaseObstacleCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj)
|
||||
{
|
||||
double score = 0.0;
|
||||
for (unsigned int i = 0; i < traj.poses.size(); ++i) {
|
||||
double pose_score = scorePose(traj.poses[i]);
|
||||
// Optimized/branchless version of if (sum_scores_) score += pose_score,
|
||||
// else score = pose_score;
|
||||
score = static_cast<double>(sum_scores_) * score + pose_score;
|
||||
}
|
||||
return score;
|
||||
}
|
||||
|
||||
double BaseObstacleCritic::scorePose(const geometry_msgs::msg::Pose2D & pose)
|
||||
{
|
||||
unsigned int cell_x, cell_y;
|
||||
if (!costmap_->worldToMap(pose.x, pose.y, cell_x, cell_y)) {
|
||||
throw dwb_core::
|
||||
IllegalTrajectoryException(name_, "Trajectory Goes Off Grid.");
|
||||
}
|
||||
unsigned char cost = costmap_->getCost(cell_x, cell_y);
|
||||
if (!isValidCost(cost)) {
|
||||
throw dwb_core::
|
||||
IllegalTrajectoryException(name_, "Trajectory Hits Obstacle.");
|
||||
}
|
||||
return cost;
|
||||
}
|
||||
|
||||
bool BaseObstacleCritic::isValidCost(const unsigned char cost)
|
||||
{
|
||||
return cost != nav2_costmap_2d::LETHAL_OBSTACLE &&
|
||||
cost != nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE &&
|
||||
cost != nav2_costmap_2d::NO_INFORMATION;
|
||||
}
|
||||
|
||||
void BaseObstacleCritic::addCriticVisualization(
|
||||
std::vector<std::pair<std::string, std::vector<float>>> & cost_channels)
|
||||
{
|
||||
std::pair<std::string, std::vector<float>> grid_scores;
|
||||
grid_scores.first = name_;
|
||||
|
||||
unsigned int size_x = costmap_->getSizeInCellsX();
|
||||
unsigned int size_y = costmap_->getSizeInCellsY();
|
||||
grid_scores.second.resize(size_x * size_y);
|
||||
unsigned int i = 0;
|
||||
for (unsigned int cy = 0; cy < size_y; cy++) {
|
||||
for (unsigned int cx = 0; cx < size_x; cx++) {
|
||||
grid_scores.second[i] = costmap_->getCost(cx, cy);
|
||||
i++;
|
||||
}
|
||||
}
|
||||
cost_channels.push_back(grid_scores);
|
||||
}
|
||||
|
||||
} // namespace dwb_critics
|
||||
@@ -0,0 +1,86 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 "dwb_critics/goal_align.hpp"
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include "dwb_critics/alignment_util.hpp"
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
#include "nav_2d_utils/parameters.hpp"
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
|
||||
void GoalAlignCritic::onInit()
|
||||
{
|
||||
GoalDistCritic::onInit();
|
||||
stop_on_failure_ = false;
|
||||
|
||||
auto node = node_.lock();
|
||||
if (!node) {
|
||||
throw std::runtime_error{"Failed to lock node"};
|
||||
}
|
||||
|
||||
forward_point_distance_ = nav_2d_utils::searchAndGetParam(
|
||||
node,
|
||||
dwb_plugin_name_ + "." + name_ + ".forward_point_distance", 0.325);
|
||||
}
|
||||
|
||||
bool GoalAlignCritic::prepare(
|
||||
const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
|
||||
const geometry_msgs::msg::Pose2D & goal,
|
||||
const nav_2d_msgs::msg::Path2D & global_plan)
|
||||
{
|
||||
// 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
|
||||
double angle_to_goal = atan2(goal.y - pose.y, goal.x - pose.x);
|
||||
|
||||
nav_2d_msgs::msg::Path2D target_poses = global_plan;
|
||||
target_poses.poses.back().x += forward_point_distance_ * cos(angle_to_goal);
|
||||
target_poses.poses.back().y += forward_point_distance_ * sin(angle_to_goal);
|
||||
|
||||
return GoalDistCritic::prepare(pose, vel, goal, target_poses);
|
||||
}
|
||||
|
||||
double GoalAlignCritic::scorePose(const geometry_msgs::msg::Pose2D & pose)
|
||||
{
|
||||
return GoalDistCritic::scorePose(getForwardPose(pose, forward_point_distance_));
|
||||
}
|
||||
|
||||
} // namespace dwb_critics
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(dwb_critics::GoalAlignCritic, dwb_core::TrajectoryCritic)
|
||||
@@ -0,0 +1,106 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 "dwb_critics/goal_dist.hpp"
|
||||
#include <vector>
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
#include "nav_2d_utils/path_ops.hpp"
|
||||
#include "nav2_costmap_2d/cost_values.hpp"
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
bool GoalDistCritic::prepare(
|
||||
const geometry_msgs::msg::Pose2D &, const nav_2d_msgs::msg::Twist2D &,
|
||||
const geometry_msgs::msg::Pose2D &,
|
||||
const nav_2d_msgs::msg::Path2D & global_plan)
|
||||
{
|
||||
reset();
|
||||
|
||||
unsigned int local_goal_x, local_goal_y;
|
||||
if (!getLastPoseOnCostmap(global_plan, local_goal_x, local_goal_y)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Enqueue just the last pose
|
||||
int index = costmap_->getIndex(local_goal_x, local_goal_y);
|
||||
cell_values_[index] = 0.0;
|
||||
queue_->enqueueCell(local_goal_x, local_goal_y);
|
||||
|
||||
propogateManhattanDistances();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool GoalDistCritic::getLastPoseOnCostmap(
|
||||
const nav_2d_msgs::msg::Path2D & global_plan,
|
||||
unsigned int & x, unsigned int & y)
|
||||
{
|
||||
nav_2d_msgs::msg::Path2D adjusted_global_plan = nav_2d_utils::adjustPlanResolution(
|
||||
global_plan,
|
||||
costmap_->getResolution());
|
||||
bool started_path = false;
|
||||
|
||||
// skip global path points until we reach the border of the local map
|
||||
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 (costmap_->worldToMap(
|
||||
g_x, g_y, map_x,
|
||||
map_y) && costmap_->getCost(map_x, map_y) != nav2_costmap_2d::NO_INFORMATION)
|
||||
{
|
||||
// Still on the costmap. Continue.
|
||||
x = map_x;
|
||||
y = map_y;
|
||||
started_path = true;
|
||||
} else if (started_path) {
|
||||
// Off the costmap after being on the costmap. Return the last saved indices.
|
||||
return true;
|
||||
}
|
||||
// else, we have not yet found a point on the costmap, so we just continue
|
||||
}
|
||||
|
||||
if (started_path) {
|
||||
return true;
|
||||
} else {
|
||||
RCLCPP_ERROR(
|
||||
rclcpp::get_logger(
|
||||
"GoalDistCritic"), "None of the points of the global plan were in the local costmap.");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace dwb_critics
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(dwb_critics::GoalDistCritic, dwb_core::TrajectoryCritic)
|
||||
@@ -0,0 +1,189 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 "dwb_critics/map_grid.hpp"
|
||||
#include <cmath>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <utility>
|
||||
#include <algorithm>
|
||||
#include <memory>
|
||||
#include "dwb_core/exceptions.hpp"
|
||||
#include "nav2_costmap_2d/cost_values.hpp"
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
|
||||
using std::abs;
|
||||
using costmap_queue::CellData;
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
|
||||
// Customization of the CostmapQueue validCellToQueue method
|
||||
bool MapGridCritic::MapGridQueue::validCellToQueue(const costmap_queue::CellData & /*cell*/)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
void MapGridCritic::onInit()
|
||||
{
|
||||
costmap_ = costmap_ros_->getCostmap();
|
||||
queue_ = std::make_shared<MapGridQueue>(*costmap_, *this);
|
||||
|
||||
// Always set to true, but can be overriden by subclasses
|
||||
stop_on_failure_ = true;
|
||||
|
||||
auto node = node_.lock();
|
||||
if (!node) {
|
||||
throw std::runtime_error{"Failed to lock node"};
|
||||
}
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node,
|
||||
dwb_plugin_name_ + "." + name_ + ".aggregation_type",
|
||||
rclcpp::ParameterValue(std::string("last")));
|
||||
|
||||
std::string aggro_str;
|
||||
node->get_parameter(dwb_plugin_name_ + "." + name_ + ".aggregation_type", aggro_str);
|
||||
std::transform(aggro_str.begin(), aggro_str.end(), aggro_str.begin(), ::tolower);
|
||||
if (aggro_str == "last") {
|
||||
aggregationType_ = ScoreAggregationType::Last;
|
||||
} else if (aggro_str == "sum") {
|
||||
aggregationType_ = ScoreAggregationType::Sum;
|
||||
} else if (aggro_str == "product") {
|
||||
aggregationType_ = ScoreAggregationType::Product;
|
||||
} else {
|
||||
RCLCPP_ERROR(
|
||||
rclcpp::get_logger(
|
||||
"MapGridCritic"), "aggregation_type parameter \"%s\" invalid. Using Last.",
|
||||
aggro_str.c_str());
|
||||
aggregationType_ = ScoreAggregationType::Last;
|
||||
}
|
||||
}
|
||||
|
||||
void MapGridCritic::setAsObstacle(unsigned int index)
|
||||
{
|
||||
cell_values_[index] = obstacle_score_;
|
||||
}
|
||||
|
||||
void MapGridCritic::reset()
|
||||
{
|
||||
queue_->reset();
|
||||
cell_values_.resize(costmap_->getSizeInCellsX() * costmap_->getSizeInCellsY());
|
||||
obstacle_score_ = static_cast<double>(cell_values_.size());
|
||||
unreachable_score_ = obstacle_score_ + 1.0;
|
||||
std::fill(cell_values_.begin(), cell_values_.end(), unreachable_score_);
|
||||
}
|
||||
|
||||
void MapGridCritic::propogateManhattanDistances()
|
||||
{
|
||||
while (!queue_->isEmpty()) {
|
||||
costmap_queue::CellData cell = queue_->getNextCell();
|
||||
cell_values_[cell.index_] = CellData::absolute_difference(cell.src_x_, cell.x_) +
|
||||
CellData::absolute_difference(cell.src_y_, cell.y_);
|
||||
}
|
||||
}
|
||||
|
||||
double MapGridCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj)
|
||||
{
|
||||
double score = 0.0;
|
||||
unsigned int start_index = 0;
|
||||
if (aggregationType_ == ScoreAggregationType::Product) {
|
||||
score = 1.0;
|
||||
} else if (aggregationType_ == ScoreAggregationType::Last && !stop_on_failure_) {
|
||||
start_index = traj.poses.size() - 1;
|
||||
}
|
||||
double grid_dist;
|
||||
|
||||
for (unsigned int i = start_index; i < traj.poses.size(); ++i) {
|
||||
grid_dist = scorePose(traj.poses[i]);
|
||||
if (stop_on_failure_) {
|
||||
if (grid_dist == obstacle_score_) {
|
||||
throw dwb_core::
|
||||
IllegalTrajectoryException(name_, "Trajectory Hits Obstacle.");
|
||||
} else if (grid_dist == unreachable_score_) {
|
||||
throw dwb_core::
|
||||
IllegalTrajectoryException(name_, "Trajectory Hits Unreachable Area.");
|
||||
}
|
||||
}
|
||||
|
||||
switch (aggregationType_) {
|
||||
case ScoreAggregationType::Last:
|
||||
score = grid_dist;
|
||||
break;
|
||||
case ScoreAggregationType::Sum:
|
||||
score += grid_dist;
|
||||
break;
|
||||
case ScoreAggregationType::Product:
|
||||
if (score > 0) {
|
||||
score *= grid_dist;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return score;
|
||||
}
|
||||
|
||||
double MapGridCritic::scorePose(const geometry_msgs::msg::Pose2D & pose)
|
||||
{
|
||||
unsigned int cell_x, cell_y;
|
||||
// we won't allow trajectories that go off the map... shouldn't happen that often anyways
|
||||
if (!costmap_->worldToMap(pose.x, pose.y, cell_x, cell_y)) {
|
||||
throw dwb_core::
|
||||
IllegalTrajectoryException(name_, "Trajectory Goes Off Grid.");
|
||||
}
|
||||
return getScore(cell_x, cell_y);
|
||||
}
|
||||
|
||||
void MapGridCritic::addCriticVisualization(
|
||||
std::vector<std::pair<std::string, std::vector<float>>> & cost_channels)
|
||||
{
|
||||
std::pair<std::string, std::vector<float>> grid_scores;
|
||||
grid_scores.first = name_;
|
||||
|
||||
nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
|
||||
unsigned int size_x = costmap->getSizeInCellsX();
|
||||
unsigned int size_y = costmap->getSizeInCellsY();
|
||||
grid_scores.second.resize(size_x * size_y);
|
||||
unsigned int i = 0;
|
||||
for (unsigned int cy = 0; cy < size_y; cy++) {
|
||||
for (unsigned int cx = 0; cx < size_x; cx++) {
|
||||
grid_scores.second[i] = getScore(cx, cy);
|
||||
i++;
|
||||
}
|
||||
}
|
||||
cost_channels.push_back(grid_scores);
|
||||
}
|
||||
|
||||
} // namespace dwb_critics
|
||||
@@ -0,0 +1,165 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 "dwb_critics/obstacle_footprint.hpp"
|
||||
#include <algorithm>
|
||||
#include <vector>
|
||||
#include "dwb_critics/line_iterator.hpp"
|
||||
#include "dwb_core/exceptions.hpp"
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
#include "nav2_costmap_2d/cost_values.hpp"
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(dwb_critics::ObstacleFootprintCritic, dwb_core::TrajectoryCritic)
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
Footprint getOrientedFootprint(
|
||||
const geometry_msgs::msg::Pose2D & pose,
|
||||
const Footprint & footprint_spec)
|
||||
{
|
||||
std::vector<geometry_msgs::msg::Point> oriented_footprint;
|
||||
oriented_footprint.resize(footprint_spec.size());
|
||||
double cos_th = cos(pose.theta);
|
||||
double sin_th = sin(pose.theta);
|
||||
for (unsigned int i = 0; i < footprint_spec.size(); ++i) {
|
||||
geometry_msgs::msg::Point & new_pt = oriented_footprint[i];
|
||||
new_pt.x = pose.x + footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th;
|
||||
new_pt.y = pose.y + footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th;
|
||||
}
|
||||
return oriented_footprint;
|
||||
}
|
||||
|
||||
bool ObstacleFootprintCritic::prepare(
|
||||
const geometry_msgs::msg::Pose2D &, const nav_2d_msgs::msg::Twist2D &,
|
||||
const geometry_msgs::msg::Pose2D &, const nav_2d_msgs::msg::Path2D &)
|
||||
{
|
||||
footprint_spec_ = costmap_ros_->getRobotFootprint();
|
||||
if (footprint_spec_.size() == 0) {
|
||||
RCLCPP_ERROR(
|
||||
rclcpp::get_logger("ObstacleFootprintCritic"),
|
||||
"Footprint spec is empty, maybe missing call to setFootprint?");
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
double ObstacleFootprintCritic::scorePose(const geometry_msgs::msg::Pose2D & pose)
|
||||
{
|
||||
unsigned int cell_x, cell_y;
|
||||
if (!costmap_->worldToMap(pose.x, pose.y, cell_x, cell_y)) {
|
||||
throw dwb_core::
|
||||
IllegalTrajectoryException(name_, "Trajectory Goes Off Grid.");
|
||||
}
|
||||
return scorePose(pose, getOrientedFootprint(pose, footprint_spec_));
|
||||
}
|
||||
|
||||
double ObstacleFootprintCritic::scorePose(
|
||||
const geometry_msgs::msg::Pose2D &,
|
||||
const Footprint & footprint)
|
||||
{
|
||||
// now we really have to lay down the footprint in the costmap grid
|
||||
unsigned int x0, x1, y0, y1;
|
||||
double line_cost = 0.0;
|
||||
double footprint_cost = 0.0;
|
||||
|
||||
// we need to rasterize each line in the footprint
|
||||
for (unsigned int i = 0; i < footprint.size() - 1; ++i) {
|
||||
// get the cell coord of the first point
|
||||
if (!costmap_->worldToMap(footprint[i].x, footprint[i].y, x0, y0)) {
|
||||
throw dwb_core::
|
||||
IllegalTrajectoryException(name_, "Footprint Goes Off Grid.");
|
||||
}
|
||||
|
||||
// get the cell coord of the second point
|
||||
if (!costmap_->worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1)) {
|
||||
throw dwb_core::
|
||||
IllegalTrajectoryException(name_, "Footprint Goes Off Grid.");
|
||||
}
|
||||
|
||||
line_cost = lineCost(x0, x1, y0, y1);
|
||||
footprint_cost = std::max(line_cost, footprint_cost);
|
||||
}
|
||||
|
||||
// we also need to connect the first point in the footprint to the last point
|
||||
// get the cell coord of the last point
|
||||
if (!costmap_->worldToMap(footprint.back().x, footprint.back().y, x0, y0)) {
|
||||
throw dwb_core::
|
||||
IllegalTrajectoryException(name_, "Footprint Goes Off Grid.");
|
||||
}
|
||||
|
||||
// get the cell coord of the first point
|
||||
if (!costmap_->worldToMap(footprint.front().x, footprint.front().y, x1, y1)) {
|
||||
throw dwb_core::
|
||||
IllegalTrajectoryException(name_, "Footprint Goes Off Grid.");
|
||||
}
|
||||
|
||||
line_cost = lineCost(x0, x1, y0, y1);
|
||||
footprint_cost = std::max(line_cost, footprint_cost);
|
||||
|
||||
// if all line costs are legal... then we can return that the footprint is legal
|
||||
return footprint_cost;
|
||||
}
|
||||
|
||||
double ObstacleFootprintCritic::lineCost(int x0, int x1, int y0, int y1)
|
||||
{
|
||||
double line_cost = 0.0;
|
||||
double point_cost = -1.0;
|
||||
|
||||
for (LineIterator line(x0, y0, x1, y1); line.isValid(); line.advance()) {
|
||||
point_cost = pointCost(line.getX(), line.getY()); // Score the current point
|
||||
|
||||
if (line_cost < point_cost) {
|
||||
line_cost = point_cost;
|
||||
}
|
||||
}
|
||||
|
||||
return line_cost;
|
||||
}
|
||||
|
||||
double ObstacleFootprintCritic::pointCost(int x, int y)
|
||||
{
|
||||
unsigned char cost = costmap_->getCost(x, y);
|
||||
// if the cell is in an obstacle the path is invalid or unknown
|
||||
if (cost == nav2_costmap_2d::LETHAL_OBSTACLE) {
|
||||
throw dwb_core::
|
||||
IllegalTrajectoryException(name_, "Trajectory Hits Obstacle.");
|
||||
} else if (cost == nav2_costmap_2d::NO_INFORMATION) {
|
||||
throw dwb_core::
|
||||
IllegalTrajectoryException(name_, "Trajectory Hits Unknown Region.");
|
||||
}
|
||||
|
||||
return cost;
|
||||
}
|
||||
|
||||
} // namespace dwb_critics
|
||||
@@ -0,0 +1,234 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 "dwb_critics/oscillation.hpp"
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include "nav_2d_utils/parameters.hpp"
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
#include "dwb_core/exceptions.hpp"
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(dwb_critics::OscillationCritic, dwb_core::TrajectoryCritic)
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
|
||||
|
||||
OscillationCritic::CommandTrend::CommandTrend()
|
||||
{
|
||||
reset();
|
||||
}
|
||||
|
||||
void OscillationCritic::CommandTrend::reset()
|
||||
{
|
||||
sign_ = Sign::ZERO;
|
||||
positive_only_ = false;
|
||||
negative_only_ = false;
|
||||
}
|
||||
|
||||
bool OscillationCritic::CommandTrend::update(double velocity)
|
||||
{
|
||||
bool flag_set = false;
|
||||
if (velocity < 0.0) {
|
||||
if (sign_ == Sign::POSITIVE) {
|
||||
negative_only_ = true;
|
||||
flag_set = true;
|
||||
}
|
||||
sign_ = Sign::NEGATIVE;
|
||||
} else if (velocity > 0.0) {
|
||||
if (sign_ == Sign::NEGATIVE) {
|
||||
positive_only_ = true;
|
||||
flag_set = true;
|
||||
}
|
||||
sign_ = Sign::POSITIVE;
|
||||
}
|
||||
return flag_set;
|
||||
}
|
||||
|
||||
bool OscillationCritic::CommandTrend::isOscillating(double velocity)
|
||||
{
|
||||
return (positive_only_ && velocity < 0.0) || (negative_only_ && velocity > 0.0);
|
||||
}
|
||||
|
||||
bool OscillationCritic::CommandTrend::hasSignFlipped()
|
||||
{
|
||||
return positive_only_ || negative_only_;
|
||||
}
|
||||
|
||||
void OscillationCritic::onInit()
|
||||
{
|
||||
auto node = node_.lock();
|
||||
if (!node) {
|
||||
throw std::runtime_error{"Failed to lock node"};
|
||||
}
|
||||
|
||||
clock_ = node->get_clock();
|
||||
|
||||
oscillation_reset_dist_ = nav_2d_utils::searchAndGetParam(
|
||||
node,
|
||||
dwb_plugin_name_ + "." + name_ + ".oscillation_reset_dist", 0.05);
|
||||
oscillation_reset_dist_sq_ = oscillation_reset_dist_ * oscillation_reset_dist_;
|
||||
oscillation_reset_angle_ = nav_2d_utils::searchAndGetParam(
|
||||
node,
|
||||
dwb_plugin_name_ + "." + name_ + ".oscillation_reset_angle", 0.2);
|
||||
oscillation_reset_time_ = rclcpp::Duration::from_seconds(
|
||||
nav_2d_utils::searchAndGetParam(
|
||||
node,
|
||||
dwb_plugin_name_ + "." + name_ + ".oscillation_reset_time", -1.0));
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node,
|
||||
dwb_plugin_name_ + "." + name_ + ".x_only_threshold", rclcpp::ParameterValue(0.05));
|
||||
|
||||
/**
|
||||
* Historical Parameter Loading
|
||||
* If x_only_threshold is set, use that.
|
||||
* If min_speed_xy is set in the namespace (as it is often used for trajectory generation), use that.
|
||||
* If min_trans_vel is set in the namespace, as it used to be used for trajectory generation, complain then use that.
|
||||
* Otherwise, set x_only_threshold_ to 0.05
|
||||
*/
|
||||
node->get_parameter(dwb_plugin_name_ + "." + name_ + ".x_only_threshold", x_only_threshold_);
|
||||
// TODO(crdelsey): How to handle searchParam?
|
||||
// std::string resolved_name;
|
||||
// if (node->hasParam("x_only_threshold"))
|
||||
// {
|
||||
// node->param("x_only_threshold", x_only_threshold_);
|
||||
// }
|
||||
// else if (node->searchParam("min_speed_xy", resolved_name))
|
||||
// {
|
||||
// node->param(resolved_name, x_only_threshold_);
|
||||
// }
|
||||
// else if (node->searchParam("min_trans_vel", resolved_name))
|
||||
// {
|
||||
// ROS_WARN_NAMED("OscillationCritic",
|
||||
// "Parameter min_trans_vel is deprecated. "
|
||||
// "Please use the name min_speed_xy or x_only_threshold instead.");
|
||||
// node->param(resolved_name, x_only_threshold_);
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// x_only_threshold_ = 0.05;
|
||||
// }
|
||||
|
||||
reset();
|
||||
}
|
||||
|
||||
bool OscillationCritic::prepare(
|
||||
const geometry_msgs::msg::Pose2D & pose,
|
||||
const nav_2d_msgs::msg::Twist2D &,
|
||||
const geometry_msgs::msg::Pose2D &,
|
||||
const nav_2d_msgs::msg::Path2D &)
|
||||
{
|
||||
pose_ = pose;
|
||||
return true;
|
||||
}
|
||||
|
||||
void OscillationCritic::debrief(const nav_2d_msgs::msg::Twist2D & cmd_vel)
|
||||
{
|
||||
if (setOscillationFlags(cmd_vel)) {
|
||||
prev_stationary_pose_ = pose_;
|
||||
prev_reset_time_ = clock_->now();
|
||||
}
|
||||
|
||||
// if we've got restrictions... check if we can reset any oscillation flags
|
||||
if (x_trend_.hasSignFlipped() || y_trend_.hasSignFlipped() || theta_trend_.hasSignFlipped()) {
|
||||
// Reset flags if enough time or distance has passed
|
||||
if (resetAvailable()) {
|
||||
reset();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool OscillationCritic::resetAvailable()
|
||||
{
|
||||
if (oscillation_reset_dist_ >= 0.0) {
|
||||
double x_diff = pose_.x - prev_stationary_pose_.x;
|
||||
double y_diff = pose_.y - prev_stationary_pose_.y;
|
||||
double sq_dist = x_diff * x_diff + y_diff * y_diff;
|
||||
if (sq_dist > oscillation_reset_dist_sq_) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
if (oscillation_reset_angle_ >= 0.0) {
|
||||
double th_diff = pose_.theta - prev_stationary_pose_.theta;
|
||||
if (fabs(th_diff) > oscillation_reset_angle_) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
if (oscillation_reset_time_ >= rclcpp::Duration::from_seconds(0.0)) {
|
||||
auto t_diff = (clock_->now() - prev_reset_time_);
|
||||
if (t_diff > oscillation_reset_time_) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void OscillationCritic::reset()
|
||||
{
|
||||
x_trend_.reset();
|
||||
y_trend_.reset();
|
||||
theta_trend_.reset();
|
||||
}
|
||||
|
||||
bool OscillationCritic::setOscillationFlags(const nav_2d_msgs::msg::Twist2D & cmd_vel)
|
||||
{
|
||||
bool flag_set = false;
|
||||
// set oscillation flags for moving forward and backward
|
||||
flag_set |= x_trend_.update(cmd_vel.x);
|
||||
|
||||
// we'll only set flags for strafing and rotating when we're not moving forward at all
|
||||
if (x_only_threshold_ < 0.0 || fabs(cmd_vel.x) <= x_only_threshold_) {
|
||||
flag_set |= y_trend_.update(cmd_vel.y);
|
||||
flag_set |= theta_trend_.update(cmd_vel.theta);
|
||||
}
|
||||
return flag_set;
|
||||
}
|
||||
|
||||
double OscillationCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj)
|
||||
{
|
||||
if (x_trend_.isOscillating(traj.velocity.x) ||
|
||||
y_trend_.isOscillating(traj.velocity.y) ||
|
||||
theta_trend_.isOscillating(traj.velocity.theta))
|
||||
{
|
||||
throw dwb_core::
|
||||
IllegalTrajectoryException(name_, "Trajectory is oscillating.");
|
||||
}
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
} // namespace dwb_critics
|
||||
@@ -0,0 +1,95 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 "dwb_critics/path_align.hpp"
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include "dwb_critics/alignment_util.hpp"
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
#include "nav_2d_utils/parameters.hpp"
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
|
||||
void PathAlignCritic::onInit()
|
||||
{
|
||||
PathDistCritic::onInit();
|
||||
stop_on_failure_ = false;
|
||||
|
||||
auto node = node_.lock();
|
||||
if (!node) {
|
||||
throw std::runtime_error{"Failed to lock node"};
|
||||
}
|
||||
|
||||
forward_point_distance_ = nav_2d_utils::searchAndGetParam(
|
||||
node,
|
||||
dwb_plugin_name_ + "." + name_ + ".forward_point_distance", 0.325);
|
||||
}
|
||||
|
||||
bool PathAlignCritic::prepare(
|
||||
const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
|
||||
const geometry_msgs::msg::Pose2D & goal,
|
||||
const nav_2d_msgs::msg::Path2D & global_plan)
|
||||
{
|
||||
double dx = pose.x - goal.x;
|
||||
double dy = pose.y - goal.y;
|
||||
double sq_dist = dx * dx + dy * dy;
|
||||
if (sq_dist > forward_point_distance_ * forward_point_distance_) {
|
||||
zero_scale_ = false;
|
||||
} else {
|
||||
// once we are close to goal, trying to keep the nose close to anything destabilizes behavior.
|
||||
zero_scale_ = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
return PathDistCritic::prepare(pose, vel, goal, global_plan);
|
||||
}
|
||||
|
||||
double PathAlignCritic::getScale() const
|
||||
{
|
||||
if (zero_scale_) {
|
||||
return 0.0;
|
||||
} else {
|
||||
return costmap_->getResolution() * 0.5 * scale_;
|
||||
}
|
||||
}
|
||||
|
||||
double PathAlignCritic::scorePose(const geometry_msgs::msg::Pose2D & pose)
|
||||
{
|
||||
return PathDistCritic::scorePose(getForwardPose(pose, forward_point_distance_));
|
||||
}
|
||||
|
||||
} // namespace dwb_critics
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(dwb_critics::PathAlignCritic, dwb_core::TrajectoryCritic)
|
||||
@@ -0,0 +1,95 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 "dwb_critics/path_dist.hpp"
|
||||
#include <vector>
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
#include "nav_2d_utils/path_ops.hpp"
|
||||
#include "nav2_costmap_2d/cost_values.hpp"
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
bool PathDistCritic::prepare(
|
||||
const geometry_msgs::msg::Pose2D &, const nav_2d_msgs::msg::Twist2D &,
|
||||
const geometry_msgs::msg::Pose2D &,
|
||||
const nav_2d_msgs::msg::Path2D & global_plan)
|
||||
{
|
||||
reset();
|
||||
bool started_path = false;
|
||||
|
||||
nav_2d_msgs::msg::Path2D adjusted_global_plan =
|
||||
nav_2d_utils::adjustPlanResolution(global_plan, costmap_->getResolution());
|
||||
|
||||
if (adjusted_global_plan.poses.size() != global_plan.poses.size()) {
|
||||
RCLCPP_DEBUG(
|
||||
rclcpp::get_logger(
|
||||
"PathDistCritic"), "Adjusted global plan resolution, added %zu points",
|
||||
adjusted_global_plan.poses.size() - global_plan.poses.size());
|
||||
}
|
||||
|
||||
unsigned int i;
|
||||
// put global path points into local map until we reach the border of the local map
|
||||
for (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 (costmap_->worldToMap(
|
||||
g_x, g_y, map_x,
|
||||
map_y) && costmap_->getCost(map_x, map_y) != nav2_costmap_2d::NO_INFORMATION)
|
||||
{
|
||||
int index = costmap_->getIndex(map_x, map_y);
|
||||
cell_values_[index] = 0.0;
|
||||
queue_->enqueueCell(map_x, map_y);
|
||||
started_path = true;
|
||||
} else if (started_path) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!started_path) {
|
||||
RCLCPP_ERROR(
|
||||
rclcpp::get_logger("PathDistCritic"),
|
||||
"None of the %d first of %zu (%zu) points of the global plan were in "
|
||||
"the local costmap and free",
|
||||
i, adjusted_global_plan.poses.size(), global_plan.poses.size());
|
||||
return false;
|
||||
}
|
||||
|
||||
propogateManhattanDistances();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace dwb_critics
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(dwb_critics::PathDistCritic, dwb_core::TrajectoryCritic)
|
||||
@@ -0,0 +1,88 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 "dwb_critics/prefer_forward.hpp"
|
||||
#include <math.h>
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(dwb_critics::PreferForwardCritic, dwb_core::TrajectoryCritic)
|
||||
|
||||
using nav2_util::declare_parameter_if_not_declared;
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
|
||||
void PreferForwardCritic::onInit()
|
||||
{
|
||||
auto node = node_.lock();
|
||||
if (!node) {
|
||||
throw std::runtime_error{"Failed to lock node"};
|
||||
}
|
||||
|
||||
declare_parameter_if_not_declared(
|
||||
node,
|
||||
dwb_plugin_name_ + "." + name_ + ".penalty", rclcpp::ParameterValue(1.0));
|
||||
declare_parameter_if_not_declared(
|
||||
node,
|
||||
dwb_plugin_name_ + "." + name_ + ".strafe_x", rclcpp::ParameterValue(0.1));
|
||||
declare_parameter_if_not_declared(
|
||||
node, dwb_plugin_name_ + "." + name_ + ".strafe_theta",
|
||||
rclcpp::ParameterValue(0.2));
|
||||
declare_parameter_if_not_declared(
|
||||
node, dwb_plugin_name_ + "." + name_ + ".theta_scale",
|
||||
rclcpp::ParameterValue(10.0));
|
||||
|
||||
node->get_parameter(dwb_plugin_name_ + "." + name_ + ".penalty", penalty_);
|
||||
node->get_parameter(dwb_plugin_name_ + "." + name_ + ".strafe_x", strafe_x_);
|
||||
node->get_parameter(dwb_plugin_name_ + "." + name_ + ".strafe_theta", strafe_theta_);
|
||||
node->get_parameter(dwb_plugin_name_ + "." + name_ + ".theta_scale", theta_scale_);
|
||||
}
|
||||
|
||||
double PreferForwardCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj)
|
||||
{
|
||||
// backward motions bad on a robot without backward sensors
|
||||
if (traj.velocity.x < 0.0) {
|
||||
return penalty_;
|
||||
}
|
||||
// strafing motions also bad on such a robot
|
||||
if (traj.velocity.x < strafe_x_ && fabs(traj.velocity.theta) < strafe_theta_) {
|
||||
return penalty_;
|
||||
}
|
||||
|
||||
// the more we rotate, the less we progress forward
|
||||
return fabs(traj.velocity.theta) * theta_scale_;
|
||||
}
|
||||
|
||||
} // namespace dwb_critics
|
||||
@@ -0,0 +1,135 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 "dwb_critics/rotate_to_goal.hpp"
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include "nav_2d_utils/parameters.hpp"
|
||||
#include "dwb_core/exceptions.hpp"
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
#include "dwb_core/trajectory_utils.hpp"
|
||||
#include "angles/angles.h"
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(dwb_critics::RotateToGoalCritic, dwb_core::TrajectoryCritic)
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
|
||||
inline double hypot_sq(double dx, double dy)
|
||||
{
|
||||
return dx * dx + dy * dy;
|
||||
}
|
||||
|
||||
void RotateToGoalCritic::onInit()
|
||||
{
|
||||
auto node = node_.lock();
|
||||
if (!node) {
|
||||
throw std::runtime_error{"Failed to lock node"};
|
||||
}
|
||||
|
||||
xy_goal_tolerance_ = nav_2d_utils::searchAndGetParam(
|
||||
node,
|
||||
dwb_plugin_name_ + ".xy_goal_tolerance", 0.25);
|
||||
xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
|
||||
double stopped_xy_velocity = nav_2d_utils::searchAndGetParam(
|
||||
node,
|
||||
dwb_plugin_name_ + ".trans_stopped_velocity", 0.25);
|
||||
stopped_xy_velocity_sq_ = stopped_xy_velocity * stopped_xy_velocity;
|
||||
slowing_factor_ = nav_2d_utils::searchAndGetParam(
|
||||
node,
|
||||
dwb_plugin_name_ + "." + name_ + ".slowing_factor", 5.0);
|
||||
lookahead_time_ = nav_2d_utils::searchAndGetParam(
|
||||
node,
|
||||
dwb_plugin_name_ + "." + name_ + ".lookahead_time", -1.0);
|
||||
reset();
|
||||
}
|
||||
|
||||
void RotateToGoalCritic::reset()
|
||||
{
|
||||
in_window_ = false;
|
||||
rotating_ = false;
|
||||
}
|
||||
|
||||
bool RotateToGoalCritic::prepare(
|
||||
const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
|
||||
const geometry_msgs::msg::Pose2D & goal,
|
||||
const nav_2d_msgs::msg::Path2D &)
|
||||
{
|
||||
double dxy_sq = hypot_sq(pose.x - goal.x, pose.y - goal.y);
|
||||
in_window_ = in_window_ || dxy_sq <= xy_goal_tolerance_sq_;
|
||||
current_xy_speed_sq_ = hypot_sq(vel.x, vel.y);
|
||||
rotating_ = rotating_ || (in_window_ && current_xy_speed_sq_ <= stopped_xy_velocity_sq_);
|
||||
goal_yaw_ = goal.theta;
|
||||
return true;
|
||||
}
|
||||
|
||||
double RotateToGoalCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj)
|
||||
{
|
||||
// If we're not sufficiently close to the goal, we don't care what the twist is
|
||||
if (!in_window_) {
|
||||
return 0.0;
|
||||
} else if (!rotating_) {
|
||||
double speed_sq = hypot_sq(traj.velocity.x, traj.velocity.y);
|
||||
if (speed_sq >= current_xy_speed_sq_) {
|
||||
throw dwb_core::IllegalTrajectoryException(name_, "Not slowing down near goal.");
|
||||
}
|
||||
return speed_sq * slowing_factor_ + scoreRotation(traj);
|
||||
}
|
||||
|
||||
// If we're sufficiently close to the goal, any transforming velocity is invalid
|
||||
if (fabs(traj.velocity.x) > 0 || fabs(traj.velocity.y) > 0) {
|
||||
throw dwb_core::
|
||||
IllegalTrajectoryException(name_, "Nonrotation command near goal.");
|
||||
}
|
||||
|
||||
return scoreRotation(traj);
|
||||
}
|
||||
|
||||
double RotateToGoalCritic::scoreRotation(const dwb_msgs::msg::Trajectory2D & traj)
|
||||
{
|
||||
if (traj.poses.empty()) {
|
||||
throw dwb_core::IllegalTrajectoryException(name_, "Empty trajectory.");
|
||||
}
|
||||
|
||||
double end_yaw;
|
||||
if (lookahead_time_ >= 0.0) {
|
||||
geometry_msgs::msg::Pose2D eval_pose = dwb_core::projectPose(traj, lookahead_time_);
|
||||
end_yaw = eval_pose.theta;
|
||||
} else {
|
||||
end_yaw = traj.poses.back().theta;
|
||||
}
|
||||
return fabs(angles::shortest_angular_distance(end_yaw, goal_yaw_));
|
||||
}
|
||||
|
||||
} // namespace dwb_critics
|
||||
@@ -0,0 +1,56 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 "dwb_critics/twirling.hpp"
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
void TwirlingCritic::onInit()
|
||||
{
|
||||
auto node = node_.lock();
|
||||
if (!node) {
|
||||
throw std::runtime_error{"Failed to lock node"};
|
||||
}
|
||||
// Scale is set to 0 by default, so if it was not set otherwise, set to 0
|
||||
node->get_parameter(dwb_plugin_name_ + "." + name_ + ".scale", scale_);
|
||||
}
|
||||
|
||||
double TwirlingCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj)
|
||||
{
|
||||
return fabs(traj.velocity.theta); // add cost for making the robot spin
|
||||
}
|
||||
} // namespace dwb_critics
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(dwb_critics::TwirlingCritic, dwb_core::TrajectoryCritic)
|
||||
@@ -0,0 +1,14 @@
|
||||
ament_add_gtest(prefer_forward_tests prefer_forward_test.cpp)
|
||||
target_link_libraries(prefer_forward_tests dwb_critics)
|
||||
|
||||
ament_add_gtest(base_obstacle_tests base_obstacle_test.cpp)
|
||||
target_link_libraries(base_obstacle_tests dwb_critics)
|
||||
|
||||
ament_add_gtest(obstacle_footprint_tests obstacle_footprint_test.cpp)
|
||||
target_link_libraries(obstacle_footprint_tests dwb_critics)
|
||||
|
||||
ament_add_gtest(alignment_util_tests alignment_util_test.cpp)
|
||||
target_link_libraries(alignment_util_tests dwb_critics)
|
||||
|
||||
ament_add_gtest(twirling_tests twirling_test.cpp)
|
||||
target_link_libraries(twirling_tests dwb_critics)
|
||||
@@ -0,0 +1,77 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2020, Samsung Research America
|
||||
* 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 <vector>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "dwb_critics/alignment_util.hpp"
|
||||
#include "dwb_core/exceptions.hpp"
|
||||
|
||||
TEST(AlignmentUtil, TestProjection)
|
||||
{
|
||||
geometry_msgs::msg::Pose2D pose, pose_out;
|
||||
pose.x = 1.0;
|
||||
pose.y = -1.0;
|
||||
double distance = 1.0;
|
||||
pose_out = dwb_critics::getForwardPose(pose, distance);
|
||||
EXPECT_EQ(pose_out.x, 2.0);
|
||||
EXPECT_EQ(pose_out.y, -1.0);
|
||||
EXPECT_EQ(pose_out.theta, pose.theta);
|
||||
|
||||
pose.x = 2.0;
|
||||
pose.y = -10.0;
|
||||
pose.theta = 0.54;
|
||||
pose_out = dwb_critics::getForwardPose(pose, distance);
|
||||
EXPECT_NEAR(pose_out.x, 2.8577, 0.01);
|
||||
EXPECT_NEAR(pose_out.y, -9.4858, 0.01);
|
||||
EXPECT_EQ(pose_out.theta, pose.theta);
|
||||
}
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
|
||||
// initialize ROS
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
bool all_successful = RUN_ALL_TESTS();
|
||||
|
||||
// shutdown ROS
|
||||
rclcpp::shutdown();
|
||||
|
||||
return all_successful;
|
||||
}
|
||||
@@ -0,0 +1,173 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2018, Wilco Bonestroo
|
||||
* 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 <vector>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "dwb_critics/obstacle_footprint.hpp"
|
||||
#include "dwb_core/exceptions.hpp"
|
||||
|
||||
TEST(BaseObstacle, IsValidCost)
|
||||
{
|
||||
std::shared_ptr<dwb_critics::BaseObstacleCritic> critic =
|
||||
std::make_shared<dwb_critics::BaseObstacleCritic>();
|
||||
|
||||
for (int i = 0; i < 256; i++) {
|
||||
// for these 3 values the cost is not "valid"
|
||||
if (i == nav2_costmap_2d::LETHAL_OBSTACLE ||
|
||||
i == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE ||
|
||||
i == nav2_costmap_2d::NO_INFORMATION)
|
||||
{
|
||||
ASSERT_FALSE(critic->isValidCost(i));
|
||||
} else {
|
||||
ASSERT_TRUE(critic->isValidCost(i));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST(BaseObstacle, ScorePose)
|
||||
{
|
||||
std::shared_ptr<dwb_critics::BaseObstacleCritic> critic =
|
||||
std::make_shared<dwb_critics::BaseObstacleCritic>();
|
||||
|
||||
auto node = nav2_util::LifecycleNode::make_shared("base_obstacle_critic_tester");
|
||||
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_global_costmap");
|
||||
costmap_ros->configure();
|
||||
|
||||
std::string name = "name";
|
||||
std::string ns = "ns";
|
||||
|
||||
critic->initialize(node, name, ns, costmap_ros);
|
||||
|
||||
costmap_ros->getCostmap()->setCost(0, 0, nav2_costmap_2d::LETHAL_OBSTACLE);
|
||||
costmap_ros->getCostmap()->setCost(0, 1, nav2_costmap_2d::NO_INFORMATION);
|
||||
const int some_other_cost = 128;
|
||||
costmap_ros->getCostmap()->setCost(0, 2, some_other_cost);
|
||||
|
||||
// The pose is in "world" coordinates. The (default) resolution is 0.1 m.
|
||||
geometry_msgs::msg::Pose2D pose;
|
||||
pose.x = 0;
|
||||
pose.y = 0;
|
||||
|
||||
ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
|
||||
|
||||
pose.x = 0;
|
||||
pose.y = 0.15;
|
||||
ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
|
||||
|
||||
pose.y = 0.25;
|
||||
pose.x = 0.05;
|
||||
ASSERT_EQ(critic->scorePose(pose), some_other_cost);
|
||||
|
||||
// The theta should not influence the cost
|
||||
for (int i = -50; i < 150; i++) {
|
||||
pose.theta = (1.0 / 50) * i * M_PI;
|
||||
ASSERT_EQ(critic->scorePose(pose), some_other_cost);
|
||||
}
|
||||
|
||||
// Poses outside the map should throw an exception.
|
||||
pose.x = 1.0;
|
||||
pose.y = -0.1;
|
||||
ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
|
||||
|
||||
pose.x = costmap_ros->getCostmap()->getSizeInMetersX() + 0.1;
|
||||
pose.y = 1.0;
|
||||
ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
|
||||
|
||||
pose.x = 1.0;
|
||||
pose.y = costmap_ros->getCostmap()->getSizeInMetersY() + 0.1;
|
||||
ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
|
||||
|
||||
pose.x = -0.1;
|
||||
pose.y = 1.0;
|
||||
ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
|
||||
}
|
||||
|
||||
TEST(BaseObstacle, CriticVisualization)
|
||||
{
|
||||
std::shared_ptr<dwb_critics::BaseObstacleCritic> critic =
|
||||
std::make_shared<dwb_critics::BaseObstacleCritic>();
|
||||
|
||||
auto node = nav2_util::LifecycleNode::make_shared("base_obstacle_critic_tester");
|
||||
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_global_costmap");
|
||||
costmap_ros->configure();
|
||||
|
||||
std::string name = "name";
|
||||
std::string ns = "ns";
|
||||
|
||||
critic->initialize(node, name, ns, costmap_ros);
|
||||
|
||||
costmap_ros->getCostmap()->setCost(0, 0, nav2_costmap_2d::LETHAL_OBSTACLE);
|
||||
costmap_ros->getCostmap()->setCost(0, 1, nav2_costmap_2d::NO_INFORMATION);
|
||||
// Some random values
|
||||
costmap_ros->getCostmap()->setCost(3, 2, 64);
|
||||
costmap_ros->getCostmap()->setCost(30, 12, 85);
|
||||
costmap_ros->getCostmap()->setCost(10, 49, 24);
|
||||
costmap_ros->getCostmap()->setCost(45, 2, 12);
|
||||
|
||||
std::vector<std::pair<std::string, std::vector<float>>> cost_channels;
|
||||
critic->addCriticVisualization(cost_channels);
|
||||
|
||||
unsigned int size_x = costmap_ros->getCostmap()->getSizeInCellsX();
|
||||
unsigned int size_y = costmap_ros->getCostmap()->getSizeInCellsY();
|
||||
|
||||
// The values in the pointcloud should be equal to the values in the costmap
|
||||
for (unsigned int y = 0; y < size_y; y++) {
|
||||
for (unsigned int x = 0; x < size_x; x++) {
|
||||
float pointValue = cost_channels[0].second[y * size_y + x];
|
||||
ASSERT_EQ(static_cast<int>(pointValue), costmap_ros->getCostmap()->getCost(x, y));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
|
||||
// initialize ROS
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
bool all_successful = RUN_ALL_TESTS();
|
||||
|
||||
// shutdown ROS
|
||||
rclcpp::shutdown();
|
||||
|
||||
return all_successful;
|
||||
}
|
||||
@@ -0,0 +1,264 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2018, Wilco Bonestroo
|
||||
* 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 <vector>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "dwb_critics/obstacle_footprint.hpp"
|
||||
#include "dwb_core/exceptions.hpp"
|
||||
|
||||
class OpenObstacleFootprintCritic : public dwb_critics::ObstacleFootprintCritic
|
||||
{
|
||||
public:
|
||||
double pointCost(int x, int y)
|
||||
{
|
||||
return dwb_critics::ObstacleFootprintCritic::pointCost(x, y);
|
||||
}
|
||||
|
||||
double lineCost(int x0, int x1, int y0, int y1)
|
||||
{
|
||||
return dwb_critics::ObstacleFootprintCritic::lineCost(x0, x1, y0, y1);
|
||||
}
|
||||
};
|
||||
|
||||
// Rotate the given point for angle radians around the origin.
|
||||
geometry_msgs::msg::Point rotate_origin(geometry_msgs::msg::Point p, double angle)
|
||||
{
|
||||
double s = sin(angle);
|
||||
double c = cos(angle);
|
||||
|
||||
// rotate point
|
||||
double xnew = p.x * c - p.y * s;
|
||||
double ynew = p.x * s + p.y * c;
|
||||
|
||||
p.x = xnew;
|
||||
p.y = ynew;
|
||||
|
||||
return p;
|
||||
}
|
||||
|
||||
// Auxilary function to create a Point with given x and y values.
|
||||
geometry_msgs::msg::Point getPoint(double x, double y)
|
||||
{
|
||||
geometry_msgs::msg::Point p;
|
||||
p.x = x;
|
||||
p.y = y;
|
||||
return p;
|
||||
}
|
||||
|
||||
// Variables
|
||||
double footprint_size_x_half = 1.8;
|
||||
double footprint_size_y_half = 1.6;
|
||||
|
||||
std::vector<geometry_msgs::msg::Point> getFootprint()
|
||||
{
|
||||
std::vector<geometry_msgs::msg::Point> footprint;
|
||||
footprint.push_back(getPoint(footprint_size_x_half, footprint_size_y_half));
|
||||
footprint.push_back(getPoint(footprint_size_x_half, -footprint_size_y_half));
|
||||
footprint.push_back(getPoint(-footprint_size_x_half, -footprint_size_y_half));
|
||||
footprint.push_back(getPoint(-footprint_size_x_half, footprint_size_y_half));
|
||||
return footprint;
|
||||
}
|
||||
|
||||
TEST(ObstacleFootprint, GetOrientedFootprint)
|
||||
{
|
||||
double theta = 0.1234;
|
||||
|
||||
std::vector<geometry_msgs::msg::Point> footprint_before = getFootprint();
|
||||
|
||||
std::vector<geometry_msgs::msg::Point> footprint_after;
|
||||
geometry_msgs::msg::Pose2D pose;
|
||||
pose.theta = theta;
|
||||
footprint_after = dwb_critics::getOrientedFootprint(pose, footprint_before);
|
||||
|
||||
uint i;
|
||||
for (i = 0; i < footprint_before.size(); i++) {
|
||||
ASSERT_EQ(rotate_origin(footprint_before[i], theta), footprint_after[i]);
|
||||
}
|
||||
|
||||
theta = 5.123;
|
||||
pose.theta = theta;
|
||||
footprint_after = dwb_critics::getOrientedFootprint(pose, footprint_before);
|
||||
|
||||
for (unsigned int i = 0; i < footprint_before.size(); i++) {
|
||||
ASSERT_EQ(rotate_origin(footprint_before[i], theta), footprint_after[i]);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(ObstacleFootprint, Prepare)
|
||||
{
|
||||
std::shared_ptr<dwb_critics::ObstacleFootprintCritic> critic =
|
||||
std::make_shared<dwb_critics::ObstacleFootprintCritic>();
|
||||
|
||||
auto node = nav2_util::LifecycleNode::make_shared("costmap_tester");
|
||||
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_global_costmap");
|
||||
costmap_ros->configure();
|
||||
|
||||
std::string name = "name";
|
||||
std::string ns = "ns";
|
||||
critic->initialize(node, name, ns, costmap_ros);
|
||||
|
||||
geometry_msgs::msg::Pose2D pose;
|
||||
nav_2d_msgs::msg::Twist2D vel;
|
||||
geometry_msgs::msg::Pose2D goal;
|
||||
nav_2d_msgs::msg::Path2D global_plan;
|
||||
|
||||
// no footprint set in the costmap. Prepare should return false;
|
||||
std::vector<geometry_msgs::msg::Point> footprint;
|
||||
costmap_ros->setRobotFootprint(footprint);
|
||||
ASSERT_FALSE(critic->prepare(pose, vel, goal, global_plan));
|
||||
|
||||
costmap_ros->setRobotFootprint(getFootprint());
|
||||
ASSERT_TRUE(critic->prepare(pose, vel, goal, global_plan));
|
||||
|
||||
double epsilon = 0.01;
|
||||
// If the robot footprint goes of the map, it should throw an exception
|
||||
// The following cases put the robot over the edge of the map on the left, bottom, right and top
|
||||
|
||||
pose.x = footprint_size_x_half; // This gives an error
|
||||
pose.y = footprint_size_y_half + epsilon;
|
||||
ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
|
||||
|
||||
pose.x = footprint_size_x_half + epsilon;
|
||||
pose.y = footprint_size_y_half; // error
|
||||
ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
|
||||
|
||||
pose.x = costmap_ros->getCostmap()->getSizeInMetersX() - footprint_size_x_half; // error
|
||||
pose.y = costmap_ros->getCostmap()->getSizeInMetersY() + footprint_size_y_half - epsilon;
|
||||
ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
|
||||
|
||||
pose.x = costmap_ros->getCostmap()->getSizeInMetersX() - footprint_size_x_half - epsilon;
|
||||
pose.y = costmap_ros->getCostmap()->getSizeInMetersY() + footprint_size_y_half; // error
|
||||
ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
|
||||
|
||||
pose.x = footprint_size_x_half + epsilon;
|
||||
pose.y = footprint_size_y_half + epsilon;
|
||||
ASSERT_EQ(critic->scorePose(pose), 0.0);
|
||||
|
||||
for (unsigned int i = 1; i < costmap_ros->getCostmap()->getSizeInCellsX(); i++) {
|
||||
costmap_ros->getCostmap()->setCost(i, 10, nav2_costmap_2d::LETHAL_OBSTACLE);
|
||||
}
|
||||
// It should now hit an obstacle (throw an expection)
|
||||
ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
|
||||
}
|
||||
|
||||
// todo: wilcobonestroo Add tests for other footprint shapes and costmaps.
|
||||
|
||||
TEST(ObstacleFootprint, PointCost)
|
||||
{
|
||||
std::shared_ptr<OpenObstacleFootprintCritic> critic =
|
||||
std::make_shared<OpenObstacleFootprintCritic>();
|
||||
|
||||
auto node = nav2_util::LifecycleNode::make_shared("costmap_tester");
|
||||
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_global_costmap");
|
||||
costmap_ros->configure();
|
||||
|
||||
std::string name = "name";
|
||||
std::string ns = "ns";
|
||||
critic->initialize(node, name, ns, costmap_ros);
|
||||
|
||||
costmap_ros->getCostmap()->setCost(0, 0, nav2_costmap_2d::LETHAL_OBSTACLE);
|
||||
costmap_ros->getCostmap()->setCost(0, 1, nav2_costmap_2d::NO_INFORMATION);
|
||||
costmap_ros->getCostmap()->setCost(0, 2, 128);
|
||||
|
||||
ASSERT_THROW(critic->pointCost(0, 0), dwb_core::IllegalTrajectoryException);
|
||||
ASSERT_THROW(critic->pointCost(0, 1), dwb_core::IllegalTrajectoryException);
|
||||
ASSERT_EQ(critic->pointCost(0, 2), 128);
|
||||
}
|
||||
|
||||
TEST(ObstacleFootprint, LineCost)
|
||||
{
|
||||
std::shared_ptr<OpenObstacleFootprintCritic> critic =
|
||||
std::make_shared<OpenObstacleFootprintCritic>();
|
||||
|
||||
auto node = nav2_util::LifecycleNode::make_shared("costmap_tester");
|
||||
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_global_costmap");
|
||||
costmap_ros->configure();
|
||||
|
||||
std::string name = "name";
|
||||
std::string ns = "ns";
|
||||
critic->initialize(node, name, ns, costmap_ros);
|
||||
|
||||
costmap_ros->getCostmap()->setCost(3, 3, nav2_costmap_2d::LETHAL_OBSTACLE);
|
||||
costmap_ros->getCostmap()->setCost(3, 4, nav2_costmap_2d::LETHAL_OBSTACLE);
|
||||
costmap_ros->getCostmap()->setCost(4, 3, nav2_costmap_2d::LETHAL_OBSTACLE);
|
||||
costmap_ros->getCostmap()->setCost(4, 4, nav2_costmap_2d::LETHAL_OBSTACLE);
|
||||
|
||||
ASSERT_THROW(critic->lineCost(0, 5, 2, 6), dwb_core::IllegalTrajectoryException);
|
||||
ASSERT_THROW(critic->lineCost(5, 0, 6, 2), dwb_core::IllegalTrajectoryException);
|
||||
|
||||
ASSERT_THROW(critic->lineCost(2, 4, 0, 10), dwb_core::IllegalTrajectoryException);
|
||||
ASSERT_THROW(critic->lineCost(4, 2, 10, 0), dwb_core::IllegalTrajectoryException);
|
||||
|
||||
// These all miss the obstacle
|
||||
ASSERT_EQ(critic->lineCost(2, 2, 0, 10), 0.0);
|
||||
ASSERT_EQ(critic->lineCost(2, 2, 10, 0), 0.0);
|
||||
ASSERT_EQ(critic->lineCost(5, 5, 0, 10), 0.0);
|
||||
ASSERT_EQ(critic->lineCost(5, 5, 10, 0), 0.0);
|
||||
ASSERT_EQ(critic->lineCost(0, 50, 2, 2), 0.0);
|
||||
ASSERT_EQ(critic->lineCost(50, 0, 2, 2), 0.0);
|
||||
ASSERT_EQ(critic->lineCost(0, 50, 5, 5), 0.0);
|
||||
ASSERT_EQ(critic->lineCost(50, 0, 5, 5), 0.0);
|
||||
|
||||
// Use valid costs
|
||||
costmap_ros->getCostmap()->setCost(3, 3, 50);
|
||||
costmap_ros->getCostmap()->setCost(3, 4, 50);
|
||||
costmap_ros->getCostmap()->setCost(4, 3, 100);
|
||||
costmap_ros->getCostmap()->setCost(4, 4, 100);
|
||||
|
||||
ASSERT_EQ(critic->lineCost(3, 3, 0, 50), 50); // all 50
|
||||
ASSERT_EQ(critic->lineCost(4, 4, 0, 10), 100); // all 100
|
||||
ASSERT_EQ(critic->lineCost(0, 50, 3, 3), 100); // pass 50 and 100
|
||||
}
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
|
||||
// initialize ROS
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
bool all_successful = RUN_ALL_TESTS();
|
||||
|
||||
// shutdown ROS
|
||||
rclcpp::shutdown();
|
||||
|
||||
return all_successful;
|
||||
}
|
||||
@@ -0,0 +1,229 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2018, Wilco Bonestroo
|
||||
* 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 <math.h>
|
||||
#include <vector>
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <limits>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "dwb_critics/prefer_forward.hpp"
|
||||
#include "nav2_costmap_2d/costmap_2d.hpp"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
|
||||
static constexpr double default_penalty = 1.0;
|
||||
static constexpr double default_strafe_x = 0.1;
|
||||
static constexpr double default_strafe_theta = 0.2;
|
||||
static constexpr double default_theta_scale = 10.0;
|
||||
|
||||
TEST(PreferForward, StartNode)
|
||||
{
|
||||
auto critic = std::make_shared<dwb_critics::PreferForwardCritic>();
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_global_costmap");
|
||||
auto node = nav2_util::LifecycleNode::make_shared("costmap_tester");
|
||||
node->configure();
|
||||
node->activate();
|
||||
|
||||
std::string name = "test";
|
||||
std::string ns = "ns";
|
||||
critic->initialize(node, name, ns, costmap_ros);
|
||||
|
||||
EXPECT_EQ(node->get_parameter(ns + "." + name + ".penalty").as_double(), default_penalty);
|
||||
EXPECT_EQ(node->get_parameter(ns + "." + name + ".strafe_x").as_double(), default_strafe_x);
|
||||
EXPECT_EQ(
|
||||
node->get_parameter(ns + "." + name + ".strafe_theta").as_double(), default_strafe_theta);
|
||||
EXPECT_EQ(node->get_parameter(ns + "." + name + ".theta_scale").as_double(), default_theta_scale);
|
||||
}
|
||||
|
||||
TEST(PreferForward, NegativeVelocityX)
|
||||
{
|
||||
auto critic = std::make_shared<dwb_critics::PreferForwardCritic>();
|
||||
dwb_msgs::msg::Trajectory2D trajectory;
|
||||
|
||||
// score must be equal to the penalty (1.0) for any negative x velocity
|
||||
trajectory.velocity.x = -1.0;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), default_penalty);
|
||||
|
||||
trajectory.velocity.x = -0.00001;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), default_penalty);
|
||||
|
||||
trajectory.velocity.x = -0.1;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), default_penalty);
|
||||
|
||||
trajectory.velocity.x = std::numeric_limits<double>::lowest();
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), default_penalty);
|
||||
|
||||
trajectory.velocity.x = -std::numeric_limits<double>::min();
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), default_penalty);
|
||||
}
|
||||
|
||||
TEST(PreferForward, Strafe)
|
||||
{
|
||||
auto critic = std::make_shared<dwb_critics::PreferForwardCritic>();
|
||||
dwb_msgs::msg::Trajectory2D trajectory;
|
||||
|
||||
// score must be equal to the penalty (1.0) when x vel is lower than 0.1
|
||||
// and theta is between -0.2 and 0.2
|
||||
trajectory.velocity.x = 0.05;
|
||||
trajectory.velocity.theta = -0.1;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), default_penalty);
|
||||
|
||||
trajectory.velocity.x = 0.0999999;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), default_penalty);
|
||||
|
||||
trajectory.velocity.x = 0.000001;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), default_penalty);
|
||||
|
||||
trajectory.velocity.theta = -0.19;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), default_penalty);
|
||||
|
||||
trajectory.velocity.theta = 0.19;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), default_penalty);
|
||||
}
|
||||
|
||||
TEST(PreferForward, Normal)
|
||||
{
|
||||
auto critic = std::make_shared<dwb_critics::PreferForwardCritic>();
|
||||
dwb_msgs::msg::Trajectory2D trajectory;
|
||||
|
||||
// score must be equal to the theta * scaling factor (10.0)
|
||||
trajectory.velocity.x = 0.2;
|
||||
trajectory.velocity.theta = -0.1;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), 0.1 * default_theta_scale);
|
||||
|
||||
trajectory.velocity.theta = 0.1;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), 0.1 * default_theta_scale);
|
||||
|
||||
trajectory.velocity.theta = -0.2;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), 0.2 * default_theta_scale);
|
||||
|
||||
trajectory.velocity.theta = 0.2;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), 0.2 * default_theta_scale);
|
||||
|
||||
trajectory.velocity.theta = 1.5;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), 1.5 * default_theta_scale);
|
||||
}
|
||||
|
||||
TEST(PreferForward, NoneDefaultValues)
|
||||
{
|
||||
auto critic = std::make_shared<dwb_critics::PreferForwardCritic>();
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_global_costmap");
|
||||
auto node = nav2_util::LifecycleNode::make_shared("costmap_tester");
|
||||
node->configure();
|
||||
node->activate();
|
||||
|
||||
double penalty = 18.3;
|
||||
double strafe_x = 0.5;
|
||||
double strafe_theta = 0.4;
|
||||
double theta_scale = 15.0;
|
||||
|
||||
std::string name = "test";
|
||||
std::string ns = "ns";
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, ns + "." + name + ".penalty",
|
||||
rclcpp::ParameterValue(penalty));
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, ns + "." + name + ".strafe_x",
|
||||
rclcpp::ParameterValue(strafe_x));
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, ns + "." + name + ".strafe_theta",
|
||||
rclcpp::ParameterValue(strafe_theta));
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, ns + "." + name + ".theta_scale",
|
||||
rclcpp::ParameterValue(theta_scale));
|
||||
|
||||
critic->initialize(node, name, ns, costmap_ros);
|
||||
critic->onInit();
|
||||
|
||||
dwb_msgs::msg::Trajectory2D trajectory;
|
||||
trajectory.velocity.x = 0.05;
|
||||
trajectory.velocity.theta = -0.1;
|
||||
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), penalty);
|
||||
|
||||
// score must be equal to the penalty when x vel is lower than strafe_x
|
||||
// and theta is between -strafe_theta and strafe_theta
|
||||
trajectory.velocity.x = 0.4;
|
||||
trajectory.velocity.theta = -0.39;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), penalty);
|
||||
|
||||
trajectory.velocity.x = 0.0999999;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), penalty);
|
||||
|
||||
trajectory.velocity.x = 0.000001;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), penalty);
|
||||
|
||||
trajectory.velocity.theta = -0.09999;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), penalty);
|
||||
|
||||
trajectory.velocity.theta = 0.09999;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), penalty);
|
||||
|
||||
// score must be equal to the theta * scaling factor (10.0)
|
||||
trajectory.velocity.x = 0.5;
|
||||
trajectory.velocity.theta = -0.1;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), 0.1 * theta_scale);
|
||||
|
||||
trajectory.velocity.theta = 0.1;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), 0.1 * theta_scale);
|
||||
|
||||
trajectory.velocity.theta = -0.2;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), 0.2 * theta_scale);
|
||||
|
||||
trajectory.velocity.theta = 0.2;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), 0.2 * theta_scale);
|
||||
|
||||
trajectory.velocity.theta = 1.5;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), 1.5 * theta_scale);
|
||||
}
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
|
||||
// initialize ROS
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
bool all_successful = RUN_ALL_TESTS();
|
||||
|
||||
// shutdown ROS
|
||||
rclcpp::shutdown();
|
||||
|
||||
return all_successful;
|
||||
}
|
||||
@@ -0,0 +1,78 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2020, Samsung Research America
|
||||
* 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 <vector>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "dwb_critics/twirling.hpp"
|
||||
#include "dwb_core/exceptions.hpp"
|
||||
|
||||
TEST(TwirlingTests, Scoring)
|
||||
{
|
||||
std::shared_ptr<dwb_critics::TwirlingCritic> critic =
|
||||
std::make_shared<dwb_critics::TwirlingCritic>();
|
||||
|
||||
auto node = nav2_util::LifecycleNode::make_shared("costmap_tester");
|
||||
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_global_costmap");
|
||||
costmap_ros->configure();
|
||||
|
||||
std::string name = "name";
|
||||
std::string ns = "ns";
|
||||
critic->initialize(node, name, ns, costmap_ros);
|
||||
|
||||
dwb_msgs::msg::Trajectory2D traj;
|
||||
traj.velocity.theta = 1.0;
|
||||
EXPECT_EQ(critic->scoreTrajectory(traj), 1.0);
|
||||
traj.velocity.theta = -1.0;
|
||||
EXPECT_EQ(critic->scoreTrajectory(traj), 1.0);
|
||||
}
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
|
||||
// initialize ROS
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
bool all_successful = RUN_ALL_TESTS();
|
||||
|
||||
// shutdown ROS
|
||||
rclcpp::shutdown();
|
||||
|
||||
return all_successful;
|
||||
}
|
||||
Reference in New Issue
Block a user