Created path segmenter controller
This commit is contained in:
131
toid_spinner_controller/src/path_handler.cpp
Normal file
131
toid_spinner_controller/src/path_handler.cpp
Normal file
@@ -0,0 +1,131 @@
|
||||
#include "toid_spinner_controller/path_handler.hpp"
|
||||
#include "angles/angles.h"
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "nav2_core/controller_exceptions.hpp"
|
||||
#include "nav2_util/geometry_utils.hpp"
|
||||
#include "nav_msgs/msg/path.hpp"
|
||||
#include "tf2/LinearMath/Quaternion.hpp"
|
||||
#include "tf2/LinearMath/Vector3.hpp"
|
||||
#include "tf2/exceptions.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <iterator>
|
||||
#include <memory>
|
||||
#include <numeric>
|
||||
|
||||
namespace toid {
|
||||
PathHandler::PathHandler(
|
||||
tf2::Duration tranform_tolerance, std::shared_ptr<tf2_ros::Buffer> tf,
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
|
||||
: transform_tolerance_(tranform_tolerance), tf_(tf),
|
||||
costmap_ros_(costmap_ros) {}
|
||||
|
||||
nav_msgs::msg::Path
|
||||
PathHandler::transformGlobalPlan(const geometry_msgs::msg::PoseStamped &pose,
|
||||
double max_robot_pose_search_dist,
|
||||
double angle_tolerance) {
|
||||
|
||||
if (global_plan_.poses.empty()) {
|
||||
throw nav2_core::InvalidPath("Received plan with length of one");
|
||||
}
|
||||
|
||||
geometry_msgs::msg::PoseStamped robot_pose;
|
||||
if (!tranformPose(global_plan_.header.frame_id, pose, robot_pose)) {
|
||||
throw nav2_core::ControllerTFError(
|
||||
"Unable to transform robot pose into global plan's frame");
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
const double robot_orientation = tf2::getYaw(robot_pose.pose.orientation);
|
||||
|
||||
// Calculate the closeset pose and keep track of the next rotation point
|
||||
auto closest_pose = global_plan_.poses.begin();
|
||||
auto last_rotation = global_plan_.poses.end();
|
||||
auto previous_it = global_plan_.poses.begin();
|
||||
auto it = global_plan_.poses.begin();
|
||||
double closest_dist = INFINITY;
|
||||
|
||||
for (; it != closest_pose_upper_bound; it++) {
|
||||
const double it_orientation = tf2::getYaw(it->pose.orientation);
|
||||
const double angle_diff = std::abs(
|
||||
angles::shortest_angular_distance(it_orientation, robot_orientation));
|
||||
|
||||
if (previous_it->pose.position == it->pose.position) {
|
||||
last_rotation = it + 1;
|
||||
}
|
||||
|
||||
if (angle_diff <= angle_tolerance) {
|
||||
double dist = dist2(robot_pose, *it);
|
||||
if (dist <= closest_dist) {
|
||||
closest_dist = dist;
|
||||
closest_pose = it;
|
||||
last_rotation = global_plan_.poses.end();
|
||||
}
|
||||
}
|
||||
previous_it = it;
|
||||
}
|
||||
if(last_rotation == closest_pose_upper_bound || last_rotation == global_plan_.poses.end()) {
|
||||
for (; it != global_plan_.poses.end(); it++) {
|
||||
if (previous_it->pose.position == it->pose.position) {
|
||||
last_rotation = it + 1;
|
||||
} else if (last_rotation != global_plan_.poses.end()) {
|
||||
break;
|
||||
}
|
||||
previous_it = it;
|
||||
}
|
||||
}
|
||||
|
||||
auto transformGlobalPoseToLocal = [&](const auto &global_plan_pose) {
|
||||
geometry_msgs::msg::PoseStamped stamped_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;
|
||||
stamped_pose.pose.position.z = 0.0;
|
||||
return stamped_pose;
|
||||
};
|
||||
|
||||
nav_msgs::msg::Path out_path;
|
||||
std::transform(closest_pose, last_rotation,
|
||||
std::back_inserter(out_path.poses),
|
||||
transformGlobalPoseToLocal);
|
||||
|
||||
out_path.header.frame_id = global_plan_.header.frame_id;
|
||||
out_path.header.stamp = robot_pose.header.stamp;
|
||||
|
||||
global_plan_.poses.erase(begin(global_plan_.poses), closest_pose);
|
||||
|
||||
return out_path;
|
||||
}
|
||||
|
||||
bool PathHandler::tranformPose(const std::string frame,
|
||||
const geometry_msgs::msg::PoseStamped &in_pose,
|
||||
geometry_msgs::msg::PoseStamped &out_pose) {
|
||||
if (in_pose.header.frame_id == frame) {
|
||||
out_pose = in_pose;
|
||||
return true;
|
||||
}
|
||||
|
||||
try {
|
||||
tf_->transform(in_pose, out_pose, frame, transform_tolerance_);
|
||||
out_pose.header.frame_id = frame;
|
||||
return true;
|
||||
} catch (tf2::TransformException &ex) {
|
||||
RCLCPP_ERROR(logger_, "Exception in tranformPose: %s", ex.what());
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
double PathHandler::dist2(const geometry_msgs::msg::PoseStamped &p1,
|
||||
const geometry_msgs::msg::PoseStamped &p2) {
|
||||
const double x = p1.pose.position.x - p2.pose.position.x;
|
||||
const double y = p1.pose.position.y - p2.pose.position.y;
|
||||
return x * x + y * y;
|
||||
}
|
||||
|
||||
} // namespace toid
|
||||
206
toid_spinner_controller/src/toid_spinner_controller.cpp
Normal file
206
toid_spinner_controller/src/toid_spinner_controller.cpp
Normal file
@@ -0,0 +1,206 @@
|
||||
#include "toid_spinner_controller/toid_spinner_controller.hpp"
|
||||
#include "geometry_msgs/msg/point_stamped.hpp"
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "geometry_msgs/msg/twist_stamped.hpp"
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
#include "pluginlib/exceptions.hpp"
|
||||
#include "rclcpp/logging.hpp"
|
||||
#include "toid_spinner_controller/path_handler.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <angles/angles.h>
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#include <memory>
|
||||
#include <rclcpp/duration.hpp>
|
||||
#include <tf2/time.hpp>
|
||||
#include <tf2/utils.hpp>
|
||||
|
||||
namespace toid {
|
||||
SpinnerController::SpinnerController()
|
||||
: lp_loader_("nav2_core", "nav2_core::Controller") {}
|
||||
|
||||
void SpinnerController::configure(
|
||||
const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent, std::string name,
|
||||
std::shared_ptr<tf2_ros::Buffer> tf,
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) {
|
||||
|
||||
position_goal_checker_ =
|
||||
std::make_unique<nav2_controller::PositionGoalChecker>();
|
||||
position_goal_checker_->initialize(parent, ".position_checker", costmap_ros);
|
||||
plugin_name_ = name;
|
||||
node_ = parent;
|
||||
auto node = node_.lock();
|
||||
|
||||
tf_ = tf;
|
||||
logger_ = node->get_logger();
|
||||
clock_ = node->get_clock();
|
||||
|
||||
std::string primary_controller;
|
||||
double control_frequency;
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, plugin_name_ + ".angular_dist_threshold",
|
||||
rclcpp::ParameterValue(0.785));
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, plugin_name_ + ".angular_disengage_threshold",
|
||||
rclcpp::ParameterValue(0.785 / 2.0));
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, plugin_name_ + ".forward_sampling_distance",
|
||||
rclcpp::ParameterValue(0.5));
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2));
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, plugin_name_ + ".rotate_to_heading", rclcpp::ParameterValue(1.0));
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, plugin_name_ + ".primary_controller.plugin",
|
||||
rclcpp::PARAMETER_STRING);
|
||||
|
||||
node->get_parameter(plugin_name_ + ".angular_dist_threshold",
|
||||
angular_dist_threshold_);
|
||||
node->get_parameter(plugin_name_ + ".angular_disengage_threshold",
|
||||
angular_disengage_threshold_);
|
||||
node->get_parameter(plugin_name_ + ".forward_sampling_distance",
|
||||
forward_sampling_distance_);
|
||||
node->get_parameter(plugin_name_ + ".max_angular_accel", max_angular_accel_);
|
||||
node->get_parameter(plugin_name_ + ".rotate_to_heading", rotate_to_heading_);
|
||||
|
||||
primary_controller =
|
||||
node->get_parameter(plugin_name_ + ".primary_controller.plugin")
|
||||
.as_string();
|
||||
|
||||
node->get_parameter("controller_frequency", control_frequency);
|
||||
control_duration_ = 1.0 / control_frequency;
|
||||
|
||||
try {
|
||||
primary_controller_ = lp_loader_.createUniqueInstance(primary_controller);
|
||||
RCLCPP_INFO(logger_,
|
||||
"Created internal controller for multiplexer: %s of type %s",
|
||||
plugin_name_.c_str(), primary_controller.c_str());
|
||||
} catch (const pluginlib::PluginlibException &ex) {
|
||||
RCLCPP_FATAL(
|
||||
logger_,
|
||||
"Failed to create primary controller for multiplexer. Exception: %s",
|
||||
ex.what());
|
||||
return;
|
||||
}
|
||||
primary_controller_->configure(parent, name + ".primary_controller", tf,
|
||||
costmap_ros);
|
||||
|
||||
path_handler_ = std::make_unique<PathHandler>(tf2::durationFromSec(0.1), tf_,
|
||||
costmap_ros);
|
||||
|
||||
closest_pt_pub_ = node->create_publisher<geometry_msgs::msg::PointStamped>(
|
||||
"start_point", 1);
|
||||
}
|
||||
|
||||
void SpinnerController::cleanup() {
|
||||
RCLCPP_INFO(logger_, "Cleaning SpinnerController");
|
||||
primary_controller_->cleanup();
|
||||
primary_controller_->reset();
|
||||
position_goal_checker_.reset();
|
||||
path_handler_.reset();
|
||||
closest_pt_pub_.reset();
|
||||
}
|
||||
|
||||
void SpinnerController::activate() {
|
||||
RCLCPP_INFO(logger_, "Activating SpinnerController");
|
||||
last_angular_vel_ = std::numeric_limits<double>::max();
|
||||
new_path_recieved = false;
|
||||
auto node = node_.lock();
|
||||
closest_pt_pub_->on_activate();
|
||||
primary_controller_->activate();
|
||||
position_goal_checker_->reset();
|
||||
}
|
||||
|
||||
void SpinnerController::deactivate() {
|
||||
RCLCPP_INFO(logger_, "Deactivating SpinnerController");
|
||||
closest_pt_pub_.reset();
|
||||
primary_controller_->deactivate();
|
||||
closest_pt_pub_->on_deactivate();
|
||||
}
|
||||
|
||||
void SpinnerController::setPlan(const nav_msgs::msg::Path &path) {
|
||||
path_handler_->setPlan(path);
|
||||
new_path_recieved = true;
|
||||
primary_controller_->setPlan(path);
|
||||
}
|
||||
|
||||
geometry_msgs::msg::TwistStamped SpinnerController::computeVelocityCommands(
|
||||
const geometry_msgs::msg::PoseStamped &pose,
|
||||
const geometry_msgs::msg::Twist &velocity,
|
||||
nav2_core::GoalChecker *goal_checker) {
|
||||
|
||||
auto path = path_handler_->transformGlobalPlan(pose, 1.0, M_PI / 8);
|
||||
|
||||
if (path.poses.size() > 0) {
|
||||
geometry_msgs::msg::PointStamped pt;
|
||||
const auto &pose = path.poses.back();
|
||||
pt.point = pose.pose.position;
|
||||
pt.header.frame_id = path.header.frame_id;
|
||||
pt.header.stamp = path.header.stamp;
|
||||
closest_pt_pub_->publish(pt);
|
||||
}
|
||||
|
||||
geometry_msgs::msg::PoseStamped robot_pose;
|
||||
path_handler_->tranformPose(path.header.frame_id, pose, robot_pose);
|
||||
const double robot_angle = tf2::getYaw(robot_pose.pose.orientation);
|
||||
|
||||
if (path.poses.front().pose.position == path.poses.back().pose.position) {
|
||||
const double path_angle = tf2::getYaw(path.poses.back().pose.orientation);
|
||||
const double angle =
|
||||
angles::shortest_angular_distance(robot_angle, path_angle);
|
||||
return computeRotateToHeadingCommand(angle, pose);
|
||||
}
|
||||
|
||||
if (new_path_recieved) {
|
||||
new_path_recieved = false;
|
||||
primary_controller_->setPlan(path);
|
||||
}
|
||||
|
||||
auto twist = primary_controller_->computeVelocityCommands(pose, velocity,
|
||||
goal_checker);
|
||||
last_angular_vel_ = twist.twist.angular.z;
|
||||
return twist;
|
||||
}
|
||||
|
||||
geometry_msgs::msg::TwistStamped
|
||||
SpinnerController::computeRotateToHeadingCommand(
|
||||
const double &angular_distance_to_heading,
|
||||
const geometry_msgs::msg::PoseStamped &pose) {
|
||||
geometry_msgs::msg::TwistStamped cmd;
|
||||
|
||||
const double last_angular_vel =
|
||||
(last_angular_vel_ == std::numeric_limits<double>::max())
|
||||
? 0
|
||||
: last_angular_vel_;
|
||||
const double sign = angular_distance_to_heading > 0.0 ? 1.0 : -1.0;
|
||||
const double speed_upper_bound =
|
||||
last_angular_vel + control_duration_ * max_angular_accel_;
|
||||
const double speed_lower_bound =
|
||||
last_angular_vel - control_duration_ * max_angular_accel_;
|
||||
|
||||
const double speed_until_overshoot =
|
||||
0.9 * std::sqrt(2.0 * max_angular_accel_ *
|
||||
std::fabs(angular_distance_to_heading));
|
||||
const double requested_speed = sign * std::fmin(speed_until_overshoot,rotate_to_heading_);
|
||||
const double speed =
|
||||
std::clamp(requested_speed, speed_lower_bound, speed_upper_bound);
|
||||
|
||||
cmd.header.frame_id = pose.header.frame_id;
|
||||
cmd.header.stamp = pose.header.stamp;
|
||||
cmd.twist.angular.z = speed;
|
||||
last_angular_vel_ = speed;
|
||||
return cmd;
|
||||
}
|
||||
|
||||
void SpinnerController::setSpeedLimit(const double &speed_limit,
|
||||
const bool &percentage) {
|
||||
primary_controller_->setSpeedLimit(speed_limit, percentage);
|
||||
}
|
||||
|
||||
bool SpinnerController::cancel() { return primary_controller_->cancel(); }
|
||||
|
||||
} // namespace toid
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(toid::SpinnerController, nav2_core::Controller)
|
||||
Reference in New Issue
Block a user