X-lanni
127 lines
4.6 KiB
C++
127 lines
4.6 KiB
C++
// Copyright (c) 2022 Samsung Research America
|
|
//
|
|
// Licensed under the Apache License, Version 2.0 (the "License");
|
|
// you may not use this file except in compliance with the License.
|
|
// You may obtain a copy of the License at
|
|
//
|
|
// http://www.apache.org/licenses/LICENSE-2.0
|
|
//
|
|
// Unless required by applicable law or agreed to in writing, software
|
|
// distributed under the License is distributed on an "AS IS" BASIS,
|
|
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
// See the License for the specific language governing permissions and
|
|
// limitations under the License.
|
|
|
|
#include <algorithm>
|
|
#include <string>
|
|
#include <limits>
|
|
#include <memory>
|
|
#include <vector>
|
|
#include <utility>
|
|
|
|
#include "nav2_util/node_utils.hpp"
|
|
#include "nav2_util/geometry_utils.hpp"
|
|
#include "nav_2d_utils/tf_help.hpp"
|
|
#include "nav2_graceful_controller/path_handler.hpp"
|
|
#include "nav2_core/exceptions.hpp"
|
|
|
|
namespace nav2_graceful_controller
|
|
{
|
|
|
|
using nav2_util::geometry_utils::euclidean_distance;
|
|
|
|
PathHandler::PathHandler(
|
|
tf2::Duration transform_tolerance,
|
|
std::shared_ptr<tf2_ros::Buffer> tf,
|
|
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
|
|
: transform_tolerance_(transform_tolerance), tf_buffer_(tf), costmap_ros_(costmap_ros)
|
|
{
|
|
}
|
|
|
|
nav_msgs::msg::Path PathHandler::transformGlobalPlan(
|
|
const geometry_msgs::msg::PoseStamped & pose,
|
|
double max_robot_pose_search_dist)
|
|
{
|
|
// Check first if the plan is empty
|
|
if (global_plan_.poses.empty()) {
|
|
throw nav2_core::PlannerException("Received plan with zero length");
|
|
}
|
|
|
|
// Let's get the pose of the robot in the frame of the plan
|
|
geometry_msgs::msg::PoseStamped robot_pose;
|
|
if (!nav_2d_utils::transformPose(
|
|
tf_buffer_, global_plan_.header.frame_id, pose, robot_pose,
|
|
transform_tolerance_))
|
|
{
|
|
throw nav2_core::PlannerException("Unable to transform robot pose into global plan's frame");
|
|
}
|
|
|
|
// Find the first pose in the global plan that's further than max_robot_pose_search_dist
|
|
// from the robot using integrated distance
|
|
auto closest_pose_upper_bound =
|
|
nav2_util::geometry_utils::first_after_integrated_distance(
|
|
global_plan_.poses.begin(), global_plan_.poses.end(), max_robot_pose_search_dist);
|
|
|
|
// First find the closest pose on the path to the robot
|
|
// bounded by when the path turns around (if it does) so we don't get a pose from a later
|
|
// portion of the path
|
|
auto transformation_begin =
|
|
nav2_util::geometry_utils::min_by(
|
|
global_plan_.poses.begin(), closest_pose_upper_bound,
|
|
[&robot_pose](const geometry_msgs::msg::PoseStamped & ps) {
|
|
return euclidean_distance(robot_pose, ps);
|
|
});
|
|
|
|
// We'll discard points on the plan that are outside the local costmap
|
|
double dist_threshold = std::max(
|
|
costmap_ros_->getCostmap()->getSizeInMetersX(),
|
|
costmap_ros_->getCostmap()->getSizeInMetersY()) / 2.0;
|
|
auto transformation_end = std::find_if(
|
|
transformation_begin, global_plan_.poses.end(),
|
|
[&](const auto & global_plan_pose) {
|
|
return euclidean_distance(global_plan_pose, robot_pose) > dist_threshold;
|
|
});
|
|
|
|
// Lambda to transform a PoseStamped from global frame to local
|
|
auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) {
|
|
geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose;
|
|
stamped_pose.header.frame_id = global_plan_.header.frame_id;
|
|
stamped_pose.header.stamp = robot_pose.header.stamp;
|
|
stamped_pose.pose = global_plan_pose.pose;
|
|
if (!nav_2d_utils::transformPose(
|
|
tf_buffer_, costmap_ros_->getBaseFrameID(), stamped_pose,
|
|
transformed_pose, transform_tolerance_))
|
|
{
|
|
throw nav2_core::PlannerException("Unable to transform plan pose into local frame");
|
|
}
|
|
transformed_pose.pose.position.z = 0.0;
|
|
return transformed_pose;
|
|
};
|
|
|
|
// Transform the near part of the global plan into the robot's frame of reference.
|
|
nav_msgs::msg::Path transformed_plan;
|
|
transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID();
|
|
transformed_plan.header.stamp = robot_pose.header.stamp;
|
|
std::transform(
|
|
transformation_begin, transformation_end,
|
|
std::back_inserter(transformed_plan.poses),
|
|
transformGlobalPoseToLocal);
|
|
|
|
// Remove the portion of the global plan that we've already passed so we don't
|
|
// process it on the next iteration (this is called path pruning)
|
|
global_plan_.poses.erase(begin(global_plan_.poses), transformation_begin);
|
|
|
|
if (transformed_plan.poses.empty()) {
|
|
throw nav2_core::PlannerException("Resulting plan has 0 poses in it.");
|
|
}
|
|
|
|
return transformed_plan;
|
|
}
|
|
|
|
void PathHandler::setPlan(const nav_msgs::msg::Path & path)
|
|
{
|
|
global_plan_ = path;
|
|
}
|
|
|
|
} // namespace nav2_graceful_controller
|