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

Changed behavior tree actions to ignore costmap

Browse Source
This commit is contained in:
pimpest
2026-04-01 17:51:29 +02:00
parent 88ebe9fb68
commit ff1fbf5abe
5 changed files with 24 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
toid_behaviors/include/toid_behaviors/move_coords.hpp
View File

@@ -47,6 +47,7 @@ protected:
double target_angle_;
double target_sign_;
bool backwards_;
unsigned char mode_;
//State
double last_speed_;

40
toid_behaviors/src/move_coords.cpp
View File

@@ -74,6 +74,8 @@ ResultStatus MoveCoords::onStart(
node->get_parameter(behavior_name_ + ".max_vel_speed", max_vel_speed_);
}
mode_ = command->mode;
scl_.k_phi = k_phi_;
scl_.k_delta = k_delta_;
scl_.bbeta = beta_;
@@ -94,28 +96,30 @@ double MoveCoords::distanceToTarget(
{
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;
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) {
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_);
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++) {
const bool check_map = !(mode_ & MoveAction::Goal::IGNORE_OBSTACLES);
for (int i = 0; i < samples; i++) {
scl_.step(target_pose_, current_pose, control_duration_, backwards_);
geometry_msgs::msg::Pose2D p;
@@ -123,17 +127,18 @@ bool MoveCoords::collisionDetection(const geometry_msgs::msg::Pose &pose, geomet
p.y = current_pose.position.y;
p.theta = tf2::getYaw(current_pose.orientation);
if(!local_collision_checker_->isCollisionFree(p, i==0)) {
if (check_map && !local_collision_checker_->isCollisionFree(p, i == 0)) {
return true;
}
if(check_rival_collision(p)) {
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) {
const double dist_left =
distanceToTarget(current_pose, target_pose_.position, target_angle_, backwards_);
if (dist_left < 0.005) {
return false;
}
}
@@ -144,7 +149,6 @@ ResultStatus MoveCoords::updateVel(
const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist &,
geometry_msgs::msg::Twist & out_vel)
{
double dist_left = distanceToTarget(pose, target_pose_.position, target_angle_, backwards_);
if (dist_left <= 0.001) {
@@ -157,10 +161,10 @@ ResultStatus MoveCoords::updateVel(
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) {
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 {
@@ -168,8 +172,6 @@ ResultStatus MoveCoords::updateVel(
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);
@@ -185,11 +187,9 @@ ResultStatus MoveCoords::updateVel(
return ResultStatus{Status::RUNNING};
}
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);