HiepLM/mir_amr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit"

Browse Source
This commit is contained in:
HiepLM
2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
navigations/dwb_critics/src/alignment_util.cpp Executable file
View File

@@ -0,0 +1,47 @@
/*
* 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.h>
namespace dwb_critics
{
geometry_msgs::Pose2D getForwardPose(const geometry_msgs::Pose2D& pose, double distance)
{
geometry_msgs::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

102
navigations/dwb_critics/src/base_obstacle.cpp Executable file
View File

@@ -0,0 +1,102 @@
/*
* 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/base_obstacle.h>
#include <nav_grid/coordinate_conversion.h>
#include <nav_core2/exceptions.h>
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(dwb_critics::BaseObstacleCritic, dwb_local_planner::TrajectoryCritic)
namespace dwb_critics
{
void BaseObstacleCritic::onInit()
{
critic_nh_.param("sum_scores", sum_scores_, false);
}
double BaseObstacleCritic::scoreTrajectory(const dwb_msgs::Trajectory2D& traj)
{
const nav_core2::Costmap& costmap = *costmap_;
double score = 0.0;
for (unsigned int i = 0; i < traj.poses.size(); ++i)
{
double pose_score = scorePose(costmap, 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 nav_core2::Costmap& costmap, const geometry_msgs::Pose2D& pose)
{
unsigned int cell_x, cell_y;
if (!worldToGridBounded(costmap.getInfo(), pose.x, pose.y, cell_x, cell_y))
throw nav_core2::IllegalTrajectoryException(name_, "Trajectory Goes Off Grid.");
unsigned char cost = costmap(cell_x, cell_y);
if (!isValidCost(cost))
throw nav_core2::IllegalTrajectoryException(name_, "Trajectory Hits Obstacle.");
return cost;
}
bool BaseObstacleCritic::isValidCost(const unsigned char cost)
{
return cost != costmap_->LETHAL_OBSTACLE &&
cost != costmap_->INSCRIBED_INFLATED_OBSTACLE &&
cost != costmap_->NO_INFORMATION;
}
void BaseObstacleCritic::addCriticVisualization(sensor_msgs::PointCloud& pc)
{
sensor_msgs::ChannelFloat32 grid_scores;
grid_scores.name = name_;
const nav_core2::Costmap& costmap = *costmap_;
unsigned int size_x = costmap.getWidth();
unsigned int size_y = costmap.getHeight();
grid_scores.values.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.values[i] = costmap(cx, cy);
i++;
}
}
pc.channels.push_back(grid_scores);
}
} // namespace dwb_critics

76
navigations/dwb_critics/src/goal_align.cpp Executable file
View File

@@ -0,0 +1,76 @@
/*
* 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.h>
#include <dwb_critics/alignment_util.h>
#include <pluginlib/class_list_macros.h>
#include <nav_2d_utils/parameters.h>
#include <vector>
#include <string>
namespace dwb_critics
{
void GoalAlignCritic::onInit()
{
GoalDistCritic::onInit();
stop_on_failure_ = false;
forward_point_distance_ = nav_2d_utils::searchAndGetParam(critic_nh_, "forward_point_distance", 0.325);
}
bool GoalAlignCritic::prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel,
const geometry_msgs::Pose2D& goal,
const nav_2d_msgs::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::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::Pose2D& pose)
{
return GoalDistCritic::scorePose(getForwardPose(pose, forward_point_distance_));
}
} // namespace dwb_critics
PLUGINLIB_EXPORT_CLASS(dwb_critics::GoalAlignCritic, dwb_local_planner::TrajectoryCritic)

104
navigations/dwb_critics/src/goal_dist.cpp Executable file
View File

@@ -0,0 +1,104 @@
/*
* 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.h>
#include <nav_grid/coordinate_conversion.h>
#include <pluginlib/class_list_macros.h>
#include <nav_2d_utils/path_ops.h>
#include <vector>
namespace dwb_critics
{
bool GoalDistCritic::prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel,
const geometry_msgs::Pose2D& goal,
const nav_2d_msgs::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
cell_values_.setValue(local_goal_x, local_goal_y, 0.0);
queue_->enqueueCell(local_goal_x, local_goal_y);
propogateManhattanDistances();
return true;
}
bool GoalDistCritic::getLastPoseOnCostmap(const nav_2d_msgs::Path2D& global_plan, unsigned int& x, unsigned int& y)
{
const nav_core2::Costmap& costmap = *costmap_;
const nav_grid::NavGridInfo& info = costmap.getInfo();
nav_2d_msgs::Path2D adjusted_global_plan = nav_2d_utils::adjustPlanResolution(global_plan, info.resolution);
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 (worldToGridBounded(info, g_x, g_y, map_x, map_y) && costmap(map_x, map_y) != costmap.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
{
ROS_ERROR_NAMED("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_local_planner::TrajectoryCritic)

202
navigations/dwb_critics/src/map_grid.cpp Executable file
View File

@@ -0,0 +1,202 @@
/*
* 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.h>
#include <nav_grid/coordinate_conversion.h>
#include <nav_core2/exceptions.h>
#include <algorithm>
#include <memory>
#include <string>
namespace dwb_critics
{
// Customization of the CostmapQueue validCellToQueue method
bool MapGridCritic::MapGridQueue::validCellToQueue(const costmap_queue::CellData& cell)
{
unsigned char cost = costmap_(cell.x_, cell.y_);
if (cost == costmap_.LETHAL_OBSTACLE ||
cost == costmap_.INSCRIBED_INFLATED_OBSTACLE ||
cost == costmap_.NO_INFORMATION)
{
parent_.setAsObstacle(cell.x_, cell.y_);
return false;
}
return true;
}
void MapGridCritic::onInit()
{
queue_ = std::make_shared<MapGridQueue>(*costmap_, *this);
// Always set to true, but can be overriden by subclasses
stop_on_failure_ = true;
std::string aggro_str;
critic_nh_.param("aggregation_type", aggro_str, std::string("last"));
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
{
ROS_ERROR_NAMED("MapGridCritic", "aggregation_type parameter \"%s\" invalid. Using Last.", aggro_str.c_str());
aggregationType_ = ScoreAggregationType::Last;
}
}
void MapGridCritic::setAsObstacle(unsigned int x, unsigned int y)
{
cell_values_.setValue(x, y, obstacle_score_);
}
void MapGridCritic::reset()
{
queue_->reset();
if (costmap_->getInfo() == cell_values_.getInfo())
{
cell_values_.reset();
}
else
{
obstacle_score_ = static_cast<double>(costmap_->getWidth() * costmap_->getHeight());
unreachable_score_ = obstacle_score_ + 1.0;
cell_values_.setDefaultValue(unreachable_score_);
cell_values_.setInfo(costmap_->getInfo());
}
}
void MapGridCritic::propogateManhattanDistances()
{
while (!queue_->isEmpty())
{
costmap_queue::CellData cell = queue_->getNextCell();
cell_values_.setValue(cell.x_, cell.y_,
std::abs(static_cast<int>(cell.src_x_) - static_cast<int>(cell.x_))
+ std::abs(static_cast<int>(cell.src_y_) - static_cast<int>(cell.y_)));
}
}
double MapGridCritic::scoreTrajectory(const dwb_msgs::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 nav_core2::IllegalTrajectoryException(name_, "Trajectory Hits Obstacle.");
}
else if (grid_dist == unreachable_score_)
{
throw nav_core2::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::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 (!worldToGridBounded(costmap_->getInfo(), pose.x, pose.y, cell_x, cell_y))
{
throw nav_core2::IllegalTrajectoryException(name_, "Trajectory Goes Off Grid.");
}
return getScore(cell_x, cell_y);
}
void MapGridCritic::addCriticVisualization(sensor_msgs::PointCloud& pc)
{
sensor_msgs::ChannelFloat32 grid_scores;
grid_scores.name = name_;
const nav_core2::Costmap& costmap = *costmap_;
unsigned int size_x = costmap.getWidth();
unsigned int size_y = costmap.getHeight();
grid_scores.values.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.values[i] = getScore(cx, cy);
i++;
}
}
pc.channels.push_back(grid_scores);
}
} // namespace dwb_critics

362
navigations/dwb_critics/src/metratronik_technik.cpp Executable file
View File

@@ -0,0 +1,362 @@
/*
* 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/metratronik_technik.h>
#include <nav_2d_utils/parameters.h>
#include <nav_2d_utils/path_ops.h>
#include <nav_grid/coordinate_conversion.h>
#include <nav_core2/exceptions.h>
#include <pluginlib/class_list_macros.h>
const double EPSILON = 1E-5;
PLUGINLIB_EXPORT_CLASS(dwb_critics::MetratronikTechnikCritic, dwb_local_planner::TrajectoryCritic)
namespace dwb_critics
{
inline double hypot_sq(double dx, double dy)
{
return dx * dx + dy * dy;
}
void MetratronikTechnikCritic::onInit()
{
xy_local_goal_tolerance_ = nav_2d_utils::searchAndGetParam(critic_nh_, "xy_goal_tolerance", 0.25);
xy_local_goal_tolerance_sq_ = xy_local_goal_tolerance_ * xy_local_goal_tolerance_;
stopped_velocity_ = nav_2d_utils::searchAndGetParam(critic_nh_, "trans_stopped_velocity", 0.25);
critic_nh_.param("sq_window", sq_window_, 1.0);
critic_nh_.param("slowing_factor", slowing_factor_, 5.0);
critic_nh_.param("angle_threshold", angle_threshold_, 0.436332313);
critic_nh_.param("Lm", Lm_, 0.436332313);
reset();
}
void MetratronikTechnikCritic::reset()
{
in_window_ = false;
running_ = false;
}
bool MetratronikTechnikCritic::prepare(const geometry_msgs::Pose2D &pose, const nav_2d_msgs::Twist2D &vel,
const geometry_msgs::Pose2D &goal, const nav_2d_msgs::Path2D &global_plan)
{
if (!getPosition(pose, global_plan, current_position_))
{
return false;
}
// ROS_INFO("current_position_");
dxy_sq_ = hypot_sq(pose.x - goal.x, pose.y - goal.y);
in_window_ = in_window_ || dxy_sq_ <= sq_window_;
current_speed_ = vel.x;
running_ = running_ || (in_window_ && vel.x > 0.0 && vel.x <= stopped_velocity_);
return true;
}
double MetratronikTechnikCritic::scoreTrajectory(const dwb_msgs::Trajectory2D &traj)
{
if (!in_window_)
{
return 0.0;
}
else if (!running_)
{
return fabs(slowing_factor_ + scoreMetratronikTechnik(traj));
}
// If we're sufficiently close to the goal, any transforming velocity is invalid
if (fabs(traj.velocity.x) > stopped_velocity_ || fabs(traj.velocity.y) > stopped_velocity_)
{
return fabs(traj.velocity.x + slowing_factor_ + scoreMetratronikTechnik(traj));
}
return 0.01 * scoreMetratronikTechnik(traj);
}
double MetratronikTechnikCritic::scoreMetratronikTechnik(const dwb_msgs::Trajectory2D &traj)
{
if (traj.poses.empty())
{
throw nav_core2::IllegalTrajectoryException(name_, "Empty trajectory.");
}
double k_reduce_v = cos(((M_PI / 2) / Lm_) * current_position_);
double result_linear_x = traj.velocity.x * k_reduce_v;
double k_reduce_w_v = cos(((M_PI / 2) / stopped_velocity_) * result_linear_x);
double result_angular_z = atan(current_position_ / Lm_) * fabs(k_reduce_w_v);
double running_time = traj.time_offsets.back().toSec();
double score_linear_x = fabs(traj.velocity.x * running_time - sqrt(dxy_sq_));
// ROS_INFO("%f %f %f %f", running_time,fabs(traj.velocity.x*running_time), sqrt(dxy_sq_), score_linear_x);
double score_angular_z = fabs(traj.velocity.theta - result_angular_z);
// ROS_INFO("%f %f %f %f %f %f %f", result_linear_x, traj.velocity.x, result_angular_z, traj.velocity.theta, score_linear_x, score_angular_z, score_angular_z + score_linear_x);
return score_angular_z + score_linear_x;
}
bool MetratronikTechnikCritic::getPosition(const geometry_msgs::Pose2D &robot_pose, const nav_2d_msgs::Path2D &global_plan, double &position)
{
const nav_core2::Costmap &costmap = *costmap_;
const nav_grid::NavGridInfo &info = costmap.getInfo();
if (global_plan.poses.empty())
{
ROS_ERROR_NAMED("MetratronikTechnikCritic", "The global plan was empty.");
return false;
}
// Chuyển đổi dữ liệu kế hoạch đường đi 'nav_2d_msgs::Path2D' sang 'std::vector<geometry_msgs::Pose2D>'
std::vector<geometry_msgs::Pose2D> plan = nav_2d_utils::adjustPlanResolution(global_plan, info.resolution).poses;
// unsigned int start_index = 0;
// double distance_to_start = std::numeric_limits<double>::infinity();
// bool started_path = false;
// for (unsigned int i = 0; i < plan.size(); i++)
// {
// double g_x = plan[i].x;
// double g_y = plan[i].y;
// unsigned int map_x, map_y;
// if (worldToGridBounded(info, g_x, g_y, map_x, map_y) // chuyển tọa độ từ đơn vị mét sang pixel
// && costmap(map_x, map_y) != nav_core2::Costmap::NO_INFORMATION)
// { // chi phí phải khác -1
// // Still on the costmap. Continue.
// double distance = nav_2d_utils::poseDistance(plan[i], robot_pose); // Tính khoảng cách từ vị trí của robot đến pose đang xét
// if (distance_to_start > distance)
// {
// start_index = i;
// distance_to_start = distance;
// started_path = true;
// }
// else
// {
// // Plan is going away from the robot again. It's possible that it comes back and we would find a pose that's
// // even closer to the robot, but then we would skip over parts of the plan.
// break;
// }
// }
// else if (started_path)
// {
// // Off the costmap after being on the costmap.
// break;
// }
// // else, we have not yet found a point on the costmap, so we just continue
// }
// // Nếu khồng tìm được điểm gần với robot nhất thì trả về false
// if (!started_path)
// {
// ROS_ERROR_NAMED("MetratronikTechnikCritic", "None of the points of the global plan were in the local costmap.");
// return false;
// }
// // current_index_ = start_index;
// // find the "last valid pose", i.e. the last pose on the plan after the start pose that is still on the local map
// // Tìm điểm pose cuôi cùng sau điểm pose bắt đầu và có tồn tại trong local map
// unsigned int last_valid_index = start_index;
// for (unsigned int i = start_index + 1; i < plan.size(); i++)
// {
// double g_x = plan[i].x;
// double g_y = plan[i].y;
// unsigned int map_x, map_y;
// if (worldToGridBounded(info, g_x, g_y, map_x, map_y) && costmap(map_x, map_y) != nav_core2::Costmap::NO_INFORMATION)
// {
// // Still on the costmap. Continue.
// last_valid_index = i;
// }
// else
// {
// // Off the costmap after being on the costmap.
// break;
// }
// }
// // find the "goal pose" by walking along the plan as long as each step leads far enough away from the starting point,
// // i.e. is within angle_threshold_ of the starting direction.
// // Tìm điểm mục tiêu chạy từ điểm start pose đến last valid pose dọc thep plan
// unsigned int goal_index = start_index;
// while (goal_index < last_valid_index && critic_nh_.ok())
// {
// // Tìm kiếm vị trí điểm mục tiêu phù hợp
// goal_index = getGoalIndex(robot_pose, plan, start_index, last_valid_index);
// // check if goal already reached
// bool goal_already_reached = false;
// for (auto &reached_intermediate_goal : reached_intermediate_goals_)
// {
// double distance = nav_2d_utils::poseDistance(reached_intermediate_goal, plan[goal_index]);
// if (distance < xy_local_goal_tolerance_)
// {
// goal_already_reached = true;
// // choose a new start_index by walking along the plan until we're outside xy_local_goal_tolerance_ and try again
// // (start_index is now > goal_index)
// for (start_index = goal_index; start_index <= last_valid_index; ++start_index)
// {
// distance = nav_2d_utils::poseDistance(reached_intermediate_goal, plan[start_index]);
// if (distance >= xy_local_goal_tolerance_)
// break;
// }
// break;
// }
// }
// if (!goal_already_reached)
// {
// // new goal not in reached_intermediate_goals_
// double distance = nav_2d_utils::poseDistance(plan[goal_index], robot_pose);
// if (distance < xy_local_goal_tolerance_)
// {
// // the robot has currently reached the goal
// reached_intermediate_goals_.push_back(plan[goal_index]);
// ROS_DEBUG_NAMED("MetratronikTechnikCritic", "Number of reached_intermediate goals: %zu", reached_intermediate_goals_.size());
// }
// else
// {
// // we've found a new goal!
// break;
// }
// }
// }
// if (start_index >= goal_index)
// return false;
lineEquation leq;
getLine(plan.front(), plan.back(), leq);
position = calculateError(leq, Lm_, robot_pose, 0).error_line;
return true;
}
unsigned int MetratronikTechnikCritic::getGoalIndex(const geometry_msgs::Pose2D &robot_pose,
const std::vector<geometry_msgs::Pose2D> &plan,
unsigned int start_index,
unsigned int last_valid_index) const
{
if (start_index >= last_valid_index)
return last_valid_index;
unsigned int goal_index = start_index;
for (unsigned int i = start_index + 1; i <= last_valid_index; ++i)
{
const double current_direction_x = plan[i].x - plan[i - 1].x;
const double current_direction_y = plan[i].y - plan[i - 1].y;
if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9)
{
double current_angle = atan2(current_direction_y, current_direction_x);
if (fabs(remainder(robot_pose.theta - current_angle, 2 * M_PI)) > angle_threshold_)
break;
goal_index = i;
}
}
if (nav_2d_utils::poseDistance(plan[goal_index], plan[last_valid_index]) <= xy_local_goal_tolerance_)
goal_index = last_valid_index;
return goal_index;
}
bool MetratronikTechnikCritic::getLine(geometry_msgs::Pose2D start_point, geometry_msgs::Pose2D end_point, lineEquation &leq)
{
/**
* Phương trình đường thẳng có dạng:
* (y - yi)/(yj - yi) = (x - xi)/(xj - xi)
*/
leq.Gi = start_point;
leq.Gj = end_point;
leq.a = (leq.Gj.y - leq.Gi.y);
leq.b = (leq.Gi.x - leq.Gj.x);
leq.c = (leq.Gj.x * leq.Gi.y - leq.Gi.x * leq.Gj.y);
return leq.a != 0 || leq.b != 0;
}
presentPosition MetratronikTechnikCritic::calculateError(const lineEquation &leq, const float Lm, const geometry_msgs::Pose2D robot_pose, float pha)
{
presentPosition present_pose;
float Yaw_rad = robot_pose.theta;
float Yaw = ((Yaw_rad * 180) / M_PI);
Yaw_rad = (Yaw - pha) * M_PI / 180;
/*
* Tính tọa độ điểm R
*/
float G_xr = float(robot_pose.x);
float G_yr = float(robot_pose.y);
/*
* Tính tọa độ điểm H
*/
present_pose.Gxh = G_xr + Lm * cos(Yaw_rad);
present_pose.Gyh = G_yr + Lm * sin(Yaw_rad);
/**
* Phương trình đường thẳng (rh) có dạng:
* (y - yr)/(yh - yr) = (x - xr)/(xh - xr)
*/
float A_rh = (present_pose.Gyh - G_yr);
float B_rh = (G_xr - present_pose.Gxh);
float C_rh = (present_pose.Gxh * G_yr - G_xr * present_pose.Gyh);
/*
* Phương trình đường thẳng vuông góc (y - yr)/(yh - yr) = (x - xr)/(xh - xr)
* và đi qua Gh có dạng:
* -B_rh*(x - Gxh) + Arh*(y - Gyh) = 0
*/
float A_he = -B_rh;
float B_he = A_rh;
float C_he = (-B_rh) * (-present_pose.Gxh) + A_rh * (-present_pose.Gyh);
/*
* Đường thẳng (he) giao với đường thẳng (ij) tại e
* vậy G_xe và G_ye là nghiệm của hệ phương trình 2 ẩn của 2 đường thẳng trên:
*/
float D = A_he * leq.b - leq.a * B_he;
float Dx = (-C_he) * leq.b - (-leq.c) * B_he;
float Dy = A_he * (-leq.c) - leq.a * (-C_he);
/*
* Tính tọa độ điểm E
*/
if (D == 0)
{
if (Dx + Dy == 0)
{
/*
* Vì hai đường thẳng trùng nhau nên sai số giữa robot và đường đi là bằng 0
*/
present_pose.error_line = 0;
}
else
throw NoSolution();
}
else
{
float G_xe = Dx / D;
float G_ye = Dy / D;
/*
* Tính khoảng cách từ điểm E đến đường thẳng (RH)
*/
present_pose.error_line = -(A_rh * G_xe + B_rh * G_ye + C_rh) / (sqrt(pow(A_rh, 2) + pow(B_rh, 2)));
}
present_pose.distanceToEnd = sqrt(pow((leq.Gj.x - G_xr), 2) + pow((leq.Gj.y - G_yr), 2));
present_pose.distanceToStart = sqrt(pow((leq.Gi.x - G_xr), 2) + pow((leq.Gi.y - G_yr), 2));
return present_pose;
}
} /* namespace dwb_critics */

