From cfba1f948d8fa19992832bc35a05923893d7ae93 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Wed, 4 Mar 2026 19:28:49 +0100 Subject: [PATCH] Added simple translation behavior --- toid_behaviors/CMakeLists.txt | 1 + .../toid_behaviors/simple_translate_x.hpp | 53 ++++++++++ toid_behaviors/src/simple_translate_x.cpp | 96 +++++++++++++++++++ toid_behaviors/toid_behaviors.xml | 3 + toid_msgs/CMakeLists.txt | 1 + toid_msgs/action/SimpleTranslateX.action | 12 +++ .../params/toid_general_params.yaml | 4 +- 7 files changed, 169 insertions(+), 1 deletion(-) create mode 100644 toid_behaviors/include/toid_behaviors/simple_translate_x.hpp create mode 100644 toid_behaviors/src/simple_translate_x.cpp create mode 100644 toid_msgs/action/SimpleTranslateX.action diff --git a/toid_behaviors/CMakeLists.txt b/toid_behaviors/CMakeLists.txt index a6526d4..dec5493 100644 --- a/toid_behaviors/CMakeLists.txt +++ b/toid_behaviors/CMakeLists.txt @@ -29,6 +29,7 @@ set( SOURCES src/simple_move.cpp src/simple_rotate.cpp + src/simple_translate_x.cpp ) find_package(ament_cmake REQUIRED) diff --git a/toid_behaviors/include/toid_behaviors/simple_translate_x.hpp b/toid_behaviors/include/toid_behaviors/simple_translate_x.hpp new file mode 100644 index 0000000..0b5329e --- /dev/null +++ b/toid_behaviors/include/toid_behaviors/simple_translate_x.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include "toid_behaviors/simple_move.hpp" +#include "toid_msgs/action/simple_translate_x.hpp" + +namespace toid +{ +using TranslateAction = toid_msgs::action::SimpleTranslateX; +using namespace nav2_behaviors; + +class SimpleTranslateXBehavior : public SimpleMove +{ +public: + SimpleTranslateXBehavior(); + ~SimpleTranslateXBehavior(); + + void configureCB() override; + + ResultStatus onStart( + const std::shared_ptr command, const geometry_msgs::msg::Pose & pose, + const geometry_msgs::msg::Twist & vel) override; + + ResultStatus updateVel( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & vel, + geometry_msgs::msg::Twist & out_vel) override; + + virtual nav2_core::CostmapInfoType getResourceInfo() override { + return nav2_core::CostmapInfoType::NONE; + } + +protected: + + //Goal + double target_distance_; + double target_angle_; + double target_sign_; + geometry_msgs::msg::Pose initial_pose_; + + //State + double speed_; + + //Config + double max_vel_accel_; + double max_vel_decel_; + double max_vel_speed_; + double min_vel_speed_; + double max_angular_speed_; + + double kp_; +}; + +} // namespace toid + diff --git a/toid_behaviors/src/simple_translate_x.cpp b/toid_behaviors/src/simple_translate_x.cpp new file mode 100644 index 0000000..1dcd53f --- /dev/null +++ b/toid_behaviors/src/simple_translate_x.cpp @@ -0,0 +1,96 @@ +#include "toid_behaviors/simple_translate_x.hpp" + +#include + +#include "angles/angles.h" +#include "tf2/convert.hpp" + +namespace toid +{ + +SimpleTranslateXBehavior::SimpleTranslateXBehavior() : SimpleMove() {} +SimpleTranslateXBehavior::~SimpleTranslateXBehavior() {} + +void SimpleTranslateXBehavior::configureCB() +{ + auto node = node_.lock(); + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".max_vel_accel", rclcpp::ParameterValue(2.0)); + node->get_parameter(behavior_name_ + ".max_vel_accel", max_vel_accel_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".max_vel_decel", rclcpp::ParameterValue(1.0)); + node->get_parameter(behavior_name_ + ".max_vel_decel", max_vel_decel_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".max_vel_speed", rclcpp::ParameterValue(0.4)); + node->get_parameter(behavior_name_ + ".max_vel_speed", max_vel_speed_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".min_vel_speed", rclcpp::ParameterValue(0.007)); + node->get_parameter(behavior_name_ + ".min_vel_speed", min_vel_speed_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".max_angular_speed", rclcpp::ParameterValue(1.0)); + node->get_parameter(behavior_name_ + ".max_angular_speed", max_angular_speed_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".kp", rclcpp::ParameterValue(5.0)); + node->get_parameter(behavior_name_ + ".kp", kp_); +} + +ResultStatus SimpleTranslateXBehavior::onStart( + const std::shared_ptr command, const geometry_msgs::msg::Pose & pose, + const geometry_msgs::msg::Twist & vel) +{ + target_distance_ = command->distance; + target_angle_ = tf2::getYaw(pose.orientation); + target_sign_ = (target_distance_ < 0) ? -1.0 : 1.0; + + target_distance_ *= target_sign_; + + initial_pose_ = pose; + speed_ = vel.angular.x; + + return ResultStatus{Status::SUCCEEDED}; +} + +ResultStatus SimpleTranslateXBehavior::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_dist = angles::shortest_angular_distance(current_yaw, target_angle_); + + const double dx = pose.position.x - initial_pose_.position.x; + const double dy = pose.position.y - initial_pose_.position.y; + const double dist_passed = target_sign_ * (dx * cos(target_angle_) + dy * sin(target_angle_)); + + const double lower_bound = speed_ - control_duration_ * max_vel_accel_; + const double upper_bound = speed_ + control_duration_ * max_vel_accel_; + + float vel = std::clamp(max_vel_speed_, lower_bound, upper_bound); + float max_vel_to_stop = + 0.8 * std::sqrt(2.0 * max_vel_decel_ * std::abs(target_distance_ - dist_passed)); + + vel = std::min(vel, max_vel_to_stop); + const double w = std::clamp(kp_ * angle_dist, -max_angular_speed_, max_angular_speed_); + + if (dist_passed >= target_distance_) { + out_vel.linear.x = 0; + out_vel.angular.z = 0; + return ResultStatus{Status::SUCCEEDED}; + } + + out_vel.linear.x = target_sign_ * vel; + out_vel.angular.z = w; + + speed_ = vel; + + return ResultStatus{Status::RUNNING}; +} + +} // namespace toid + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(toid::SimpleTranslateXBehavior, nav2_core::Behavior); \ No newline at end of file diff --git a/toid_behaviors/toid_behaviors.xml b/toid_behaviors/toid_behaviors.xml index a8ad4b3..48af7a1 100644 --- a/toid_behaviors/toid_behaviors.xml +++ b/toid_behaviors/toid_behaviors.xml @@ -3,5 +3,8 @@ + + + \ No newline at end of file diff --git a/toid_msgs/CMakeLists.txt b/toid_msgs/CMakeLists.txt index 4a27bd4..1b60254 100644 --- a/toid_msgs/CMakeLists.txt +++ b/toid_msgs/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "srv/SendDouble.srv" "action/SimpleRotate.action" + "action/SimpleTranslateX.action" ) ament_package() \ No newline at end of file diff --git a/toid_msgs/action/SimpleTranslateX.action b/toid_msgs/action/SimpleTranslateX.action new file mode 100644 index 0000000..59545bb --- /dev/null +++ b/toid_msgs/action/SimpleTranslateX.action @@ -0,0 +1,12 @@ +uint8 IGNORE_OBSTACLES=1 + +float64 distance +uint8 mode 0 +--- +uint16 NONE=0 +uint16 TF_ERROR=1 + +builtin_interfaces/Duration total_elapsed_time +uint16 error_code +string error_msg +--- diff --git a/toid_navigation/params/toid_general_params.yaml b/toid_navigation/params/toid_general_params.yaml index 66d5205..54152e7 100644 --- a/toid_navigation/params/toid_general_params.yaml +++ b/toid_navigation/params/toid_general_params.yaml @@ -22,7 +22,7 @@ behavior_server: local_footprint_topic: local_costmap/published_footprint global_footprint_topic: global_costmap/published_footprint cycle_frequency: 50.0 - behavior_plugins: ["backup", "spin", "drive_on_heading", "assisted_teleop", "wait", "rotate"] + behavior_plugins: ["backup", "spin", "drive_on_heading", "assisted_teleop", "wait", "rotate", "translateX"] spin: plugin: "nav2_behaviors::Spin" backup: @@ -37,6 +37,8 @@ behavior_server: plugin: "toid::SimpleRotateBehavior" max_angular_accel: 4.0 max_angular_decel: 1.0 + translateX: + plugin: "toid::SimpleTranslateXBehavior" local_frame: map global_frame: map robot_base_frame: base_footprint