add humble-navigation2
This commit is contained in:
X-lanni
@@ -0,0 +1,51 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "dwb_critics/alignment_util.hpp"
|
||||
#include <cmath>
|
||||
|
||||
using std::cos;
|
||||
using std::sin;
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
geometry_msgs::msg::Pose2D getForwardPose(const geometry_msgs::msg::Pose2D & pose, double distance)
|
||||
{
|
||||
geometry_msgs::msg::Pose2D forward_pose;
|
||||
forward_pose.x = pose.x + distance * cos(pose.theta);
|
||||
forward_pose.y = pose.y + distance * sin(pose.theta);
|
||||
forward_pose.theta = pose.theta;
|
||||
return forward_pose;
|
||||
}
|
||||
} // namespace dwb_critics
|
||||
@@ -0,0 +1,117 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
|
||||
#include "dwb_critics/base_obstacle.hpp"
|
||||
#include "dwb_core/exceptions.hpp"
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
#include "nav2_costmap_2d/cost_values.hpp"
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(dwb_critics::BaseObstacleCritic, dwb_core::TrajectoryCritic)
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
|
||||
void BaseObstacleCritic::onInit()
|
||||
{
|
||||
costmap_ = costmap_ros_->getCostmap();
|
||||
|
||||
auto node = node_.lock();
|
||||
if (!node) {
|
||||
throw std::runtime_error{"Failed to lock node"};
|
||||
}
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node,
|
||||
dwb_plugin_name_ + "." + name_ + ".sum_scores", rclcpp::ParameterValue(false));
|
||||
node->get_parameter(dwb_plugin_name_ + "." + name_ + ".sum_scores", sum_scores_);
|
||||
}
|
||||
|
||||
double BaseObstacleCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj)
|
||||
{
|
||||
double score = 0.0;
|
||||
for (unsigned int i = 0; i < traj.poses.size(); ++i) {
|
||||
double pose_score = scorePose(traj.poses[i]);
|
||||
// Optimized/branchless version of if (sum_scores_) score += pose_score,
|
||||
// else score = pose_score;
|
||||
score = static_cast<double>(sum_scores_) * score + pose_score;
|
||||
}
|
||||
return score;
|
||||
}
|
||||
|
||||
double BaseObstacleCritic::scorePose(const geometry_msgs::msg::Pose2D & pose)
|
||||
{
|
||||
unsigned int cell_x, cell_y;
|
||||
if (!costmap_->worldToMap(pose.x, pose.y, cell_x, cell_y)) {
|
||||
throw dwb_core::
|
||||
IllegalTrajectoryException(name_, "Trajectory Goes Off Grid.");
|
||||
}
|
||||
unsigned char cost = costmap_->getCost(cell_x, cell_y);
|
||||
if (!isValidCost(cost)) {
|
||||
throw dwb_core::
|
||||
IllegalTrajectoryException(name_, "Trajectory Hits Obstacle.");
|
||||
}
|
||||
return cost;
|
||||
}
|
||||
|
||||
bool BaseObstacleCritic::isValidCost(const unsigned char cost)
|
||||
{
|
||||
return cost != nav2_costmap_2d::LETHAL_OBSTACLE &&
|
||||
cost != nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE &&
|
||||
cost != nav2_costmap_2d::NO_INFORMATION;
|
||||
}
|
||||
|
||||
void BaseObstacleCritic::addCriticVisualization(
|
||||
std::vector<std::pair<std::string, std::vector<float>>> & cost_channels)
|
||||
{
|
||||
std::pair<std::string, std::vector<float>> grid_scores;
|
||||
grid_scores.first = name_;
|
||||
|
||||
unsigned int size_x = costmap_->getSizeInCellsX();
|
||||
unsigned int size_y = costmap_->getSizeInCellsY();
|
||||
grid_scores.second.resize(size_x * size_y);
|
||||
unsigned int i = 0;
|
||||
for (unsigned int cy = 0; cy < size_y; cy++) {
|
||||
for (unsigned int cx = 0; cx < size_x; cx++) {
|
||||
grid_scores.second[i] = costmap_->getCost(cx, cy);
|
||||
i++;
|
||||
}
|
||||
}
|
||||
cost_channels.push_back(grid_scores);
|
||||
}
|
||||
|
||||
} // namespace dwb_critics
|
||||
@@ -0,0 +1,86 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "dwb_critics/goal_align.hpp"
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include "dwb_critics/alignment_util.hpp"
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
#include "nav_2d_utils/parameters.hpp"
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
|
||||
void GoalAlignCritic::onInit()
|
||||
{
|
||||
GoalDistCritic::onInit();
|
||||
stop_on_failure_ = false;
|
||||
|
||||
auto node = node_.lock();
|
||||
if (!node) {
|
||||
throw std::runtime_error{"Failed to lock node"};
|
||||
}
|
||||
|
||||
forward_point_distance_ = nav_2d_utils::searchAndGetParam(
|
||||
node,
|
||||
dwb_plugin_name_ + "." + name_ + ".forward_point_distance", 0.325);
|
||||
}
|
||||
|
||||
bool GoalAlignCritic::prepare(
|
||||
const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
|
||||
const geometry_msgs::msg::Pose2D & goal,
|
||||
const nav_2d_msgs::msg::Path2D & global_plan)
|
||||
{
|
||||
// we want the robot nose to be drawn to its final position
|
||||
// (before robot turns towards goal orientation), not the end of the
|
||||
// path for the robot center. Choosing the final position after
|
||||
// turning towards goal orientation causes instability when the
|
||||
// robot needs to make a 180 degree turn at the end
|
||||
double angle_to_goal = atan2(goal.y - pose.y, goal.x - pose.x);
|
||||
|
||||
nav_2d_msgs::msg::Path2D target_poses = global_plan;
|
||||
target_poses.poses.back().x += forward_point_distance_ * cos(angle_to_goal);
|
||||
target_poses.poses.back().y += forward_point_distance_ * sin(angle_to_goal);
|
||||
|
||||
return GoalDistCritic::prepare(pose, vel, goal, target_poses);
|
||||
}
|
||||
|
||||
double GoalAlignCritic::scorePose(const geometry_msgs::msg::Pose2D & pose)
|
||||
{
|
||||
return GoalDistCritic::scorePose(getForwardPose(pose, forward_point_distance_));
|
||||
}
|
||||
|
||||
} // namespace dwb_critics
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(dwb_critics::GoalAlignCritic, dwb_core::TrajectoryCritic)
|
||||
@@ -0,0 +1,106 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "dwb_critics/goal_dist.hpp"
|
||||
#include <vector>
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
#include "nav_2d_utils/path_ops.hpp"
|
||||
#include "nav2_costmap_2d/cost_values.hpp"
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
bool GoalDistCritic::prepare(
|
||||
const geometry_msgs::msg::Pose2D &, const nav_2d_msgs::msg::Twist2D &,
|
||||
const geometry_msgs::msg::Pose2D &,
|
||||
const nav_2d_msgs::msg::Path2D & global_plan)
|
||||
{
|
||||
reset();
|
||||
|
||||
unsigned int local_goal_x, local_goal_y;
|
||||
if (!getLastPoseOnCostmap(global_plan, local_goal_x, local_goal_y)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Enqueue just the last pose
|
||||
int index = costmap_->getIndex(local_goal_x, local_goal_y);
|
||||
cell_values_[index] = 0.0;
|
||||
queue_->enqueueCell(local_goal_x, local_goal_y);
|
||||
|
||||
propogateManhattanDistances();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool GoalDistCritic::getLastPoseOnCostmap(
|
||||
const nav_2d_msgs::msg::Path2D & global_plan,
|
||||
unsigned int & x, unsigned int & y)
|
||||
{
|
||||
nav_2d_msgs::msg::Path2D adjusted_global_plan = nav_2d_utils::adjustPlanResolution(
|
||||
global_plan,
|
||||
costmap_->getResolution());
|
||||
bool started_path = false;
|
||||
|
||||
// skip global path points until we reach the border of the local map
|
||||
for (unsigned int i = 0; i < adjusted_global_plan.poses.size(); ++i) {
|
||||
double g_x = adjusted_global_plan.poses[i].x;
|
||||
double g_y = adjusted_global_plan.poses[i].y;
|
||||
unsigned int map_x, map_y;
|
||||
if (costmap_->worldToMap(
|
||||
g_x, g_y, map_x,
|
||||
map_y) && costmap_->getCost(map_x, map_y) != nav2_costmap_2d::NO_INFORMATION)
|
||||
{
|
||||
// Still on the costmap. Continue.
|
||||
x = map_x;
|
||||
y = map_y;
|
||||
started_path = true;
|
||||
} else if (started_path) {
|
||||
// Off the costmap after being on the costmap. Return the last saved indices.
|
||||
return true;
|
||||
}
|
||||
// else, we have not yet found a point on the costmap, so we just continue
|
||||
}
|
||||
|
||||
if (started_path) {
|
||||
return true;
|
||||
} else {
|
||||
RCLCPP_ERROR(
|
||||
rclcpp::get_logger(
|
||||
"GoalDistCritic"), "None of the points of the global plan were in the local costmap.");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace dwb_critics
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(dwb_critics::GoalDistCritic, dwb_core::TrajectoryCritic)
|
||||
@@ -0,0 +1,189 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "dwb_critics/map_grid.hpp"
|
||||
#include <cmath>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <utility>
|
||||
#include <algorithm>
|
||||
#include <memory>
|
||||
#include "dwb_core/exceptions.hpp"
|
||||
#include "nav2_costmap_2d/cost_values.hpp"
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
|
||||
using std::abs;
|
||||
using costmap_queue::CellData;
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
|
||||
// Customization of the CostmapQueue validCellToQueue method
|
||||
bool MapGridCritic::MapGridQueue::validCellToQueue(const costmap_queue::CellData & /*cell*/)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
void MapGridCritic::onInit()
|
||||
{
|
||||
costmap_ = costmap_ros_->getCostmap();
|
||||
queue_ = std::make_shared<MapGridQueue>(*costmap_, *this);
|
||||
|
||||
// Always set to true, but can be overriden by subclasses
|
||||
stop_on_failure_ = true;
|
||||
|
||||
auto node = node_.lock();
|
||||
if (!node) {
|
||||
throw std::runtime_error{"Failed to lock node"};
|
||||
}
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node,
|
||||
dwb_plugin_name_ + "." + name_ + ".aggregation_type",
|
||||
rclcpp::ParameterValue(std::string("last")));
|
||||
|
||||
std::string aggro_str;
|
||||
node->get_parameter(dwb_plugin_name_ + "." + name_ + ".aggregation_type", aggro_str);
|
||||
std::transform(aggro_str.begin(), aggro_str.end(), aggro_str.begin(), ::tolower);
|
||||
if (aggro_str == "last") {
|
||||
aggregationType_ = ScoreAggregationType::Last;
|
||||
} else if (aggro_str == "sum") {
|
||||
aggregationType_ = ScoreAggregationType::Sum;
|
||||
} else if (aggro_str == "product") {
|
||||
aggregationType_ = ScoreAggregationType::Product;
|
||||
} else {
|
||||
RCLCPP_ERROR(
|
||||
rclcpp::get_logger(
|
||||
"MapGridCritic"), "aggregation_type parameter \"%s\" invalid. Using Last.",
|
||||
aggro_str.c_str());
|
||||
aggregationType_ = ScoreAggregationType::Last;
|
||||
}
|
||||
}
|
||||
|
||||
void MapGridCritic::setAsObstacle(unsigned int index)
|
||||
{
|
||||
cell_values_[index] = obstacle_score_;
|
||||
}
|
||||
|
||||
void MapGridCritic::reset()
|
||||
{
|
||||
queue_->reset();
|
||||
cell_values_.resize(costmap_->getSizeInCellsX() * costmap_->getSizeInCellsY());
|
||||
obstacle_score_ = static_cast<double>(cell_values_.size());
|
||||
unreachable_score_ = obstacle_score_ + 1.0;
|
||||
std::fill(cell_values_.begin(), cell_values_.end(), unreachable_score_);
|
||||
}
|
||||
|
||||
void MapGridCritic::propogateManhattanDistances()
|
||||
{
|
||||
while (!queue_->isEmpty()) {
|
||||
costmap_queue::CellData cell = queue_->getNextCell();
|
||||
cell_values_[cell.index_] = CellData::absolute_difference(cell.src_x_, cell.x_) +
|
||||
CellData::absolute_difference(cell.src_y_, cell.y_);
|
||||
}
|
||||
}
|
||||
|
||||
double MapGridCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj)
|
||||
{
|
||||
double score = 0.0;
|
||||
unsigned int start_index = 0;
|
||||
if (aggregationType_ == ScoreAggregationType::Product) {
|
||||
score = 1.0;
|
||||
} else if (aggregationType_ == ScoreAggregationType::Last && !stop_on_failure_) {
|
||||
start_index = traj.poses.size() - 1;
|
||||
}
|
||||
double grid_dist;
|
||||
|
||||
for (unsigned int i = start_index; i < traj.poses.size(); ++i) {
|
||||
grid_dist = scorePose(traj.poses[i]);
|
||||
if (stop_on_failure_) {
|
||||
if (grid_dist == obstacle_score_) {
|
||||
throw dwb_core::
|
||||
IllegalTrajectoryException(name_, "Trajectory Hits Obstacle.");
|
||||
} else if (grid_dist == unreachable_score_) {
|
||||
throw dwb_core::
|
||||
IllegalTrajectoryException(name_, "Trajectory Hits Unreachable Area.");
|
||||
}
|
||||
}
|
||||
|
||||
switch (aggregationType_) {
|
||||
case ScoreAggregationType::Last:
|
||||
score = grid_dist;
|
||||
break;
|
||||
case ScoreAggregationType::Sum:
|
||||
score += grid_dist;
|
||||
break;
|
||||
case ScoreAggregationType::Product:
|
||||
if (score > 0) {
|
||||
score *= grid_dist;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return score;
|
||||
}
|
||||
|
||||
double MapGridCritic::scorePose(const geometry_msgs::msg::Pose2D & pose)
|
||||
{
|
||||
unsigned int cell_x, cell_y;
|
||||
// we won't allow trajectories that go off the map... shouldn't happen that often anyways
|
||||
if (!costmap_->worldToMap(pose.x, pose.y, cell_x, cell_y)) {
|
||||
throw dwb_core::
|
||||
IllegalTrajectoryException(name_, "Trajectory Goes Off Grid.");
|
||||
}
|
||||
return getScore(cell_x, cell_y);
|
||||
}
|
||||
|
||||
void MapGridCritic::addCriticVisualization(
|
||||
std::vector<std::pair<std::string, std::vector<float>>> & cost_channels)
|
||||
{
|
||||
std::pair<std::string, std::vector<float>> grid_scores;
|
||||
grid_scores.first = name_;
|
||||
|
||||
nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
|
||||
unsigned int size_x = costmap->getSizeInCellsX();
|
||||
unsigned int size_y = costmap->getSizeInCellsY();
|
||||
grid_scores.second.resize(size_x * size_y);
|
||||
unsigned int i = 0;
|
||||
for (unsigned int cy = 0; cy < size_y; cy++) {
|
||||
for (unsigned int cx = 0; cx < size_x; cx++) {
|
||||
grid_scores.second[i] = getScore(cx, cy);
|
||||
i++;
|
||||
}
|
||||
}
|
||||
cost_channels.push_back(grid_scores);
|
||||
}
|
||||
|
||||
} // namespace dwb_critics
|
||||
@@ -0,0 +1,165 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "dwb_critics/obstacle_footprint.hpp"
|
||||
#include <algorithm>
|
||||
#include <vector>
|
||||
#include "dwb_critics/line_iterator.hpp"
|
||||
#include "dwb_core/exceptions.hpp"
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
#include "nav2_costmap_2d/cost_values.hpp"
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(dwb_critics::ObstacleFootprintCritic, dwb_core::TrajectoryCritic)
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
Footprint getOrientedFootprint(
|
||||
const geometry_msgs::msg::Pose2D & pose,
|
||||
const Footprint & footprint_spec)
|
||||
{
|
||||
std::vector<geometry_msgs::msg::Point> oriented_footprint;
|
||||
oriented_footprint.resize(footprint_spec.size());
|
||||
double cos_th = cos(pose.theta);
|
||||
double sin_th = sin(pose.theta);
|
||||
for (unsigned int i = 0; i < footprint_spec.size(); ++i) {
|
||||
geometry_msgs::msg::Point & new_pt = oriented_footprint[i];
|
||||
new_pt.x = pose.x + footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th;
|
||||
new_pt.y = pose.y + footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th;
|
||||
}
|
||||
return oriented_footprint;
|
||||
}
|
||||
|
||||
bool ObstacleFootprintCritic::prepare(
|
||||
const geometry_msgs::msg::Pose2D &, const nav_2d_msgs::msg::Twist2D &,
|
||||
const geometry_msgs::msg::Pose2D &, const nav_2d_msgs::msg::Path2D &)
|
||||
{
|
||||
footprint_spec_ = costmap_ros_->getRobotFootprint();
|
||||
if (footprint_spec_.size() == 0) {
|
||||
RCLCPP_ERROR(
|
||||
rclcpp::get_logger("ObstacleFootprintCritic"),
|
||||
"Footprint spec is empty, maybe missing call to setFootprint?");
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
double ObstacleFootprintCritic::scorePose(const geometry_msgs::msg::Pose2D & pose)
|
||||
{
|
||||
unsigned int cell_x, cell_y;
|
||||
if (!costmap_->worldToMap(pose.x, pose.y, cell_x, cell_y)) {
|
||||
throw dwb_core::
|
||||
IllegalTrajectoryException(name_, "Trajectory Goes Off Grid.");
|
||||
}
|
||||
return scorePose(pose, getOrientedFootprint(pose, footprint_spec_));
|
||||
}
|
||||
|
||||
double ObstacleFootprintCritic::scorePose(
|
||||
const geometry_msgs::msg::Pose2D &,
|
||||
const Footprint & footprint)
|
||||
{
|
||||
// now we really have to lay down the footprint in the costmap grid
|
||||
unsigned int x0, x1, y0, y1;
|
||||
double line_cost = 0.0;
|
||||
double footprint_cost = 0.0;
|
||||
|
||||
// we need to rasterize each line in the footprint
|
||||
for (unsigned int i = 0; i < footprint.size() - 1; ++i) {
|
||||
// get the cell coord of the first point
|
||||
if (!costmap_->worldToMap(footprint[i].x, footprint[i].y, x0, y0)) {
|
||||
throw dwb_core::
|
||||
IllegalTrajectoryException(name_, "Footprint Goes Off Grid.");
|
||||
}
|
||||
|
||||
// get the cell coord of the second point
|
||||
if (!costmap_->worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1)) {
|
||||
throw dwb_core::
|
||||
IllegalTrajectoryException(name_, "Footprint Goes Off Grid.");
|
||||
}
|
||||
|
||||
line_cost = lineCost(x0, x1, y0, y1);
|
||||
footprint_cost = std::max(line_cost, footprint_cost);
|
||||
}
|
||||
|
||||
// we also need to connect the first point in the footprint to the last point
|
||||
// get the cell coord of the last point
|
||||
if (!costmap_->worldToMap(footprint.back().x, footprint.back().y, x0, y0)) {
|
||||
throw dwb_core::
|
||||
IllegalTrajectoryException(name_, "Footprint Goes Off Grid.");
|
||||
}
|
||||
|
||||
// get the cell coord of the first point
|
||||
if (!costmap_->worldToMap(footprint.front().x, footprint.front().y, x1, y1)) {
|
||||
throw dwb_core::
|
||||
IllegalTrajectoryException(name_, "Footprint Goes Off Grid.");
|
||||
}
|
||||
|
||||
line_cost = lineCost(x0, x1, y0, y1);
|
||||
footprint_cost = std::max(line_cost, footprint_cost);
|
||||
|
||||
// if all line costs are legal... then we can return that the footprint is legal
|
||||
return footprint_cost;
|
||||
}
|
||||
|
||||
double ObstacleFootprintCritic::lineCost(int x0, int x1, int y0, int y1)
|
||||
{
|
||||
double line_cost = 0.0;
|
||||
double point_cost = -1.0;
|
||||
|
||||
for (LineIterator line(x0, y0, x1, y1); line.isValid(); line.advance()) {
|
||||
point_cost = pointCost(line.getX(), line.getY()); // Score the current point
|
||||
|
||||
if (line_cost < point_cost) {
|
||||
line_cost = point_cost;
|
||||
}
|
||||
}
|
||||
|
||||
return line_cost;
|
||||
}
|
||||
|
||||
double ObstacleFootprintCritic::pointCost(int x, int y)
|
||||
{
|
||||
unsigned char cost = costmap_->getCost(x, y);
|
||||
// if the cell is in an obstacle the path is invalid or unknown
|
||||
if (cost == nav2_costmap_2d::LETHAL_OBSTACLE) {
|
||||
throw dwb_core::
|
||||
IllegalTrajectoryException(name_, "Trajectory Hits Obstacle.");
|
||||
} else if (cost == nav2_costmap_2d::NO_INFORMATION) {
|
||||
throw dwb_core::
|
||||
IllegalTrajectoryException(name_, "Trajectory Hits Unknown Region.");
|
||||
}
|
||||
|
||||
return cost;
|
||||
}
|
||||
|
||||
} // namespace dwb_critics
|
||||
@@ -0,0 +1,234 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "dwb_critics/oscillation.hpp"
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include "nav_2d_utils/parameters.hpp"
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
#include "dwb_core/exceptions.hpp"
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(dwb_critics::OscillationCritic, dwb_core::TrajectoryCritic)
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
|
||||
|
||||
OscillationCritic::CommandTrend::CommandTrend()
|
||||
{
|
||||
reset();
|
||||
}
|
||||
|
||||
void OscillationCritic::CommandTrend::reset()
|
||||
{
|
||||
sign_ = Sign::ZERO;
|
||||
positive_only_ = false;
|
||||
negative_only_ = false;
|
||||
}
|
||||
|
||||
bool OscillationCritic::CommandTrend::update(double velocity)
|
||||
{
|
||||
bool flag_set = false;
|
||||
if (velocity < 0.0) {
|
||||
if (sign_ == Sign::POSITIVE) {
|
||||
negative_only_ = true;
|
||||
flag_set = true;
|
||||
}
|
||||
sign_ = Sign::NEGATIVE;
|
||||
} else if (velocity > 0.0) {
|
||||
if (sign_ == Sign::NEGATIVE) {
|
||||
positive_only_ = true;
|
||||
flag_set = true;
|
||||
}
|
||||
sign_ = Sign::POSITIVE;
|
||||
}
|
||||
return flag_set;
|
||||
}
|
||||
|
||||
bool OscillationCritic::CommandTrend::isOscillating(double velocity)
|
||||
{
|
||||
return (positive_only_ && velocity < 0.0) || (negative_only_ && velocity > 0.0);
|
||||
}
|
||||
|
||||
bool OscillationCritic::CommandTrend::hasSignFlipped()
|
||||
{
|
||||
return positive_only_ || negative_only_;
|
||||
}
|
||||
|
||||
void OscillationCritic::onInit()
|
||||
{
|
||||
auto node = node_.lock();
|
||||
if (!node) {
|
||||
throw std::runtime_error{"Failed to lock node"};
|
||||
}
|
||||
|
||||
clock_ = node->get_clock();
|
||||
|
||||
oscillation_reset_dist_ = nav_2d_utils::searchAndGetParam(
|
||||
node,
|
||||
dwb_plugin_name_ + "." + name_ + ".oscillation_reset_dist", 0.05);
|
||||
oscillation_reset_dist_sq_ = oscillation_reset_dist_ * oscillation_reset_dist_;
|
||||
oscillation_reset_angle_ = nav_2d_utils::searchAndGetParam(
|
||||
node,
|
||||
dwb_plugin_name_ + "." + name_ + ".oscillation_reset_angle", 0.2);
|
||||
oscillation_reset_time_ = rclcpp::Duration::from_seconds(
|
||||
nav_2d_utils::searchAndGetParam(
|
||||
node,
|
||||
dwb_plugin_name_ + "." + name_ + ".oscillation_reset_time", -1.0));
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node,
|
||||
dwb_plugin_name_ + "." + name_ + ".x_only_threshold", rclcpp::ParameterValue(0.05));
|
||||
|
||||
/**
|
||||
* Historical Parameter Loading
|
||||
* If x_only_threshold is set, use that.
|
||||
* If min_speed_xy is set in the namespace (as it is often used for trajectory generation), use that.
|
||||
* If min_trans_vel is set in the namespace, as it used to be used for trajectory generation, complain then use that.
|
||||
* Otherwise, set x_only_threshold_ to 0.05
|
||||
*/
|
||||
node->get_parameter(dwb_plugin_name_ + "." + name_ + ".x_only_threshold", x_only_threshold_);
|
||||
// TODO(crdelsey): How to handle searchParam?
|
||||
// std::string resolved_name;
|
||||
// if (node->hasParam("x_only_threshold"))
|
||||
// {
|
||||
// node->param("x_only_threshold", x_only_threshold_);
|
||||
// }
|
||||
// else if (node->searchParam("min_speed_xy", resolved_name))
|
||||
// {
|
||||
// node->param(resolved_name, x_only_threshold_);
|
||||
// }
|
||||
// else if (node->searchParam("min_trans_vel", resolved_name))
|
||||
// {
|
||||
// ROS_WARN_NAMED("OscillationCritic",
|
||||
// "Parameter min_trans_vel is deprecated. "
|
||||
// "Please use the name min_speed_xy or x_only_threshold instead.");
|
||||
// node->param(resolved_name, x_only_threshold_);
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// x_only_threshold_ = 0.05;
|
||||
// }
|
||||
|
||||
reset();
|
||||
}
|
||||
|
||||
bool OscillationCritic::prepare(
|
||||
const geometry_msgs::msg::Pose2D & pose,
|
||||
const nav_2d_msgs::msg::Twist2D &,
|
||||
const geometry_msgs::msg::Pose2D &,
|
||||
const nav_2d_msgs::msg::Path2D &)
|
||||
{
|
||||
pose_ = pose;
|
||||
return true;
|
||||
}
|
||||
|
||||
void OscillationCritic::debrief(const nav_2d_msgs::msg::Twist2D & cmd_vel)
|
||||
{
|
||||
if (setOscillationFlags(cmd_vel)) {
|
||||
prev_stationary_pose_ = pose_;
|
||||
prev_reset_time_ = clock_->now();
|
||||
}
|
||||
|
||||
// if we've got restrictions... check if we can reset any oscillation flags
|
||||
if (x_trend_.hasSignFlipped() || y_trend_.hasSignFlipped() || theta_trend_.hasSignFlipped()) {
|
||||
// Reset flags if enough time or distance has passed
|
||||
if (resetAvailable()) {
|
||||
reset();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool OscillationCritic::resetAvailable()
|
||||
{
|
||||
if (oscillation_reset_dist_ >= 0.0) {
|
||||
double x_diff = pose_.x - prev_stationary_pose_.x;
|
||||
double y_diff = pose_.y - prev_stationary_pose_.y;
|
||||
double sq_dist = x_diff * x_diff + y_diff * y_diff;
|
||||
if (sq_dist > oscillation_reset_dist_sq_) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
if (oscillation_reset_angle_ >= 0.0) {
|
||||
double th_diff = pose_.theta - prev_stationary_pose_.theta;
|
||||
if (fabs(th_diff) > oscillation_reset_angle_) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
if (oscillation_reset_time_ >= rclcpp::Duration::from_seconds(0.0)) {
|
||||
auto t_diff = (clock_->now() - prev_reset_time_);
|
||||
if (t_diff > oscillation_reset_time_) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void OscillationCritic::reset()
|
||||
{
|
||||
x_trend_.reset();
|
||||
y_trend_.reset();
|
||||
theta_trend_.reset();
|
||||
}
|
||||
|
||||
bool OscillationCritic::setOscillationFlags(const nav_2d_msgs::msg::Twist2D & cmd_vel)
|
||||
{
|
||||
bool flag_set = false;
|
||||
// set oscillation flags for moving forward and backward
|
||||
flag_set |= x_trend_.update(cmd_vel.x);
|
||||
|
||||
// we'll only set flags for strafing and rotating when we're not moving forward at all
|
||||
if (x_only_threshold_ < 0.0 || fabs(cmd_vel.x) <= x_only_threshold_) {
|
||||
flag_set |= y_trend_.update(cmd_vel.y);
|
||||
flag_set |= theta_trend_.update(cmd_vel.theta);
|
||||
}
|
||||
return flag_set;
|
||||
}
|
||||
|
||||
double OscillationCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj)
|
||||
{
|
||||
if (x_trend_.isOscillating(traj.velocity.x) ||
|
||||
y_trend_.isOscillating(traj.velocity.y) ||
|
||||
theta_trend_.isOscillating(traj.velocity.theta))
|
||||
{
|
||||
throw dwb_core::
|
||||
IllegalTrajectoryException(name_, "Trajectory is oscillating.");
|
||||
}
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
} // namespace dwb_critics
|
||||
@@ -0,0 +1,95 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "dwb_critics/path_align.hpp"
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include "dwb_critics/alignment_util.hpp"
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
#include "nav_2d_utils/parameters.hpp"
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
|
||||
void PathAlignCritic::onInit()
|
||||
{
|
||||
PathDistCritic::onInit();
|
||||
stop_on_failure_ = false;
|
||||
|
||||
auto node = node_.lock();
|
||||
if (!node) {
|
||||
throw std::runtime_error{"Failed to lock node"};
|
||||
}
|
||||
|
||||
forward_point_distance_ = nav_2d_utils::searchAndGetParam(
|
||||
node,
|
||||
dwb_plugin_name_ + "." + name_ + ".forward_point_distance", 0.325);
|
||||
}
|
||||
|
||||
bool PathAlignCritic::prepare(
|
||||
const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
|
||||
const geometry_msgs::msg::Pose2D & goal,
|
||||
const nav_2d_msgs::msg::Path2D & global_plan)
|
||||
{
|
||||
double dx = pose.x - goal.x;
|
||||
double dy = pose.y - goal.y;
|
||||
double sq_dist = dx * dx + dy * dy;
|
||||
if (sq_dist > forward_point_distance_ * forward_point_distance_) {
|
||||
zero_scale_ = false;
|
||||
} else {
|
||||
// once we are close to goal, trying to keep the nose close to anything destabilizes behavior.
|
||||
zero_scale_ = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
return PathDistCritic::prepare(pose, vel, goal, global_plan);
|
||||
}
|
||||
|
||||
double PathAlignCritic::getScale() const
|
||||
{
|
||||
if (zero_scale_) {
|
||||
return 0.0;
|
||||
} else {
|
||||
return costmap_->getResolution() * 0.5 * scale_;
|
||||
}
|
||||
}
|
||||
|
||||
double PathAlignCritic::scorePose(const geometry_msgs::msg::Pose2D & pose)
|
||||
{
|
||||
return PathDistCritic::scorePose(getForwardPose(pose, forward_point_distance_));
|
||||
}
|
||||
|
||||
} // namespace dwb_critics
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(dwb_critics::PathAlignCritic, dwb_core::TrajectoryCritic)
|
||||
@@ -0,0 +1,95 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "dwb_critics/path_dist.hpp"
|
||||
#include <vector>
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
#include "nav_2d_utils/path_ops.hpp"
|
||||
#include "nav2_costmap_2d/cost_values.hpp"
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
bool PathDistCritic::prepare(
|
||||
const geometry_msgs::msg::Pose2D &, const nav_2d_msgs::msg::Twist2D &,
|
||||
const geometry_msgs::msg::Pose2D &,
|
||||
const nav_2d_msgs::msg::Path2D & global_plan)
|
||||
{
|
||||
reset();
|
||||
bool started_path = false;
|
||||
|
||||
nav_2d_msgs::msg::Path2D adjusted_global_plan =
|
||||
nav_2d_utils::adjustPlanResolution(global_plan, costmap_->getResolution());
|
||||
|
||||
if (adjusted_global_plan.poses.size() != global_plan.poses.size()) {
|
||||
RCLCPP_DEBUG(
|
||||
rclcpp::get_logger(
|
||||
"PathDistCritic"), "Adjusted global plan resolution, added %zu points",
|
||||
adjusted_global_plan.poses.size() - global_plan.poses.size());
|
||||
}
|
||||
|
||||
unsigned int i;
|
||||
// put global path points into local map until we reach the border of the local map
|
||||
for (i = 0; i < adjusted_global_plan.poses.size(); ++i) {
|
||||
double g_x = adjusted_global_plan.poses[i].x;
|
||||
double g_y = adjusted_global_plan.poses[i].y;
|
||||
unsigned int map_x, map_y;
|
||||
if (costmap_->worldToMap(
|
||||
g_x, g_y, map_x,
|
||||
map_y) && costmap_->getCost(map_x, map_y) != nav2_costmap_2d::NO_INFORMATION)
|
||||
{
|
||||
int index = costmap_->getIndex(map_x, map_y);
|
||||
cell_values_[index] = 0.0;
|
||||
queue_->enqueueCell(map_x, map_y);
|
||||
started_path = true;
|
||||
} else if (started_path) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!started_path) {
|
||||
RCLCPP_ERROR(
|
||||
rclcpp::get_logger("PathDistCritic"),
|
||||
"None of the %d first of %zu (%zu) points of the global plan were in "
|
||||
"the local costmap and free",
|
||||
i, adjusted_global_plan.poses.size(), global_plan.poses.size());
|
||||
return false;
|
||||
}
|
||||
|
||||
propogateManhattanDistances();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace dwb_critics
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(dwb_critics::PathDistCritic, dwb_core::TrajectoryCritic)
|
||||
@@ -0,0 +1,88 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "dwb_critics/prefer_forward.hpp"
|
||||
#include <math.h>
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(dwb_critics::PreferForwardCritic, dwb_core::TrajectoryCritic)
|
||||
|
||||
using nav2_util::declare_parameter_if_not_declared;
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
|
||||
void PreferForwardCritic::onInit()
|
||||
{
|
||||
auto node = node_.lock();
|
||||
if (!node) {
|
||||
throw std::runtime_error{"Failed to lock node"};
|
||||
}
|
||||
|
||||
declare_parameter_if_not_declared(
|
||||
node,
|
||||
dwb_plugin_name_ + "." + name_ + ".penalty", rclcpp::ParameterValue(1.0));
|
||||
declare_parameter_if_not_declared(
|
||||
node,
|
||||
dwb_plugin_name_ + "." + name_ + ".strafe_x", rclcpp::ParameterValue(0.1));
|
||||
declare_parameter_if_not_declared(
|
||||
node, dwb_plugin_name_ + "." + name_ + ".strafe_theta",
|
||||
rclcpp::ParameterValue(0.2));
|
||||
declare_parameter_if_not_declared(
|
||||
node, dwb_plugin_name_ + "." + name_ + ".theta_scale",
|
||||
rclcpp::ParameterValue(10.0));
|
||||
|
||||
node->get_parameter(dwb_plugin_name_ + "." + name_ + ".penalty", penalty_);
|
||||
node->get_parameter(dwb_plugin_name_ + "." + name_ + ".strafe_x", strafe_x_);
|
||||
node->get_parameter(dwb_plugin_name_ + "." + name_ + ".strafe_theta", strafe_theta_);
|
||||
node->get_parameter(dwb_plugin_name_ + "." + name_ + ".theta_scale", theta_scale_);
|
||||
}
|
||||
|
||||
double PreferForwardCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj)
|
||||
{
|
||||
// backward motions bad on a robot without backward sensors
|
||||
if (traj.velocity.x < 0.0) {
|
||||
return penalty_;
|
||||
}
|
||||
// strafing motions also bad on such a robot
|
||||
if (traj.velocity.x < strafe_x_ && fabs(traj.velocity.theta) < strafe_theta_) {
|
||||
return penalty_;
|
||||
}
|
||||
|
||||
// the more we rotate, the less we progress forward
|
||||
return fabs(traj.velocity.theta) * theta_scale_;
|
||||
}
|
||||
|
||||
} // namespace dwb_critics
|
||||
@@ -0,0 +1,135 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "dwb_critics/rotate_to_goal.hpp"
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include "nav_2d_utils/parameters.hpp"
|
||||
#include "dwb_core/exceptions.hpp"
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
#include "dwb_core/trajectory_utils.hpp"
|
||||
#include "angles/angles.h"
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(dwb_critics::RotateToGoalCritic, dwb_core::TrajectoryCritic)
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
|
||||
inline double hypot_sq(double dx, double dy)
|
||||
{
|
||||
return dx * dx + dy * dy;
|
||||
}
|
||||
|
||||
void RotateToGoalCritic::onInit()
|
||||
{
|
||||
auto node = node_.lock();
|
||||
if (!node) {
|
||||
throw std::runtime_error{"Failed to lock node"};
|
||||
}
|
||||
|
||||
xy_goal_tolerance_ = nav_2d_utils::searchAndGetParam(
|
||||
node,
|
||||
dwb_plugin_name_ + ".xy_goal_tolerance", 0.25);
|
||||
xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
|
||||
double stopped_xy_velocity = nav_2d_utils::searchAndGetParam(
|
||||
node,
|
||||
dwb_plugin_name_ + ".trans_stopped_velocity", 0.25);
|
||||
stopped_xy_velocity_sq_ = stopped_xy_velocity * stopped_xy_velocity;
|
||||
slowing_factor_ = nav_2d_utils::searchAndGetParam(
|
||||
node,
|
||||
dwb_plugin_name_ + "." + name_ + ".slowing_factor", 5.0);
|
||||
lookahead_time_ = nav_2d_utils::searchAndGetParam(
|
||||
node,
|
||||
dwb_plugin_name_ + "." + name_ + ".lookahead_time", -1.0);
|
||||
reset();
|
||||
}
|
||||
|
||||
void RotateToGoalCritic::reset()
|
||||
{
|
||||
in_window_ = false;
|
||||
rotating_ = false;
|
||||
}
|
||||
|
||||
bool RotateToGoalCritic::prepare(
|
||||
const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
|
||||
const geometry_msgs::msg::Pose2D & goal,
|
||||
const nav_2d_msgs::msg::Path2D &)
|
||||
{
|
||||
double dxy_sq = hypot_sq(pose.x - goal.x, pose.y - goal.y);
|
||||
in_window_ = in_window_ || dxy_sq <= xy_goal_tolerance_sq_;
|
||||
current_xy_speed_sq_ = hypot_sq(vel.x, vel.y);
|
||||
rotating_ = rotating_ || (in_window_ && current_xy_speed_sq_ <= stopped_xy_velocity_sq_);
|
||||
goal_yaw_ = goal.theta;
|
||||
return true;
|
||||
}
|
||||
|
||||
double RotateToGoalCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj)
|
||||
{
|
||||
// If we're not sufficiently close to the goal, we don't care what the twist is
|
||||
if (!in_window_) {
|
||||
return 0.0;
|
||||
} else if (!rotating_) {
|
||||
double speed_sq = hypot_sq(traj.velocity.x, traj.velocity.y);
|
||||
if (speed_sq >= current_xy_speed_sq_) {
|
||||
throw dwb_core::IllegalTrajectoryException(name_, "Not slowing down near goal.");
|
||||
}
|
||||
return speed_sq * slowing_factor_ + scoreRotation(traj);
|
||||
}
|
||||
|
||||
// If we're sufficiently close to the goal, any transforming velocity is invalid
|
||||
if (fabs(traj.velocity.x) > 0 || fabs(traj.velocity.y) > 0) {
|
||||
throw dwb_core::
|
||||
IllegalTrajectoryException(name_, "Nonrotation command near goal.");
|
||||
}
|
||||
|
||||
return scoreRotation(traj);
|
||||
}
|
||||
|
||||
double RotateToGoalCritic::scoreRotation(const dwb_msgs::msg::Trajectory2D & traj)
|
||||
{
|
||||
if (traj.poses.empty()) {
|
||||
throw dwb_core::IllegalTrajectoryException(name_, "Empty trajectory.");
|
||||
}
|
||||
|
||||
double end_yaw;
|
||||
if (lookahead_time_ >= 0.0) {
|
||||
geometry_msgs::msg::Pose2D eval_pose = dwb_core::projectPose(traj, lookahead_time_);
|
||||
end_yaw = eval_pose.theta;
|
||||
} else {
|
||||
end_yaw = traj.poses.back().theta;
|
||||
}
|
||||
return fabs(angles::shortest_angular_distance(end_yaw, goal_yaw_));
|
||||
}
|
||||
|
||||
} // namespace dwb_critics
|
||||
@@ -0,0 +1,56 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "dwb_critics/twirling.hpp"
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
|
||||
namespace dwb_critics
|
||||
{
|
||||
void TwirlingCritic::onInit()
|
||||
{
|
||||
auto node = node_.lock();
|
||||
if (!node) {
|
||||
throw std::runtime_error{"Failed to lock node"};
|
||||
}
|
||||
// Scale is set to 0 by default, so if it was not set otherwise, set to 0
|
||||
node->get_parameter(dwb_plugin_name_ + "." + name_ + ".scale", scale_);
|
||||
}
|
||||
|
||||
double TwirlingCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj)
|
||||
{
|
||||
return fabs(traj.velocity.theta); // add cost for making the robot spin
|
||||
}
|
||||
} // namespace dwb_critics
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(dwb_critics::TwirlingCritic, dwb_core::TrajectoryCritic)
|
||||
Reference in New Issue
Block a user