99
navigations/dwb_critics/src/obstacle_footprint.cpp Executable file
View File

@@ -0,0 +1,99 @@
/*
* 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.h>
#include <nav_grid/coordinate_conversion.h>
#include <nav_grid_iterators/polygon_outline.h>
#include <nav_2d_utils/polygons.h>
#include <nav_2d_utils/footprint.h>
#include <nav_core2/exceptions.h>
#include <pluginlib/class_list_macros.h>
#include <algorithm>
#include <vector>
PLUGINLIB_EXPORT_CLASS(dwb_critics::ObstacleFootprintCritic, dwb_local_planner::TrajectoryCritic)
namespace dwb_critics
{
void ObstacleFootprintCritic::onInit()
{
BaseObstacleCritic::onInit();
footprint_spec_ = nav_2d_utils::footprintFromParams(critic_nh_);
}
bool ObstacleFootprintCritic::prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel,
const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan)
{
if (footprint_spec_.points.size() == 0)
{
ROS_ERROR_NAMED("ObstacleFootprintCritic", "Footprint spec is empty, maybe missing call to setFootprint?");
return false;
}
return true;
}
double ObstacleFootprintCritic::scorePose(const nav_core2::Costmap& costmap, const geometry_msgs::Pose2D& pose)
{
unsigned int cell_x, cell_y;
if (!worldToGridBounded(costmap.getInfo(), pose.x, pose.y, cell_x, cell_y))
throw nav_core2::IllegalTrajectoryException(name_, "Trajectory Goes Off Grid.");
return scorePose(costmap, pose, nav_2d_utils::movePolygonToPose(footprint_spec_, pose));
}
double ObstacleFootprintCritic::scorePose(const nav_core2::Costmap& costmap, const geometry_msgs::Pose2D& pose,
const nav_2d_msgs::Polygon2D& footprint)
{
unsigned char footprint_cost = 0;
nav_grid::NavGridInfo info = costmap.getInfo();
for (nav_grid::Index index : nav_grid_iterators::PolygonOutline(&info, footprint))
{
unsigned char cost = costmap(index.x, index.y);
// if the cell is in an obstacle the path is invalid or unknown
if (cost == costmap.LETHAL_OBSTACLE)
{
throw nav_core2::IllegalTrajectoryException(name_, "Trajectory Hits Obstacle.");
}
else if (cost == costmap.NO_INFORMATION)
{
throw nav_core2::IllegalTrajectoryException(name_, "Trajectory Hits Unknown Region.");
}
footprint_cost = std::max(cost, footprint_cost);
}
// if all line costs are legal... then we can return that the footprint is legal
return footprint_cost;
}
} // namespace dwb_critics

223
navigations/dwb_critics/src/oscillation.cpp Executable file
View File

@@ -0,0 +1,223 @@
/*
* 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 <nav_2d_utils/parameters.h>
#include <dwb_critics/oscillation.h>
#include <nav_core2/exceptions.h>
#include <pluginlib/class_list_macros.h>
#include <cmath>
#include <string>
#include <vector>
PLUGINLIB_EXPORT_CLASS(dwb_critics::OscillationCritic, dwb_local_planner::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()
{
oscillation_reset_dist_ = nav_2d_utils::searchAndGetParam(critic_nh_, "oscillation_reset_dist", 0.05);
oscillation_reset_dist_sq_ = oscillation_reset_dist_ * oscillation_reset_dist_;
oscillation_reset_angle_ = nav_2d_utils::searchAndGetParam(critic_nh_, "oscillation_reset_angle", 0.2);
oscillation_reset_time_ = nav_2d_utils::searchAndGetParam(critic_nh_, "oscillation_reset_time", -1.0);
/**
* 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
*/
std::string resolved_name;
if (critic_nh_.hasParam("x_only_threshold"))
{
critic_nh_.getParam("x_only_threshold", x_only_threshold_);
}
else if (critic_nh_.searchParam("min_speed_xy", resolved_name))
{
critic_nh_.getParam(resolved_name, x_only_threshold_);
}
else if (critic_nh_.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.");
critic_nh_.getParam(resolved_name, x_only_threshold_);
}
else
{
x_only_threshold_ = 0.05;
}
reset();
}
bool OscillationCritic::prepare(const geometry_msgs::Pose2D& pose,
const nav_2d_msgs::Twist2D& vel,
const geometry_msgs::Pose2D& goal,
const nav_2d_msgs::Path2D& global_plan)
{
pose_ = pose;
return true;
}
void OscillationCritic::debrief(const nav_2d_msgs::Twist2D& cmd_vel)
{
if (setOscillationFlags(cmd_vel))
{
prev_stationary_pose_ = pose_;
prev_reset_time_ = ros::Time::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_ >= 0.0)
{
double t_diff = (ros::Time::now() - prev_reset_time_).toSec();
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::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::Trajectory2D& traj)
{
if (x_trend_.isOscillating(traj.velocity.x) ||
y_trend_.isOscillating(traj.velocity.y) ||
theta_trend_.isOscillating(traj.velocity.theta))
{
throw nav_core2::IllegalTrajectoryException(name_, "Trajectory is oscillating.");
}
return 0.0;
}
} // namespace dwb_critics

