Pimpest/toid
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

added collision detection

Browse Source
This commit is contained in:
pimpest
2026-04-01 17:46:49 +02:00
parent 089b630538
commit 88ebe9fb68
7 changed files with 301 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files

120
toid_behaviors/src/move_coords.cpp
View File

@@ -69,39 +69,83 @@ ResultStatus MoveCoords::onStart(
target_sign_ = backwards_ ? -1.0 : 1.0;
max_vel_speed_ = command->max_speed;
if(command->max_speed == 0) {
if (command->max_speed == 0) {
auto node = node_.lock();
node->get_parameter(behavior_name_ + ".max_vel_speed", max_vel_speed_);
}
scl.k_phi = k_phi_;
scl.k_delta = k_delta_;
scl.bbeta = beta_;
scl.lam = lambda_;
scl.slowdown_radius = slowdown_radius_;
scl.v_angular_max = max_angular_speed_;
scl.v_linear_min = min_vel_speed_;
scl.v_linear_max = max_vel_speed_;
scl_.k_phi = k_phi_;
scl_.k_delta = k_delta_;
scl_.bbeta = beta_;
scl_.lam = lambda_;
scl_.slowdown_radius = slowdown_radius_;
scl_.v_angular_max = max_angular_speed_;
scl_.v_linear_min = min_vel_speed_;
scl_.v_linear_max = max_vel_speed_;
last_speed_ = vel.angular.x;
return ResultStatus{Status::SUCCEEDED};
}
double MoveCoords::distanceToTarget(
const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & target_point,
const double target_theta, bool backwards)
{
const double dx = target_point.x - pose.position.x;
const double dy = target_point.y - pose.position.y;
const double target_sign = backwards? -1.0 : 1.0;
return target_sign * (dx * cos(target_theta) + dy * sin(target_theta));
}
double MoveCoords::velocityTarget(const double dist_left) {
const double lower_bound = last_speed_ - control_duration_ * max_vel_accel_;
const double upper_bound = last_speed_ + control_duration_ * max_vel_accel_;
double vel = max_vel_speed_;
double max_vel_to_stop = 0.8 * std::sqrt(2.0 * max_vel_decel_ * dist_left);
vel = std::min(vel, max_vel_to_stop);
return std::clamp(target_sign_ * vel, lower_bound, upper_bound);
}
bool MoveCoords::collisionDetection(const geometry_msgs::msg::Pose &pose, geometry_msgs::msg::Pose &last_ok_pose) {
const int samples = static_cast<int>(0.5/control_duration_);
geometry_msgs::msg::Pose current_pose = pose;
last_ok_pose = pose;
for(int i = 0; i < samples; i++) {
scl_.step(target_pose_, current_pose, control_duration_, backwards_);
geometry_msgs::msg::Pose2D p;
p.x = current_pose.position.x;
p.y = current_pose.position.y;
p.theta = tf2::getYaw(current_pose.orientation);
if(!local_collision_checker_->isCollisionFree(p, i==0)) {
return true;
}
if(check_rival_collision(p)) {
return true;
}
last_ok_pose = current_pose;
const double dist_left = distanceToTarget(current_pose, target_pose_.position, target_angle_, backwards_);
if(dist_left < 0.005) {
return false;
}
}
return false;
}
ResultStatus MoveCoords::updateVel(
const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist &,
geometry_msgs::msg::Twist & out_vel)
{
const double current_yaw = tf2::getYaw(pose.orientation);
double angle_dist = angles::shortest_angular_distance(current_yaw, target_angle_);
const double dx = target_pose_.position.x - pose.position.x;
const double dy = target_pose_.position.y - pose.position.y;
const double dist_left = target_sign_ * (dx * cos(target_angle_) + dy * sin(target_angle_));
const double lower_bound = last_speed_ - control_duration_ * max_vel_accel_;
const double upper_bound = last_speed_ + control_duration_ * max_vel_accel_;
double dist_left = distanceToTarget(pose, target_pose_.position, target_angle_, backwards_);
if (dist_left <= 0.001) {
out_vel.linear.x = 0;
@@ -109,30 +153,52 @@ ResultStatus MoveCoords::updateVel(
return ResultStatus{Status::SUCCEEDED};
}
double vel = max_vel_speed_;
double max_vel_to_stop = 0.8 * std::sqrt(2.0 * max_vel_decel_ * dist_left);
vel = std::min(vel, max_vel_to_stop);
const double current_yaw = tf2::getYaw(pose.orientation);
double angle_dist = angles::shortest_angular_distance(current_yaw, target_angle_);
geometry_msgs::msg::Pose last_ok_pose;
if(collisionDetection(pose, last_ok_pose)) {
dist_left = distanceToTarget(pose, last_ok_pose.position, tf2::getYaw(last_ok_pose.orientation), backwards_);
if(dist_left <= 0.02) {
out_vel.linear.x = 0;
out_vel.angular.z = 0;
} else {
scl_.v_linear_max = target_sign_ * velocityTarget(dist_left);
scl_.calculate_vel(last_ok_pose, pose, out_vel, backwards_);
}
last_speed_ = out_vel.linear.x;
RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Distance Left: %lf", dist_left);
RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Max Speed: %lf", scl_.v_linear_max);
return ResultStatus{Status::RUNNING};
}
if (dist_left <= 0.02) {
out_vel.linear.x = std::clamp(target_sign_ * vel, lower_bound, upper_bound);
out_vel.linear.x = velocityTarget(dist_left);
out_vel.angular.z = std::clamp(kp_ * angle_dist, -max_angular_speed_, max_angular_speed_);
last_speed_ = out_vel.linear.x;
return ResultStatus{Status::RUNNING};
}
scl.v_linear_max = target_sign_ * std::clamp(target_sign_ * vel, lower_bound, upper_bound);
scl.calculate_vel(target_pose_, pose, out_vel, backwards_);
scl_.v_linear_max = target_sign_ * velocityTarget(dist_left);
scl_.calculate_vel(target_pose_, pose, out_vel, backwards_);
last_speed_ = out_vel.linear.x;
RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Distance Left: %lf", dist_left);
RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Max Speed: %lf", scl.v_linear_max);
RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Distance Left: %lf", dist_left);
RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Max Speed: %lf", scl_.v_linear_max);
return ResultStatus{Status::RUNNING};
}
} // namespace toid
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(toid::MoveCoords, nav2_core::Behavior);

31
toid_behaviors/src/scl.cpp
View File

@@ -3,20 +3,21 @@
#include <cmath>
#include "angles/angles.h"
#include "nav2_util/geometry_utils.hpp"
#include "tf2/utils.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
namespace toid {
namespace toid
{
void SmoothControlLaw::calculate_vel(
const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current,
geometry_msgs::msg::Twist &out_speed, bool backwards)
geometry_msgs::msg::Twist & out_speed, bool backwards)
{
double r, phi, delta;
egocentric_polar(target, current, backwards, r, phi, delta);
double curvature = this->curvature(r, phi, delta);
curvature = backwards? -curvature : curvature;
curvature = backwards ? -curvature : curvature;
double v = v_linear_max / (1.0 + bbeta * std::pow(fabs(curvature), lam));
@@ -44,8 +45,8 @@ double SmoothControlLaw::curvature(double r, double phi, double delta)
}
void SmoothControlLaw::egocentric_polar(
const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current, bool backwards, double & r,
double & phi, double & delta)
const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current, bool backwards,
double & r, double & phi, double & delta)
{
const double dx = target.position.x - current.position.x;
const double dy = target.position.y - current.position.y;
@@ -59,4 +60,20 @@ void SmoothControlLaw::egocentric_polar(
delta = angles::normalize_angle(ctheta + los);
}
}
void SmoothControlLaw::step(
const geometry_msgs::msg::Pose & target, geometry_msgs::msg::Pose & current, double dt,
bool backwards)
{
geometry_msgs::msg::Twist twist;
calculate_vel(target, current, twist, backwards);
double theta = tf2::getYaw(current.orientation);
double dx = twist.linear.x * dt * cos(theta);
double dy = twist.linear.x * dt * sin(theta);
current.orientation =
nav2_util::geometry_utils::orientationAroundZAxis(theta + twist.angular.z * dt);
current.position.x += dx;
current.position.y += dy;
}
} // namespace toid

