m5p3nc3r/agv_pro_ros2
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

add humble-navigation2

Browse Source
This commit is contained in:
X-lanni
2025-05-27 19:03:40 +08:00
parent 974abb5e1e
commit e74ec539c2
1280 changed files with 204114 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
navigation2/nav2_dwb_controller/README.md
+58
View File
@@ -0,0 +1,58 @@
# DWB Controller
The DWB controller is the successor to the base local planner and DWA controllers in ROS 1. It was created in ROS 1 by David Lu!! at Locus Robotics as part of the `robot_navigation` project. It was then ported to ROS 2 for use in Nav2 as its critic-based controller algorithm.
DWB improves on DWA in a few major ways:
- It implements plugin-based critics to allow users to specify new critic functions to use in the system. They can be dynamically reconfigured, reweighted, and tuned to gain very particular behavior in your robot system.
- It implements plugin-based trajectory generation techniques, so that users can generate trajectories any number of ways and for any number of types of vehicles
- Includes a number of plugin implementations for common use
It is possible to tune DWB to gain both DWA and base local planner behaviors, as well as expansions using new plugins for totally use-case specific behaviors. The current trajectory generator plugins work for omnidirectional and differential drive robots, though an ackermann generator would be trivial to add. The current critic plugins work for both circular and non-circular robots and include many of the cost functions needed to build a path tracking system with various attributes.
See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-dwb-controller.html) for additional parameter descriptions.
## DWB Plugins
DWB is highly configurable through the use of plugins. There are three types of
plugins used. For each of them, a few implementations are available but you can
create custom ones if you need to.
### Trajectory Generator Plugins
These plugins generate the set of possible trajectories that should be evaluated
by the critics. The trajectory with the best score determines the output command
velocity.
There are two trajectory generators provided with Navigation 2. Only one can be
loaded at a time.
* **StandardTrajectoryGenerator** - This is similar to the trajectory rollout
algorithm used in base_local_planner in ROS 1.
* **LimitedAccelGenerator** - This is similar to DWA used in ROS 1.
### Critic Plugins
These plugins score the trajectories generated by the trajectory generator.
Multiple plugins can be loaded and the sum of their scores determines the chosen
command velocity.
* **BaseObstacle** - Scores a trajectory based on where the path passes over the
costmap. To use this properly, you must use the inflation layer in costmap to
expand obstacles by the robot's radius.
* **ObstacleFootprint** - Scores a trajectory based on verifying all points along
the robot's footprint don't touch an obstacle marked in the costmap.
* **GoalAlign** - Scores a trajectory based on how well aligned the trajectory is
with the goal pose.
* **GoalDist** - Scores a trajectory based on how close the trajectory gets the robot
to the goal pose.
* **PathAlign** - Scores a trajectory based on how well it is aligned to the path
provided by the global planner.
* **PathDist** - Scores a trajectory based on how far it ends up from the path
provided by the global planner.
* **PreferForward** - Scores trajectories that move the robot forwards more highly
* **RotateToGoal** - Only allows the robot to rotate to the goal orientation when it
is sufficiently close to the goal location
* **Oscillation** - Prevents the robot from just moving backwards and forwards.
* **Twirling** - Prevents holonomic robots from spinning as they make their way to
the goal.
navigation2/nav2_dwb_controller/costmap_queue/CMakeLists.txt
+57
View File
@@ -0,0 +1,57 @@
cmake_minimum_required(VERSION 3.5)
project(costmap_queue)
find_package(ament_cmake REQUIRED)
find_package(nav2_common REQUIRED)
find_package(nav2_costmap_2d REQUIRED)
find_package(rclcpp REQUIRED)
nav2_package()
include_directories(
include
)
add_library(${PROJECT_NAME} SHARED
src/costmap_queue.cpp
src/limited_costmap_queue.cpp
)
set(dependencies
rclcpp
nav2_costmap_2d
)
ament_target_dependencies(${PROJECT_NAME}
${dependencies}
)
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)
ament_add_gtest(mbq_test test/mbq_test.cpp)
ament_target_dependencies(mbq_test ${dependencies})
ament_add_gtest(utest test/utest.cpp)
ament_target_dependencies(utest ${dependencies})
target_link_libraries(utest ${PROJECT_NAME})
endif()
install(TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
install(DIRECTORY include/
DESTINATION include/
)
ament_export_include_directories(include)
ament_export_libraries(${PROJECT_NAME})
ament_package()
navigation2/nav2_dwb_controller/costmap_queue/include/costmap_queue/costmap_queue.hpp
+189
View File
@@ -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.
*/
#ifndef COSTMAP_QUEUE__COSTMAP_QUEUE_HPP_
#define COSTMAP_QUEUE__COSTMAP_QUEUE_HPP_
#include <cmath>
#include <vector>
#include <limits>
#include <memory>
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "costmap_queue/map_based_queue.hpp"
namespace costmap_queue
{
/**
* @class CellData
* @brief Storage for cell information used during queue expansion
*/
class CellData
{
public:
/**
* @brief Real Constructor
* @param d The distance to the nearest obstacle
* @param i The index of the cell in the costmap. Redundant with the following two parameters.
* @param x The x coordinate of the cell in the cost map
* @param y The y coordinate of the cell in the cost map
* @param sx The x coordinate of the closest source cell in the costmap
* @param sy The y coordinate of the closest source cell in the costmap
*/
CellData(
const double d, const unsigned int i, const unsigned int x, const unsigned int y,
const unsigned int sx, const unsigned int sy)
: distance_(d), index_(i), x_(x), y_(y), src_x_(sx), src_y_(sy)
{
}
/**
* @brief Default Constructor - Should be used sparingly
*/
CellData()
: distance_(std::numeric_limits<double>::max()), index_(0), x_(0), y_(0), src_x_(0), src_y_(0)
{
}
static unsigned absolute_difference(const unsigned x, const unsigned y)
{
return (x > y) ? (x - y) : (y - x);
}
double distance_;
unsigned int index_;
unsigned int x_, y_;
unsigned int src_x_, src_y_;
};
/**
* @class CostmaQueue
* @brief A tool for finding the cells closest to some set of originating cells.
*
* A common operation with costmaps is to define a set of cells in the costmap, and then
* perform some operation on all the other cells based on which cell in the original set
* the other cells are closest to. This operation is done in the inflation layer to figure out
* how far each cell is from an obstacle, and is also used in a number of Trajectory cost functions.
*
* It is implemented with a queue. The standard operation is to enqueueCell the original set, and then
* retreive the other cells with the isEmpty/getNextCell iterator-like functionality. getNextCell
* returns an object that contains the coordinates of this cell and the origin cell, as well as
* the distance between them. By default, the Euclidean distance is used for ordering, but passing in
* manhattan=true to the constructor will use the Manhattan distance.
*
* The validCellToQueue overridable-function allows for deriving classes to limit the queue traversal
* to a subset of all costmap cells. LimitedCostmapQueue does this by ignoring distances above a limit.
*
*/
class CostmapQueue : public MapBasedQueue<CellData>
{
public:
/**
* @brief constructor
* @param costmap Costmap which defines the size/number of cells
* @param manhattan If true, sort cells by Manhattan distance, otherwise use Euclidean distance
*/
explicit CostmapQueue(nav2_costmap_2d::Costmap2D & costmap, bool manhattan = false);
/**
* @brief Clear the queue
*/
void reset() override;
/**
* @brief Add a cell the queue
* @param x X coordinate of the cell
* @param y Y coordinate of the cell
*/
void enqueueCell(unsigned int x, unsigned int y);
/**
* @brief Get the next cell to examine, and enqueue its neighbors as needed
* @return The next cell
*
* NB: Assumes that isEmpty has been called before this call and returned false
*/
CellData getNextCell();
/**
* @brief Check to see if we should add this cell to the queue. Always true unless overridden.
* @param cell The cell to check
* @return True, unless overriden
*/
virtual bool validCellToQueue(const CellData & /*cell*/) {return true;}
/**
* @brief convenience typedef for a pointer
*/
typedef std::shared_ptr<CostmapQueue> Ptr;
protected:
/**
* @brief Enqueue a cell with the given coordinates and the given source cell
*/
void enqueueCell(
unsigned int index, unsigned int cur_x, unsigned int cur_y, unsigned int src_x,
unsigned int src_y);
/**
* @brief Compute the cached distances
*/
void computeCache();
nav2_costmap_2d::Costmap2D & costmap_;
std::vector<bool> seen_;
int max_distance_;
bool manhattan_;
protected:
/**
* @brief Lookup pre-computed distances
* @param cur_x The x coordinate of the current cell
* @param cur_y The y coordinate of the current cell
* @param src_x The x coordinate of the source cell
* @param src_y The y coordinate of the source cell
* @return
*/
inline double distanceLookup(
const unsigned int cur_x, const unsigned int cur_y,
const unsigned int src_x, const unsigned int src_y)
{
unsigned int dx = CellData::absolute_difference(cur_x, src_x);
unsigned int dy = CellData::absolute_difference(cur_y, src_y);
return cached_distances_[dx][dy];
}
std::vector<std::vector<double>> cached_distances_;
int cached_max_distance_;
};
} // namespace costmap_queue
#endif // COSTMAP_QUEUE__COSTMAP_QUEUE_HPP_
navigation2/nav2_dwb_controller/costmap_queue/include/costmap_queue/limited_costmap_queue.hpp
+58
View File
@@ -0,0 +1,58 @@
/*
* 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 COSTMAP_QUEUE__LIMITED_COSTMAP_QUEUE_HPP_
#define COSTMAP_QUEUE__LIMITED_COSTMAP_QUEUE_HPP_
#include "costmap_queue/costmap_queue.hpp"
namespace costmap_queue
{
/**
* @class LimitedCostmapQueue
* @brief Extension of Costmap Queue where distances are limited to a given distance from source cells.
*/
class LimitedCostmapQueue : public CostmapQueue
{
public:
/**
* @brief Constructor with limit as an integer number of cells.
*/
LimitedCostmapQueue(nav2_costmap_2d::Costmap2D & costmap, const int cell_distance_limit);
bool validCellToQueue(const CellData & cell) override;
};
} // namespace costmap_queue
#endif // COSTMAP_QUEUE__LIMITED_COSTMAP_QUEUE_HPP_
navigation2/nav2_dwb_controller/costmap_queue/include/costmap_queue/map_based_queue.hpp
+172
View File
@@ -0,0 +1,172 @@
/*
* 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 COSTMAP_QUEUE__MAP_BASED_QUEUE_HPP_
#define COSTMAP_QUEUE__MAP_BASED_QUEUE_HPP_
#include <algorithm>
#include <map>
#include <stdexcept>
#include <utility>
#include <vector>
namespace costmap_queue
{
/**
* @brief Templatized interface for a priority queue
*
* This is faster than the std::priority_queue implementation in certain cases because iterating does
* not require resorting after every element is examined.
* Based on https://github.com/ros-planning/navigation/pull/525
* The relative speed of this against the priority queue depends how many items with each
* priority are inserted into the queue.
*
* One additional speed up depends on the patterns of priorities during each iteration of the queue.
* If the same priorities are inserted into the queue on every iteration, then it is quicker to
* set reset_bins = false, such that the priority bins are not reset and will not have to be recreated
* on each iteration.
*/
template<class item_t>
class MapBasedQueue
{
public:
/**
* @brief Default Constructor
*/
explicit MapBasedQueue(bool reset_bins = true)
: reset_bins_(reset_bins), item_count_(0)
{
reset();
}
/**
* @brief Clear the queue
*/
virtual void reset()
{
if (reset_bins_ || item_count_ > 0) {
item_bins_.clear();
item_count_ = 0;
}
iter_ = last_insert_iter_ = item_bins_.end();
}
/**
* @brief Add a new item to the queue with a set priority
* @param priority Priority of the item
* @param item Payload item
*/
void enqueue(const double priority, item_t item)
{
// We keep track of the last priority we inserted. If this items priority
// matches the previous insertion we can avoid searching through all the
// bins.
if (last_insert_iter_ == item_bins_.end() || last_insert_iter_->first != priority) {
last_insert_iter_ = item_bins_.find(priority);
// If not found, create a new bin
if (last_insert_iter_ == item_bins_.end()) {
auto map_item = std::make_pair(priority, std::move(std::vector<item_t>()));
// Inserts an item if it doesn't exist. Returns an iterator to the item
// whether it existed or was inserted.
std::pair<ItemMapIterator, bool> insert_result = item_bins_.insert(std::move(map_item));
last_insert_iter_ = insert_result.first;
}
}
// Add the item to the vector for this map key
last_insert_iter_->second.push_back(item);
item_count_++;
// Use short circuiting to check if we want to update the iterator
if (iter_ == item_bins_.end() || priority < iter_->first) {
iter_ = last_insert_iter_;
}
}
/**
* @brief Check to see if there is anything in the queue
* @return True if there is nothing in the queue
*
* Must be called prior to front/pop.
*/
bool isEmpty()
{
return item_count_ == 0;
}
/**
* @brief Return the item at the front of the queue
* @return The item at the front of the queue
*/
item_t & front()
{
if (iter_ == item_bins_.end()) {
throw std::out_of_range("front() called on empty costmap_queue::MapBasedQueue!");
}
return iter_->second.back();
}
/**
* @brief Remove (and destroy) the item at the front of the queue
*/
void pop()
{
if (iter_ != item_bins_.end() && !iter_->second.empty()) {
iter_->second.pop_back();
item_count_--;
}
auto not_empty = [](const typename ItemMap::value_type & key_val) {
return !key_val.second.empty();
};
iter_ = std::find_if(iter_, item_bins_.end(), not_empty);
}
protected:
using ItemMap = std::map<double, std::vector<item_t>>;
using ItemMapIterator = typename ItemMap::iterator;
bool reset_bins_;
ItemMap item_bins_;
unsigned int item_count_;
ItemMapIterator iter_;
ItemMapIterator last_insert_iter_;
};
} // namespace costmap_queue
#endif // COSTMAP_QUEUE__MAP_BASED_QUEUE_HPP_
navigation2/nav2_dwb_controller/costmap_queue/package.xml
+22
View File
@@ -0,0 +1,22 @@
<?xml version="1.0"?>
<package format="2">
<name>costmap_queue</name>
<version>1.1.18</version>
<description>The costmap_queue 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>nav2_costmap_2d</depend>
<depend>rclcpp</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>
navigation2/nav2_dwb_controller/costmap_queue/src/costmap_queue.cpp
+137
View File
@@ -0,0 +1,137 @@
/*
* 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 "costmap_queue/costmap_queue.hpp"
#include <algorithm>
#include <cmath>
#include <vector>
using std::hypot;
namespace costmap_queue
{
CostmapQueue::CostmapQueue(nav2_costmap_2d::Costmap2D & costmap, bool manhattan)
: MapBasedQueue(), costmap_(costmap), max_distance_(-1), manhattan_(manhattan),
cached_max_distance_(-1)
{
reset();
}
void CostmapQueue::reset()
{
unsigned int size_x = costmap_.getSizeInCellsX(), size_y = costmap_.getSizeInCellsY();
if (seen_.size() != size_x * size_y) {
seen_.resize(size_x * size_y);
}
std::fill(seen_.begin(), seen_.end(), false);
computeCache();
MapBasedQueue::reset();
}
void CostmapQueue::enqueueCell(unsigned int x, unsigned int y)
{
unsigned int index = costmap_.getIndex(x, y);
enqueueCell(index, x, y, x, y);
}
void CostmapQueue::enqueueCell(
unsigned int index, unsigned int cur_x, unsigned int cur_y,
unsigned int src_x, unsigned int src_y)
{
if (seen_[index]) {return;}
// we compute our distance table one cell further than the inflation radius
// dictates so we can make the check below
double distance = distanceLookup(cur_x, cur_y, src_x, src_y);
CellData data(distance, index, cur_x, cur_y, src_x, src_y);
if (validCellToQueue(data)) {
seen_[index] = true;
enqueue(distance, data);
}
}
CellData CostmapQueue::getNextCell()
{
// get the highest priority cell and pop it off the priority queue
CellData current_cell = front();
pop();
unsigned int index = current_cell.index_;
unsigned int mx = current_cell.x_;
unsigned int my = current_cell.y_;
unsigned int sx = current_cell.src_x_;
unsigned int sy = current_cell.src_y_;
// attempt to put the neighbors of the current cell onto the queue
unsigned int size_x = costmap_.getSizeInCellsX();
if (mx > 0) {
enqueueCell(index - 1, mx - 1, my, sx, sy);
}
if (my > 0) {
enqueueCell(index - size_x, mx, my - 1, sx, sy);
}
if (mx < size_x - 1) {
enqueueCell(index + 1, mx + 1, my, sx, sy);
}
if (my < costmap_.getSizeInCellsY() - 1) {
enqueueCell(index + size_x, mx, my + 1, sx, sy);
}
return current_cell;
}
void CostmapQueue::computeCache()
{
if (max_distance_ == -1) {
max_distance_ = std::max(costmap_.getSizeInCellsX(), costmap_.getSizeInCellsY());
}
if (max_distance_ == cached_max_distance_) {return;}
cached_distances_.clear();
cached_distances_.resize(max_distance_ + 2);
for (unsigned int i = 0; i < cached_distances_.size(); ++i) {
cached_distances_[i].resize(max_distance_ + 2);
for (unsigned int j = 0; j < cached_distances_[i].size(); ++j) {
if (manhattan_) {
cached_distances_[i][j] = i + j;
} else {
cached_distances_[i][j] = hypot(i, j);
}
}
}
cached_max_distance_ = max_distance_;
}
} // namespace costmap_queue
navigation2/nav2_dwb_controller/costmap_queue/src/limited_costmap_queue.cpp
+54
View File
@@ -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.
*/
#include "costmap_queue/limited_costmap_queue.hpp"
namespace costmap_queue
{
LimitedCostmapQueue::LimitedCostmapQueue(
nav2_costmap_2d::Costmap2D & costmap,
const int distance_limit)
: CostmapQueue(costmap)
{
max_distance_ = distance_limit;
reset();
}
bool LimitedCostmapQueue::validCellToQueue(const CellData & cell)
{
return cell.distance_ <= max_distance_;
}
} // namespace costmap_queue
navigation2/nav2_dwb_controller/costmap_queue/test/mbq_test.cpp
+117
View File
@@ -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 <string>
#include "gtest/gtest.h"
#include "costmap_queue/map_based_queue.hpp"
using costmap_queue::MapBasedQueue;
void letter_test(MapBasedQueue<char> & q, const char test_letter)
{
ASSERT_FALSE(q.isEmpty());
char c = q.front();
EXPECT_EQ(c, test_letter);
q.pop();
}
TEST(MapBasedQueue, emptyQueue)
{
MapBasedQueue<char> q;
EXPECT_TRUE(q.isEmpty());
q.enqueue(1.0, 'A');
EXPECT_FALSE(q.isEmpty());
}
TEST(MapBasedQueue, checkOrdering)
{
MapBasedQueue<char> q;
q.enqueue(1.0, 'A');
q.enqueue(3.0, 'B');
q.enqueue(2.0, 'C');
q.enqueue(5.0, 'D');
q.enqueue(0.0, 'E');
std::string expected = "EACBD";
for (unsigned int i = 0; i < expected.size(); i++) {
letter_test(q, expected[i]);
}
EXPECT_TRUE(q.isEmpty());
}
TEST(MapBasedQueue, checkDynamicOrdering)
{
MapBasedQueue<char> q;
q.enqueue(1.0, 'A');
q.enqueue(3.0, 'B');
q.enqueue(2.0, 'C');
q.enqueue(5.0, 'D');
std::string expected = "ACB";
for (unsigned int i = 0; i < expected.size(); i++) {
letter_test(q, expected[i]);
}
q.enqueue(0.0, 'E');
letter_test(q, 'E');
}
TEST(MapBasedQueue, checkDynamicOrdering2)
{
MapBasedQueue<char> q;
q.enqueue(1.0, 'A');
q.enqueue(2.0, 'B');
letter_test(q, 'A');
q.enqueue(3.0, 'C');
letter_test(q, 'B');
}
TEST(MapBasedQueue, checkDynamicOrdering3)
{
MapBasedQueue<char> q;
q.enqueue(1.0, 'A');
q.enqueue(2.0, 'B');
q.enqueue(5.0, 'D');
letter_test(q, 'A');
letter_test(q, 'B');
q.enqueue(1.0, 'C');
letter_test(q, 'C');
letter_test(q, 'D');
}
int main(int argc, char ** argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
navigation2/nav2_dwb_controller/costmap_queue/test/utest.cpp
+142
View File
@@ -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.
*/
#include <cmath>
#include <memory>
#include <algorithm>
#include "gtest/gtest.h"
#include "costmap_queue/costmap_queue.hpp"
#include "costmap_queue/limited_costmap_queue.hpp"
#include "rclcpp/rclcpp.hpp"
using std::hypot;
nav2_costmap_2d::Costmap2D costmap(5, 5, 1.0, 0.0, 0.0);
TEST(CostmapQueue, basicQueue)
{
costmap_queue::CostmapQueue q(costmap);
int count = 0;
q.enqueueCell(0, 0);
while (!q.isEmpty()) {
costmap_queue::CellData cell = q.getNextCell();
EXPECT_EQ(cell.distance_, hypot(cell.x_, cell.y_));
count++;
}
EXPECT_EQ(count, 25);
}
TEST(CostmapQueue, bigTest)
{
nav2_costmap_2d::Costmap2D big_map(500, 500, 1.0, 0.0, 0.0);
costmap_queue::CostmapQueue q(big_map);
int count = 0;
q.enqueueCell(0, 0);
while (!q.isEmpty()) {
costmap_queue::CellData cell = q.getNextCell();
EXPECT_EQ(cell.distance_, hypot(cell.x_, cell.y_));
count++;
}
EXPECT_EQ(count, 500 * 500);
}
TEST(CostmapQueue, linearQueue)
{
costmap_queue::CostmapQueue q(costmap);
int count = 0;
q.enqueueCell(0, 0);
q.enqueueCell(0, 1);
q.enqueueCell(0, 2);
q.enqueueCell(0, 3);
q.enqueueCell(0, 4);
while (!q.isEmpty()) {
costmap_queue::CellData cell = q.getNextCell();
EXPECT_EQ(cell.distance_, cell.x_);
count++;
}
EXPECT_EQ(count, 25);
}
TEST(CostmapQueue, crossQueue)
{
costmap_queue::CostmapQueue q(costmap);
int count = 0;
int xs[] = {1, 2, 2, 3};
int ys[] = {2, 1, 3, 2};
int N = 4;
for (int i = 0; i < N; i++) {
q.enqueueCell(xs[i], ys[i]);
}
while (!q.isEmpty()) {
costmap_queue::CellData cell = q.getNextCell();
double min_d = 1000;
for (int i = 0; i < N; i++) {
double dd = hypot(xs[i] - static_cast<float>(cell.x_), ys[i] - static_cast<float>(cell.y_));
min_d = std::min(min_d, dd);
}
EXPECT_NEAR(cell.distance_, min_d, 0.00001);
count++;
}
EXPECT_EQ(count, 25);
}
TEST(CostmapQueue, limitedQueue)
{
costmap_queue::LimitedCostmapQueue q(costmap, 5);
int count = 0;
q.enqueueCell(0, 0);
while (!q.isEmpty()) {
costmap_queue::CellData cell = q.getNextCell();
EXPECT_EQ(cell.distance_, hypot(cell.x_, cell.y_));
count++;
}
EXPECT_EQ(count, 24);
costmap_queue::LimitedCostmapQueue q2(costmap, 3);
count = 0;
q2.enqueueCell(0, 0);
while (!q2.isEmpty()) {
q2.getNextCell();
count++;
}
EXPECT_EQ(count, 11);
}
int main(int argc, char ** argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
navigation2/nav2_dwb_controller/dwb_core/CMakeLists.txt
+81
View File
@@ -0,0 +1,81 @@
cmake_minimum_required(VERSION 3.5)
project(dwb_core)
find_package(ament_cmake REQUIRED)
find_package(nav2_common REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(nav_2d_msgs REQUIRED)
find_package(dwb_msgs REQUIRED)
find_package(nav2_costmap_2d REQUIRED)
find_package(pluginlib REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(visualization_msgs REQUIRED)
find_package(nav_2d_utils REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(nav2_util REQUIRED)
find_package(nav2_core REQUIRED)
nav2_package()
include_directories(
include
)
set(dependencies
rclcpp
std_msgs
geometry_msgs
nav_2d_msgs
dwb_msgs
nav2_costmap_2d
pluginlib
sensor_msgs
visualization_msgs
nav_2d_utils
nav_msgs
tf2_ros
nav2_util
nav2_core
)
add_library(dwb_core SHARED
src/dwb_local_planner.cpp
src/publisher.cpp
src/illegal_trajectory_tracker.cpp
src/trajectory_utils.cpp
)
ament_target_dependencies(dwb_core
${dependencies}
)
install(TARGETS dwb_core
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
install(DIRECTORY include/
DESTINATION include/
)
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(dwb_core)
ament_export_dependencies(${dependencies})
pluginlib_export_plugin_description_file(nav2_core local_planner_plugin.xml)
ament_package()
navigation2/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp
+244
View File
@@ -0,0 +1,244 @@
/*
* 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_CORE__DWB_LOCAL_PLANNER_HPP_
#define DWB_CORE__DWB_LOCAL_PLANNER_HPP_
#include <memory>
#include <string>
#include <vector>
#include "nav2_core/controller.hpp"
#include "nav2_core/goal_checker.hpp"
#include "dwb_core/publisher.hpp"
#include "dwb_core/trajectory_critic.hpp"
#include "dwb_core/trajectory_generator.hpp"
#include "nav_2d_msgs/msg/pose2_d_stamped.hpp"
#include "nav_2d_msgs/msg/twist2_d_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "pluginlib/class_loader.hpp"
#include "pluginlib/class_list_macros.hpp"
namespace dwb_core
{
/**
* @class DWBLocalPlanner
* @brief Plugin-based flexible controller
*/
class DWBLocalPlanner : public nav2_core::Controller
{
public:
/**
* @brief Constructor that brings up pluginlib loaders
*/
DWBLocalPlanner();
void configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
virtual ~DWBLocalPlanner() {}
/**
* @brief Activate lifecycle node
*/
void activate() override;
/**
* @brief Deactivate lifecycle node
*/
void deactivate() override;
/**
* @brief Cleanup lifecycle node
*/
void cleanup() override;
/**
* @brief nav2_core setPlan - Sets the global plan
* @param path The global plan
*/
void setPlan(const nav_msgs::msg::Path & path) override;
/**
* @brief nav2_core computeVelocityCommands - calculates the best command given the current pose and velocity
*
* It is presumed that the global plan is already set.
*
* This is mostly a wrapper for the protected computeVelocityCommands
* function which has additional debugging info.
*
* @param pose Current robot pose
* @param velocity Current robot velocity
* @param goal_checker Ptr to the goal checker for this task in case useful in computing commands
* @return The best command for the robot to drive
*/
geometry_msgs::msg::TwistStamped computeVelocityCommands(
const geometry_msgs::msg::PoseStamped & pose,
const geometry_msgs::msg::Twist & velocity,
nav2_core::GoalChecker * /*goal_checker*/) override;
/**
* @brief Score a given command. Can be used for testing.
*
* Given a trajectory, calculate the score where lower scores are better.
* If the given (positive) score exceeds the best_score, calculation may be cut short, as the
* score can only go up from there.
*
* @param traj Trajectory to check
* @param best_score If positive, the threshold for early termination
* @return The full scoring of the input trajectory
*/
virtual dwb_msgs::msg::TrajectoryScore scoreTrajectory(
const dwb_msgs::msg::Trajectory2D & traj,
double best_score = -1);
/**
* @brief Compute the best command given the current pose and velocity, with possible debug information
*
* Same as above computeVelocityCommands, but with debug results.
* If the results pointer is not null, additional information about the twists
* evaluated will be in results after the call.
*
* @param pose Current robot pose
* @param velocity Current robot velocity
* @param results Output param, if not NULL, will be filled in with full evaluation results
* @return Best command
*/
virtual nav_2d_msgs::msg::Twist2DStamped computeVelocityCommands(
const nav_2d_msgs::msg::Pose2DStamped & pose,
const nav_2d_msgs::msg::Twist2D & velocity,
std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> & results);
/**
* @brief Limits the maximum linear speed of the robot.
* @param speed_limit expressed in absolute value (in m/s)
* or in percentage from maximum robot speed.
* @param percentage Setting speed limit in percentage if true
* or in absolute values in false case.
*/
void setSpeedLimit(const double & speed_limit, const bool & percentage) override
{
if (traj_generator_) {
traj_generator_->setSpeedLimit(speed_limit, percentage);
}
}
protected:
/**
* @brief Helper method for two common operations for the operating on the global_plan
*
* Transforms the global plan (stored in global_plan_) relative to the pose and saves it in
* transformed_plan and possibly publishes it. Then it takes the last pose and transforms it
* to match the local costmap's frame
*/
void prepareGlobalPlan(
const nav_2d_msgs::msg::Pose2DStamped & pose, nav_2d_msgs::msg::Path2D & transformed_plan,
nav_2d_msgs::msg::Pose2DStamped & goal_pose, bool publish_plan = true);
/**
* @brief Iterate through all the twists and find the best one
*/
virtual dwb_msgs::msg::TrajectoryScore coreScoringAlgorithm(
const geometry_msgs::msg::Pose2D & pose,
const nav_2d_msgs::msg::Twist2D velocity,
std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> & results);
/**
* @brief Transforms global plan into same frame as pose, clips far away poses and possibly prunes passed poses
*
* Three key operations
* 1) Transforms global plan into frame of the given pose
* 2) Only returns poses that are near the robot, i.e. whether they are likely on the local costmap
* 3) If prune_plan_ is true, it will remove all points that we've already passed from both the transformed plan
* and the saved global_plan_. Technically, it iterates to a pose on the path that is within prune_distance_
* of the robot and erases all poses before that.
*
* Additionally, shorten_transformed_plan_ determines whether we will pass the full plan all
* the way to the nav goal on to the critics or just a subset of the plan near the robot.
* True means pass just a subset. This gives DWB less discretion to decide how it gets to the
* nav goal. Instead it is encouraged to try to get on to the path generated by the global planner.
*/
virtual nav_2d_msgs::msg::Path2D transformGlobalPlan(
const nav_2d_msgs::msg::Pose2DStamped & pose);
nav_2d_msgs::msg::Path2D global_plan_; ///< Saved Global Plan
bool prune_plan_;
double prune_distance_;
bool debug_trajectory_details_;
rclcpp::Duration transform_tolerance_{0, 0};
bool shorten_transformed_plan_;
double forward_prune_distance_;
/**
* @brief try to resolve a possibly shortened critic name with the default namespaces and the suffix "Critic"
*
* @param base_name The name of the critic as read in from the parameter server
* @return Our attempted resolution of the name, with namespace prepended and/or the suffix Critic appended
*/
std::string resolveCriticClassName(std::string base_name);
/**
* @brief Load the critic parameters from the namespace
* @param name The namespace of this planner.
*/
virtual void loadCritics();
rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
rclcpp::Clock::SharedPtr clock_;
rclcpp::Logger logger_{rclcpp::get_logger("DWBLocalPlanner")};
std::shared_ptr<tf2_ros::Buffer> tf_;
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
std::unique_ptr<DWBPublisher> pub_;
std::vector<std::string> default_critic_namespaces_;
// Plugin handling
pluginlib::ClassLoader<TrajectoryGenerator> traj_gen_loader_;
TrajectoryGenerator::Ptr traj_generator_;
pluginlib::ClassLoader<TrajectoryCritic> critic_loader_;
std::vector<TrajectoryCritic::Ptr> critics_;
std::string dwb_plugin_name_;
bool short_circuit_trajectory_evaluation_;
};
} // namespace dwb_core
#endif // DWB_CORE__DWB_LOCAL_PLANNER_HPP_
navigation2/nav2_dwb_controller/dwb_core/include/dwb_core/exceptions.hpp
+74
View File
@@ -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_CORE__EXCEPTIONS_HPP_
#define DWB_CORE__EXCEPTIONS_HPP_
#include <stdexcept>
#include <string>
#include <memory>
#include "nav2_core/exceptions.hpp"
namespace dwb_core
{
/**
* @class PlannerTFException
* @brief Thrown when the planner cannot complete its operation due to TF errors
*/
class PlannerTFException : public nav2_core::PlannerException
{
public:
explicit PlannerTFException(const std::string description)
: nav2_core::PlannerException(description) {}
};
/**
* @class IllegalTrajectoryException
* @brief Thrown when one of the critics encountered a fatal error
*/
class IllegalTrajectoryException : public nav2_core::PlannerException
{
public:
IllegalTrajectoryException(const std::string critic_name, const std::string description)
: nav2_core::PlannerException(description), critic_name_(critic_name) {}
std::string getCriticName() const {return critic_name_;}
protected:
std::string critic_name_;
};
} // namespace dwb_core
#endif // DWB_CORE__EXCEPTIONS_HPP_
navigation2/nav2_dwb_controller/dwb_core/include/dwb_core/illegal_trajectory_tracker.hpp
+80
View File
@@ -0,0 +1,80 @@
/*
* 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_CORE__ILLEGAL_TRAJECTORY_TRACKER_HPP_
#define DWB_CORE__ILLEGAL_TRAJECTORY_TRACKER_HPP_
#include <map>
#include <utility>
#include <string>
#include "dwb_core/exceptions.hpp"
#include "nav2_core/exceptions.hpp"
namespace dwb_core
{
class IllegalTrajectoryTracker
{
public:
IllegalTrajectoryTracker()
: legal_count_(0), illegal_count_(0) {}
void addIllegalTrajectory(const IllegalTrajectoryException & e);
void addLegalTrajectory();
std::map<std::pair<std::string, std::string>, double> getPercentages() const;
std::string getMessage() const;
protected:
std::map<std::pair<std::string, std::string>, unsigned int> counts_;
unsigned int legal_count_, illegal_count_;
};
/**
* @class NoLegalTrajectoriesException
* @brief Thrown when all the trajectories explored are illegal
*/
class NoLegalTrajectoriesException
: public nav2_core::PlannerException
{
public:
explicit NoLegalTrajectoriesException(const IllegalTrajectoryTracker & tracker)
: PlannerException(tracker.getMessage()),
tracker_(tracker) {}
IllegalTrajectoryTracker tracker_;
};
} // namespace dwb_core
#endif // DWB_CORE__ILLEGAL_TRAJECTORY_TRACKER_HPP_
navigation2/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp
+136
View File
@@ -0,0 +1,136 @@
/*
* 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_CORE__PUBLISHER_HPP_
#define DWB_CORE__PUBLISHER_HPP_
#include <memory>
#include <string>
#include <vector>
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "dwb_core/trajectory_critic.hpp"
#include "dwb_msgs/msg/local_plan_evaluation.hpp"
#include "nav_msgs/msg/path.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "visualization_msgs/msg/marker_array.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "builtin_interfaces/msg/duration.hpp"
using rclcpp_lifecycle::LifecyclePublisher;
namespace dwb_core
{
/**
* @class DWBPublisher
* @brief Consolidation of all the publishing logic for the DWB Local Planner.
*
* Right now, it can publish
* 1) The Global Plan (as passed in using setPath)
* 2) The Local Plan (after it is calculated)
* 3) The Transformed Global Plan (since it may be different than the global)
* 4) The Full LocalPlanEvaluation
* 5) Markers representing the different trajectories evaluated
* 6) The CostGrid (in the form of a complex PointCloud2)
*/
class DWBPublisher
{
public:
explicit DWBPublisher(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
const std::string & plugin_name);
nav2_util::CallbackReturn on_configure();
nav2_util::CallbackReturn on_activate();
nav2_util::CallbackReturn on_deactivate();
nav2_util::CallbackReturn on_cleanup();
/**
* @brief Does the publisher require that the LocalPlanEvaluation be saved
* @return True if the Evaluation is needed to publish either directly or as trajectories
*/
bool shouldRecordEvaluation() {return publish_evaluation_ || publish_trajectories_;}
/**
* @brief If the pointer is not null, publish the evaluation and trajectories as needed
*/
void publishEvaluation(std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> results);
void publishLocalPlan(
const std_msgs::msg::Header & header,
const dwb_msgs::msg::Trajectory2D & traj);
void publishCostGrid(
const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros,
const std::vector<TrajectoryCritic::Ptr> critics);
void publishGlobalPlan(const nav_2d_msgs::msg::Path2D plan);
void publishTransformedPlan(const nav_2d_msgs::msg::Path2D plan);
void publishLocalPlan(const nav_2d_msgs::msg::Path2D plan);
protected:
void publishTrajectories(const dwb_msgs::msg::LocalPlanEvaluation & results);
// Helper function for publishing other plans
void publishGenericPlan(
const nav_2d_msgs::msg::Path2D plan,
rclcpp::Publisher<nav_msgs::msg::Path> & pub, bool flag);
// Flags for turning on/off publishing specific components
bool publish_evaluation_;
bool publish_global_plan_;
bool publish_transformed_;
bool publish_local_plan_;
bool publish_trajectories_;
bool publish_cost_grid_pc_;
bool publish_input_params_;
// Marker Lifetime
builtin_interfaces::msg::Duration marker_lifetime_;
// Publisher Objects
std::shared_ptr<LifecyclePublisher<dwb_msgs::msg::LocalPlanEvaluation>> eval_pub_;
std::shared_ptr<LifecyclePublisher<nav_msgs::msg::Path>> global_pub_;
std::shared_ptr<LifecyclePublisher<nav_msgs::msg::Path>> transformed_pub_;
std::shared_ptr<LifecyclePublisher<nav_msgs::msg::Path>> local_pub_;
std::shared_ptr<LifecyclePublisher<visualization_msgs::msg::MarkerArray>> marker_pub_;
std::shared_ptr<LifecyclePublisher<sensor_msgs::msg::PointCloud2>> cost_grid_pc_pub_;
rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
rclcpp::Clock::SharedPtr clock_;
std::string plugin_name_;
};
} // namespace dwb_core
#endif // DWB_CORE__PUBLISHER_HPP_
navigation2/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_critic.hpp
+190
View File
@@ -0,0 +1,190 @@
/*
* 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_CORE__TRAJECTORY_CRITIC_HPP_
#define DWB_CORE__TRAJECTORY_CRITIC_HPP_
#include <string>
#include <vector>
#include <memory>
#include <utility>
#include "rclcpp/rclcpp.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "geometry_msgs/msg/pose2_d.hpp"
#include "nav_2d_msgs/msg/twist2_d.hpp"
#include "nav_2d_msgs/msg/path2_d.hpp"
#include "dwb_msgs/msg/trajectory2_d.hpp"
#include "sensor_msgs/msg/point_cloud.hpp"
#include "nav2_util/lifecycle_node.hpp"
namespace dwb_core
{
/**
* @class TrajectoryCritic
* @brief Evaluates a Trajectory2D to produce a score
*
* This class defines the plugin interface for the TrajectoryCritic which
* gives scores to trajectories, where lower numbers are better, but negative
* scores are considered invalid.
*
* The general lifecycle is
* 1) initialize is called once at the beginning which in turn calls onInit.
* Derived classes may override onInit to load parameters as needed.
* 2) prepare is called once before each set of trajectories.
* It is presumed that there are multiple trajectories that we want to evaluate,
* and there may be some shared work that can be done beforehand to optimize
* the scoring of each individual trajectory.
* 3) scoreTrajectory is called once per trajectory and returns the score.
* 4) debrief is called after each set of trajectories with the chosen trajectory.
* This can be used for stateful critics that monitor the trajectory through time.
*
* Optionally, there is also a debugging mechanism for certain types of critics in the
* addCriticVisualization method. If the score for a trajectory depends on its relationship to
* the costmap, addCriticVisualization can provide that information to the dwb_core
* which will publish the grid scores as a PointCloud2.
*/
class TrajectoryCritic
{
public:
using Ptr = std::shared_ptr<dwb_core::TrajectoryCritic>;
virtual ~TrajectoryCritic() {}
/**
* @brief Initialize the critic with appropriate pointers and parameters
*
* The name and costmap are stored as member variables.
* A NodeHandle is created using the combination of the parent namespace and the critic name
*
* @param name The name of this critic
* @param parent_namespace The namespace of the planner
* @param costmap_ros Pointer to the costmap
*/
void initialize(
const nav2_util::LifecycleNode::SharedPtr & nh,
const std::string & name,
const std::string & ns,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
{
node_ = nh;
name_ = name;
costmap_ros_ = costmap_ros;
dwb_plugin_name_ = ns;
if (!nh->has_parameter(dwb_plugin_name_ + "." + name_ + ".scale")) {
nh->declare_parameter(
dwb_plugin_name_ + "." + name_ + ".scale",
rclcpp::ParameterValue(1.0));
}
nh->get_parameter(dwb_plugin_name_ + "." + name_ + ".scale", scale_);
onInit();
}
virtual void onInit() {}
/**
* @brief Reset the state of the critic
*
* Reset is called when the planner receives a new global plan.
* This can be used to discard information specific to one plan.
*/
virtual void reset() {}
/**
* @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
*
* Subclasses may overwrite. Return false in case there is any error.
*
* @param pose Current pose (costmap frame)
* @param vel Current velocity
* @param goal The final goal (costmap frame)
* @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
*/
virtual bool prepare(
const geometry_msgs::msg::Pose2D &, const nav_2d_msgs::msg::Twist2D &,
const geometry_msgs::msg::Pose2D &,
const nav_2d_msgs::msg::Path2D &)
{
return true;
}
/**
* @brief Return a raw score for the given trajectory.
*
* scores < 0 are considered invalid/errors, such as collisions
* This is the raw score in that the scale should not be applied to it.
*/
virtual double scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) = 0;
/**
* @brief debrief informs the critic what the chosen cmd_vel was (if it cares)
*/
virtual void debrief(const nav_2d_msgs::msg::Twist2D &) {}
/**
* @brief Add information to the given pointcloud for debugging costmap-grid based scores
*
* addCriticVisualization is an optional debugging mechanism for providing rich information
* about the cost for certain trajectories. Some critics will have scoring mechanisms
* wherein there will be some score for each cell in the costmap. This could be as
* straightforward as the cost in the costmap, or it could be the number of cells away
* from the goal pose.
*
* Prior to calling this, dwb_core will load the PointCloud's header and the points
* in row-major order. The critic may then add a ChannelFloat to the channels member of the PC
* with the same number of values as the points array. This information may then be converted
* and published as a PointCloud2.
*
* @param pc PointCloud to add channels to
*/
virtual void addCriticVisualization(std::vector<std::pair<std::string, std::vector<float>>> &) {}
std::string getName()
{
return name_;
}
virtual double getScale() const {return scale_;}
void setScale(const double scale) {scale_ = scale;}
protected:
std::string name_;
std::string dwb_plugin_name_;
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
double scale_;
rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
};
} // namespace dwb_core
#endif // DWB_CORE__TRAJECTORY_CRITIC_HPP_
navigation2/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_generator.hpp
+139
View File
@@ -0,0 +1,139 @@
/*
* 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_CORE__TRAJECTORY_GENERATOR_HPP_
#define DWB_CORE__TRAJECTORY_GENERATOR_HPP_
#include <vector>
#include <string>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "nav_2d_msgs/msg/twist2_d.hpp"
#include "dwb_msgs/msg/trajectory2_d.hpp"
#include "nav2_util/lifecycle_node.hpp"
namespace dwb_core
{
/**
* @class TrajectoryGenerator
* @brief Interface for iterating through possible velocities and creating trajectories
*
* This class defines the plugin interface for two separate but related components.
*
* First, this class provides an iterator interface for exploring all of the velocities
* to search, given the current velocity.
*
* Second, the class gives an independent interface for creating a trajectory from a twist,
* i.e. projecting it out in time and space.
*
* Both components rely heavily on the robot's kinematic model, and can share many parameters,
* which is why they are grouped into a singular class.
*/
class TrajectoryGenerator
{
public:
typedef std::shared_ptr<dwb_core::TrajectoryGenerator> Ptr;
virtual ~TrajectoryGenerator() {}
/**
* @brief Initialize parameters as needed
* @param nh NodeHandle to read parameters from
*/
virtual void initialize(
const nav2_util::LifecycleNode::SharedPtr & nh,
const std::string & plugin_name) = 0;
virtual void reset() {}
/**
* @brief Start a new iteration based on the current velocity
* @param current_velocity
*/
virtual void startNewIteration(const nav_2d_msgs::msg::Twist2D & current_velocity) = 0;
/**
* @brief Test to see whether there are more twists to test
* @return True if more twists, false otherwise
*/
virtual bool hasMoreTwists() = 0;
/**
* @brief Return the next twist and advance the iteration
* @return The Twist!
*/
virtual nav_2d_msgs::msg::Twist2D nextTwist() = 0;
/**
* @brief Get all the twists for an iteration.
*
* Note: Resets the iterator if one is in process
*
* @param current_velocity
* @return all the twists
*/
virtual std::vector<nav_2d_msgs::msg::Twist2D> getTwists(
const nav_2d_msgs::msg::Twist2D & current_velocity)
{
std::vector<nav_2d_msgs::msg::Twist2D> twists;
startNewIteration(current_velocity);
while (hasMoreTwists()) {
twists.push_back(nextTwist());
}
return twists;
}
/**
* @brief Given a cmd_vel in the robot's frame and initial conditions, generate a Trajectory2D
* @param start_pose Current robot location
* @param start_vel Current robot velocity
* @param cmd_vel The desired command velocity
*/
virtual dwb_msgs::msg::Trajectory2D generateTrajectory(
const geometry_msgs::msg::Pose2D & start_pose,
const nav_2d_msgs::msg::Twist2D & start_vel,
const nav_2d_msgs::msg::Twist2D & cmd_vel) = 0;
/**
* @brief Limits the maximum linear speed of the robot.
* @param speed_limit expressed in absolute value (in m/s)
* or in percentage from maximum robot speed.
* @param percentage Setting speed limit in percentage if true
* or in absolute values in false case.
*/
virtual void setSpeedLimit(const double & speed_limit, const bool & percentage) = 0;
};
} // namespace dwb_core
#endif // DWB_CORE__TRAJECTORY_GENERATOR_HPP_
navigation2/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_utils.hpp
+70
View File
@@ -0,0 +1,70 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, 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_CORE__TRAJECTORY_UTILS_HPP_
#define DWB_CORE__TRAJECTORY_UTILS_HPP_
#include "rclcpp/rclcpp.hpp"
#include "dwb_msgs/msg/trajectory2_d.hpp"
namespace dwb_core
{
/**
* @brief Helper function to find a pose in the trajectory with a particular time time_offset
* @param trajectory The trajectory to search
* @param time_offset The desired time_offset
* @return reference to the pose that is closest to the particular time offset
*
* Linearly searches through the poses. Once the poses time_offset is greater than the desired time_offset,
* the search ends, since the poses have increasing time_offsets.
*/
const geometry_msgs::msg::Pose2D & getClosestPose(
const dwb_msgs::msg::Trajectory2D & trajectory,
const double time_offset);
/**
* @brief Helper function to create a pose with an exact time_offset by linearly interpolating between existing poses
* @param trajectory The trajectory with pose and time offset information
* @param time_offset The desired time_offset
* @return New Pose2D with interpolated values
* @note If the given time offset is outside the bounds of the trajectory, the return pose will be either the first or last pose.
*/
geometry_msgs::msg::Pose2D projectPose(
const dwb_msgs::msg::Trajectory2D & trajectory,
const double time_offset);
} // namespace dwb_core
#endif // DWB_CORE__TRAJECTORY_UTILS_HPP_
navigation2/nav2_dwb_controller/dwb_core/local_planner_plugin.xml
+7
View File
@@ -0,0 +1,7 @@
<class_libraries>
<library path="dwb_core">
<class type="dwb_core::DWBLocalPlanner" base_class_type="nav2_core::Controller">
<description></description>
</class>
</library>
</class_libraries>
navigation2/nav2_dwb_controller/dwb_core/package.xml
+50
View File
@@ -0,0 +1,50 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>dwb_core</name>
<version>1.1.18</version>
<description>TODO</description>
<maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
<license>BSD-3-Clause</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>nav2_common</build_depend>
<build_depend>rclcpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>nav_2d_msgs</build_depend>
<build_depend>dwb_msgs</build_depend>
<build_depend>nav2_costmap_2d</build_depend>
<build_depend>pluginlib</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>visualization_msgs</build_depend>
<build_depend>nav_2d_utils</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>tf2_ros</build_depend>
<build_depend>nav2_util</build_depend>
<build_depend>nav2_core</build_depend>
<exec_depend>rclcpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>rclcpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>dwb_msgs</exec_depend>
<exec_depend>nav2_costmap_2d</exec_depend>
<exec_depend>nav_2d_utils</exec_depend>
<exec_depend>pluginlib</exec_depend>
<exec_depend>nav_msgs</exec_depend>
<exec_depend>tf2_ros</exec_depend>
<exec_depend>nav2_util</exec_depend>
<exec_depend>nav2_core</exec_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>
<nav2_core plugin="${prefix}/local_planner_plugin.xml" />
</export>
</package>
navigation2/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
+552
View File
@@ -0,0 +1,552 @@
/*
* 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 <algorithm>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "dwb_core/dwb_local_planner.hpp"
#include "dwb_core/exceptions.hpp"
#include "dwb_core/illegal_trajectory_tracker.hpp"
#include "dwb_msgs/msg/critic_score.hpp"
#include "nav_2d_msgs/msg/twist2_d.hpp"
#include "nav_2d_utils/conversions.hpp"
#include "nav_2d_utils/parameters.hpp"
#include "nav_2d_utils/tf_help.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/node_utils.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "nav_msgs/msg/path.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"
using nav2_util::declare_parameter_if_not_declared;
using nav2_util::geometry_utils::euclidean_distance;
namespace dwb_core
{
DWBLocalPlanner::DWBLocalPlanner()
: traj_gen_loader_("dwb_core", "dwb_core::TrajectoryGenerator"),
critic_loader_("dwb_core", "dwb_core::TrajectoryCritic")
{
}
void DWBLocalPlanner::configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
{
node_ = parent;
auto node = node_.lock();
logger_ = node->get_logger();
clock_ = node->get_clock();
costmap_ros_ = costmap_ros;
tf_ = tf;
dwb_plugin_name_ = name;
declare_parameter_if_not_declared(
node, dwb_plugin_name_ + ".critics",
rclcpp::PARAMETER_STRING_ARRAY);
declare_parameter_if_not_declared(
node, dwb_plugin_name_ + ".default_critic_namespaces",
rclcpp::ParameterValue(std::vector<std::string>()));
declare_parameter_if_not_declared(
node, dwb_plugin_name_ + ".prune_plan",
rclcpp::ParameterValue(true));
declare_parameter_if_not_declared(
node, dwb_plugin_name_ + ".prune_distance",
rclcpp::ParameterValue(2.0));
declare_parameter_if_not_declared(
node, dwb_plugin_name_ + ".forward_prune_distance",
rclcpp::ParameterValue(2.0));
declare_parameter_if_not_declared(
node, dwb_plugin_name_ + ".debug_trajectory_details",
rclcpp::ParameterValue(false));
declare_parameter_if_not_declared(
node, dwb_plugin_name_ + ".trajectory_generator_name",
rclcpp::ParameterValue(std::string("dwb_plugins::StandardTrajectoryGenerator")));
declare_parameter_if_not_declared(
node, dwb_plugin_name_ + ".transform_tolerance",
rclcpp::ParameterValue(0.1));
declare_parameter_if_not_declared(
node, dwb_plugin_name_ + ".shorten_transformed_plan",
rclcpp::ParameterValue(true));
declare_parameter_if_not_declared(
node, dwb_plugin_name_ + ".short_circuit_trajectory_evaluation",
rclcpp::ParameterValue(true));
std::string traj_generator_name;
double transform_tolerance;
node->get_parameter(dwb_plugin_name_ + ".transform_tolerance", transform_tolerance);
transform_tolerance_ = rclcpp::Duration::from_seconds(transform_tolerance);
RCLCPP_INFO(logger_, "Setting transform_tolerance to %f", transform_tolerance);
node->get_parameter(dwb_plugin_name_ + ".prune_plan", prune_plan_);
node->get_parameter(dwb_plugin_name_ + ".prune_distance", prune_distance_);
node->get_parameter(dwb_plugin_name_ + ".forward_prune_distance", forward_prune_distance_);
node->get_parameter(dwb_plugin_name_ + ".debug_trajectory_details", debug_trajectory_details_);
node->get_parameter(dwb_plugin_name_ + ".trajectory_generator_name", traj_generator_name);
node->get_parameter(
dwb_plugin_name_ + ".short_circuit_trajectory_evaluation",
short_circuit_trajectory_evaluation_);
node->get_parameter(dwb_plugin_name_ + ".shorten_transformed_plan", shorten_transformed_plan_);
pub_ = std::make_unique<DWBPublisher>(node, dwb_plugin_name_);
pub_->on_configure();
traj_generator_ = traj_gen_loader_.createUniqueInstance(traj_generator_name);
traj_generator_->initialize(node, dwb_plugin_name_);
try {
loadCritics();
} catch (const std::exception & e) {
RCLCPP_ERROR(logger_, "Couldn't load critics! Caught exception: %s", e.what());
throw;
}
}
void
DWBLocalPlanner::activate()
{
pub_->on_activate();
}
void
DWBLocalPlanner::deactivate()
{
pub_->on_deactivate();
}
void
DWBLocalPlanner::cleanup()
{
pub_->on_cleanup();
traj_generator_.reset();
}
std::string
DWBLocalPlanner::resolveCriticClassName(std::string base_name)
{
if (base_name.find("Critic") == std::string::npos) {
base_name = base_name + "Critic";
}
if (base_name.find("::") == std::string::npos) {
for (unsigned int j = 0; j < default_critic_namespaces_.size(); j++) {
std::string full_name = default_critic_namespaces_[j] + "::" + base_name;
if (critic_loader_.isClassAvailable(full_name)) {
return full_name;
}
}
}
return base_name;
}
void
DWBLocalPlanner::loadCritics()
{
auto node = node_.lock();
if (!node) {
throw std::runtime_error{"Failed to lock node"};
}
node->get_parameter(dwb_plugin_name_ + ".default_critic_namespaces", default_critic_namespaces_);
if (default_critic_namespaces_.empty()) {
default_critic_namespaces_.emplace_back("dwb_critics");
}
std::vector<std::string> critic_names;
if (!node->get_parameter(dwb_plugin_name_ + ".critics", critic_names)) {
throw std::runtime_error("No critics defined for " + dwb_plugin_name_);
}
node->get_parameter(dwb_plugin_name_ + ".critics", critic_names);
for (unsigned int i = 0; i < critic_names.size(); i++) {
std::string critic_plugin_name = critic_names[i];
std::string plugin_class;
declare_parameter_if_not_declared(
node, dwb_plugin_name_ + "." + critic_plugin_name + ".class",
rclcpp::ParameterValue(critic_plugin_name));
node->get_parameter(dwb_plugin_name_ + "." + critic_plugin_name + ".class", plugin_class);
plugin_class = resolveCriticClassName(plugin_class);
TrajectoryCritic::Ptr plugin = critic_loader_.createUniqueInstance(plugin_class);
RCLCPP_INFO(
logger_,
"Using critic \"%s\" (%s)", critic_plugin_name.c_str(), plugin_class.c_str());
critics_.push_back(plugin);
try {
plugin->initialize(node, critic_plugin_name, dwb_plugin_name_, costmap_ros_);
} catch (const std::exception & e) {
RCLCPP_ERROR(logger_, "Couldn't initialize critic plugin!");
throw;
}
RCLCPP_INFO(logger_, "Critic plugin initialized");
}
}
void
DWBLocalPlanner::setPlan(const nav_msgs::msg::Path & path)
{
auto path2d = nav_2d_utils::pathToPath2D(path);
for (TrajectoryCritic::Ptr & critic : critics_) {
critic->reset();
}
traj_generator_->reset();
pub_->publishGlobalPlan(path2d);
global_plan_ = path2d;
}
geometry_msgs::msg::TwistStamped
DWBLocalPlanner::computeVelocityCommands(
const geometry_msgs::msg::PoseStamped & pose,
const geometry_msgs::msg::Twist & velocity,
nav2_core::GoalChecker * /*goal_checker*/)
{
std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> results = nullptr;
if (pub_->shouldRecordEvaluation()) {
results = std::make_shared<dwb_msgs::msg::LocalPlanEvaluation>();
}
try {
nav_2d_msgs::msg::Twist2DStamped cmd_vel2d = computeVelocityCommands(
nav_2d_utils::poseStampedToPose2D(pose),
nav_2d_utils::twist3Dto2D(velocity), results);
pub_->publishEvaluation(results);
geometry_msgs::msg::TwistStamped cmd_vel;
cmd_vel.twist = nav_2d_utils::twist2Dto3D(cmd_vel2d.velocity);
return cmd_vel;
} catch (const nav2_core::PlannerException & e) {
pub_->publishEvaluation(results);
throw;
}
}
void
DWBLocalPlanner::prepareGlobalPlan(
const nav_2d_msgs::msg::Pose2DStamped & pose, nav_2d_msgs::msg::Path2D & transformed_plan,
nav_2d_msgs::msg::Pose2DStamped & goal_pose, bool publish_plan)
{
transformed_plan = transformGlobalPlan(pose);
if (publish_plan) {
pub_->publishTransformedPlan(transformed_plan);
}
goal_pose.header.frame_id = global_plan_.header.frame_id;
goal_pose.pose = global_plan_.poses.back();
nav_2d_utils::transformPose(
tf_, costmap_ros_->getGlobalFrameID(), goal_pose,
goal_pose, transform_tolerance_);
}
nav_2d_msgs::msg::Twist2DStamped
DWBLocalPlanner::computeVelocityCommands(
const nav_2d_msgs::msg::Pose2DStamped & pose,
const nav_2d_msgs::msg::Twist2D & velocity,
std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> & results)
{
if (results) {
results->header.frame_id = pose.header.frame_id;
results->header.stamp = clock_->now();
}
nav_2d_msgs::msg::Path2D transformed_plan;
nav_2d_msgs::msg::Pose2DStamped goal_pose;
prepareGlobalPlan(pose, transformed_plan, goal_pose);
nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));
for (TrajectoryCritic::Ptr & critic : critics_) {
if (!critic->prepare(pose.pose, velocity, goal_pose.pose, transformed_plan)) {
RCLCPP_WARN(rclcpp::get_logger("DWBLocalPlanner"), "A scoring function failed to prepare");
}
}
try {
dwb_msgs::msg::TrajectoryScore best = coreScoringAlgorithm(pose.pose, velocity, results);
// Return Value
nav_2d_msgs::msg::Twist2DStamped cmd_vel;
cmd_vel.header.stamp = clock_->now();
cmd_vel.velocity = best.traj.velocity;
// debrief stateful scoring functions
for (TrajectoryCritic::Ptr & critic : critics_) {
critic->debrief(cmd_vel.velocity);
}
lock.unlock();
pub_->publishLocalPlan(pose.header, best.traj);
pub_->publishCostGrid(costmap_ros_, critics_);
return cmd_vel;
} catch (const dwb_core::NoLegalTrajectoriesException & e) {
nav_2d_msgs::msg::Twist2D empty_cmd;
dwb_msgs::msg::Trajectory2D empty_traj;
// debrief stateful scoring functions
for (TrajectoryCritic::Ptr & critic : critics_) {
critic->debrief(empty_cmd);
}
lock.unlock();
pub_->publishLocalPlan(pose.header, empty_traj);
pub_->publishCostGrid(costmap_ros_, critics_);
throw;
}
}
dwb_msgs::msg::TrajectoryScore
DWBLocalPlanner::coreScoringAlgorithm(
const geometry_msgs::msg::Pose2D & pose,
const nav_2d_msgs::msg::Twist2D velocity,
std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> & results)
{
nav_2d_msgs::msg::Twist2D twist;
dwb_msgs::msg::Trajectory2D traj;
dwb_msgs::msg::TrajectoryScore best, worst;
best.total = -1;
worst.total = -1;
IllegalTrajectoryTracker tracker;
traj_generator_->startNewIteration(velocity);
while (traj_generator_->hasMoreTwists()) {
twist = traj_generator_->nextTwist();
traj = traj_generator_->generateTrajectory(pose, velocity, twist);
try {
dwb_msgs::msg::TrajectoryScore score = scoreTrajectory(traj, best.total);
tracker.addLegalTrajectory();
if (results) {
results->twists.push_back(score);
}
if (best.total < 0 || score.total < best.total) {
best = score;
if (results) {
results->best_index = results->twists.size() - 1;
}
}
if (worst.total < 0 || score.total > worst.total) {
worst = score;
if (results) {
results->worst_index = results->twists.size() - 1;
}
}
} catch (const dwb_core::IllegalTrajectoryException & e) {
if (results) {
dwb_msgs::msg::TrajectoryScore failed_score;
failed_score.traj = traj;
dwb_msgs::msg::CriticScore cs;
cs.name = e.getCriticName();
cs.raw_score = -1.0;
failed_score.scores.push_back(cs);
failed_score.total = -1.0;
results->twists.push_back(failed_score);
}
tracker.addIllegalTrajectory(e);
}
}
if (best.total < 0) {
if (debug_trajectory_details_) {
RCLCPP_ERROR(rclcpp::get_logger("DWBLocalPlanner"), "%s", tracker.getMessage().c_str());
for (auto const & x : tracker.getPercentages()) {
RCLCPP_ERROR(
rclcpp::get_logger(
"DWBLocalPlanner"), "%.2f: %10s/%s", x.second,
x.first.first.c_str(), x.first.second.c_str());
}
}
throw NoLegalTrajectoriesException(tracker);
}
return best;
}
dwb_msgs::msg::TrajectoryScore
DWBLocalPlanner::scoreTrajectory(
const dwb_msgs::msg::Trajectory2D & traj,
double best_score)
{
dwb_msgs::msg::TrajectoryScore score;
score.traj = traj;
for (TrajectoryCritic::Ptr & critic : critics_) {
dwb_msgs::msg::CriticScore cs;
cs.name = critic->getName();
cs.scale = critic->getScale();
if (cs.scale == 0.0) {
score.scores.push_back(cs);
continue;
}
double critic_score = critic->scoreTrajectory(traj);
cs.raw_score = critic_score;
score.scores.push_back(cs);
score.total += critic_score * cs.scale;
if (short_circuit_trajectory_evaluation_ && best_score > 0 && score.total > best_score) {
// since we keep adding positives, once we are worse than the best, we will stay worse
break;
}
}
return score;
}
nav_2d_msgs::msg::Path2D
DWBLocalPlanner::transformGlobalPlan(
const nav_2d_msgs::msg::Pose2DStamped & pose)
{
if (global_plan_.poses.empty()) {
throw nav2_core::PlannerException("Received plan with zero length");
}
// let's get the pose of the robot in the frame of the plan
nav_2d_msgs::msg::Pose2DStamped robot_pose;
if (!nav_2d_utils::transformPose(
tf_, global_plan_.header.frame_id, pose,
robot_pose, transform_tolerance_))
{
throw dwb_core::
PlannerTFException("Unable to transform robot pose into global plan's frame");
}
// we'll discard points on the plan that are outside the local costmap
nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
double dist_threshold = std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()) *
costmap->getResolution() / 2.0;
// If prune_plan is enabled (it is by default) then we want to restrict the
// plan to distances within that range as well.
double prune_dist = prune_distance_;
// Set the maximum distance we'll include points before getting to the part
// of the path where the robot is located (the start of the plan). Basically,
// these are the points the robot has already passed.
double transform_start_threshold;
if (prune_plan_) {
transform_start_threshold = std::min(dist_threshold, prune_dist);
} else {
transform_start_threshold = dist_threshold;
}
// Set the maximum distance we'll include points after the part of the plan
// near the robot (the end of the plan). This determines the amount of the
// plan passed on to the critics
double transform_end_threshold;
double forward_prune_dist = forward_prune_distance_;
if (shorten_transformed_plan_) {
transform_end_threshold = std::min(dist_threshold, forward_prune_dist);
} else {
transform_end_threshold = dist_threshold;
}
// Find the first pose in the global plan that's further than prune distance
// from the robot using integrated distance
auto prune_point = nav2_util::geometry_utils::first_after_integrated_distance(
global_plan_.poses.begin(), global_plan_.poses.end(), prune_dist);
// Find the first pose in the plan (upto prune_point) that's less than transform_start_threshold
// from the robot.
auto transformation_begin = std::find_if(
begin(global_plan_.poses), prune_point,
[&](const auto & global_plan_pose) {
return euclidean_distance(robot_pose.pose, global_plan_pose) < transform_start_threshold;
});
// Find the first pose in the end of the plan that's further than transform_end_threshold
// from the robot using integrated distance
auto transformation_end = std::find_if(
transformation_begin, global_plan_.poses.end(),
[&](const auto & pose) {
return euclidean_distance(pose, robot_pose.pose) > transform_end_threshold;
});
// Transform the near part of the global plan into the robot's frame of reference.
nav_2d_msgs::msg::Path2D transformed_plan;
transformed_plan.header.frame_id = costmap_ros_->getGlobalFrameID();
transformed_plan.header.stamp = pose.header.stamp;
// Helper function for the transform below. Converts a pose2D from global
// frame to local
auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) {
nav_2d_msgs::msg::Pose2DStamped stamped_pose, transformed_pose;
stamped_pose.header.frame_id = global_plan_.header.frame_id;
stamped_pose.pose = global_plan_pose;
nav_2d_utils::transformPose(
tf_, transformed_plan.header.frame_id,
stamped_pose, transformed_pose, transform_tolerance_);
return transformed_pose.pose;
};
std::transform(
transformation_begin, transformation_end,
std::back_inserter(transformed_plan.poses),
transformGlobalPoseToLocal);
// Remove the portion of the global plan that we've already passed so we don't
// process it on the next iteration.
if (prune_plan_) {
global_plan_.poses.erase(begin(global_plan_.poses), transformation_begin);
pub_->publishGlobalPlan(global_plan_);
}
if (transformed_plan.poses.empty()) {
throw nav2_core::PlannerException("Resulting plan has 0 poses in it.");
}
return transformed_plan;
}
} // namespace dwb_core
// Register this controller as a nav2_core plugin
PLUGINLIB_EXPORT_CLASS(
dwb_core::DWBLocalPlanner,
nav2_core::Controller)
navigation2/nav2_dwb_controller/dwb_core/src/illegal_trajectory_tracker.cpp
+80
View File
@@ -0,0 +1,80 @@
/*
* 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_core/illegal_trajectory_tracker.hpp"
#include <map>
#include <utility>
#include <string>
#include <sstream>
namespace dwb_core
{
void IllegalTrajectoryTracker::addIllegalTrajectory(
const dwb_core::IllegalTrajectoryException & e)
{
counts_[std::make_pair(e.getCriticName(), e.what())]++;
illegal_count_++;
}
void IllegalTrajectoryTracker::addLegalTrajectory()
{
legal_count_++;
}
std::map<std::pair<std::string, std::string>,
double> IllegalTrajectoryTracker::getPercentages() const
{
std::map<std::pair<std::string, std::string>, double> percents;
double denominator = static_cast<double>(legal_count_ + illegal_count_);
for (auto const & x : counts_) {
percents[x.first] = static_cast<double>(x.second) / denominator;
}
return percents;
}
std::string IllegalTrajectoryTracker::getMessage() const
{
std::ostringstream msg;
if (legal_count_ == 0) {
msg << "No valid trajectories out of " << illegal_count_ << "! ";
} else {
unsigned int total = legal_count_ + illegal_count_;
msg << legal_count_ << " valid trajectories found (";
msg << static_cast<double>(100 * legal_count_) / static_cast<double>(total);
msg << "% of " << total << "). ";
}
return msg.str();
}
} // namespace dwb_core
navigation2/nav2_dwb_controller/dwb_core/src/publisher.cpp
+369
View File
@@ -0,0 +1,369 @@
/*
* 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_core/publisher.hpp"
#include <algorithm>
#include <memory>
#include <string>
#include <vector>
#include <utility>
#include "sensor_msgs/point_cloud2_iterator.hpp"
#include "nav_2d_utils/conversions.hpp"
#include "nav2_util/node_utils.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "visualization_msgs/msg/marker_array.hpp"
#include "visualization_msgs/msg/marker.hpp"
using std::max;
using std::string;
using nav2_util::declare_parameter_if_not_declared;
namespace dwb_core
{
DWBPublisher::DWBPublisher(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
const std::string & plugin_name)
: node_(parent),
plugin_name_(plugin_name)
{
auto node = node_.lock();
clock_ = node->get_clock();
}
nav2_util::CallbackReturn
DWBPublisher::on_configure()
{
auto node = node_.lock();
if (!node) {
throw std::runtime_error{"Failed to lock node"};
}
declare_parameter_if_not_declared(
node, plugin_name_ + ".publish_evaluation",
rclcpp::ParameterValue(true));
declare_parameter_if_not_declared(
node, plugin_name_ + ".publish_global_plan",
rclcpp::ParameterValue(true));
declare_parameter_if_not_declared(
node, plugin_name_ + ".publish_transformed_plan",
rclcpp::ParameterValue(true));
declare_parameter_if_not_declared(
node, plugin_name_ + ".publish_local_plan",
rclcpp::ParameterValue(true));
declare_parameter_if_not_declared(
node, plugin_name_ + ".publish_trajectories",
rclcpp::ParameterValue(true));
declare_parameter_if_not_declared(
node, plugin_name_ + ".publish_cost_grid_pc",
rclcpp::ParameterValue(false));
declare_parameter_if_not_declared(
node, plugin_name_ + ".marker_lifetime",
rclcpp::ParameterValue(0.1));
node->get_parameter(plugin_name_ + ".publish_evaluation", publish_evaluation_);
node->get_parameter(plugin_name_ + ".publish_global_plan", publish_global_plan_);
node->get_parameter(plugin_name_ + ".publish_transformed_plan", publish_transformed_);
node->get_parameter(plugin_name_ + ".publish_local_plan", publish_local_plan_);
node->get_parameter(plugin_name_ + ".publish_trajectories", publish_trajectories_);
node->get_parameter(plugin_name_ + ".publish_cost_grid_pc", publish_cost_grid_pc_);
eval_pub_ = node->create_publisher<dwb_msgs::msg::LocalPlanEvaluation>("evaluation", 1);
global_pub_ = node->create_publisher<nav_msgs::msg::Path>("received_global_plan", 1);
transformed_pub_ = node->create_publisher<nav_msgs::msg::Path>("transformed_global_plan", 1);
local_pub_ = node->create_publisher<nav_msgs::msg::Path>("local_plan", 1);
marker_pub_ = node->create_publisher<visualization_msgs::msg::MarkerArray>("marker", 1);
cost_grid_pc_pub_ = node->create_publisher<sensor_msgs::msg::PointCloud2>("cost_cloud", 1);
double marker_lifetime = 0.0;
node->get_parameter(plugin_name_ + ".marker_lifetime", marker_lifetime);
marker_lifetime_ = rclcpp::Duration::from_seconds(marker_lifetime);
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
DWBPublisher::on_activate()
{
eval_pub_->on_activate();
global_pub_->on_activate();
transformed_pub_->on_activate();
local_pub_->on_activate();
marker_pub_->on_activate();
cost_grid_pc_pub_->on_activate();
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
DWBPublisher::on_deactivate()
{
eval_pub_->on_deactivate();
global_pub_->on_deactivate();
transformed_pub_->on_deactivate();
local_pub_->on_deactivate();
marker_pub_->on_deactivate();
cost_grid_pc_pub_->on_deactivate();
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
DWBPublisher::on_cleanup()
{
eval_pub_.reset();
global_pub_.reset();
transformed_pub_.reset();
local_pub_.reset();
marker_pub_.reset();
cost_grid_pc_pub_.reset();
return nav2_util::CallbackReturn::SUCCESS;
}
void
DWBPublisher::publishEvaluation(std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> results)
{
if (results) {
if (publish_evaluation_ && eval_pub_->get_subscription_count() > 0) {
auto msg = std::make_unique<dwb_msgs::msg::LocalPlanEvaluation>(*results);
eval_pub_->publish(std::move(msg));
}
publishTrajectories(*results);
}
}
void
DWBPublisher::publishTrajectories(const dwb_msgs::msg::LocalPlanEvaluation & results)
{
if (marker_pub_->get_subscription_count() < 1) {return;}
if (!publish_trajectories_) {return;}
auto ma = std::make_unique<visualization_msgs::msg::MarkerArray>();
visualization_msgs::msg::Marker m;
if (results.twists.size() == 0) {return;}
geometry_msgs::msg::Point pt;
m.header = results.header;
m.type = m.LINE_STRIP;
m.pose.orientation.w = 1;
m.scale.x = 0.002;
m.color.a = 1.0;
m.lifetime = marker_lifetime_;
double best_cost = results.twists[results.best_index].total;
double worst_cost = results.twists[results.worst_index].total;
double denominator = worst_cost - best_cost;
if (std::fabs(denominator) < 1e-9) {
denominator = 1.0;
}
unsigned currentValidId = 0;
unsigned currentInvalidId = 0;
string validNamespace("ValidTrajectories");
string invalidNamespace("InvalidTrajectories");
for (unsigned int i = 0; i < results.twists.size(); i++) {
const dwb_msgs::msg::TrajectoryScore & twist = results.twists[i];
double displayLevel = (twist.total - best_cost) / denominator;
if (twist.total >= 0) {
m.color.r = displayLevel;
m.color.g = 1.0 - displayLevel;
m.color.b = 0;
m.color.a = 1.0;
m.ns = validNamespace;
m.id = currentValidId;
++currentValidId;
} else {
m.color.r = 0;
m.color.g = 0;
m.color.b = 0;
m.color.a = 1.0;
m.ns = invalidNamespace;
m.id = currentInvalidId;
++currentInvalidId;
}
m.points.clear();
for (unsigned int j = 0; j < twist.traj.poses.size(); ++j) {
pt.x = twist.traj.poses[j].x;
pt.y = twist.traj.poses[j].y;
pt.z = 0;
m.points.push_back(pt);
}
ma->markers.push_back(m);
}
marker_pub_->publish(std::move(ma));
}
void
DWBPublisher::publishLocalPlan(
const std_msgs::msg::Header & header,
const dwb_msgs::msg::Trajectory2D & traj)
{
if (!publish_local_plan_) {return;}
auto path =
std::make_unique<nav_msgs::msg::Path>(
nav_2d_utils::poses2DToPath(
traj.poses, header.frame_id,
header.stamp));
if (local_pub_->get_subscription_count() > 0) {
local_pub_->publish(std::move(path));
}
}
void
DWBPublisher::publishCostGrid(
const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros,
const std::vector<TrajectoryCritic::Ptr> critics)
{
if (cost_grid_pc_pub_->get_subscription_count() < 1) {return;}
if (!publish_cost_grid_pc_) {return;}
auto cost_grid_pc = std::make_unique<sensor_msgs::msg::PointCloud2>();
cost_grid_pc->header.frame_id = costmap_ros->getGlobalFrameID();
cost_grid_pc->header.stamp = clock_->now();
nav2_costmap_2d::Costmap2D * costmap = costmap_ros->getCostmap();
double x_coord, y_coord;
unsigned int size_x = costmap->getSizeInCellsX();
unsigned int size_y = costmap->getSizeInCellsY();
std::vector<std::pair<std::string, std::vector<float>>> cost_channels;
std::vector<float> total_cost(size_x * size_y, 0.0);
for (TrajectoryCritic::Ptr critic : critics) {
unsigned int channel_index = cost_channels.size();
critic->addCriticVisualization(cost_channels);
if (channel_index == cost_channels.size()) {
// No channels were added, so skip to next critic
continue;
}
double scale = critic->getScale();
for (unsigned int i = 0; i < size_x * size_y; i++) {
total_cost[i] += cost_channels[channel_index].second[i] * scale;
}
}
cost_channels.push_back(std::make_pair("total_cost", total_cost));
cost_grid_pc->width = size_x * size_y;
cost_grid_pc->height = 1;
cost_grid_pc->fields.resize(3 + cost_channels.size()); // x,y,z, + cost channels
cost_grid_pc->is_dense = true;
cost_grid_pc->is_bigendian = false;
int offset = 0;
for (size_t i = 0; i < cost_grid_pc->fields.size(); ++i, offset += 4) {
cost_grid_pc->fields[i].offset = offset;
cost_grid_pc->fields[i].count = 1;
cost_grid_pc->fields[i].datatype = sensor_msgs::msg::PointField::FLOAT32;
if (i >= 3) {
cost_grid_pc->fields[i].name = cost_channels[i - 3].first;
}
}
cost_grid_pc->fields[0].name = "x";
cost_grid_pc->fields[1].name = "y";
cost_grid_pc->fields[2].name = "z";
cost_grid_pc->point_step = offset;
cost_grid_pc->row_step = cost_grid_pc->point_step * cost_grid_pc->width;
cost_grid_pc->data.resize(cost_grid_pc->row_step * cost_grid_pc->height);
std::vector<sensor_msgs::PointCloud2Iterator<float>> cost_grid_pc_iter;
for (size_t i = 0; i < cost_grid_pc->fields.size(); ++i) {
sensor_msgs::PointCloud2Iterator<float> iter(*cost_grid_pc, cost_grid_pc->fields[i].name);
cost_grid_pc_iter.push_back(iter);
}
unsigned int j = 0;
for (unsigned int cy = 0; cy < size_y; cy++) {
for (unsigned int cx = 0; cx < size_x; cx++) {
costmap->mapToWorld(cx, cy, x_coord, y_coord);
*cost_grid_pc_iter[0] = x_coord;
*cost_grid_pc_iter[1] = y_coord;
*cost_grid_pc_iter[2] = 0.0; // z value
for (size_t i = 3; i < cost_grid_pc_iter.size(); ++i) {
*cost_grid_pc_iter[i] = cost_channels[i - 3].second[j];
++cost_grid_pc_iter[i];
}
++cost_grid_pc_iter[0];
++cost_grid_pc_iter[1];
++cost_grid_pc_iter[2];
j++;
}
}
cost_grid_pc_pub_->publish(std::move(cost_grid_pc));
}
void
DWBPublisher::publishGlobalPlan(const nav_2d_msgs::msg::Path2D plan)
{
publishGenericPlan(plan, *global_pub_, publish_global_plan_);
}
void
DWBPublisher::publishTransformedPlan(const nav_2d_msgs::msg::Path2D plan)
{
publishGenericPlan(plan, *transformed_pub_, publish_transformed_);
}
void
DWBPublisher::publishLocalPlan(const nav_2d_msgs::msg::Path2D plan)
{
publishGenericPlan(plan, *local_pub_, publish_local_plan_);
}
void
DWBPublisher::publishGenericPlan(
const nav_2d_msgs::msg::Path2D plan,
rclcpp::Publisher<nav_msgs::msg::Path> & pub, bool flag)
{
if (pub.get_subscription_count() < 1) {return;}
if (!flag) {return;}
auto path = std::make_unique<nav_msgs::msg::Path>(nav_2d_utils::pathToPath(plan));
pub.publish(std::move(path));
}
} // namespace dwb_core
navigation2/nav2_dwb_controller/dwb_core/src/trajectory_utils.cpp
+109
View File
@@ -0,0 +1,109 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, 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_core/trajectory_utils.hpp>
#include <cmath>
#include "rclcpp/duration.hpp"
#include "dwb_core/exceptions.hpp"
namespace dwb_core
{
const geometry_msgs::msg::Pose2D & getClosestPose(
const dwb_msgs::msg::Trajectory2D & trajectory,
const double time_offset)
{
rclcpp::Duration goal_time = rclcpp::Duration::from_seconds(time_offset);
const unsigned int num_poses = trajectory.poses.size();
if (num_poses == 0) {
throw nav2_core::PlannerException("Cannot call getClosestPose on empty trajectory.");
}
unsigned int closest_index = num_poses;
double closest_diff = 0.0;
for (unsigned int i = 0; i < num_poses; ++i) {
double diff = std::fabs((rclcpp::Duration(trajectory.time_offsets[i]) - goal_time).seconds());
if (closest_index == num_poses || diff < closest_diff) {
closest_index = i;
closest_diff = diff;
}
if (goal_time < rclcpp::Duration(trajectory.time_offsets[i])) {
break;
}
}
return trajectory.poses[closest_index];
}
geometry_msgs::msg::Pose2D projectPose(
const dwb_msgs::msg::Trajectory2D & trajectory,
const double time_offset)
{
rclcpp::Duration goal_time = rclcpp::Duration::from_seconds(time_offset);
const unsigned int num_poses = trajectory.poses.size();
if (num_poses == 0) {
throw nav2_core::PlannerException("Cannot call projectPose on empty trajectory.");
}
if (goal_time <= (trajectory.time_offsets[0])) {
return trajectory.poses[0];
} else if (goal_time >= rclcpp::Duration(trajectory.time_offsets[num_poses - 1])) {
return trajectory.poses[num_poses - 1];
}
for (unsigned int i = 0; i < num_poses - 1; ++i) {
if (goal_time >= rclcpp::Duration(trajectory.time_offsets[i]) &&
goal_time < rclcpp::Duration(trajectory.time_offsets[i + 1]))
{
double time_diff =
(rclcpp::Duration(trajectory.time_offsets[i + 1]) -
rclcpp::Duration(trajectory.time_offsets[i])).seconds();
double ratio = (goal_time - rclcpp::Duration(trajectory.time_offsets[i])).seconds() /
time_diff;
double inv_ratio = 1.0 - ratio;
const geometry_msgs::msg::Pose2D & pose_a = trajectory.poses[i];
const geometry_msgs::msg::Pose2D & pose_b = trajectory.poses[i + 1];
geometry_msgs::msg::Pose2D projected;
projected.x = pose_a.x * inv_ratio + pose_b.x * ratio;
projected.y = pose_a.y * inv_ratio + pose_b.y * ratio;
projected.theta = pose_a.theta * inv_ratio + pose_b.theta * ratio;
return projected;
}
}
// Should not reach this point
return trajectory.poses[num_poses - 1];
}
} // namespace dwb_core
navigation2/nav2_dwb_controller/dwb_core/test/CMakeLists.txt
+2
View File
@@ -0,0 +1,2 @@
ament_add_gtest(utils_test utils_test.cpp)
target_link_libraries(utils_test dwb_core)
navigation2/nav2_dwb_controller/dwb_core/test/utils_test.cpp
+114
View File
@@ -0,0 +1,114 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, 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 "gtest/gtest.h"
#include "dwb_core/trajectory_utils.hpp"
using dwb_core::getClosestPose;
using dwb_core::projectPose;
TEST(Utils, ClosestPose)
{
dwb_msgs::msg::Trajectory2D traj;
traj.poses.resize(4);
traj.time_offsets.resize(4);
for (unsigned int i = 0; i < traj.poses.size(); i++) {
double d = static_cast<double>(i);
traj.poses[i].x = d;
traj.time_offsets[i] = rclcpp::Duration::from_seconds(d);
}
EXPECT_DOUBLE_EQ(getClosestPose(traj, 0.0).x, traj.poses[0].x);
EXPECT_DOUBLE_EQ(getClosestPose(traj, -1.0).x, traj.poses[0].x);
EXPECT_DOUBLE_EQ(getClosestPose(traj, 0.4).x, traj.poses[0].x);
EXPECT_DOUBLE_EQ(getClosestPose(traj, 0.5).x, traj.poses[0].x);
EXPECT_DOUBLE_EQ(getClosestPose(traj, 0.51).x, traj.poses[1].x);
EXPECT_DOUBLE_EQ(getClosestPose(traj, 1.0).x, traj.poses[1].x);
EXPECT_DOUBLE_EQ(getClosestPose(traj, 1.4999).x, traj.poses[1].x);
EXPECT_DOUBLE_EQ(getClosestPose(traj, 2.0).x, traj.poses[2].x);
EXPECT_DOUBLE_EQ(getClosestPose(traj, 2.51).x, traj.poses[3].x);
EXPECT_DOUBLE_EQ(getClosestPose(traj, 3.5).x, traj.poses[3].x);
}
TEST(Utils, ProjectPose)
{
dwb_msgs::msg::Trajectory2D traj;
traj.poses.resize(4);
traj.time_offsets.resize(4);
for (unsigned int i = 0; i < traj.poses.size(); i++) {
double d = static_cast<double>(i);
traj.poses[i].x = d;
traj.poses[i].y = 30.0 - 2.0 * d;
traj.poses[i].theta = 0.42;
traj.time_offsets[i] = rclcpp::Duration::from_seconds(d);
}
EXPECT_DOUBLE_EQ(projectPose(traj, 0.0).x, 0.0);
EXPECT_DOUBLE_EQ(projectPose(traj, 0.0).y, 30.0);
EXPECT_DOUBLE_EQ(projectPose(traj, 0.0).theta, 0.42);
EXPECT_DOUBLE_EQ(projectPose(traj, -1.0).x, 0.0);
EXPECT_DOUBLE_EQ(projectPose(traj, -1.0).y, 30.0);
EXPECT_DOUBLE_EQ(projectPose(traj, -1.0).theta, 0.42);
EXPECT_DOUBLE_EQ(projectPose(traj, 0.4).x, 0.4);
EXPECT_DOUBLE_EQ(projectPose(traj, 0.4).y, 29.2);
EXPECT_DOUBLE_EQ(projectPose(traj, 0.4).theta, 0.42);
EXPECT_DOUBLE_EQ(projectPose(traj, 0.5).x, 0.5);
EXPECT_DOUBLE_EQ(projectPose(traj, 0.5).y, 29.0);
EXPECT_DOUBLE_EQ(projectPose(traj, 0.5).theta, 0.42);
EXPECT_DOUBLE_EQ(projectPose(traj, 0.51).x, 0.51);
EXPECT_DOUBLE_EQ(projectPose(traj, 0.51).y, 28.98);
EXPECT_DOUBLE_EQ(projectPose(traj, 0.51).theta, 0.42);
EXPECT_DOUBLE_EQ(projectPose(traj, 1.0).x, 1.0);
EXPECT_DOUBLE_EQ(projectPose(traj, 1.0).y, 28.0);
EXPECT_DOUBLE_EQ(projectPose(traj, 1.0).theta, 0.42);
EXPECT_DOUBLE_EQ(projectPose(traj, 1.4999).x, 1.4999);
EXPECT_DOUBLE_EQ(projectPose(traj, 1.4999).y, 27.0002);
EXPECT_DOUBLE_EQ(projectPose(traj, 1.4999).theta, 0.42);
EXPECT_DOUBLE_EQ(projectPose(traj, 2.0).x, 2.0);
EXPECT_DOUBLE_EQ(projectPose(traj, 2.0).y, 26.0);
EXPECT_DOUBLE_EQ(projectPose(traj, 2.0).theta, 0.42);
EXPECT_FLOAT_EQ(projectPose(traj, 2.51).x, 2.51);
EXPECT_FLOAT_EQ(projectPose(traj, 2.51).y, 24.98);
EXPECT_DOUBLE_EQ(projectPose(traj, 2.51).theta, 0.42);
EXPECT_DOUBLE_EQ(projectPose(traj, 3.5).x, 3.0);
EXPECT_DOUBLE_EQ(projectPose(traj, 3.5).y, 24.0);
EXPECT_DOUBLE_EQ(projectPose(traj, 3.5).theta, 0.42);
}
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
navigation2/nav2_dwb_controller/dwb_critics/CMakeLists.txt
+87
View File
@@ -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()
navigation2/nav2_dwb_controller/dwb_critics/default_critics.xml
+42
View File
@@ -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>
navigation2/nav2_dwb_controller/dwb_critics/include/dwb_critics/alignment_util.hpp
+54
View File
@@ -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_
navigation2/nav2_dwb_controller/dwb_critics/include/dwb_critics/base_obstacle.hpp
+84
View File
@@ -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_
navigation2/nav2_dwb_controller/dwb_critics/include/dwb_critics/goal_align.hpp
+68
View File
@@ -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_
navigation2/nav2_dwb_controller/dwb_critics/include/dwb_critics/goal_dist.hpp
+64
View File
@@ -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_
navigation2/nav2_dwb_controller/dwb_critics/include/dwb_critics/line_iterator.hpp
+151
View File
@@ -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_
navigation2/nav2_dwb_controller/dwb_critics/include/dwb_critics/map_grid.hpp
+142
View File
@@ -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_
navigation2/nav2_dwb_controller/dwb_critics/include/dwb_critics/obstacle_footprint.hpp
+100
View File
@@ -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_
navigation2/nav2_dwb_controller/dwb_critics/include/dwb_critics/oscillation.hpp
+162
View File
@@ -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_
navigation2/nav2_dwb_controller/dwb_critics/include/dwb_critics/path_align.hpp
+74
View File
@@ -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_
navigation2/nav2_dwb_controller/dwb_critics/include/dwb_critics/path_dist.hpp
+54
View File
@@ -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_
navigation2/nav2_dwb_controller/dwb_critics/include/dwb_critics/prefer_forward.hpp
+66
View File
@@ -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_
navigation2/nav2_dwb_controller/dwb_critics/include/dwb_critics/rotate_to_goal.hpp
+101
View File
@@ -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_
navigation2/nav2_dwb_controller/dwb_critics/include/dwb_critics/twirling.hpp
+59
View File
@@ -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_
navigation2/nav2_dwb_controller/dwb_critics/package.xml
+31
View File
@@ -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>
navigation2/nav2_dwb_controller/dwb_critics/src/alignment_util.cpp
+51
View File
@@ -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
navigation2/nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp
+117
View File
@@ -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
navigation2/nav2_dwb_controller/dwb_critics/src/goal_align.cpp
+86
View File
@@ -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)
navigation2/nav2_dwb_controller/dwb_critics/src/goal_dist.cpp
+106
View File
@@ -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)
navigation2/nav2_dwb_controller/dwb_critics/src/map_grid.cpp
+189
View File
@@ -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
navigation2/nav2_dwb_controller/dwb_critics/src/obstacle_footprint.cpp
+165
View File
@@ -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
navigation2/nav2_dwb_controller/dwb_critics/src/oscillation.cpp
+234
View File
@@ -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
navigation2/nav2_dwb_controller/dwb_critics/src/path_align.cpp
+95
View File
@@ -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)
navigation2/nav2_dwb_controller/dwb_critics/src/path_dist.cpp
+95
View File
@@ -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)
navigation2/nav2_dwb_controller/dwb_critics/src/prefer_forward.cpp
+88
View File
@@ -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
navigation2/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp
+135
View File
@@ -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
navigation2/nav2_dwb_controller/dwb_critics/src/twirling.cpp
+56
View File
@@ -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)
navigation2/nav2_dwb_controller/dwb_critics/test/CMakeLists.txt
+14
View File
@@ -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)
navigation2/nav2_dwb_controller/dwb_critics/test/alignment_util_test.cpp
+77
View File
@@ -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;
}
navigation2/nav2_dwb_controller/dwb_critics/test/base_obstacle_test.cpp
+173
View File
@@ -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;
}
navigation2/nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp
+264
View File
@@ -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;
}
navigation2/nav2_dwb_controller/dwb_critics/test/prefer_forward_test.cpp
+229
View File
@@ -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;
}
navigation2/nav2_dwb_controller/dwb_critics/test/twirling_test.cpp
+78
View File
@@ -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;
}
navigation2/nav2_dwb_controller/dwb_msgs/CMakeLists.txt
+26
View File
@@ -0,0 +1,26 @@
cmake_minimum_required(VERSION 3.5)
project(dwb_msgs)
find_package(builtin_interfaces REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(nav_2d_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(dwb_msgs
"msg/CriticScore.msg"
"msg/LocalPlanEvaluation.msg"
"msg/Trajectory2D.msg"
"msg/TrajectoryScore.msg"
"srv/DebugLocalPlan.srv"
"srv/GenerateTrajectory.srv"
"srv/GenerateTwists.srv"
"srv/GetCriticScore.srv"
"srv/ScoreTrajectory.srv"
DEPENDENCIES geometry_msgs std_msgs nav_2d_msgs nav_msgs builtin_interfaces
)
ament_export_dependencies(rosidl_default_runtime)
ament_package()
navigation2/nav2_dwb_controller/dwb_msgs/msg/CriticScore.msg
+7
View File
@@ -0,0 +1,7 @@
# The result from one critic scoring a Twist.
# Name of the critic
string name
# Score for the critic, not multiplied by the scale
float32 raw_score
# Scale for the critic, multiplied by the raw_score and added to the total score
float32 scale
navigation2/nav2_dwb_controller/dwb_msgs/msg/LocalPlanEvaluation.msg
+10
View File
@@ -0,0 +1,10 @@
# Full Scoring for running the local planner
# Header, used for timestamp
std_msgs/Header header
# All the trajectories evaluated and their scores
TrajectoryScore[] twists
# Convenience index of the best (lowest) score in the twists array
uint16 best_index
# Convenience index of the worst (highest) score in the twists array. Useful for scaling.
uint16 worst_index
navigation2/nav2_dwb_controller/dwb_msgs/msg/Trajectory2D.msg
+8
View File
@@ -0,0 +1,8 @@
# For a given velocity command, the poses that the robot will go to in the allotted time.
# Input Velocity
nav_2d_msgs/Twist2D velocity
# Time difference between first and last poses
builtin_interfaces/Duration[] time_offsets
# Poses the robot will go to, given our kinematic model
geometry_msgs/Pose2D[] poses
navigation2/nav2_dwb_controller/dwb_msgs/msg/TrajectoryScore.msg
+8
View File
@@ -0,0 +1,8 @@
# Complete scoring for a given twist.
# The trajectory being scored
Trajectory2D traj
# The Scores for each of the critics employed
CriticScore[] scores
# Convenience member that totals the critic scores
float32 total
navigation2/nav2_dwb_controller/dwb_msgs/package.xml
+24
View File
@@ -0,0 +1,24 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>dwb_msgs</name>
<version>1.1.18</version>
<description>Message/Service definitions specifically for the dwb_core</description>
<maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
<license>BSD-3-Clause</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>builtin_interfaces</depend>
<depend>geometry_msgs</depend>
<depend>nav_2d_msgs</depend>
<depend>std_msgs</depend>
<depend>nav_msgs</depend>
<depend>rosidl_default_runtime</depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
navigation2/nav2_dwb_controller/dwb_msgs/srv/DebugLocalPlan.srv
+6
View File
@@ -0,0 +1,6 @@
# For a given pose velocity and global_plan, run the local planner and return full results
nav_2d_msgs/Pose2DStamped pose
nav_2d_msgs/Twist2D velocity
nav_2d_msgs/Path2D global_plan
---
LocalPlanEvaluation results
navigation2/nav2_dwb_controller/dwb_msgs/srv/GenerateTrajectory.srv
+6
View File
@@ -0,0 +1,6 @@
# For a given start pose, velocity and desired velocity, generate which poses will be visited
geometry_msgs/Pose2D start_pose
nav_2d_msgs/Twist2D start_vel
nav_2d_msgs/Twist2D cmd_vel
---
Trajectory2D traj
navigation2/nav2_dwb_controller/dwb_msgs/srv/GenerateTwists.srv
+4
View File
@@ -0,0 +1,4 @@
# For a given velocity, generate which twist commands will be evaluated
nav_2d_msgs/Twist2D current_vel
---
nav_2d_msgs/Twist2D[] twists
navigation2/nav2_dwb_controller/dwb_msgs/srv/GetCriticScore.srv
+7
View File
@@ -0,0 +1,7 @@
nav_2d_msgs/Pose2DStamped pose
nav_2d_msgs/Twist2D velocity
nav_2d_msgs/Path2D global_plan
Trajectory2D traj
string critic_name
---
CriticScore score
navigation2/nav2_dwb_controller/dwb_msgs/srv/ScoreTrajectory.srv
+6
View File
@@ -0,0 +1,6 @@
nav_2d_msgs/Pose2DStamped pose
nav_2d_msgs/Twist2D velocity
nav_2d_msgs/Path2D global_plan
Trajectory2D traj
---
TrajectoryScore score
navigation2/nav2_dwb_controller/dwb_plugins/CMakeLists.txt
+64
View File
@@ -0,0 +1,64 @@
cmake_minimum_required(VERSION 3.5)
project(dwb_plugins)
find_package(ament_cmake REQUIRED)
find_package(nav2_common REQUIRED)
find_package(angles REQUIRED)
find_package(dwb_core REQUIRED)
find_package(nav_2d_msgs REQUIRED)
find_package(nav_2d_utils REQUIRED)
find_package(pluginlib REQUIRED)
find_package(rclcpp REQUIRED)
find_package(nav2_util REQUIRED)
nav2_package()
set(dependencies
angles
dwb_core
nav_2d_msgs
nav_2d_utils
pluginlib
rclcpp
nav2_util
)
include_directories(
include
)
add_library(standard_traj_generator SHARED
src/standard_traj_generator.cpp
src/limited_accel_generator.cpp
src/kinematic_parameters.cpp
src/xy_theta_iterator.cpp)
ament_target_dependencies(standard_traj_generator ${dependencies})
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()
install(TARGETS standard_traj_generator
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
install(DIRECTORY include/
DESTINATION include/
)
install(FILES plugins.xml
DESTINATION share/${PROJECT_NAME}
)
ament_export_include_directories(include)
ament_export_libraries(standard_traj_generator)
pluginlib_export_plugin_description_file(dwb_core plugins.xml)
ament_package()
navigation2/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp
+138
View File
@@ -0,0 +1,138 @@
/*
* 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_PLUGINS__KINEMATIC_PARAMETERS_HPP_
#define DWB_PLUGINS__KINEMATIC_PARAMETERS_HPP_
#include <memory>
#include <string>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "nav2_util/lifecycle_node.hpp"
namespace dwb_plugins
{
/**
* @struct KinematicParameters
* @brief A struct containing one representation of the robot's kinematics
*/
struct KinematicParameters
{
friend class KinematicsHandler;
inline double getMinX() {return min_vel_x_;}
inline double getMaxX() {return max_vel_x_;}
inline double getAccX() {return acc_lim_x_;}
inline double getDecelX() {return decel_lim_x_;}
inline double getMinY() {return min_vel_y_;}
inline double getMaxY() {return max_vel_y_;}
inline double getAccY() {return acc_lim_y_;}
inline double getDecelY() {return decel_lim_y_;}
inline double getMinSpeedXY() {return min_speed_xy_;}
inline double getMaxSpeedXY() {return max_speed_xy_;}
inline double getMinTheta() {return -max_vel_theta_;}
inline double getMaxTheta() {return max_vel_theta_;}
inline double getAccTheta() {return acc_lim_theta_;}
inline double getDecelTheta() {return decel_lim_theta_;}
inline double getMinSpeedTheta() {return min_speed_theta_;}
inline double getMinSpeedXY_SQ() {return min_speed_xy_sq_;}
inline double getMaxSpeedXY_SQ() {return max_speed_xy_sq_;}
protected:
// For parameter descriptions, see cfg/KinematicParams.cfg
double min_vel_x_{0};
double min_vel_y_{0};
double max_vel_x_{0};
double max_vel_y_{0};
double base_max_vel_x_{0};
double base_max_vel_y_{0};
double max_vel_theta_{0};
double base_max_vel_theta_{0};
double min_speed_xy_{0};
double max_speed_xy_{0};
double base_max_speed_xy_{0};
double min_speed_theta_{0};
double acc_lim_x_{0};
double acc_lim_y_{0};
double acc_lim_theta_{0};
double decel_lim_x_{0};
double decel_lim_y_{0};
double decel_lim_theta_{0};
// Cached square values of min_speed_xy and max_speed_xy
double min_speed_xy_sq_{0};
double max_speed_xy_sq_{0};
};
/**
* @class KinematicsHandler
* @brief A class managing the representation of the robot's kinematics
*/
class KinematicsHandler
{
public:
KinematicsHandler();
~KinematicsHandler();
void initialize(const nav2_util::LifecycleNode::SharedPtr & nh, const std::string & plugin_name);
inline KinematicParameters getKinematics() {return *kinematics_.load();}
void setSpeedLimit(const double & speed_limit, const bool & percentage);
using Ptr = std::shared_ptr<KinematicsHandler>;
protected:
std::atomic<KinematicParameters *> kinematics_;
// Dynamic parameters handler
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
/**
* @brief Callback executed when a paramter change is detected
* @param parameters list of changed parameters
*/
rcl_interfaces::msg::SetParametersResult
dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
void update_kinematics(KinematicParameters kinematics);
std::string plugin_name_;
};
} // namespace dwb_plugins
#endif // DWB_PLUGINS__KINEMATIC_PARAMETERS_HPP_
navigation2/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/limited_accel_generator.hpp
+78
View File
@@ -0,0 +1,78 @@
/*
* 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_PLUGINS__LIMITED_ACCEL_GENERATOR_HPP_
#define DWB_PLUGINS__LIMITED_ACCEL_GENERATOR_HPP_
#include <memory>
#include <string>
#include "dwb_plugins/standard_traj_generator.hpp"
#include "nav2_util/lifecycle_node.hpp"
namespace dwb_plugins
{
/**
* @class LimitedAccelGenerator
* @brief Limits the acceleration in the generated trajectories to a fraction of the simulated time.
*/
class LimitedAccelGenerator : public StandardTrajectoryGenerator
{
public:
void initialize(
const nav2_util::LifecycleNode::SharedPtr & nh,
const std::string & plugin_name) override;
void startNewIteration(const nav_2d_msgs::msg::Twist2D & current_velocity) override;
protected:
/**
* @brief Calculate the velocity after a set period of time, given the desired velocity and acceleration limits
*
* Unlike the StandardTrajectoryGenerator, the velocity remains constant in the LimitedAccelGenerator
*
* @param cmd_vel Desired velocity
* @param start_vel starting velocity
* @param dt amount of time in seconds
* @return cmd_vel
*/
nav_2d_msgs::msg::Twist2D computeNewVelocity(
const nav_2d_msgs::msg::Twist2D & cmd_vel,
const nav_2d_msgs::msg::Twist2D & start_vel,
const double dt) override;
double acceleration_time_;
std::string plugin_name_;
};
} // namespace dwb_plugins
#endif // DWB_PLUGINS__LIMITED_ACCEL_GENERATOR_HPP_
navigation2/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/one_d_velocity_iterator.hpp
+170
View File
@@ -0,0 +1,170 @@
/*
* 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_PLUGINS__ONE_D_VELOCITY_ITERATOR_HPP_
#define DWB_PLUGINS__ONE_D_VELOCITY_ITERATOR_HPP_
#include <algorithm>
#include <cmath>
namespace dwb_plugins
{
const double EPSILON = 1E-5;
/**
* @brief Given initial conditions and a time, figure out the end velocity
*
* @param v0 Initial velocity
* @param accel The acceleration rate
* @param decel The decceleration rate
* @param dt Delta time - amount of time to project into the future
* @param target target velocity
* @return The velocity dt seconds after v0.
*/
inline double projectVelocity(double v0, double accel, double decel, double dt, double target)
{
double v1;
if (v0 < target) {
v1 = v0 + accel * dt;
return std::min(target, v1);
} else {
v1 = v0 + decel * dt;
return std::max(target, v1);
}
}
/**
* @class OneDVelocityIterator
* @brief An iterator for generating a number of samples in a range
*
* In its simplest usage, this gives us N (num_samples) different velocities that are reachable
* given our current velocity. However, there is some fancy logic around zero velocities and
* the min/max velocities
*
* If the current velocity is 2 m/s, and the acceleration limit is 1 m/ss and the acc_time is 1 s,
* this class would provide velocities between 1 m/s and 3 m/s.
*
*
*
*/
class OneDVelocityIterator
{
public:
/**
* @brief Constructor for the velocity iterator
*
* @param current Current velocity
* @param min Minimum velocity allowable
* @param max Maximum velocity allowable
* @param acc_limit Acceleration Limit
* @param decel_limit Deceleration Limit
* @param num_samples The number of samples to return
*/
OneDVelocityIterator(
double current, double min, double max, double acc_limit, double decel_limit, double acc_time,
int num_samples)
{
if (current < min) {
current = min;
} else if (current > max) {
current = max;
}
max_vel_ = projectVelocity(current, acc_limit, decel_limit, acc_time, max);
min_vel_ = projectVelocity(current, acc_limit, decel_limit, acc_time, min);
reset();
if (fabs(min_vel_ - max_vel_) < EPSILON) {
increment_ = 1.0;
return;
}
num_samples = std::max(2, num_samples);
// e.g. for 4 samples, split distance in 3 even parts
increment_ = (max_vel_ - min_vel_) / std::max(1, (num_samples - 1));
}
/**
* @brief Get the next velocity available
*/
double getVelocity() const
{
if (return_zero_now_) {return 0.0;}
return current_;
}
/**
* @brief Increment the iterator
*/
OneDVelocityIterator & operator++()
{
if (return_zero_ && current_ < 0.0 && current_ + increment_ > 0.0 &&
current_ + increment_ <= max_vel_ + EPSILON)
{
return_zero_now_ = true;
return_zero_ = false;
} else {
current_ += increment_;
return_zero_now_ = false;
}
return *this;
}
/**
* @brief Reset back to the first velocity
*/
void reset()
{
current_ = min_vel_;
return_zero_ = true;
return_zero_now_ = false;
}
/**
* If we have returned all the velocities for this iteration
*/
bool isFinished() const
{
return current_ > max_vel_ + EPSILON;
}
private:
bool return_zero_, return_zero_now_;
double min_vel_, max_vel_;
double current_;
double increment_;
};
} // namespace dwb_plugins
#endif // DWB_PLUGINS__ONE_D_VELOCITY_ITERATOR_HPP_
navigation2/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp
+172
View File
@@ -0,0 +1,172 @@
/*
* 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_PLUGINS__STANDARD_TRAJ_GENERATOR_HPP_
#define DWB_PLUGINS__STANDARD_TRAJ_GENERATOR_HPP_
#include <vector>
#include <memory>
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "dwb_core/trajectory_generator.hpp"
#include "dwb_plugins/velocity_iterator.hpp"
#include "dwb_plugins/kinematic_parameters.hpp"
#include "nav2_util/lifecycle_node.hpp"
namespace dwb_plugins
{
/**
* @class StandardTrajectoryGenerator
* @brief Standard DWA-like trajectory generator.
*/
class StandardTrajectoryGenerator : public dwb_core::TrajectoryGenerator
{
public:
// Standard TrajectoryGenerator interface
void initialize(
const nav2_util::LifecycleNode::SharedPtr & nh,
const std::string & plugin_name) override;
void startNewIteration(const nav_2d_msgs::msg::Twist2D & current_velocity) override;
bool hasMoreTwists() override;
nav_2d_msgs::msg::Twist2D nextTwist() override;
dwb_msgs::msg::Trajectory2D generateTrajectory(
const geometry_msgs::msg::Pose2D & start_pose,
const nav_2d_msgs::msg::Twist2D & start_vel,
const nav_2d_msgs::msg::Twist2D & cmd_vel) override;
/**
* @brief Limits the maximum linear speed of the robot.
* @param speed_limit expressed in absolute value (in m/s)
* or in percentage from maximum robot speed.
* @param percentage Setting speed limit in percentage if true
* or in absolute values in false case.
*/
void setSpeedLimit(const double & speed_limit, const bool & percentage) override
{
if (kinematics_handler_) {
kinematics_handler_->setSpeedLimit(speed_limit, percentage);
}
}
protected:
/**
* @brief Initialize the VelocityIterator pointer. Put in its own function for easy overriding
*/
virtual void initializeIterator(const nav2_util::LifecycleNode::SharedPtr & nh);
/**
* @brief Calculate the velocity after a set period of time, given the desired velocity and acceleration limits
*
* @param cmd_vel Desired velocity
* @param start_vel starting velocity
* @param dt amount of time in seconds
* @return new velocity after dt seconds
*/
virtual nav_2d_msgs::msg::Twist2D computeNewVelocity(
const nav_2d_msgs::msg::Twist2D & cmd_vel, const nav_2d_msgs::msg::Twist2D & start_vel,
const double dt);
/**
* @brief Use the robot's kinematic model to predict new positions for the robot
*
* @param start_pose Starting pose
* @param vel Actual robot velocity (assumed to be within acceleration limits)
* @param dt amount of time in seconds
* @return New pose after dt seconds
*/
virtual geometry_msgs::msg::Pose2D computeNewPosition(
const geometry_msgs::msg::Pose2D start_pose, const nav_2d_msgs::msg::Twist2D & vel,
const double dt);
/**
* @brief Compute an array of time deltas between the points in the generated trajectory.
*
* @param cmd_vel The desired command velocity
* @return vector of the difference between each time step in the generated trajectory
*
* If we are discretizing by time, the returned vector will be the same constant time_granularity
* for all cmd_vels. Otherwise, you will get times based on the linear/angular granularity.
*
* Right now the vector contains a single value repeated many times, but this method could be overridden
* to allow for dynamic spacing
*/
virtual std::vector<double> getTimeSteps(const nav_2d_msgs::msg::Twist2D & cmd_vel);
KinematicsHandler::Ptr kinematics_handler_;
std::shared_ptr<VelocityIterator> velocity_iterator_;
double sim_time_;
// Sampling Parameters
bool discretize_by_time_;
/// @brief If discretizing by time, the amount of time between each point in the traj
double time_granularity_;
/// @brief If not discretizing by time, the amount of linear space between points
double linear_granularity_;
/// @brief If not discretizing by time, the amount of angular space between points
double angular_granularity_;
/// @brief the name of the overlying plugin ID
std::string plugin_name_;
/// @brief Option to limit velocity in the trajectory generator by using current velocity
bool limit_vel_cmd_in_traj_;
/* Backwards Compatibility Parameter: include_last_point
*
* dwa had an off-by-one error built into it.
* It generated N trajectory points, where N = ceil(sim_time / time_delta).
* If for example, sim_time=3.0 and time_delta=1.5, it would generate trajectories with 2 points, which
* indeed were time_delta seconds apart. However, the points would be at t=0 and t=1.5, and thus the
* actual sim_time was much less than advertised.
*
* This is remedied by adding one final point at t=sim_time, but only if include_last_point_ is true.
*
* Nothing I could find actually used the time_delta variable or seemed to care that the trajectories
* were not projected out as far as they intended.
*/
bool include_last_point_;
};
} // namespace dwb_plugins
#endif // DWB_PLUGINS__STANDARD_TRAJ_GENERATOR_HPP_
navigation2/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/velocity_iterator.hpp
+62
View File
@@ -0,0 +1,62 @@
/*
* 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_PLUGINS__VELOCITY_ITERATOR_HPP_
#define DWB_PLUGINS__VELOCITY_ITERATOR_HPP_
#include <memory>
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "nav_2d_msgs/msg/twist2_d.hpp"
#include "dwb_plugins/kinematic_parameters.hpp"
#include "nav2_util/lifecycle_node.hpp"
namespace dwb_plugins
{
class VelocityIterator
{
public:
virtual ~VelocityIterator() {}
virtual void initialize(
const nav2_util::LifecycleNode::SharedPtr & nh,
KinematicsHandler::Ptr kinematics,
const std::string & plugin_name) = 0;
virtual void startNewIteration(const nav_2d_msgs::msg::Twist2D & current_velocity, double dt) = 0;
virtual bool hasMoreTwists() = 0;
virtual nav_2d_msgs::msg::Twist2D nextTwist() = 0;
};
} // namespace dwb_plugins
#endif // DWB_PLUGINS__VELOCITY_ITERATOR_HPP_
navigation2/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/xy_theta_iterator.hpp
+82
View File
@@ -0,0 +1,82 @@
/*
* 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_PLUGINS__XY_THETA_ITERATOR_HPP_
#define DWB_PLUGINS__XY_THETA_ITERATOR_HPP_
#include <memory>
#include <string>
#include "dwb_plugins/velocity_iterator.hpp"
#include "dwb_plugins/one_d_velocity_iterator.hpp"
#include "nav2_util/lifecycle_node.hpp"
namespace dwb_plugins
{
class XYThetaIterator : public VelocityIterator
{
public:
XYThetaIterator()
: kinematics_handler_(nullptr), x_it_(nullptr), y_it_(nullptr), th_it_(nullptr) {}
void initialize(
const nav2_util::LifecycleNode::SharedPtr & nh,
KinematicsHandler::Ptr kinematics,
const std::string & plugin_name) override;
void startNewIteration(const nav_2d_msgs::msg::Twist2D & current_velocity, double dt) override;
bool hasMoreTwists() override;
nav_2d_msgs::msg::Twist2D nextTwist() override;
protected:
/**
* @brief Check to see whether the combined x/y/theta velocities are valid
* @return True if the magnitude hypot(x,y) and theta are within the robot's absolute limits
*
* This is based on three parameters: min_speed_xy, max_speed_xy and min_speed_theta.
* The speed is valid if
* 1) The combined magnitude hypot(x,y) is less than max_speed_xy (or max_speed_xy is negative)
* AND
* 2) min_speed_xy is negative or min_speed_theta is negative or
* hypot(x,y) is greater than min_speed_xy or fabs(theta) is greater than min_speed_theta.
*/
bool isValidSpeed(double x, double y, double theta);
virtual bool isValidVelocity();
void iterateToValidVelocity();
int vx_samples_, vy_samples_, vtheta_samples_;
KinematicsHandler::Ptr kinematics_handler_;
std::shared_ptr<OneDVelocityIterator> x_it_, y_it_, th_it_;
};
} // namespace dwb_plugins
#endif // DWB_PLUGINS__XY_THETA_ITERATOR_HPP_
navigation2/nav2_dwb_controller/dwb_plugins/package.xml
+30
View File
@@ -0,0 +1,30 @@
<?xml version="1.0"?>
<package format="2">
<name>dwb_plugins</name>
<version>1.1.18</version>
<description>
Standard implementations of the GoalChecker
and TrajectoryGenerators for dwb_core
</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>dwb_core</depend>
<depend>nav_2d_msgs</depend>
<depend>nav_2d_utils</depend>
<depend>pluginlib</depend>
<depend>rclcpp</depend>
<depend>nav2_util</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>
navigation2/nav2_dwb_controller/dwb_plugins/plugins.xml
+20
View File
@@ -0,0 +1,20 @@
<class_libraries>
<library path="simple_goal_checker">
<class type="dwb_plugins::SimpleGoalChecker" base_class_type="nav2_core::GoalChecker">
<description></description>
</class>
</library>
<library path="standard_traj_generator">
<class type="dwb_plugins::StandardTrajectoryGenerator" base_class_type="dwb_core::TrajectoryGenerator">
<description></description>
</class>
<class type="dwb_plugins::LimitedAccelGenerator" base_class_type="dwb_core::TrajectoryGenerator">
<description></description>
</class>
</library>
<library path="stopped_goal_checker">
<class type="dwb_plugins::StoppedGoalChecker" base_class_type="nav2_core::GoalChecker">
<description></description>
</class>
</library>
</class_libraries>
navigation2/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp
+226
View File
@@ -0,0 +1,226 @@
/*
* 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_plugins/kinematic_parameters.hpp"
#include <memory>
#include <string>
#include <vector>
#include "nav_2d_utils/parameters.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
using nav2_util::declare_parameter_if_not_declared;
using rcl_interfaces::msg::ParameterType;
using std::placeholders::_1;
namespace dwb_plugins
{
KinematicsHandler::KinematicsHandler()
{
kinematics_.store(new KinematicParameters);
}
KinematicsHandler::~KinematicsHandler()
{
delete kinematics_.load();
}
void KinematicsHandler::initialize(
const nav2_util::LifecycleNode::SharedPtr & nh,
const std::string & plugin_name)
{
plugin_name_ = plugin_name;
declare_parameter_if_not_declared(nh, plugin_name + ".min_vel_x", rclcpp::ParameterValue(0.0));
declare_parameter_if_not_declared(nh, plugin_name + ".min_vel_y", rclcpp::ParameterValue(0.0));
declare_parameter_if_not_declared(nh, plugin_name + ".max_vel_x", rclcpp::ParameterValue(0.0));
declare_parameter_if_not_declared(nh, plugin_name + ".max_vel_y", rclcpp::ParameterValue(0.0));
declare_parameter_if_not_declared(
nh, plugin_name + ".max_vel_theta",
rclcpp::ParameterValue(0.0));
declare_parameter_if_not_declared(
nh, plugin_name + ".min_speed_xy",
rclcpp::ParameterValue(0.0));
declare_parameter_if_not_declared(
nh, plugin_name + ".max_speed_xy",
rclcpp::ParameterValue(0.0));
declare_parameter_if_not_declared(
nh, plugin_name + ".min_speed_theta",
rclcpp::ParameterValue(0.0));
declare_parameter_if_not_declared(nh, plugin_name + ".acc_lim_x", rclcpp::ParameterValue(0.0));
declare_parameter_if_not_declared(nh, plugin_name + ".acc_lim_y", rclcpp::ParameterValue(0.0));
declare_parameter_if_not_declared(
nh, plugin_name + ".acc_lim_theta",
rclcpp::ParameterValue(0.0));
declare_parameter_if_not_declared(nh, plugin_name + ".decel_lim_x", rclcpp::ParameterValue(0.0));
declare_parameter_if_not_declared(nh, plugin_name + ".decel_lim_y", rclcpp::ParameterValue(0.0));
declare_parameter_if_not_declared(
nh, plugin_name + ".decel_lim_theta",
rclcpp::ParameterValue(0.0));
KinematicParameters kinematics;
nh->get_parameter(plugin_name + ".min_vel_x", kinematics.min_vel_x_);
nh->get_parameter(plugin_name + ".min_vel_y", kinematics.min_vel_y_);
nh->get_parameter(plugin_name + ".max_vel_x", kinematics.max_vel_x_);
nh->get_parameter(plugin_name + ".max_vel_y", kinematics.max_vel_y_);
nh->get_parameter(plugin_name + ".max_vel_theta", kinematics.max_vel_theta_);
nh->get_parameter(plugin_name + ".min_speed_xy", kinematics.min_speed_xy_);
nh->get_parameter(plugin_name + ".max_speed_xy", kinematics.max_speed_xy_);
nh->get_parameter(plugin_name + ".min_speed_theta", kinematics.min_speed_theta_);
nh->get_parameter(plugin_name + ".acc_lim_x", kinematics.acc_lim_x_);
nh->get_parameter(plugin_name + ".acc_lim_y", kinematics.acc_lim_y_);
nh->get_parameter(plugin_name + ".acc_lim_theta", kinematics.acc_lim_theta_);
nh->get_parameter(plugin_name + ".decel_lim_x", kinematics.decel_lim_x_);
nh->get_parameter(plugin_name + ".decel_lim_y", kinematics.decel_lim_y_);
nh->get_parameter(plugin_name + ".decel_lim_theta", kinematics.decel_lim_theta_);
kinematics.base_max_vel_x_ = kinematics.max_vel_x_;
kinematics.base_max_vel_y_ = kinematics.max_vel_y_;
kinematics.base_max_speed_xy_ = kinematics.max_speed_xy_;
kinematics.base_max_vel_theta_ = kinematics.max_vel_theta_;
// Add callback for dynamic parameters
dyn_params_handler_ = nh->add_on_set_parameters_callback(
std::bind(&KinematicsHandler::dynamicParametersCallback, this, _1));
kinematics.min_speed_xy_sq_ = kinematics.min_speed_xy_ * kinematics.min_speed_xy_;
kinematics.max_speed_xy_sq_ = kinematics.max_speed_xy_ * kinematics.max_speed_xy_;
update_kinematics(kinematics);
}
void KinematicsHandler::setSpeedLimit(
const double & speed_limit, const bool & percentage)
{
KinematicParameters kinematics(*kinematics_.load());
if (speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT) {
// Restore default value
kinematics.max_speed_xy_ = kinematics.base_max_speed_xy_;
kinematics.max_vel_x_ = kinematics.base_max_vel_x_;
kinematics.max_vel_y_ = kinematics.base_max_vel_y_;
kinematics.max_vel_theta_ = kinematics.base_max_vel_theta_;
} else {
if (percentage) {
// Speed limit is expressed in % from maximum speed of robot
kinematics.max_speed_xy_ = kinematics.base_max_speed_xy_ * speed_limit / 100.0;
kinematics.max_vel_x_ = kinematics.base_max_vel_x_ * speed_limit / 100.0;
kinematics.max_vel_y_ = kinematics.base_max_vel_y_ * speed_limit / 100.0;
kinematics.max_vel_theta_ = kinematics.base_max_vel_theta_ * speed_limit / 100.0;
} else {
// Speed limit is expressed in absolute value
if (speed_limit < kinematics.base_max_speed_xy_) {
kinematics.max_speed_xy_ = speed_limit;
// Handling components and angular velocity changes:
// Max velocities are being changed in the same proportion
// as absolute linear speed changed in order to preserve
// robot moving trajectories to be the same after speed change.
const double ratio = speed_limit / kinematics.base_max_speed_xy_;
kinematics.max_vel_x_ = kinematics.base_max_vel_x_ * ratio;
kinematics.max_vel_y_ = kinematics.base_max_vel_y_ * ratio;
kinematics.max_vel_theta_ = kinematics.base_max_vel_theta_ * ratio;
}
}
}
// Do not forget to update max_speed_xy_sq_ as well
kinematics.max_speed_xy_sq_ = kinematics.max_speed_xy_ * kinematics.max_speed_xy_;
update_kinematics(kinematics);
}
rcl_interfaces::msg::SetParametersResult
KinematicsHandler::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
{
rcl_interfaces::msg::SetParametersResult result;
KinematicParameters kinematics(*kinematics_.load());
for (auto parameter : parameters) {
const auto & type = parameter.get_type();
const auto & name = parameter.get_name();
if (type == ParameterType::PARAMETER_DOUBLE) {
if (name == plugin_name_ + ".min_vel_x") {
kinematics.min_vel_x_ = parameter.as_double();
} else if (name == plugin_name_ + ".min_vel_y") {
kinematics.min_vel_y_ = parameter.as_double();
} else if (name == plugin_name_ + ".max_vel_x") {
kinematics.max_vel_x_ = parameter.as_double();
kinematics.base_max_vel_x_ = kinematics.max_vel_x_;
} else if (name == plugin_name_ + ".max_vel_y") {
kinematics.max_vel_y_ = parameter.as_double();
kinematics.base_max_vel_y_ = kinematics.max_vel_y_;
} else if (name == plugin_name_ + ".max_vel_theta") {
kinematics.max_vel_theta_ = parameter.as_double();
kinematics.base_max_vel_theta_ = kinematics.max_vel_theta_;
} else if (name == plugin_name_ + ".min_speed_xy") {
kinematics.min_speed_xy_ = parameter.as_double();
kinematics.min_speed_xy_sq_ = kinematics.min_speed_xy_ * kinematics.min_speed_xy_;
} else if (name == plugin_name_ + ".max_speed_xy") {
kinematics.max_speed_xy_ = parameter.as_double();
kinematics.base_max_speed_xy_ = kinematics.max_speed_xy_;
} else if (name == plugin_name_ + ".min_speed_theta") {
kinematics.min_speed_theta_ = parameter.as_double();
kinematics.max_speed_xy_sq_ = kinematics.max_speed_xy_ * kinematics.max_speed_xy_;
} else if (name == plugin_name_ + ".acc_lim_x") {
kinematics.acc_lim_x_ = parameter.as_double();
} else if (name == plugin_name_ + ".acc_lim_y") {
kinematics.acc_lim_y_ = parameter.as_double();
} else if (name == plugin_name_ + ".acc_lim_theta") {
kinematics.acc_lim_theta_ = parameter.as_double();
} else if (name == plugin_name_ + ".decel_lim_x") {
kinematics.decel_lim_x_ = parameter.as_double();
} else if (name == plugin_name_ + ".decel_lim_y") {
kinematics.decel_lim_y_ = parameter.as_double();
} else if (name == plugin_name_ + ".decel_lim_theta") {
kinematics.decel_lim_theta_ = parameter.as_double();
}
}
}
update_kinematics(kinematics);
result.successful = true;
return result;
}
void KinematicsHandler::update_kinematics(KinematicParameters kinematics)
{
delete kinematics_.load();
kinematics_.store(new KinematicParameters(kinematics));
}
} // namespace dwb_plugins
navigation2/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp
+98
View File
@@ -0,0 +1,98 @@
/*
* 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_plugins/limited_accel_generator.hpp"
#include <vector>
#include <memory>
#include <string>
#include "nav_2d_utils/parameters.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "dwb_core/exceptions.hpp"
#include "nav2_util/node_utils.hpp"
namespace dwb_plugins
{
void LimitedAccelGenerator::initialize(
const nav2_util::LifecycleNode::SharedPtr & nh,
const std::string & plugin_name)
{
plugin_name_ = plugin_name;
StandardTrajectoryGenerator::initialize(nh, plugin_name_);
try {
nav2_util::declare_parameter_if_not_declared(
nh, plugin_name + ".sim_period", rclcpp::PARAMETER_DOUBLE);
if (!nh->get_parameter(plugin_name + ".sim_period", acceleration_time_)) {
// This actually should never appear, since declare_parameter_if_not_declared()
// completed w/o exceptions guarantee that static parameter will be initialized
// with some value. However for reliability we should also process the case
// when get_parameter() will return a failure for some other reasons.
throw std::runtime_error("Failed to get 'sim_period' value");
}
} catch (std::exception &) {
RCLCPP_WARN(
rclcpp::get_logger("LimitedAccelGenerator"),
"'sim_period' parameter is not set for %s", plugin_name.c_str());
double controller_frequency = nav_2d_utils::searchAndGetParam(
nh, "controller_frequency", 20.0);
if (controller_frequency > 0) {
acceleration_time_ = 1.0 / controller_frequency;
} else {
RCLCPP_WARN(
rclcpp::get_logger("LimitedAccelGenerator"),
"A controller_frequency less than or equal to 0 has been set. "
"Ignoring the parameter, assuming a rate of 20Hz");
acceleration_time_ = 0.05;
}
}
}
void LimitedAccelGenerator::startNewIteration(const nav_2d_msgs::msg::Twist2D & current_velocity)
{
// Limit our search space to just those within the limited acceleration_time
velocity_iterator_->startNewIteration(current_velocity, acceleration_time_);
}
nav_2d_msgs::msg::Twist2D LimitedAccelGenerator::computeNewVelocity(
const nav_2d_msgs::msg::Twist2D & cmd_vel,
const nav_2d_msgs::msg::Twist2D & /*start_vel*/,
const double /*dt*/)
{
return cmd_vel;
}
} // namespace dwb_plugins
PLUGINLIB_EXPORT_CLASS(dwb_plugins::LimitedAccelGenerator, dwb_core::TrajectoryGenerator)
navigation2/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp
+228
View File
@@ -0,0 +1,228 @@
/*
* 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_plugins/standard_traj_generator.hpp"
#include <string>
#include <vector>
#include <algorithm>
#include <memory>
#include "dwb_plugins/xy_theta_iterator.hpp"
#include "nav_2d_utils/parameters.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "dwb_core/exceptions.hpp"
#include "nav2_util/node_utils.hpp"
namespace dwb_plugins
{
void StandardTrajectoryGenerator::initialize(
const nav2_util::LifecycleNode::SharedPtr & nh,
const std::string & plugin_name)
{
plugin_name_ = plugin_name;
kinematics_handler_ = std::make_shared<KinematicsHandler>();
kinematics_handler_->initialize(nh, plugin_name_);
initializeIterator(nh);
nav2_util::declare_parameter_if_not_declared(
nh,
plugin_name + ".sim_time", rclcpp::ParameterValue(1.7));
nav2_util::declare_parameter_if_not_declared(
nh,
plugin_name + ".discretize_by_time", rclcpp::ParameterValue(false));
nav2_util::declare_parameter_if_not_declared(
nh,
plugin_name + ".time_granularity", rclcpp::ParameterValue(0.5));
nav2_util::declare_parameter_if_not_declared(
nh,
plugin_name + ".linear_granularity", rclcpp::ParameterValue(0.5));
nav2_util::declare_parameter_if_not_declared(
nh,
plugin_name + ".angular_granularity", rclcpp::ParameterValue(0.025));
nav2_util::declare_parameter_if_not_declared(
nh,
plugin_name + ".include_last_point", rclcpp::ParameterValue(true));
nav2_util::declare_parameter_if_not_declared(
nh,
plugin_name + ".limit_vel_cmd_in_traj", rclcpp::ParameterValue(false));
/*
* If discretize_by_time, then sim_granularity represents the amount of time that should be between
* two successive points on the trajectory.
*
* If discretize_by_time is false, then sim_granularity is the maximum amount of distance between
* two successive points on the trajectory, and angular_sim_granularity is the maximum amount of
* angular distance between two successive points.
*/
nh->get_parameter(plugin_name + ".sim_time", sim_time_);
nh->get_parameter(plugin_name + ".discretize_by_time", discretize_by_time_);
nh->get_parameter(plugin_name + ".time_granularity", time_granularity_);
nh->get_parameter(plugin_name + ".linear_granularity", linear_granularity_);
nh->get_parameter(plugin_name + ".angular_granularity", angular_granularity_);
nh->get_parameter(plugin_name + ".include_last_point", include_last_point_);
nh->get_parameter(plugin_name + ".limit_vel_cmd_in_traj", limit_vel_cmd_in_traj_);
}
void StandardTrajectoryGenerator::initializeIterator(
const nav2_util::LifecycleNode::SharedPtr & nh)
{
velocity_iterator_ = std::make_shared<XYThetaIterator>();
velocity_iterator_->initialize(nh, kinematics_handler_, plugin_name_);
}
void StandardTrajectoryGenerator::startNewIteration(
const nav_2d_msgs::msg::Twist2D & current_velocity)
{
velocity_iterator_->startNewIteration(current_velocity, sim_time_);
}
bool StandardTrajectoryGenerator::hasMoreTwists()
{
return velocity_iterator_->hasMoreTwists();
}
nav_2d_msgs::msg::Twist2D StandardTrajectoryGenerator::nextTwist()
{
return velocity_iterator_->nextTwist();
}
std::vector<double> StandardTrajectoryGenerator::getTimeSteps(
const nav_2d_msgs::msg::Twist2D & cmd_vel)
{
std::vector<double> steps;
if (discretize_by_time_) {
steps.resize(ceil(sim_time_ / time_granularity_));
} else { // discretize by distance
double vmag = hypot(cmd_vel.x, cmd_vel.y);
// the distance the robot would travel in sim_time if it did not change velocity
double projected_linear_distance = vmag * sim_time_;
// the angle the robot would rotate in sim_time
double projected_angular_distance = fabs(cmd_vel.theta) * sim_time_;
// Pick the maximum of the two
int num_steps = ceil(
std::max(
projected_linear_distance / linear_granularity_,
projected_angular_distance / angular_granularity_));
steps.resize(num_steps);
}
if (steps.size() == 0) {
steps.resize(1);
}
std::fill(steps.begin(), steps.end(), sim_time_ / steps.size());
return steps;
}
dwb_msgs::msg::Trajectory2D StandardTrajectoryGenerator::generateTrajectory(
const geometry_msgs::msg::Pose2D & start_pose,
const nav_2d_msgs::msg::Twist2D & start_vel,
const nav_2d_msgs::msg::Twist2D & cmd_vel)
{
dwb_msgs::msg::Trajectory2D traj;
traj.velocity = cmd_vel;
// simulate the trajectory
geometry_msgs::msg::Pose2D pose = start_pose;
nav_2d_msgs::msg::Twist2D vel = start_vel;
double running_time = 0.0;
std::vector<double> steps = getTimeSteps(cmd_vel);
traj.poses.push_back(start_pose);
bool first_vel = false;
for (double dt : steps) {
// calculate velocities
vel = computeNewVelocity(cmd_vel, vel, dt);
if (!first_vel && limit_vel_cmd_in_traj_) {
traj.velocity = vel;
first_vel = true;
}
// update the position of the robot using the velocities passed in
pose = computeNewPosition(pose, vel, dt);
traj.poses.push_back(pose);
traj.time_offsets.push_back(rclcpp::Duration::from_seconds(running_time));
running_time += dt;
} // end for simulation steps
if (include_last_point_) {
traj.poses.push_back(pose);
traj.time_offsets.push_back(rclcpp::Duration::from_seconds(running_time));
}
return traj;
}
/**
* change vel using acceleration limits to converge towards sample_target-vel
*/
nav_2d_msgs::msg::Twist2D StandardTrajectoryGenerator::computeNewVelocity(
const nav_2d_msgs::msg::Twist2D & cmd_vel,
const nav_2d_msgs::msg::Twist2D & start_vel, const double dt)
{
KinematicParameters kinematics = kinematics_handler_->getKinematics();
nav_2d_msgs::msg::Twist2D new_vel;
new_vel.x = projectVelocity(
start_vel.x, kinematics.getAccX(),
kinematics.getDecelX(), dt, cmd_vel.x);
new_vel.y = projectVelocity(
start_vel.y, kinematics.getAccY(),
kinematics.getDecelY(), dt, cmd_vel.y);
new_vel.theta = projectVelocity(
start_vel.theta,
kinematics.getAccTheta(), kinematics.getDecelTheta(),
dt, cmd_vel.theta);
return new_vel;
}
geometry_msgs::msg::Pose2D StandardTrajectoryGenerator::computeNewPosition(
const geometry_msgs::msg::Pose2D start_pose,
const nav_2d_msgs::msg::Twist2D & vel, const double dt)
{
geometry_msgs::msg::Pose2D new_pose;
new_pose.x = start_pose.x +
(vel.x * cos(start_pose.theta) + vel.y * cos(M_PI_2 + start_pose.theta)) * dt;
new_pose.y = start_pose.y +
(vel.x * sin(start_pose.theta) + vel.y * sin(M_PI_2 + start_pose.theta)) * dt;
new_pose.theta = start_pose.theta + vel.theta * dt;
return new_pose;
}
} // namespace dwb_plugins
PLUGINLIB_EXPORT_CLASS(
dwb_plugins::StandardTrajectoryGenerator,
dwb_core::TrajectoryGenerator)
navigation2/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp
+154
View File
@@ -0,0 +1,154 @@
/*
* 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_plugins/xy_theta_iterator.hpp"
#include <cmath>
#include <memory>
#include <string>
#include "nav_2d_utils/parameters.hpp"
#include "nav2_util/node_utils.hpp"
#define EPSILON 1E-5
namespace dwb_plugins
{
void XYThetaIterator::initialize(
const nav2_util::LifecycleNode::SharedPtr & nh,
KinematicsHandler::Ptr kinematics,
const std::string & plugin_name)
{
kinematics_handler_ = kinematics;
nav2_util::declare_parameter_if_not_declared(
nh,
plugin_name + ".vx_samples", rclcpp::ParameterValue(20));
nav2_util::declare_parameter_if_not_declared(
nh,
plugin_name + ".vy_samples", rclcpp::ParameterValue(5));
nav2_util::declare_parameter_if_not_declared(
nh,
plugin_name + ".vtheta_samples", rclcpp::ParameterValue(20));
nh->get_parameter(plugin_name + ".vx_samples", vx_samples_);
nh->get_parameter(plugin_name + ".vy_samples", vy_samples_);
nh->get_parameter(plugin_name + ".vtheta_samples", vtheta_samples_);
}
void XYThetaIterator::startNewIteration(
const nav_2d_msgs::msg::Twist2D & current_velocity,
double dt)
{
KinematicParameters kinematics = kinematics_handler_->getKinematics();
x_it_ = std::make_shared<OneDVelocityIterator>(
current_velocity.x,
kinematics.getMinX(), kinematics.getMaxX(),
kinematics.getAccX(), kinematics.getDecelX(),
dt, vx_samples_);
y_it_ = std::make_shared<OneDVelocityIterator>(
current_velocity.y,
kinematics.getMinY(), kinematics.getMaxY(),
kinematics.getAccY(), kinematics.getDecelY(),
dt, vy_samples_);
th_it_ = std::make_shared<OneDVelocityIterator>(
current_velocity.theta,
kinematics.getMinTheta(), kinematics.getMaxTheta(),
kinematics.getAccTheta(), kinematics.getDecelTheta(),
dt, vtheta_samples_);
if (!isValidVelocity()) {
iterateToValidVelocity();
}
}
bool XYThetaIterator::isValidSpeed(double x, double y, double theta)
{
KinematicParameters kinematics = kinematics_handler_->getKinematics();
double vmag_sq = x * x + y * y;
if (kinematics.getMaxSpeedXY() >= 0.0 && vmag_sq > kinematics.getMaxSpeedXY_SQ() + EPSILON) {
return false;
}
if (kinematics.getMinSpeedXY() >= 0.0 && vmag_sq + EPSILON < kinematics.getMinSpeedXY_SQ() &&
kinematics.getMinSpeedTheta() >= 0.0 && fabs(theta) + EPSILON < kinematics.getMinSpeedTheta())
{
return false;
}
if (vmag_sq == 0.0 && th_it_->getVelocity() == 0.0) {
return false;
}
return true;
}
bool XYThetaIterator::isValidVelocity()
{
return isValidSpeed(
x_it_->getVelocity(), y_it_->getVelocity(),
th_it_->getVelocity());
}
bool XYThetaIterator::hasMoreTwists()
{
return x_it_ && !x_it_->isFinished();
}
nav_2d_msgs::msg::Twist2D XYThetaIterator::nextTwist()
{
nav_2d_msgs::msg::Twist2D velocity;
velocity.x = x_it_->getVelocity();
velocity.y = y_it_->getVelocity();
velocity.theta = th_it_->getVelocity();
iterateToValidVelocity();
return velocity;
}
void XYThetaIterator::iterateToValidVelocity()
{
bool valid = false;
while (!valid && hasMoreTwists()) {
++(*th_it_);
if (th_it_->isFinished()) {
th_it_->reset();
++(*y_it_);
if (y_it_->isFinished()) {
y_it_->reset();
++(*x_it_);
}
}
valid = isValidVelocity();
}
}
} // namespace dwb_plugins
navigation2/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt
+10
View File
@@ -0,0 +1,10 @@
ament_add_gtest(vtest velocity_iterator_test.cpp)
ament_add_gtest(twist_gen_test twist_gen.cpp)
target_link_libraries(twist_gen_test standard_traj_generator)
ament_add_gtest(kinematic_parameters_test kinematic_parameters_test.cpp)
target_link_libraries(kinematic_parameters_test standard_traj_generator)
ament_add_gtest(speed_limit_test speed_limit_test.cpp)
target_link_libraries(speed_limit_test standard_traj_generator)
navigation2/nav2_dwb_controller/dwb_plugins/test/kinematic_parameters_test.cpp
+129
View File
@@ -0,0 +1,129 @@
/*
* 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 <string>
#include <memory>
#include <vector>
#include "gtest/gtest.h"
#include "dwb_plugins/kinematic_parameters.hpp"
using rcl_interfaces::msg::Parameter;
using rcl_interfaces::msg::ParameterType;
using rcl_interfaces::msg::ParameterEvent;
class KinematicsHandlerTest : public dwb_plugins::KinematicsHandler
{
public:
void simulate_event(
std::vector<rclcpp::Parameter> parameters)
{
dynamicParametersCallback(parameters);
}
};
TEST(KinematicParameters, SetAllParameters) {
std::string nodeName = "test_node";
auto node = nav2_util::LifecycleNode::make_shared(nodeName);
KinematicsHandlerTest kh;
kh.initialize(node, nodeName);
std::vector<rclcpp::Parameter> parameters;
rclcpp::Parameter
p_minX(nodeName + ".min_vel_x", 12.34),
p_maxX(nodeName + ".max_vel_x", 23.45),
p_minY(nodeName + ".min_vel_y", 34.56),
p_maxY(nodeName + ".max_vel_y", 45.67),
p_accX(nodeName + ".acc_lim_x", 56.78),
p_decelX(nodeName + ".acc_lim_y", 67.89),
p_accY(nodeName + ".decel_lim_x", 78.90),
p_decelY(nodeName + ".decel_lim_y", 89.01),
p_minSpeedXY(nodeName + ".min_speed_xy", 90.12),
p_maxSpeedXY(nodeName + ".max_speed_xy", 123.456),
p_maxTheta(nodeName + ".max_vel_theta", 345.678),
p_accTheta(nodeName + ".acc_lim_theta", 234.567),
p_decelTheta(nodeName + ".decel_lim_theta", 456.789),
p_minSpeedTheta(nodeName + ".min_speed_theta", 567.890);
parameters.push_back(p_minX);
parameters.push_back(p_minX);
parameters.push_back(p_maxX);
parameters.push_back(p_minY);
parameters.push_back(p_maxY);
parameters.push_back(p_accX);
parameters.push_back(p_accY);
parameters.push_back(p_decelX);
parameters.push_back(p_decelY);
parameters.push_back(p_minSpeedXY);
parameters.push_back(p_maxSpeedXY);
parameters.push_back(p_maxTheta);
parameters.push_back(p_accTheta);
parameters.push_back(p_decelTheta);
parameters.push_back(p_minSpeedTheta);
kh.simulate_event(parameters);
dwb_plugins::KinematicParameters kp = kh.getKinematics();
EXPECT_EQ(kp.getMinX(), 12.34);
EXPECT_EQ(kp.getMaxX(), 23.45);
EXPECT_EQ(kp.getMinY(), 34.56);
EXPECT_EQ(kp.getMaxY(), 45.67);
EXPECT_EQ(kp.getAccX(), 56.78);
EXPECT_EQ(kp.getAccY(), 67.89);
EXPECT_EQ(kp.getDecelX(), 78.90);
EXPECT_EQ(kp.getDecelY(), 89.01);
EXPECT_EQ(kp.getMinSpeedXY(), 90.12);
EXPECT_EQ(kp.getMaxSpeedXY(), 123.456);
EXPECT_EQ(kp.getAccTheta(), 234.567);
EXPECT_EQ(kp.getMaxTheta(), 345.678);
EXPECT_EQ(kp.getDecelTheta(), 456.789);
EXPECT_EQ(kp.getMinSpeedTheta(), 567.890);
}
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;
}
navigation2/nav2_dwb_controller/dwb_plugins/test/speed_limit_test.cpp
+171
View File
@@ -0,0 +1,171 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2020, Samsung Research Russia
* 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.
*
* Author: Alexey Merzlyakov
*/
#include <gtest/gtest.h>
#include <string>
#include <memory>
#include <chrono>
#include "rclcpp/rclcpp.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
#include "dwb_plugins/kinematic_parameters.hpp"
using namespace std::chrono_literals;
static constexpr double EPSILON = 1e-5;
static const char NODE_NAME[] = "test_node";
static const double MAX_VEL_X = 40.0;
static const double MAX_VEL_Y = 30.0;
static const double MAX_VEL_THETA = 15.0;
static const double MAX_VEL_LINEAR = 50.0;
class TestNode : public ::testing::Test
{
public:
TestNode()
{
const std::string node_name = NODE_NAME;
node_ = nav2_util::LifecycleNode::make_shared(node_name);
node_->declare_parameter(
node_name + ".max_vel_x", rclcpp::ParameterValue(MAX_VEL_X));
node_->set_parameter(
rclcpp::Parameter(node_name + ".max_vel_x", MAX_VEL_X));
node_->declare_parameter(
node_name + ".max_vel_y", rclcpp::ParameterValue(MAX_VEL_Y));
node_->set_parameter(
rclcpp::Parameter(node_name + ".max_vel_y", MAX_VEL_Y));
node_->declare_parameter(
node_name + ".max_vel_theta", rclcpp::ParameterValue(MAX_VEL_THETA));
node_->set_parameter(
rclcpp::Parameter(node_name + ".max_vel_theta", MAX_VEL_THETA));
node_->declare_parameter(
node_name + ".max_speed_xy", rclcpp::ParameterValue(MAX_VEL_LINEAR));
node_->set_parameter(
rclcpp::Parameter(node_name + ".max_speed_xy", MAX_VEL_LINEAR));
}
~TestNode() {}
protected:
nav2_util::LifecycleNode::SharedPtr node_;
};
TEST_F(TestNode, TestPercentLimit)
{
dwb_plugins::KinematicsHandler kh;
kh.initialize(node_, NODE_NAME);
dwb_plugins::KinematicParameters kp = kh.getKinematics();
EXPECT_NEAR(kp.getMaxX(), MAX_VEL_X, EPSILON);
EXPECT_NEAR(kp.getMaxY(), MAX_VEL_Y, EPSILON);
EXPECT_NEAR(kp.getMaxTheta(), MAX_VEL_THETA, EPSILON);
EXPECT_NEAR(kp.getMaxSpeedXY(), MAX_VEL_LINEAR, EPSILON);
// Set speed limit 30% from maximum robot speed
kh.setSpeedLimit(30, true);
// Update KinematicParameters values from KinematicsHandler
kp = kh.getKinematics();
EXPECT_NEAR(kp.getMaxX(), MAX_VEL_X * 0.3, EPSILON);
EXPECT_NEAR(kp.getMaxY(), MAX_VEL_Y * 0.3, EPSILON);
EXPECT_NEAR(kp.getMaxTheta(), MAX_VEL_THETA * 0.3, EPSILON);
EXPECT_NEAR(kp.getMaxSpeedXY(), MAX_VEL_LINEAR * 0.3, EPSILON);
// Restore maximum speed to its default
kh.setSpeedLimit(nav2_costmap_2d::NO_SPEED_LIMIT, true);
// Update KinematicParameters values from KinematicsHandler
kp = kh.getKinematics();
EXPECT_NEAR(kp.getMaxX(), MAX_VEL_X, EPSILON);
EXPECT_NEAR(kp.getMaxY(), MAX_VEL_Y, EPSILON);
EXPECT_NEAR(kp.getMaxTheta(), MAX_VEL_THETA, EPSILON);
EXPECT_NEAR(kp.getMaxSpeedXY(), MAX_VEL_LINEAR, EPSILON);
}
TEST_F(TestNode, TestAbsoluteLimit)
{
dwb_plugins::KinematicsHandler kh;
kh.initialize(node_, NODE_NAME);
dwb_plugins::KinematicParameters kp = kh.getKinematics();
EXPECT_NEAR(kp.getMaxX(), MAX_VEL_X, EPSILON);
EXPECT_NEAR(kp.getMaxY(), MAX_VEL_Y, EPSILON);
EXPECT_NEAR(kp.getMaxTheta(), MAX_VEL_THETA, EPSILON);
EXPECT_NEAR(kp.getMaxSpeedXY(), MAX_VEL_LINEAR, EPSILON);
// Set speed limit 35.0 m/s
kh.setSpeedLimit(35.0, false);
// Update KinematicParameters values from KinematicsHandler
kp = kh.getKinematics();
EXPECT_NEAR(kp.getMaxX(), MAX_VEL_X * 35.0 / MAX_VEL_LINEAR, EPSILON);
EXPECT_NEAR(kp.getMaxY(), MAX_VEL_Y * 35.0 / MAX_VEL_LINEAR, EPSILON);
EXPECT_NEAR(kp.getMaxTheta(), MAX_VEL_THETA * 35.0 / MAX_VEL_LINEAR, EPSILON);
EXPECT_NEAR(kp.getMaxSpeedXY(), 35.0, EPSILON);
// Restore maximum speed to its default
kh.setSpeedLimit(nav2_costmap_2d::NO_SPEED_LIMIT, false);
// Update KinematicParameters values from KinematicsHandler
kp = kh.getKinematics();
EXPECT_NEAR(kp.getMaxX(), MAX_VEL_X, EPSILON);
EXPECT_NEAR(kp.getMaxY(), MAX_VEL_Y, EPSILON);
EXPECT_NEAR(kp.getMaxTheta(), MAX_VEL_THETA, EPSILON);
EXPECT_NEAR(kp.getMaxSpeedXY(), MAX_VEL_LINEAR, EPSILON);
}
int main(int argc, char ** argv)
{
// Initialize the system
testing::InitGoogleTest(&argc, argv);
rclcpp::init(argc, argv);
// Actual testing
bool test_result = RUN_ALL_TESTS();
// Shutdown
rclcpp::shutdown();
return test_result;
}
navigation2/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp
+503
View File
@@ -0,0 +1,503 @@
/*
* 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 <cmath>
#include <vector>
#include <algorithm>
#include <string>
#include "gtest/gtest.h"
#include "dwb_plugins/standard_traj_generator.hpp"
#include "dwb_plugins/limited_accel_generator.hpp"
#include "dwb_core/exceptions.hpp"
#include "nav2_util/node_utils.hpp"
using std::hypot;
using std::fabs;
using dwb_plugins::StandardTrajectoryGenerator;
geometry_msgs::msg::Pose2D origin;
nav_2d_msgs::msg::Twist2D zero;
nav_2d_msgs::msg::Twist2D forward;
class LimitedAccelGeneratorTest : public dwb_plugins::LimitedAccelGenerator
{
public:
double getAccelerationTime()
{
return acceleration_time_;
}
};
std::vector<rclcpp::Parameter> getDefaultKinematicParameters()
{
std::vector<rclcpp::Parameter> parameters;
parameters.push_back(rclcpp::Parameter("dwb.min_vel_x", 0.0));
parameters.push_back(rclcpp::Parameter("dwb.max_vel_x", 0.55));
parameters.push_back(rclcpp::Parameter("dwb.min_vel_y", -0.1));
parameters.push_back(rclcpp::Parameter("dwb.max_vel_y", 0.1));
parameters.push_back(rclcpp::Parameter("dwb.max_vel_theta", 1.0));
parameters.push_back(rclcpp::Parameter("dwb.acc_lim_x", 2.5));
parameters.push_back(rclcpp::Parameter("dwb.acc_lim_y", 2.5));
parameters.push_back(rclcpp::Parameter("dwb.acc_lim_theta", 3.2));
parameters.push_back(rclcpp::Parameter("dwb.decel_lim_x", -2.5));
parameters.push_back(rclcpp::Parameter("dwb.decel_lim_y", -2.5));
parameters.push_back(rclcpp::Parameter("dwb.decel_lim_theta", -3.2));
parameters.push_back(rclcpp::Parameter("dwb.min_speed_xy", 0.1));
parameters.push_back(rclcpp::Parameter("dwb.max_speed_xy", 0.55));
parameters.push_back(rclcpp::Parameter("dwb.min_speed_theta", 0.4));
return parameters;
}
rclcpp_lifecycle::LifecycleNode::SharedPtr makeTestNode(
const std::string & name,
const std::vector<rclcpp::Parameter> & overrides = {})
{
rclcpp::NodeOptions node_options;
node_options.parameter_overrides(getDefaultKinematicParameters());
node_options.parameter_overrides().insert(
node_options.parameter_overrides().end(), overrides.begin(), overrides.end());
auto node = rclcpp_lifecycle::LifecycleNode::make_shared(name, node_options);
node->on_configure(node->get_current_state());
node->on_activate(node->get_current_state());
return node;
}
void checkLimits(
const std::vector<nav_2d_msgs::msg::Twist2D> & twists,
double exp_min_x, double exp_max_x, double exp_min_y, double exp_max_y,
double exp_min_theta, double exp_max_theta,
double exp_max_xy = -1.0,
double exp_min_xy = -1.0, double exp_min_speed_theta = -1.0)
{
ASSERT_GT(twists.size(), 0u);
nav_2d_msgs::msg::Twist2D first = twists[0];
double min_x = first.x, max_x = first.x, min_y = first.y, max_y = first.y;
double min_theta = first.theta, max_theta = first.theta;
double max_xy = hypot(first.x, first.y);
for (nav_2d_msgs::msg::Twist2D twist : twists) {
min_x = std::min(min_x, twist.x);
min_y = std::min(min_y, twist.y);
min_theta = std::min(min_theta, twist.theta);
max_x = std::max(max_x, twist.x);
max_y = std::max(max_y, twist.y);
max_theta = std::max(max_theta, twist.theta);
double hyp = hypot(twist.x, twist.y);
max_xy = std::max(max_xy, hyp);
if (exp_min_xy >= 0 && exp_min_speed_theta >= 0) {
EXPECT_TRUE(fabs(twist.theta) >= exp_min_speed_theta || hyp >= exp_min_xy);
}
}
EXPECT_DOUBLE_EQ(min_x, exp_min_x);
EXPECT_DOUBLE_EQ(max_x, exp_max_x);
EXPECT_DOUBLE_EQ(min_y, exp_min_y);
EXPECT_DOUBLE_EQ(max_y, exp_max_y);
EXPECT_DOUBLE_EQ(min_theta, exp_min_theta);
EXPECT_DOUBLE_EQ(max_theta, exp_max_theta);
if (exp_max_xy >= 0) {
EXPECT_DOUBLE_EQ(max_xy, exp_max_xy);
}
}
double durationToSec(builtin_interfaces::msg::Duration d)
{
return d.sec + d.nanosec * 1e-9;
}
TEST(VelocityIterator, standard_gen)
{
auto nh = makeTestNode("st_gen");
StandardTrajectoryGenerator gen;
gen.initialize(nh, "dwb");
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(zero);
EXPECT_EQ(twists.size(), 1926u);
checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0, 0.55, 0.1, 0.4);
}
TEST(VelocityIterator, max_xy)
{
auto nh = makeTestNode("max_xy", {rclcpp::Parameter("dwb.max_speed_xy", 1.0)});
StandardTrajectoryGenerator gen;
gen.initialize(nh, "dwb");
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(zero);
// Expect more twists since max_speed_xy is now beyond feasible limits
EXPECT_EQ(twists.size(), 2010u);
checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0, hypot(0.55, 0.1));
}
TEST(VelocityIterator, min_xy)
{
auto nh = makeTestNode("min_xy", {rclcpp::Parameter("dwb.min_speed_xy", -1.0)});
StandardTrajectoryGenerator gen;
gen.initialize(nh, "dwb");
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(zero);
// Expect even more since theres no min_speed_xy
EXPECT_EQ(twists.size(), 2015u);
checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0);
}
TEST(VelocityIterator, min_theta)
{
auto nh = makeTestNode("min_theta", {rclcpp::Parameter("dwb.min_speed_theta", -1.0)});
StandardTrajectoryGenerator gen;
gen.initialize(nh, "dwb");
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(zero);
// Expect even more since theres no min_speed_xy
EXPECT_EQ(twists.size(), 2015u);
checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0);
}
TEST(VelocityIterator, no_limits)
{
auto nh = makeTestNode(
"no_limits", {
rclcpp::Parameter("dwb.max_speed_xy", -1.0),
rclcpp::Parameter("dwb.min_speed_xy", -1.0),
rclcpp::Parameter("dwb.min_speed_theta", -1.0)});
StandardTrajectoryGenerator gen;
gen.initialize(nh, "dwb");
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(zero);
// vx_samples * vtheta_samples * vy_samples + added zero theta samples - (0,0,0)
EXPECT_EQ(twists.size(), 20u * 20u * 5u + 100u - 1u);
checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0, hypot(0.55, 0.1), 0.0, 0.0);
}
TEST(VelocityIterator, no_limits_samples)
{
const int x_samples = 10, y_samples = 3, theta_samples = 5;
auto nh = makeTestNode(
"no_limits_samples", {
rclcpp::Parameter("dwb.max_speed_xy", -1.0),
rclcpp::Parameter("dwb.min_speed_xy", -1.0),
rclcpp::Parameter("dwb.min_speed_theta", -1.0),
rclcpp::Parameter("dwb.vx_samples", x_samples),
rclcpp::Parameter("dwb.vy_samples", y_samples),
rclcpp::Parameter("dwb.vtheta_samples", theta_samples)});
StandardTrajectoryGenerator gen;
gen.initialize(nh, "dwb");
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(zero);
EXPECT_EQ(twists.size(), static_cast<unsigned>(x_samples * y_samples * theta_samples - 1));
checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0, hypot(0.55, 0.1), 0.0, 0.0);
}
TEST(VelocityIterator, dwa_gen)
{
auto nh = makeTestNode("dwa_gen", {rclcpp::Parameter("dwb.min_speed_theta", -1.0)});
dwb_plugins::LimitedAccelGenerator gen;
gen.initialize(nh, "dwb");
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(zero);
// Same as no-limits since everything is within our velocity limits
EXPECT_EQ(twists.size(), 20u * 20u * 5u + 100u - 1u);
checkLimits(twists, 0.0, 0.125, -0.1, 0.1, -0.16, 0.16, hypot(0.125, 0.1), 0.0, 0.1);
}
TEST(VelocityIterator, dwa_gen_zero_frequency)
{
auto nh = makeTestNode("dwa_gen");
nh->declare_parameter("controller_frequency", 0.0);
LimitedAccelGeneratorTest gen;
gen.initialize(nh, "dwb");
// Default value should be 0.05
EXPECT_EQ(gen.getAccelerationTime(), 0.05);
}
TEST(VelocityIterator, dwa_gen_one_frequency)
{
auto nh = makeTestNode("dwa_gen");
nh->declare_parameter("controller_frequency", 1.0);
LimitedAccelGeneratorTest gen;
gen.initialize(nh, "dwb");
EXPECT_EQ(gen.getAccelerationTime(), 1.0);
}
TEST(VelocityIterator, dwa_gen_ten_frequency)
{
auto nh = makeTestNode("dwa_gen");
nh->declare_parameter("controller_frequency", 10.0);
LimitedAccelGeneratorTest gen;
gen.initialize(nh, "dwb");
EXPECT_EQ(gen.getAccelerationTime(), 0.1);
}
TEST(VelocityIterator, dwa_gen_fifty_frequency)
{
auto nh = makeTestNode("dwa_gen");
nh->declare_parameter("controller_frequency", 50.0);
LimitedAccelGeneratorTest gen;
gen.initialize(nh, "dwb");
EXPECT_EQ(gen.getAccelerationTime(), 0.02);
}
TEST(VelocityIterator, dwa_gen_hundred_frequency)
{
auto nh = makeTestNode("dwa_gen");
nh->declare_parameter("controller_frequency", 100.0);
LimitedAccelGeneratorTest gen;
gen.initialize(nh, "dwb");
EXPECT_EQ(gen.getAccelerationTime(), 0.01);
}
TEST(VelocityIterator, nonzero)
{
auto nh = makeTestNode("nonzero", {rclcpp::Parameter("dwb.min_speed_theta", -1.0)});
dwb_plugins::LimitedAccelGenerator gen;
gen.initialize(nh, "dwb");
nav_2d_msgs::msg::Twist2D initial;
initial.x = 0.1;
initial.y = -0.08;
initial.theta = 0.05;
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(initial);
EXPECT_EQ(twists.size(), 2519u);
checkLimits(
twists, 0.0, 0.225, -0.1, 0.045, -0.11000000000000003, 0.21,
0.24622144504490268, 0.0, 0.1);
}
void matchPose(const geometry_msgs::msg::Pose2D & a, const geometry_msgs::msg::Pose2D & b)
{
EXPECT_DOUBLE_EQ(a.x, b.x);
EXPECT_DOUBLE_EQ(a.y, b.y);
EXPECT_DOUBLE_EQ(a.theta, b.theta);
}
void matchPose(
const geometry_msgs::msg::Pose2D & a, const double x, const double y,
const double theta)
{
EXPECT_DOUBLE_EQ(a.x, x);
EXPECT_DOUBLE_EQ(a.y, y);
EXPECT_DOUBLE_EQ(a.theta, theta);
}
void matchTwist(const nav_2d_msgs::msg::Twist2D & a, const nav_2d_msgs::msg::Twist2D & b)
{
EXPECT_DOUBLE_EQ(a.x, b.x);
EXPECT_DOUBLE_EQ(a.y, b.y);
EXPECT_DOUBLE_EQ(a.theta, b.theta);
}
void matchTwist(
const nav_2d_msgs::msg::Twist2D & a, const double x, const double y,
const double theta)
{
EXPECT_DOUBLE_EQ(a.x, x);
EXPECT_DOUBLE_EQ(a.y, y);
EXPECT_DOUBLE_EQ(a.theta, theta);
}
const double DEFAULT_SIM_TIME = 1.7;
TEST(TrajectoryGenerator, basic)
{
auto nh = makeTestNode("basic", {rclcpp::Parameter("dwb.linear_granularity", 0.5)});
StandardTrajectoryGenerator gen;
gen.initialize(nh, "dwb");
dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(origin, forward, forward);
matchTwist(res.velocity, forward);
EXPECT_DOUBLE_EQ(durationToSec(res.time_offsets.back()), DEFAULT_SIM_TIME);
int n = res.poses.size();
EXPECT_EQ(n, 4);
ASSERT_GT(n, 0);
matchPose(res.poses[0], origin);
matchPose(res.poses[n - 1], DEFAULT_SIM_TIME * forward.x, 0, 0);
}
TEST(TrajectoryGenerator, basic_no_last_point)
{
auto nh = makeTestNode(
"basic_no_last_point", {
rclcpp::Parameter("dwb.include_last_point", false),
rclcpp::Parameter("dwb.linear_granularity", 0.5)});
StandardTrajectoryGenerator gen;
gen.initialize(nh, "dwb");
dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(origin, forward, forward);
matchTwist(res.velocity, forward);
EXPECT_DOUBLE_EQ(durationToSec(res.time_offsets.back()), DEFAULT_SIM_TIME / 2);
int n = res.poses.size();
EXPECT_EQ(n, 3);
ASSERT_GT(n, 0);
matchPose(res.poses[0], origin);
matchPose(res.poses[n - 2], 0.255, 0, 0);
}
TEST(TrajectoryGenerator, too_slow)
{
auto nh = makeTestNode("too_slow", {rclcpp::Parameter("dwb.linear_granularity", 0.5)});
StandardTrajectoryGenerator gen;
gen.initialize(nh, "dwb");
nav_2d_msgs::msg::Twist2D cmd;
cmd.x = 0.2;
dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(origin, cmd, cmd);
matchTwist(res.velocity, cmd);
EXPECT_DOUBLE_EQ(durationToSec(res.time_offsets.back()), DEFAULT_SIM_TIME);
int n = res.poses.size();
EXPECT_EQ(n, 3);
ASSERT_GT(n, 0);
matchPose(res.poses[0], origin);
}
TEST(TrajectoryGenerator, holonomic)
{
auto nh = makeTestNode("holonomic", {rclcpp::Parameter("dwb.linear_granularity", 0.5)});
StandardTrajectoryGenerator gen;
gen.initialize(nh, "dwb");
nav_2d_msgs::msg::Twist2D cmd;
cmd.x = 0.3;
cmd.y = 0.2;
dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(origin, cmd, cmd);
matchTwist(res.velocity, cmd);
EXPECT_DOUBLE_EQ(durationToSec(res.time_offsets.back()), DEFAULT_SIM_TIME);
int n = res.poses.size();
EXPECT_EQ(n, 4);
ASSERT_GT(n, 0);
matchPose(res.poses[0], origin);
matchPose(res.poses[n - 1], cmd.x * DEFAULT_SIM_TIME, cmd.y * DEFAULT_SIM_TIME, 0);
}
TEST(TrajectoryGenerator, twisty)
{
auto nh = makeTestNode(
"twisty", {
rclcpp::Parameter("dwb.linear_granularity", 0.5),
rclcpp::Parameter("dwb.angular_granularity", 0.025)});
StandardTrajectoryGenerator gen;
gen.initialize(nh, "dwb");
nav_2d_msgs::msg::Twist2D cmd;
cmd.x = 0.3;
cmd.y = -0.2;
cmd.theta = 0.111;
dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(origin, cmd, cmd);
matchTwist(res.velocity, cmd);
EXPECT_NEAR(durationToSec(res.time_offsets.back()), DEFAULT_SIM_TIME, 1.0E-5);
int n = res.poses.size();
EXPECT_EQ(n, 10);
ASSERT_GT(n, 0);
matchPose(res.poses[0], origin);
matchPose(
res.poses[n - 1], 0.5355173615993063, -0.29635287789821596,
cmd.theta * DEFAULT_SIM_TIME);
}
TEST(TrajectoryGenerator, sim_time)
{
const double sim_time = 2.5;
auto nh = makeTestNode(
"sim_time", {
rclcpp::Parameter("dwb.sim_time", sim_time),
rclcpp::Parameter("dwb.linear_granularity", 0.5)});
StandardTrajectoryGenerator gen;
gen.initialize(nh, "dwb");
dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(origin, forward, forward);
matchTwist(res.velocity, forward);
EXPECT_DOUBLE_EQ(durationToSec(res.time_offsets.back()), sim_time);
int n = res.poses.size();
EXPECT_EQ(n, 4);
ASSERT_GT(n, 0);
matchPose(res.poses[0], origin);
matchPose(res.poses[n - 2], sim_time * forward.x, 0, 0);
}
TEST(TrajectoryGenerator, accel)
{
auto nh = makeTestNode(
"accel", {
rclcpp::Parameter("dwb.sim_time", 5.0),
rclcpp::Parameter("dwb.discretize_by_time", true),
rclcpp::Parameter("dwb.time_granularity", 1.0),
rclcpp::Parameter("dwb.acc_lim_x", 0.1),
rclcpp::Parameter("dwb.min_speed_xy", -1.0)});
StandardTrajectoryGenerator gen;
gen.initialize(nh, "dwb");
dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(origin, zero, forward);
matchTwist(res.velocity, forward);
EXPECT_DOUBLE_EQ(durationToSec(res.time_offsets.back()), 5.0);
ASSERT_EQ(res.poses.size(), 7u);
matchPose(res.poses[0], origin);
matchPose(res.poses[1], 0.1, 0, 0);
matchPose(res.poses[2], 0.3, 0, 0);
matchPose(res.poses[3], 0.6, 0, 0);
matchPose(res.poses[4], 0.9, 0, 0);
matchPose(res.poses[5], 1.2, 0, 0);
}
TEST(TrajectoryGenerator, dwa)
{
auto nh = makeTestNode(
"dwa", {
rclcpp::Parameter("dwb.sim_period", 1.0),
rclcpp::Parameter("dwb.sim_time", 5.0),
rclcpp::Parameter("dwb.discretize_by_time", true),
rclcpp::Parameter("dwb.time_granularity", 1.0),
rclcpp::Parameter("dwb.acc_lim_x", 0.1),
rclcpp::Parameter("dwb.min_speed_xy", -1.0)});
dwb_plugins::LimitedAccelGenerator gen;
gen.initialize(nh, "dwb");
dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(origin, zero, forward);
matchTwist(res.velocity, forward);
EXPECT_DOUBLE_EQ(durationToSec(res.time_offsets.back()), 5.0);
ASSERT_EQ(res.poses.size(), 7u);
matchPose(res.poses[0], origin);
matchPose(res.poses[1], 0.3, 0, 0);
matchPose(res.poses[2], 0.6, 0, 0);
matchPose(res.poses[3], 0.9, 0, 0);
matchPose(res.poses[4], 1.2, 0, 0);
matchPose(res.poses[5], 1.5, 0, 0);
}
int main(int argc, char ** argv)
{
forward.x = 0.3;
rclcpp::init(0, nullptr);
testing::InitGoogleTest(&argc, argv);
int ret = RUN_ALL_TESTS();
rclcpp::shutdown();
return ret;
}
navigation2/nav2_dwb_controller/dwb_plugins/test/velocity_iterator_test.cpp
+143
View File
@@ -0,0 +1,143 @@
/*
* 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 "gtest/gtest.h"
#include "dwb_plugins/one_d_velocity_iterator.hpp"
using dwb_plugins::OneDVelocityIterator;
const double EPSILON = 1e-3;
TEST(VelocityIterator, basics)
{
OneDVelocityIterator it(2.0, 0.0, 5.0, 1.0, -1.0, 1.0, 2);
EXPECT_FALSE(it.isFinished());
EXPECT_NEAR(it.getVelocity(), 1.0, EPSILON);
EXPECT_FALSE(it.isFinished());
++it;
EXPECT_FALSE(it.isFinished());
EXPECT_NEAR(it.getVelocity(), 3.0, EPSILON);
EXPECT_FALSE(it.isFinished());
++it;
EXPECT_TRUE(it.isFinished());
it.reset();
EXPECT_FALSE(it.isFinished());
EXPECT_NEAR(it.getVelocity(), 1.0, EPSILON);
}
TEST(VelocityIterator, limits)
{
OneDVelocityIterator it(2.0, 1.5, 2.5, 1.0, -1.0, 1.0, 2);
EXPECT_NEAR(it.getVelocity(), 1.5, EPSILON);
++it;
EXPECT_NEAR(it.getVelocity(), 2.5, EPSILON);
}
TEST(VelocityIterator, acceleration)
{
OneDVelocityIterator it(2.0, 0.0, 5.0, 0.5, -0.5, 1.0, 2);
EXPECT_NEAR(it.getVelocity(), 1.5, EPSILON);
++it;
EXPECT_NEAR(it.getVelocity(), 2.5, EPSILON);
}
TEST(VelocityIterator, time)
{
OneDVelocityIterator it(2.0, 0.0, 5.0, 1.0, -1.0, 0.5, 2);
EXPECT_NEAR(it.getVelocity(), 1.5, EPSILON);
++it;
EXPECT_NEAR(it.getVelocity(), 2.5, EPSILON);
}
TEST(VelocityIterator, samples)
{
OneDVelocityIterator it(2.0, 0.0, 5.0, 1.0, -1.0, 1.0, 3);
EXPECT_NEAR(it.getVelocity(), 1.0, EPSILON);
++it;
EXPECT_NEAR(it.getVelocity(), 2.0, EPSILON);
++it;
EXPECT_NEAR(it.getVelocity(), 3.0, EPSILON);
++it;
EXPECT_TRUE(it.isFinished());
}
TEST(VelocityIterator, samples2)
{
OneDVelocityIterator it(2.0, 0.0, 5.0, 1.0, -1.0, 1.0, 5);
EXPECT_NEAR(it.getVelocity(), 1.0, EPSILON);
++it;
EXPECT_NEAR(it.getVelocity(), 1.5, EPSILON);
++it;
EXPECT_NEAR(it.getVelocity(), 2.0, EPSILON);
++it;
EXPECT_NEAR(it.getVelocity(), 2.5, EPSILON);
++it;
EXPECT_NEAR(it.getVelocity(), 3.0, EPSILON);
++it;
EXPECT_TRUE(it.isFinished());
}
TEST(VelocityIterator, around_zero)
{
OneDVelocityIterator it(0.0, -5.0, 5.0, 1.0, -1.0, 1.0, 2);
EXPECT_NEAR(it.getVelocity(), -1.0, EPSILON);
++it;
EXPECT_NEAR(it.getVelocity(), 0.0, EPSILON);
++it;
EXPECT_NEAR(it.getVelocity(), 1.0, EPSILON);
++it;
}
TEST(VelocityIterator, around_zero2)
{
OneDVelocityIterator it(0.0, -5.0, 5.0, 1.0, -1.0, 1.0, 4);
EXPECT_NEAR(it.getVelocity(), -1.0, EPSILON);
++it;
EXPECT_NEAR(it.getVelocity(), -0.3333, EPSILON);
++it;
EXPECT_NEAR(it.getVelocity(), 0.0, EPSILON);
++it;
EXPECT_NEAR(it.getVelocity(), 0.3333, EPSILON);
++it;
EXPECT_NEAR(it.getVelocity(), 1.0, EPSILON);
++it;
}
int main(int argc, char ** argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
navigation2/nav2_dwb_controller/images/DWB_Structure_Simplified.svg
+193
View File
File diff suppressed because one or more lines are too long
Side by Side

After

Width:  |  Height:  |  Size: 40 KiB

navigation2/nav2_dwb_controller/images/LocalPlanner.svg
+87
View File
File diff suppressed because one or more lines are too long
Side by Side

After

Width:  |  Height:  |  Size: 24 KiB

navigation2/nav2_dwb_controller/nav2_dwb_controller/CHANGELOG.rst
View File
navigation2/nav2_dwb_controller/nav2_dwb_controller/CMakeLists.txt
+10
View File
@@ -0,0 +1,10 @@
cmake_minimum_required(VERSION 3.5)
project(nav2_dwb_controller)
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
find_package(ament_cmake REQUIRED)
ament_package()
navigation2/nav2_dwb_controller/nav2_dwb_controller/package.xml
+27
View File
@@ -0,0 +1,27 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_dwb_controller</name>
<version>1.1.18</version>
<description>
ROS2 controller (DWB) metapackage
</description>
<maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>costmap_queue</depend>
<depend>dwb_core</depend>
<depend>dwb_critics</depend>
<depend>dwb_msgs</depend>
<depend>dwb_plugins</depend>
<depend>nav_2d_msgs</depend>
<depend>nav_2d_utils</depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
navigation2/nav2_dwb_controller/nav_2d_msgs/CMakeLists.txt
+20
View File
@@ -0,0 +1,20 @@
cmake_minimum_required(VERSION 3.5)
project(nav_2d_msgs)
find_package(geometry_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(nav_2d_msgs
"msg/Path2D.msg"
"msg/Pose2D32.msg"
"msg/Pose2DStamped.msg"
"msg/Twist2D.msg"
"msg/Twist2D32.msg"
"msg/Twist2DStamped.msg"
DEPENDENCIES geometry_msgs std_msgs
)
ament_export_dependencies(rosidl_default_runtime)
ament_package()
navigation2/nav2_dwb_controller/nav_2d_msgs/msg/Path2D.msg
+2
View File
@@ -0,0 +1,2 @@
std_msgs/Header header
geometry_msgs/Pose2D[] poses
navigation2/nav2_dwb_controller/nav_2d_msgs/msg/Pose2D32.msg
+3
View File
@@ -0,0 +1,3 @@
float32 x
float32 y
float32 theta
navigation2/nav2_dwb_controller/nav_2d_msgs/msg/Pose2DStamped.msg
+2
View File
@@ -0,0 +1,2 @@
std_msgs/Header header
geometry_msgs/Pose2D pose
navigation2/nav2_dwb_controller/nav_2d_msgs/msg/Twist2D.msg
+3
View File
@@ -0,0 +1,3 @@
float64 x
float64 y
float64 theta
navigation2/nav2_dwb_controller/nav_2d_msgs/msg/Twist2D32.msg
+3
View File
@@ -0,0 +1,3 @@
float32 x
float32 y
float32 theta

Some files were not shown because too many files have changed in this diff Show More