87
navigations/dwb_critics/src/path_align.cpp Executable file
View File

@@ -0,0 +1,87 @@
/*
* 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.h>
#include <dwb_critics/alignment_util.h>
#include <pluginlib/class_list_macros.h>
#include <nav_2d_utils/parameters.h>
#include <vector>
#include <string>
namespace dwb_critics
{
void PathAlignCritic::onInit()
{
PathDistCritic::onInit();
stop_on_failure_ = false;
forward_point_distance_ = nav_2d_utils::searchAndGetParam(critic_nh_, "forward_point_distance", 0.325);
}
bool PathAlignCritic::prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel,
const geometry_msgs::Pose2D& goal,
const nav_2d_msgs::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::Pose2D& pose)
{
return PathDistCritic::scorePose(getForwardPose(pose, forward_point_distance_));
}
} // namespace dwb_critics
PLUGINLIB_EXPORT_CLASS(dwb_critics::PathAlignCritic, dwb_local_planner::TrajectoryCritic)

92
navigations/dwb_critics/src/path_dist.cpp Executable file
View File

@@ -0,0 +1,92 @@
/*
* 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.h>
#include <nav_grid/coordinate_conversion.h>
#include <pluginlib/class_list_macros.h>
#include <nav_2d_utils/path_ops.h>
#include <vector>
namespace dwb_critics
{
bool PathDistCritic::prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel,
const geometry_msgs::Pose2D& goal,
const nav_2d_msgs::Path2D& global_plan)
{
reset();
const nav_core2::Costmap& costmap = *costmap_;
const nav_grid::NavGridInfo& info = costmap.getInfo();
bool started_path = false;
nav_2d_msgs::Path2D adjusted_global_plan = nav_2d_utils::adjustPlanResolution(global_plan, info.resolution);
if (adjusted_global_plan.poses.size() != global_plan.poses.size())
{
ROS_DEBUG_NAMED("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 (worldToGridBounded(info, g_x, g_y, map_x, map_y) && costmap(map_x, map_y) != costmap.NO_INFORMATION)
{
cell_values_.setValue(map_x, map_y, 0.0);
queue_->enqueueCell(map_x, map_y);
started_path = true;
}
else if (started_path)
{
break;
}
}
if (!started_path)
{
ROS_ERROR_NAMED("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_local_planner::TrajectoryCritic)

69
navigations/dwb_critics/src/prefer_forward.cpp Executable file
View File

@@ -0,0 +1,69 @@
/*
* 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.h>
#include <math.h>
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(dwb_critics::PreferForwardCritic, dwb_local_planner::TrajectoryCritic)
namespace dwb_critics
{
void PreferForwardCritic::onInit()
{
critic_nh_.param("penalty", penalty_, 1.0);
critic_nh_.param("strafe_x", strafe_x_, 0.1);
critic_nh_.param("strafe_theta", strafe_theta_, 0.2);
critic_nh_.param("theta_scale", theta_scale_, 10.0);
}
double PreferForwardCritic::scoreTrajectory(const dwb_msgs::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 */