113
toid_behaviors/src/simple_rotate.cpp
View File

@@ -4,6 +4,7 @@
#include "angles/angles.h"
#include "tf2/convert.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
namespace toid
{
@@ -30,6 +31,10 @@ void SimpleRotateBehavior::configureCB()
nav2_util::declare_parameter_if_not_declared(
node, behavior_name_ + ".max_angular_decel", rclcpp::ParameterValue(4.0));
node->get_parameter(behavior_name_ + ".max_angular_decel", max_angular_decel_);
nav2_util::declare_parameter_if_not_declared(
node, behavior_name_ + ".lookahead", rclcpp::ParameterValue(0.5));
node->get_parameter(behavior_name_ + ".lookahead", lookahead_);
}
ResultStatus SimpleRotateBehavior::onStart(
@@ -40,12 +45,19 @@ ResultStatus SimpleRotateBehavior::onStart(
min_turn_angle_ = abs(command->min_angle);
initial_direction_ = (command->min_angle >= 0.0) ? 1.0 : -1.0;
max_angular_speed_ = command->max_speed;
mode_ = command->mode;
if(command->max_speed == 0) {
if (command->max_speed == 0) {
auto node = node_.lock();
node->get_parameter(behavior_name_ + ".max_angular_vel", max_angular_speed_);
}
geometry_msgs::msg::Pose2D pose2d;
pose2d.x = pose.position.x;
pose2d.y = pose.position.y;
pose2d.theta = initial_direction_;
local_collision_checker_->isCollisionFree(pose2d, true);
last_angle_ = tf2::getYaw(pose.orientation);
angular_speed_ = vel.angular.z;
@@ -53,43 +65,108 @@ ResultStatus SimpleRotateBehavior::onStart(
return ResultStatus{Status::SUCCEEDED};
}
void SimpleRotateBehavior::calc_err_and_sign(
double last_angle, double current_yaw, double & min_turn_angle, double & err, double & sign)
{
err = angles::shortest_angular_distance(current_yaw, target_angle_);
sign = (err < 0) ? -1.0 : 1.0;
err = std::abs(err);
if (min_turn_angle > 0.0) {
const double angle_change = angles::shortest_angular_distance(last_angle, current_yaw);
min_turn_angle = std::max(0.0, min_turn_angle - initial_direction_ * angle_change);
err = std::max(initial_direction_ * sign * err, 0.0);
err = std::max(min_turn_angle, err);
sign = initial_direction_;
}
}
double SimpleRotateBehavior::calc_speed(
const double err, const double sign, const double angular_speed)
{
const double upper_vel_ = angular_speed + max_angular_accel_ * control_duration_;
const double lower_vel_ = angular_speed - max_angular_accel_ * control_duration_;
const double speed_until_overshoot = std::sqrt(2.0 * max_angular_accel_ * std::fabs(err));
const double requested_speed = sign * std::min(speed_until_overshoot, max_angular_speed_);
const double speed = std::clamp(requested_speed, lower_vel_, upper_vel_);
return speed;
}
ResultStatus SimpleRotateBehavior::updateVel(
const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist &,
geometry_msgs::msg::Twist & out_vel)
{
const double current_yaw = tf2::getYaw(pose.orientation);
const double angle_change = angles::shortest_angular_distance(last_angle_ , current_yaw);
last_angle_ = current_yaw;
double min_turn_angle = min_turn_angle_;
double angular_speed = angular_speed_;
double err, sign;
double err = angles::shortest_angular_distance(current_yaw, target_angle_);
double sign = (err < 0)? -1.0 : 1.0;
err = std::abs(err);
calc_err_and_sign(last_angle_, current_yaw, min_turn_angle, err, sign);
if (min_turn_angle_ > 0.0) {
min_turn_angle_ = std::max(0.0, min_turn_angle_ - initial_direction_ * angle_change);
err = std::max( initial_direction_ * sign * err, 0.0);
err = std::max(min_turn_angle_, err);
sign = initial_direction_;
if (!(mode_ & RotateAction::Goal::IGNORE_OBSTACLES)) {
err = check_space(pose, err, sign);
}
const double upper_vel_ = angular_speed_ + max_angular_accel_ * control_duration_;
const double lower_vel_ = angular_speed_ - max_angular_accel_ * control_duration_;
double speed = 0.0;
const double speed_until_overshoot =
std::sqrt(2.0 * max_angular_accel_ * std::fabs(err));
const double requested_speed = sign * std::min(speed_until_overshoot, max_angular_speed_);
const double speed = std::clamp(requested_speed, lower_vel_, upper_vel_);
if (err != 0.0) {
speed = calc_speed(err, sign, angular_speed);
}
if (min_turn_angle_ == 0 && std::fabs(current_yaw - target_angle_) < 0.01) {
return ResultStatus{Status::SUCCEEDED};
}
min_turn_angle_ = min_turn_angle;
last_angle_ = current_yaw;
angular_speed_ = speed;
out_vel.angular.z = speed;
return ResultStatus{Status::RUNNING};
}
double SimpleRotateBehavior::check_space(
const geometry_msgs::msg::Pose pose, const double e, const double sign)
{
geometry_msgs::msg::Pose2D pose2d;
pose2d.x = pose.position.x;
pose2d.y = pose.position.y;
double initial_theta = tf2::getYaw(pose.orientation);
pose2d.theta = initial_theta;
const double step_size = 0.1;
const double err = std::min(e, 1.0);
const bool check_map = !(mode_ & RotateAction::Goal::IGNORE_OBSTACLES);
for (int i = 1; i < err / step_size; i++) {
pose2d.theta += sign * step_size;
if (check_map && !local_collision_checker_->isCollisionFree(pose2d, false)) {
RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
return step_size * (i - 1);
}
if (check_rival_collision(pose2d)) {
RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
return step_size * (i - 1);
}
}
pose2d.theta = initial_theta + sign * err;
if (check_map && !local_collision_checker_->isCollisionFree(pose2d, false)) {
RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
return step_size * ((int)(err / step_size));
}
if (check_rival_collision(pose2d)) {
RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
return step_size * ((int)(err / step_size));
}
return e;
}
} // namespace toid
#include "pluginlib/class_list_macros.hpp"