add humble-navigation2
This commit is contained in:
X-lanni
@@ -0,0 +1,365 @@
|
||||
// Copyright (c) 2023 Alberto J. Tudela Roldán
|
||||
//
|
||||
// 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 "nav2_util/geometry_utils.hpp"
|
||||
#include "nav2_graceful_controller/graceful_controller.hpp"
|
||||
#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
|
||||
#include "nav2_core/exceptions.hpp"
|
||||
|
||||
namespace nav2_graceful_controller
|
||||
{
|
||||
|
||||
void GracefulController::configure(
|
||||
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
|
||||
std::string name, const std::shared_ptr<tf2_ros::Buffer> tf,
|
||||
const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
|
||||
{
|
||||
auto node = parent.lock();
|
||||
if (!node) {
|
||||
throw std::runtime_error("Unable to lock node!");
|
||||
}
|
||||
|
||||
costmap_ros_ = costmap_ros;
|
||||
tf_buffer_ = tf;
|
||||
plugin_name_ = name;
|
||||
logger_ = node->get_logger();
|
||||
|
||||
// Handles storage and dynamic configuration of parameters.
|
||||
// Returns pointer to data current param settings.
|
||||
param_handler_ = std::make_unique<ParameterHandler>(
|
||||
node, plugin_name_, logger_,
|
||||
costmap_ros_->getCostmap()->getSizeInMetersX());
|
||||
params_ = param_handler_->getParams();
|
||||
|
||||
// Handles global path transformations
|
||||
path_handler_ = std::make_unique<PathHandler>(
|
||||
tf2::durationFromSec(params_->transform_tolerance), tf_buffer_, costmap_ros_);
|
||||
|
||||
// Handles the control law to generate the velocity commands
|
||||
control_law_ = std::make_unique<SmoothControlLaw>(
|
||||
params_->k_phi, params_->k_delta, params_->beta, params_->lambda, params_->slowdown_radius,
|
||||
params_->v_linear_min, params_->v_linear_max, params_->v_angular_max);
|
||||
|
||||
// Initialize footprint collision checker
|
||||
collision_checker_ = std::make_unique<nav2_costmap_2d::
|
||||
FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>>(costmap_ros_->getCostmap());
|
||||
|
||||
// Publishers
|
||||
transformed_plan_pub_ = node->create_publisher<nav_msgs::msg::Path>("transformed_global_plan", 1);
|
||||
local_plan_pub_ = node->create_publisher<nav_msgs::msg::Path>("local_plan", 1);
|
||||
motion_target_pub_ = node->create_publisher<geometry_msgs::msg::PointStamped>("motion_target", 1);
|
||||
slowdown_pub_ = node->create_publisher<visualization_msgs::msg::Marker>("slowdown", 1);
|
||||
|
||||
RCLCPP_INFO(logger_, "Configured Graceful Motion Controller: %s", plugin_name_.c_str());
|
||||
}
|
||||
|
||||
void GracefulController::cleanup()
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
logger_,
|
||||
"Cleaning up controller: %s of type graceful_controller::GracefulController",
|
||||
plugin_name_.c_str());
|
||||
transformed_plan_pub_.reset();
|
||||
local_plan_pub_.reset();
|
||||
motion_target_pub_.reset();
|
||||
slowdown_pub_.reset();
|
||||
collision_checker_.reset();
|
||||
path_handler_.reset();
|
||||
param_handler_.reset();
|
||||
control_law_.reset();
|
||||
}
|
||||
|
||||
void GracefulController::activate()
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
logger_,
|
||||
"Activating controller: %s of type nav2_graceful_controller::GracefulController",
|
||||
plugin_name_.c_str());
|
||||
transformed_plan_pub_->on_activate();
|
||||
local_plan_pub_->on_activate();
|
||||
motion_target_pub_->on_activate();
|
||||
slowdown_pub_->on_activate();
|
||||
}
|
||||
|
||||
void GracefulController::deactivate()
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
logger_,
|
||||
"Deactivating controller: %s of type nav2_graceful_controller::GracefulController",
|
||||
plugin_name_.c_str());
|
||||
transformed_plan_pub_->on_deactivate();
|
||||
local_plan_pub_->on_deactivate();
|
||||
motion_target_pub_->on_deactivate();
|
||||
slowdown_pub_->on_deactivate();
|
||||
}
|
||||
|
||||
geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands(
|
||||
const geometry_msgs::msg::PoseStamped & pose,
|
||||
const geometry_msgs::msg::Twist & /*velocity*/,
|
||||
nav2_core::GoalChecker * goal_checker)
|
||||
{
|
||||
std::lock_guard<std::mutex> param_lock(param_handler_->getMutex());
|
||||
|
||||
// Update for the current goal checker's state
|
||||
geometry_msgs::msg::Pose pose_tolerance;
|
||||
geometry_msgs::msg::Twist velocity_tolerance;
|
||||
if (!goal_checker->getTolerances(pose_tolerance, velocity_tolerance)) {
|
||||
RCLCPP_WARN(logger_, "Unable to retrieve goal checker's tolerances!");
|
||||
} else {
|
||||
goal_dist_tolerance_ = pose_tolerance.position.x;
|
||||
}
|
||||
|
||||
// Update the smooth control law with the new params
|
||||
control_law_->setCurvatureConstants(
|
||||
params_->k_phi, params_->k_delta, params_->beta, params_->lambda);
|
||||
control_law_->setSlowdownRadius(params_->slowdown_radius);
|
||||
control_law_->setSpeedLimit(params_->v_linear_min, params_->v_linear_max, params_->v_angular_max);
|
||||
|
||||
// Transform path to robot base frame and publish it
|
||||
auto transformed_plan = path_handler_->transformGlobalPlan(
|
||||
pose, params_->max_robot_pose_search_dist);
|
||||
transformed_plan_pub_->publish(transformed_plan);
|
||||
|
||||
// Get the particular point on the path at the motion target distance and publish it
|
||||
auto motion_target = getMotionTarget(params_->motion_target_dist, transformed_plan);
|
||||
auto motion_target_point = nav2_graceful_controller::createMotionTargetMsg(motion_target);
|
||||
motion_target_pub_->publish(motion_target_point);
|
||||
|
||||
// Publish marker for slowdown radius around motion target for debugging / visualization
|
||||
auto slowdown_marker = nav2_graceful_controller::createSlowdownMarker(
|
||||
motion_target,
|
||||
params_->slowdown_radius);
|
||||
slowdown_pub_->publish(slowdown_marker);
|
||||
|
||||
// Compute distance to goal as the path's integrated distance to account for path curvatures
|
||||
double dist_to_goal = nav2_util::geometry_utils::calculate_path_length(transformed_plan);
|
||||
|
||||
// If the distance to the goal is less than the motion target distance, i.e.
|
||||
// the 'motion target' is the goal, then we skip the motion target orientation by pointing
|
||||
// it in the same orientation that the last segment of the path
|
||||
double angle_to_target = atan2(motion_target.pose.position.y, motion_target.pose.position.x);
|
||||
if (params_->final_rotation && dist_to_goal < params_->motion_target_dist) {
|
||||
geometry_msgs::msg::PoseStamped stl_pose =
|
||||
transformed_plan.poses[transformed_plan.poses.size() - 2];
|
||||
geometry_msgs::msg::PoseStamped goal_pose = transformed_plan.poses.back();
|
||||
double dx = goal_pose.pose.position.x - stl_pose.pose.position.x;
|
||||
double dy = goal_pose.pose.position.y - stl_pose.pose.position.y;
|
||||
double yaw = std::atan2(dy, dx);
|
||||
motion_target.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw);
|
||||
}
|
||||
|
||||
// Flip the orientation of the motion target if the robot is moving backwards
|
||||
bool reversing = false;
|
||||
if (params_->allow_backward && motion_target.pose.position.x < 0.0) {
|
||||
reversing = true;
|
||||
motion_target.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(
|
||||
tf2::getYaw(motion_target.pose.orientation) + M_PI);
|
||||
}
|
||||
|
||||
// Compute velocity command:
|
||||
// 1. Check if we are close enough to the goal to do a final rotation in place
|
||||
// 2. Check if we must do a rotation in place before moving
|
||||
// 3. Calculate the new velocity command using the smooth control law
|
||||
geometry_msgs::msg::TwistStamped cmd_vel;
|
||||
cmd_vel.header = pose.header;
|
||||
if (params_->final_rotation && (dist_to_goal < goal_dist_tolerance_ || goal_reached_)) {
|
||||
goal_reached_ = true;
|
||||
double angle_to_goal = tf2::getYaw(transformed_plan.poses.back().pose.orientation);
|
||||
cmd_vel.twist = rotateToTarget(angle_to_goal);
|
||||
} else if (params_->initial_rotation && // NOLINT
|
||||
fabs(angle_to_target) > params_->initial_rotation_min_angle)
|
||||
{
|
||||
cmd_vel.twist = rotateToTarget(angle_to_target);
|
||||
} else {
|
||||
cmd_vel.twist = control_law_->calculateRegularVelocity(motion_target.pose, reversing);
|
||||
}
|
||||
|
||||
// Transform local frame to global frame to use in collision checking
|
||||
geometry_msgs::msg::TransformStamped costmap_transform;
|
||||
try {
|
||||
costmap_transform = tf_buffer_->lookupTransform(
|
||||
costmap_ros_->getGlobalFrameID(), costmap_ros_->getBaseFrameID(),
|
||||
tf2::TimePointZero);
|
||||
} catch (tf2::TransformException & ex) {
|
||||
RCLCPP_ERROR(
|
||||
logger_, "Could not transform %s to %s: %s",
|
||||
costmap_ros_->getBaseFrameID().c_str(), costmap_ros_->getGlobalFrameID().c_str(),
|
||||
ex.what());
|
||||
return cmd_vel;
|
||||
}
|
||||
|
||||
// Generate and publish local plan for debugging / visualization
|
||||
nav_msgs::msg::Path local_plan;
|
||||
if (!simulateTrajectory(pose, motion_target, costmap_transform, local_plan, reversing)) {
|
||||
throw nav2_core::PlannerException("Collision detected in the trajectory");
|
||||
}
|
||||
local_plan.header = transformed_plan.header;
|
||||
local_plan_pub_->publish(local_plan);
|
||||
|
||||
return cmd_vel;
|
||||
}
|
||||
|
||||
void GracefulController::setPlan(const nav_msgs::msg::Path & path)
|
||||
{
|
||||
path_handler_->setPlan(path);
|
||||
goal_reached_ = false;
|
||||
}
|
||||
|
||||
void GracefulController::setSpeedLimit(
|
||||
const double & speed_limit, const bool & percentage)
|
||||
{
|
||||
std::lock_guard<std::mutex> param_lock(param_handler_->getMutex());
|
||||
|
||||
if (speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT) {
|
||||
params_->v_linear_max = params_->v_linear_max_initial;
|
||||
params_->v_angular_max = params_->v_angular_max_initial;
|
||||
} else {
|
||||
if (percentage) {
|
||||
// Speed limit is expressed in % from maximum speed of robot
|
||||
params_->v_linear_max = std::max(
|
||||
params_->v_linear_max_initial * speed_limit / 100.0, params_->v_linear_min);
|
||||
params_->v_angular_max = params_->v_angular_max_initial * speed_limit / 100.0;
|
||||
} else {
|
||||
// Speed limit is expressed in m/s
|
||||
params_->v_linear_max = std::max(speed_limit, params_->v_linear_min);
|
||||
// Limit the angular velocity to be proportional to the linear velocity
|
||||
params_->v_angular_max = params_->v_angular_max_initial *
|
||||
speed_limit / params_->v_linear_max_initial;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
geometry_msgs::msg::PoseStamped GracefulController::getMotionTarget(
|
||||
const double & motion_target_dist,
|
||||
const nav_msgs::msg::Path & transformed_plan)
|
||||
{
|
||||
// Find the first pose which is at a distance greater than the motion target distance
|
||||
auto goal_pose_it = std::find_if(
|
||||
transformed_plan.poses.begin(), transformed_plan.poses.end(), [&](const auto & ps) {
|
||||
return std::hypot(ps.pose.position.x, ps.pose.position.y) >= motion_target_dist;
|
||||
});
|
||||
|
||||
// If the pose is not far enough, take the last pose
|
||||
if (goal_pose_it == transformed_plan.poses.end()) {
|
||||
goal_pose_it = std::prev(transformed_plan.poses.end());
|
||||
}
|
||||
|
||||
return *goal_pose_it;
|
||||
}
|
||||
|
||||
bool GracefulController::simulateTrajectory(
|
||||
const geometry_msgs::msg::PoseStamped & robot_pose,
|
||||
const geometry_msgs::msg::PoseStamped & motion_target,
|
||||
const geometry_msgs::msg::TransformStamped & costmap_transform,
|
||||
nav_msgs::msg::Path & trajectory, const bool & backward)
|
||||
{
|
||||
// Check for collision before moving
|
||||
if (inCollision(
|
||||
robot_pose.pose.position.x, robot_pose.pose.position.y,
|
||||
tf2::getYaw(robot_pose.pose.orientation)))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
// First pose
|
||||
geometry_msgs::msg::PoseStamped next_pose;
|
||||
next_pose.header.frame_id = costmap_ros_->getBaseFrameID();
|
||||
next_pose.pose.orientation.w = 1.0;
|
||||
trajectory.poses.push_back(next_pose);
|
||||
|
||||
double distance = std::numeric_limits<double>::max();
|
||||
double resolution_ = costmap_ros_->getCostmap()->getResolution();
|
||||
double dt = (params_->v_linear_max > 0.0) ? resolution_ / params_->v_linear_max : 0.0;
|
||||
|
||||
// Set max iter to avoid infinite loop
|
||||
unsigned int max_iter = 2 * sqrt(
|
||||
motion_target.pose.position.x * motion_target.pose.position.x +
|
||||
motion_target.pose.position.y * motion_target.pose.position.y) / resolution_;
|
||||
|
||||
// Generate path
|
||||
do{
|
||||
// Apply velocities to calculate next pose
|
||||
next_pose.pose = control_law_->calculateNextPose(
|
||||
dt, motion_target.pose, next_pose.pose, backward);
|
||||
|
||||
// Add the pose to the trajectory for visualization
|
||||
trajectory.poses.push_back(next_pose);
|
||||
|
||||
// Check for collision
|
||||
geometry_msgs::msg::PoseStamped global_pose;
|
||||
tf2::doTransform(next_pose, global_pose, costmap_transform);
|
||||
if (inCollision(
|
||||
global_pose.pose.position.x, global_pose.pose.position.y,
|
||||
tf2::getYaw(global_pose.pose.orientation)))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
// Check if we reach the goal
|
||||
distance = nav2_util::geometry_utils::euclidean_distance(motion_target.pose, next_pose.pose);
|
||||
}while(distance > resolution_ && trajectory.poses.size() < max_iter);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
geometry_msgs::msg::Twist GracefulController::rotateToTarget(const double & angle_to_target)
|
||||
{
|
||||
geometry_msgs::msg::Twist vel;
|
||||
vel.linear.x = 0.0;
|
||||
vel.angular.z = params_->rotation_scaling_factor * angle_to_target * params_->v_angular_max;
|
||||
return vel;
|
||||
}
|
||||
|
||||
bool GracefulController::inCollision(const double & x, const double & y, const double & theta)
|
||||
{
|
||||
unsigned int mx, my;
|
||||
if (!costmap_ros_->getCostmap()->worldToMap(x, y, mx, my)) {
|
||||
RCLCPP_WARN(
|
||||
logger_, "The path is not in the costmap. Cannot check for collisions. "
|
||||
"Proceed at your own risk, slow the robot, or increase your costmap size.");
|
||||
return false;
|
||||
}
|
||||
|
||||
// Calculate the cost of the footprint at the robot's current position depending
|
||||
// on the shape of the footprint
|
||||
bool is_tracking_unknown =
|
||||
costmap_ros_->getLayeredCostmap()->isTrackingUnknown();
|
||||
bool consider_footprint = !costmap_ros_->getUseRadius();
|
||||
|
||||
double footprint_cost;
|
||||
if (consider_footprint) {
|
||||
footprint_cost = collision_checker_->footprintCostAtPose(
|
||||
x, y, theta, costmap_ros_->getRobotFootprint());
|
||||
} else {
|
||||
footprint_cost = collision_checker_->pointCost(mx, my);
|
||||
}
|
||||
|
||||
switch (static_cast<unsigned char>(footprint_cost)) {
|
||||
case (nav2_costmap_2d::LETHAL_OBSTACLE):
|
||||
return true;
|
||||
case (nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE):
|
||||
return consider_footprint ? false : true;
|
||||
case (nav2_costmap_2d::NO_INFORMATION):
|
||||
return is_tracking_unknown ? false : true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
} // namespace nav2_graceful_controller
|
||||
|
||||
// Register this controller as a nav2_core plugin
|
||||
PLUGINLIB_EXPORT_CLASS(
|
||||
nav2_graceful_controller::GracefulController,
|
||||
nav2_core::Controller)
|
||||
@@ -0,0 +1,171 @@
|
||||
// Copyright (c) 2023 Alberto J. Tudela Roldán
|
||||
//
|
||||
// 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_graceful_controller/parameter_handler.hpp"
|
||||
|
||||
namespace nav2_graceful_controller
|
||||
{
|
||||
|
||||
using nav2_util::declare_parameter_if_not_declared;
|
||||
using rcl_interfaces::msg::ParameterType;
|
||||
|
||||
ParameterHandler::ParameterHandler(
|
||||
rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::string & plugin_name,
|
||||
rclcpp::Logger & logger, const double costmap_size_x)
|
||||
{
|
||||
plugin_name_ = plugin_name;
|
||||
logger_ = logger;
|
||||
|
||||
declare_parameter_if_not_declared(
|
||||
node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1));
|
||||
declare_parameter_if_not_declared(
|
||||
node, plugin_name_ + ".motion_target_dist", rclcpp::ParameterValue(0.6));
|
||||
declare_parameter_if_not_declared(
|
||||
node, plugin_name_ + ".max_robot_pose_search_dist",
|
||||
rclcpp::ParameterValue(costmap_size_x / 2.0));
|
||||
declare_parameter_if_not_declared(node, plugin_name_ + ".k_phi", rclcpp::ParameterValue(3.0));
|
||||
declare_parameter_if_not_declared(node, plugin_name_ + ".k_delta", rclcpp::ParameterValue(2.0));
|
||||
declare_parameter_if_not_declared(node, plugin_name_ + ".beta", rclcpp::ParameterValue(0.2));
|
||||
declare_parameter_if_not_declared(node, plugin_name_ + ".lambda", rclcpp::ParameterValue(2.0));
|
||||
declare_parameter_if_not_declared(
|
||||
node, plugin_name_ + ".v_linear_min", rclcpp::ParameterValue(0.1));
|
||||
declare_parameter_if_not_declared(
|
||||
node, plugin_name_ + ".v_linear_max", rclcpp::ParameterValue(0.5));
|
||||
declare_parameter_if_not_declared(
|
||||
node, plugin_name_ + ".v_angular_max", rclcpp::ParameterValue(1.0));
|
||||
declare_parameter_if_not_declared(
|
||||
node, plugin_name_ + ".slowdown_radius", rclcpp::ParameterValue(1.5));
|
||||
declare_parameter_if_not_declared(
|
||||
node, plugin_name_ + ".initial_rotation", rclcpp::ParameterValue(true));
|
||||
declare_parameter_if_not_declared(
|
||||
node, plugin_name_ + ".initial_rotation_min_angle", rclcpp::ParameterValue(0.75));
|
||||
declare_parameter_if_not_declared(
|
||||
node, plugin_name_ + ".final_rotation", rclcpp::ParameterValue(true));
|
||||
declare_parameter_if_not_declared(
|
||||
node, plugin_name_ + ".rotation_scaling_factor", rclcpp::ParameterValue(0.5));
|
||||
declare_parameter_if_not_declared(
|
||||
node, plugin_name_ + ".allow_backward", rclcpp::ParameterValue(false));
|
||||
|
||||
node->get_parameter(plugin_name_ + ".transform_tolerance", params_.transform_tolerance);
|
||||
node->get_parameter(plugin_name_ + ".motion_target_dist", params_.motion_target_dist);
|
||||
node->get_parameter(
|
||||
plugin_name_ + ".max_robot_pose_search_dist", params_.max_robot_pose_search_dist);
|
||||
if (params_.max_robot_pose_search_dist < 0.0) {
|
||||
RCLCPP_WARN(
|
||||
logger_, "Max robot search distance is negative, setting to max to search"
|
||||
" every point on path for the closest value.");
|
||||
params_.max_robot_pose_search_dist = std::numeric_limits<double>::max();
|
||||
}
|
||||
|
||||
node->get_parameter(plugin_name_ + ".k_phi", params_.k_phi);
|
||||
node->get_parameter(plugin_name_ + ".k_delta", params_.k_delta);
|
||||
node->get_parameter(plugin_name_ + ".beta", params_.beta);
|
||||
node->get_parameter(plugin_name_ + ".lambda", params_.lambda);
|
||||
node->get_parameter(plugin_name_ + ".v_linear_min", params_.v_linear_min);
|
||||
node->get_parameter(plugin_name_ + ".v_linear_max", params_.v_linear_max);
|
||||
params_.v_linear_max_initial = params_.v_linear_max;
|
||||
node->get_parameter(plugin_name_ + ".v_angular_max", params_.v_angular_max);
|
||||
params_.v_angular_max_initial = params_.v_angular_max;
|
||||
node->get_parameter(plugin_name_ + ".slowdown_radius", params_.slowdown_radius);
|
||||
node->get_parameter(plugin_name_ + ".initial_rotation", params_.initial_rotation);
|
||||
node->get_parameter(
|
||||
plugin_name_ + ".initial_rotation_min_angle", params_.initial_rotation_min_angle);
|
||||
node->get_parameter(plugin_name_ + ".final_rotation", params_.final_rotation);
|
||||
node->get_parameter(plugin_name_ + ".rotation_scaling_factor", params_.rotation_scaling_factor);
|
||||
node->get_parameter(plugin_name_ + ".allow_backward", params_.allow_backward);
|
||||
|
||||
if (params_.initial_rotation && params_.allow_backward) {
|
||||
RCLCPP_WARN(
|
||||
logger_, "Initial rotation and allow backward parameters are both true, "
|
||||
"setting allow backward to false.");
|
||||
params_.allow_backward = false;
|
||||
}
|
||||
|
||||
dyn_params_handler_ = node->add_on_set_parameters_callback(
|
||||
std::bind(&ParameterHandler::dynamicParametersCallback, this, std::placeholders::_1));
|
||||
}
|
||||
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
ParameterHandler::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
|
||||
{
|
||||
rcl_interfaces::msg::SetParametersResult result;
|
||||
std::lock_guard<std::mutex> lock_reinit(mutex_);
|
||||
|
||||
for (auto parameter : parameters) {
|
||||
const auto & type = parameter.get_type();
|
||||
const auto & name = parameter.get_name();
|
||||
|
||||
if (type == ParameterType::PARAMETER_DOUBLE) {
|
||||
if (name == plugin_name_ + ".transform_tolerance") {
|
||||
params_.transform_tolerance = parameter.as_double();
|
||||
} else if (name == plugin_name_ + ".motion_target_dist") {
|
||||
params_.motion_target_dist = parameter.as_double();
|
||||
} else if (name == plugin_name_ + ".k_phi") {
|
||||
params_.k_phi = parameter.as_double();
|
||||
} else if (name == plugin_name_ + ".k_delta") {
|
||||
params_.k_delta = parameter.as_double();
|
||||
} else if (name == plugin_name_ + ".beta") {
|
||||
params_.beta = parameter.as_double();
|
||||
} else if (name == plugin_name_ + ".lambda") {
|
||||
params_.lambda = parameter.as_double();
|
||||
} else if (name == plugin_name_ + ".v_linear_min") {
|
||||
params_.v_linear_min = parameter.as_double();
|
||||
} else if (name == plugin_name_ + ".v_linear_max") {
|
||||
params_.v_linear_max = parameter.as_double();
|
||||
params_.v_linear_max_initial = params_.v_linear_max;
|
||||
} else if (name == plugin_name_ + ".v_angular_max") {
|
||||
params_.v_angular_max = parameter.as_double();
|
||||
params_.v_angular_max_initial = params_.v_angular_max;
|
||||
} else if (name == plugin_name_ + ".slowdown_radius") {
|
||||
params_.slowdown_radius = parameter.as_double();
|
||||
} else if (name == plugin_name_ + ".initial_rotation_min_angle") {
|
||||
params_.initial_rotation_min_angle = parameter.as_double();
|
||||
} else if (name == plugin_name_ + ".rotation_scaling_factor") {
|
||||
params_.rotation_scaling_factor = parameter.as_double();
|
||||
}
|
||||
} else if (type == ParameterType::PARAMETER_BOOL) {
|
||||
if (name == plugin_name_ + ".initial_rotation") {
|
||||
if (parameter.as_bool() && params_.allow_backward) {
|
||||
RCLCPP_WARN(
|
||||
logger_, "Initial rotation and allow backward parameters are both true, "
|
||||
"rejecting parameter change.");
|
||||
continue;
|
||||
}
|
||||
params_.initial_rotation = parameter.as_bool();
|
||||
} else if (name == plugin_name_ + ".final_rotation") {
|
||||
params_.final_rotation = parameter.as_bool();
|
||||
} else if (name == plugin_name_ + ".allow_backward") {
|
||||
if (params_.initial_rotation && parameter.as_bool()) {
|
||||
RCLCPP_WARN(
|
||||
logger_, "Initial rotation and allow backward parameters are both true, "
|
||||
"rejecting parameter change.");
|
||||
continue;
|
||||
}
|
||||
params_.allow_backward = parameter.as_bool();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
result.successful = true;
|
||||
return result;
|
||||
}
|
||||
|
||||
} // namespace nav2_graceful_controller
|
||||
@@ -0,0 +1,126 @@
|
||||
// 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
|
||||
@@ -0,0 +1,125 @@
|
||||
// Copyright (c) 2023 Alberto J. Tudela Roldán
|
||||
//
|
||||
// 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 "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "nav2_util/geometry_utils.hpp"
|
||||
#include "nav2_graceful_controller/ego_polar_coords.hpp"
|
||||
#include "nav2_graceful_controller/smooth_control_law.hpp"
|
||||
|
||||
namespace nav2_graceful_controller
|
||||
{
|
||||
|
||||
SmoothControlLaw::SmoothControlLaw(
|
||||
double k_phi, double k_delta, double beta, double lambda, double slowdown_radius,
|
||||
double v_linear_min, double v_linear_max, double v_angular_max)
|
||||
: k_phi_(k_phi), k_delta_(k_delta), beta_(beta), lambda_(lambda), slowdown_radius_(slowdown_radius),
|
||||
v_linear_min_(v_linear_min), v_linear_max_(v_linear_max), v_angular_max_(v_angular_max)
|
||||
{
|
||||
}
|
||||
|
||||
void SmoothControlLaw::setCurvatureConstants(
|
||||
double k_phi, double k_delta, double beta, double lambda)
|
||||
{
|
||||
k_phi_ = k_phi;
|
||||
k_delta_ = k_delta;
|
||||
beta_ = beta;
|
||||
lambda_ = lambda;
|
||||
}
|
||||
|
||||
void SmoothControlLaw::setSlowdownRadius(double slowdown_radius)
|
||||
{
|
||||
slowdown_radius_ = slowdown_radius;
|
||||
}
|
||||
|
||||
void SmoothControlLaw::setSpeedLimit(
|
||||
const double v_linear_min, const double v_linear_max, const double v_angular_max)
|
||||
{
|
||||
v_linear_min_ = v_linear_min;
|
||||
v_linear_max_ = v_linear_max;
|
||||
v_angular_max_ = v_angular_max;
|
||||
}
|
||||
|
||||
geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity(
|
||||
const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current,
|
||||
const bool & backward)
|
||||
{
|
||||
// Convert the target to polar coordinates
|
||||
auto ego_coords = EgocentricPolarCoordinates(target, current, backward);
|
||||
// Calculate the curvature
|
||||
double curvature = calculateCurvature(ego_coords.r, ego_coords.phi, ego_coords.delta);
|
||||
// Invert the curvature if the robot is moving backwards
|
||||
curvature = backward ? -curvature : curvature;
|
||||
|
||||
// Adjust the linear velocity as a function of the path curvature to
|
||||
// slowdown the controller as it approaches its target
|
||||
double v = v_linear_max_ / (1.0 + beta_ * std::pow(fabs(curvature), lambda_));
|
||||
|
||||
// Slowdown when the robot is near the target to remove singularity
|
||||
v = std::min(v_linear_max_ * (ego_coords.r / slowdown_radius_), v);
|
||||
|
||||
// Set some small v_min when far away from origin to promote faster
|
||||
// turning motion when the curvature is very high
|
||||
v = std::clamp(v, v_linear_min_, v_linear_max_);
|
||||
|
||||
// Set the velocity to negative if the robot is moving backwards
|
||||
v = backward ? -v : v;
|
||||
|
||||
// Compute the angular velocity
|
||||
double w = curvature * v;
|
||||
// Bound angular velocity between [-max_angular_vel, max_angular_vel]
|
||||
double w_bound = std::clamp(w, -v_angular_max_, v_angular_max_);
|
||||
// And linear velocity to follow the curvature
|
||||
v = (curvature != 0.0) ? (w_bound / curvature) : v;
|
||||
|
||||
// Return the velocity command
|
||||
geometry_msgs::msg::Twist cmd_vel;
|
||||
cmd_vel.linear.x = v;
|
||||
cmd_vel.angular.z = w_bound;
|
||||
return cmd_vel;
|
||||
}
|
||||
|
||||
geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity(
|
||||
const geometry_msgs::msg::Pose & target, const bool & backward)
|
||||
{
|
||||
return calculateRegularVelocity(target, geometry_msgs::msg::Pose(), backward);
|
||||
}
|
||||
|
||||
geometry_msgs::msg::Pose SmoothControlLaw::calculateNextPose(
|
||||
const double dt,
|
||||
const geometry_msgs::msg::Pose & target,
|
||||
const geometry_msgs::msg::Pose & current,
|
||||
const bool & backward)
|
||||
{
|
||||
geometry_msgs::msg::Twist vel = calculateRegularVelocity(target, current, backward);
|
||||
geometry_msgs::msg::Pose next;
|
||||
double yaw = tf2::getYaw(current.orientation);
|
||||
next.position.x = current.position.x + vel.linear.x * dt * cos(yaw);
|
||||
next.position.y = current.position.y + vel.linear.x * dt * sin(yaw);
|
||||
yaw += vel.angular.z * dt;
|
||||
next.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw);
|
||||
return next;
|
||||
}
|
||||
|
||||
double SmoothControlLaw::calculateCurvature(double r, double phi, double delta)
|
||||
{
|
||||
// Calculate the proportional term of the control law as the product of the gain and the error:
|
||||
// difference between the actual steering angle and the virtual control for the slow subsystem
|
||||
double prop_term = k_delta_ * (delta - std::atan(-k_phi_ * phi));
|
||||
// Calculate the feedback control law for the steering
|
||||
double feedback_term = (1.0 + (k_phi_ / (1.0 + std::pow(k_phi_ * phi, 2)))) * sin(delta);
|
||||
// Calculate the path curvature
|
||||
return -1.0 / r * (prop_term + feedback_term);
|
||||
}
|
||||
|
||||
} // namespace nav2_graceful_controller
|
||||
@@ -0,0 +1,51 @@
|
||||
// Copyright (c) 2023 Alberto J. Tudela Roldán
|
||||
//
|
||||
// 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 "nav2_graceful_controller/utils.hpp"
|
||||
|
||||
namespace nav2_graceful_controller
|
||||
{
|
||||
|
||||
geometry_msgs::msg::PointStamped createMotionTargetMsg(
|
||||
const geometry_msgs::msg::PoseStamped & motion_target)
|
||||
{
|
||||
geometry_msgs::msg::PointStamped motion_target_point;
|
||||
motion_target_point.header = motion_target.header;
|
||||
motion_target_point.point = motion_target.pose.position;
|
||||
motion_target_point.point.z = 0.01;
|
||||
return motion_target_point;
|
||||
}
|
||||
|
||||
visualization_msgs::msg::Marker createSlowdownMarker(
|
||||
const geometry_msgs::msg::PoseStamped & motion_target, const double & slowdown_radius)
|
||||
{
|
||||
visualization_msgs::msg::Marker slowdown_marker;
|
||||
slowdown_marker.header = motion_target.header;
|
||||
slowdown_marker.ns = "slowdown";
|
||||
slowdown_marker.id = 0;
|
||||
slowdown_marker.type = visualization_msgs::msg::Marker::SPHERE;
|
||||
slowdown_marker.action = visualization_msgs::msg::Marker::ADD;
|
||||
slowdown_marker.pose = motion_target.pose;
|
||||
slowdown_marker.pose.position.z = 0.01;
|
||||
slowdown_marker.scale.x = slowdown_radius * 2.0;
|
||||
slowdown_marker.scale.y = slowdown_radius * 2.0;
|
||||
slowdown_marker.scale.z = 0.02;
|
||||
slowdown_marker.color.a = 0.2;
|
||||
slowdown_marker.color.r = 0.0;
|
||||
slowdown_marker.color.g = 1.0;
|
||||
slowdown_marker.color.b = 0.0;
|
||||
return slowdown_marker;
|
||||
}
|
||||
|
||||
} // namespace nav2_graceful_controller
|
||||
Reference in New Issue
Block a user