130
navigations/dwb_critics/src/rotate_to_goal.cpp Executable file
View File

@@ -0,0 +1,130 @@
/*
* 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.h>
#include <dwb_local_planner/trajectory_utils.h>
#include <nav_2d_utils/parameters.h>
#include <nav_core2/exceptions.h>
#include <pluginlib/class_list_macros.h>
#include <angles/angles.h>
#include <string>
#include <vector>
const double EPSILON = 1E-5;
PLUGINLIB_EXPORT_CLASS(dwb_critics::RotateToGoalCritic, dwb_local_planner::TrajectoryCritic)
namespace dwb_critics
{
inline double hypot_sq(double dx, double dy)
{
return dx * dx + dy * dy;
}
void RotateToGoalCritic::onInit()
{
xy_goal_tolerance_ = nav_2d_utils::searchAndGetParam(critic_nh_, "xy_goal_tolerance", 0.25);
xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
double stopped_xy_velocity = nav_2d_utils::searchAndGetParam(critic_nh_, "trans_stopped_velocity", 0.25);
stopped_xy_velocity_sq_ = stopped_xy_velocity * stopped_xy_velocity;
critic_nh_.param("slowing_factor", slowing_factor_, 5.0);
critic_nh_.param("lookahead_time", lookahead_time_, -1.0);
reset();
}
void RotateToGoalCritic::reset()
{
in_window_ = false;
rotating_ = false;
}
bool RotateToGoalCritic::prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel,
const geometry_msgs::Pose2D& goal,
const nav_2d_msgs::Path2D& global_plan)
{
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::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 nav_core2::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) > EPSILON || fabs(traj.velocity.y) > EPSILON)
{
throw nav_core2::IllegalTrajectoryException(name_, "Nonrotation command near goal.");
}
return scoreRotation(traj);
}
double RotateToGoalCritic::scoreRotation(const dwb_msgs::Trajectory2D& traj)
{
if (traj.poses.empty())
{
throw nav_core2::IllegalTrajectoryException(name_, "Empty trajectory.");
}
double end_yaw;
if (lookahead_time_ >= 0.0)
{
geometry_msgs::Pose2D eval_pose = dwb_local_planner::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 */

55
navigations/dwb_critics/src/twirling.cpp Executable file
View File

@@ -0,0 +1,55 @@
/*
* 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.h>
#include <pluginlib/class_list_macros.h>
namespace dwb_critics
{
void TwirlingCritic::onInit()
{
// Scale is set to 0 by default, so if it was not set otherwise, set to 0
if (!critic_nh_.hasParam("scale"))
{
scale_ = 0.0;
}
}
double TwirlingCritic::scoreTrajectory(const dwb_msgs::Trajectory2D& traj)
{
return fabs(traj.velocity.theta); // add cost for making the robot spin
}
} // namespace dwb_critics
PLUGINLIB_EXPORT_CLASS(dwb_critics::TwirlingCritic, dwb_local_planner::TrajectoryCritic)