From 969ceae6f84d27886e071358d46f2b15d8b11436 Mon Sep 17 00:00:00 2001 From: pimpest <82343504+pimpest@users.noreply.github.com> Date: Tue, 17 Feb 2026 13:49:31 +0100 Subject: [PATCH] Added simple rotate behavior --- .clang-format | 19 ++ toid_behaviors/CMakeLists.txt | 82 +++++++ toid_behaviors/LICENSE | 202 ++++++++++++++++++ .../include/toid_behaviors/simple_move.hpp | 104 +++++++++ .../include/toid_behaviors/simple_rotate.hpp | 43 ++++ toid_behaviors/package.xml | 31 +++ toid_behaviors/src/simple_move.cpp | 1 + toid_behaviors/src/simple_rotate.cpp | 87 ++++++++ toid_behaviors/toid_behaviors.xml | 7 + toid_msgs/CMakeLists.txt | 1 + toid_msgs/action/SimpleRotate.action | 13 ++ 11 files changed, 590 insertions(+) create mode 100644 .clang-format create mode 100644 toid_behaviors/CMakeLists.txt create mode 100644 toid_behaviors/LICENSE create mode 100644 toid_behaviors/include/toid_behaviors/simple_move.hpp create mode 100644 toid_behaviors/include/toid_behaviors/simple_rotate.hpp create mode 100644 toid_behaviors/package.xml create mode 100644 toid_behaviors/src/simple_move.cpp create mode 100644 toid_behaviors/src/simple_rotate.cpp create mode 100644 toid_behaviors/toid_behaviors.xml create mode 100644 toid_msgs/action/SimpleRotate.action diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..8bd76ad --- /dev/null +++ b/.clang-format @@ -0,0 +1,19 @@ +--- +Language: Cpp +BasedOnStyle: Google + +AccessModifierOffset: -2 +AlignAfterOpenBracket: AlwaysBreak +BraceWrapping: + AfterClass: true + AfterFunction: true + AfterNamespace: true + AfterStruct: true + AfterEnum: true +BreakBeforeBraces: Custom +ColumnLimit: 100 +ConstructorInitializerIndentWidth: 0 +ContinuationIndentWidth: 2 +DerivePointerAlignment: false +PointerAlignment: Middle +ReflowComments: false \ No newline at end of file diff --git a/toid_behaviors/CMakeLists.txt b/toid_behaviors/CMakeLists.txt new file mode 100644 index 0000000..a6526d4 --- /dev/null +++ b/toid_behaviors/CMakeLists.txt @@ -0,0 +1,82 @@ +cmake_minimum_required(VERSION 3.8) +project(toid_behaviors) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(library_name toid_behaviors) + +set( + PACKAGE_DEPS + + rclcpp + angles + geometry_msgs + pluginlib + nav_msgs + nav2_core + nav2_behaviors + nav2_costmap_2d + nav2_util + tf2 + tf2_geometry_msgs + tf2_ros + toid_msgs +) + +set( + SOURCES + src/simple_move.cpp + src/simple_rotate.cpp +) + +find_package(ament_cmake REQUIRED) +foreach(PACKAGE ${PACKAGE_DEPS}) + find_package(${PACKAGE} REQUIRED) +endforeach() + +add_library(${library_name} SHARED ${SOURCES}) + +target_include_directories( + ${library_name} + PRIVATE + + include +) + +ament_target_dependencies( + ${library_name} + ${PACKAGE_DEPS} +) + +install(TARGETS ${library_name} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + DIRECTORY include/ + DESTINATION include/ +) + +ament_export_include_directories(include) +ament_export_libraries(${library_name}) +ament_export_dependencies(${PACKAGE_DEPS}) + +pluginlib_export_plugin_description_file(nav2_core toid_behaviors.xml) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/toid_behaviors/LICENSE b/toid_behaviors/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/toid_behaviors/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/toid_behaviors/include/toid_behaviors/simple_move.hpp b/toid_behaviors/include/toid_behaviors/simple_move.hpp new file mode 100644 index 0000000..a32f110 --- /dev/null +++ b/toid_behaviors/include/toid_behaviors/simple_move.hpp @@ -0,0 +1,104 @@ +#pragma once +#include "nav2_behaviors/timed_behavior.hpp" +#include "nav2_core/behavior.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/twist_publisher.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" +#include "toid_msgs/action/simple_rotate.hpp" + +namespace toid +{ + +template +class SimpleMove : public nav2_behaviors::TimedBehavior +{ +public: + virtual void configureCB() {} + virtual void cleanupCB() {} + + virtual void activateCB() {} + virtual void deactivateCB() {} + + virtual nav2_behaviors::ResultStatus onStart( + const std::shared_ptr command, + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & vel) = 0; + + virtual nav2_behaviors::ResultStatus updateVel( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & vel, + geometry_msgs::msg::Twist & out_vel) = 0; + + void onConfigure() override + { + rclcpp_lifecycle::LifecycleNode::SharedPtr node = this->node_.lock(); + nav2_util::declare_parameter_if_not_declared( + node, "odom_topic", rclcpp::ParameterValue("/odom")); + std::string odom_topic_name = node->get_parameter("odom_topic").as_string(); + odom_sub_ = node->create_subscription( + odom_topic_name, 1, [&](nav_msgs::msg::Odometry msg) { + std::lock_guard lock(mut_); + current_pose_ = msg.pose.pose; + current_vel_ = msg.twist.twist; + }); + control_duration_ = 1.0 / this->cycle_frequency_; + } + + void onCleanup() override + { + odom_sub_.reset(); + cleanupCB(); + } + + void activate() override + { + nav2_behaviors::TimedBehavior::activate(); + activateCB(); + } + + void deactivate() override + { + nav2_behaviors::TimedBehavior::deactivate(); + deactivateCB(); + } + + nav2_behaviors::ResultStatus onRun( + const std::shared_ptr command) override + { + geometry_msgs::msg::Pose pose; + geometry_msgs::msg::Twist vel; + { + std::lock_guard lock(mut_); + pose = current_pose_; + vel = current_vel_; + } + return onStart(command, pose, vel); + } + + nav2_behaviors::ResultStatus onCycleUpdate() override + { + geometry_msgs::msg::Pose pose; + geometry_msgs::msg::Twist vel; + geometry_msgs::msg::Twist out_vel; + auto vel_p = std::make_unique(); + { + std::lock_guard lock(mut_); + pose = current_pose_; + vel = current_vel_; + } + nav2_behaviors::ResultStatus r = updateVel(pose, vel, out_vel); + vel_p->twist = out_vel; + vel_p->header.stamp = this->clock_->now(); + vel_p->header.frame_id = this->robot_base_frame_; + this->vel_pub_->publish(std::move(vel_p)); + return r; + } + +protected: + rclcpp::Subscription::SharedPtr odom_sub_; + geometry_msgs::msg::Pose current_pose_; + geometry_msgs::msg::Twist current_vel_; + std::recursive_mutex mut_; + double control_duration_; +}; + +} // namespace toid \ No newline at end of file diff --git a/toid_behaviors/include/toid_behaviors/simple_rotate.hpp b/toid_behaviors/include/toid_behaviors/simple_rotate.hpp new file mode 100644 index 0000000..bc3d12d --- /dev/null +++ b/toid_behaviors/include/toid_behaviors/simple_rotate.hpp @@ -0,0 +1,43 @@ +#pragma once + +#include "toid_behaviors/simple_move.hpp" +#include "toid_msgs/action/simple_rotate.h" + +namespace toid +{ +using RotateAction = toid_msgs::action::SimpleRotate; +using namespace nav2_behaviors; + +class SimpleRotateBehavior : public SimpleMove +{ +public: + SimpleRotateBehavior(); + + 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; + +protected: + + //Goal + double target_angle_; + double min_turn_angle_; + double initial_direction_; + + //State + double angular_speed_; + double last_angle_; + + //Config + double max_angular_speed_; + double min_angular_speed_; + double max_angular_accel_; +}; + +} // namespace toid diff --git a/toid_behaviors/package.xml b/toid_behaviors/package.xml new file mode 100644 index 0000000..270e4cc --- /dev/null +++ b/toid_behaviors/package.xml @@ -0,0 +1,31 @@ + + + + toid_behaviors + 0.0.0 + TODO: Package description + Pimpest + Apache-2.0 + + ament_cmake + + angles + geometry_msgs + nav_msgs + nav2_costmap_2d + nav2_util + nav2_core + nav2_controller + pluginlib + tf2 + tf2_geometry_msgs + tf2_ros + toid_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/toid_behaviors/src/simple_move.cpp b/toid_behaviors/src/simple_move.cpp new file mode 100644 index 0000000..f297700 --- /dev/null +++ b/toid_behaviors/src/simple_move.cpp @@ -0,0 +1 @@ +#include "toid_behaviors/simple_move.hpp" diff --git a/toid_behaviors/src/simple_rotate.cpp b/toid_behaviors/src/simple_rotate.cpp new file mode 100644 index 0000000..0d9692a --- /dev/null +++ b/toid_behaviors/src/simple_rotate.cpp @@ -0,0 +1,87 @@ +#include "toid_behaviors/simple_rotate.hpp" + +#include + +#include "angles/angles.h" +#include "tf2/convert.hpp" + +namespace toid +{ + +SimpleRotateBehavior::SimpleRotateBehavior() {} + +void SimpleRotateBehavior::configureCB() +{ + auto node = node_.lock(); + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".max_angular_vel", rclcpp::ParameterValue(2.0)); + + node->get_parameter(behavior_name_ + ".max_angular_vel", max_angular_speed_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".min_angular_vel", rclcpp::ParameterValue(0.2)); + node->get_parameter(behavior_name_ + ".min_angular_vel", min_angular_speed_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".max_angular_accel", rclcpp::ParameterValue(4.0)); + node->get_parameter(behavior_name_ + ".max_angular_accel", max_angular_accel_); +} + +ResultStatus SimpleRotateBehavior::onStart( + const std::shared_ptr command, const geometry_msgs::msg::Pose & pose, + const geometry_msgs::msg::Twist & vel) +{ + target_angle_ = command->angle; + min_turn_angle_ = abs(command->min_angle); + initial_direction_ = (command->min_angle >= 0.0) ? 1.0 : -1.0; + + last_angle_ = tf2::getYaw(pose.orientation); + + angular_speed_ = vel.angular.z; + + return ResultStatus{Status::SUCCEEDED}; +} + +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::normalize_angle(current_yaw - last_angle_); + last_angle_ = current_yaw; + + if (min_turn_angle_ > 0.0) { + min_turn_angle_ = fmax(0.0, min_turn_angle_ - initial_direction_ * angle_change); + } + + const double upper_vel_ = angular_speed_ + max_angular_accel_ * control_duration_; + const double lower_vel_ = angular_speed_ - max_angular_accel_ * control_duration_; + + double anglular_distance_to_target = + angles::shortest_angular_distance(current_yaw, target_angle_); + double sign = (anglular_distance_to_target >= 0.0) ? 1.0 : -1.0; + + if (min_turn_angle_ > 0.0 && initial_direction_ != sign) { + anglular_distance_to_target = angles::two_pi_complement(anglular_distance_to_target); + sign = initial_direction_; + } + + const double angular_distance_to_heading = + sign * fmax(std::fabs(anglular_distance_to_target), min_turn_angle_); + + 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::min(speed_until_overshoot, max_angular_speed_); + const double speed = std::clamp(requested_speed, lower_vel_, upper_vel_); + + if (min_turn_angle_ == 0 && std::fabs(current_yaw - target_angle_) < 0.01) { + return ResultStatus{Status::SUCCEEDED}; + } + + out_vel.angular.z = speed; + + return ResultStatus{Status::RUNNING}; +} + +} // namespace toid \ No newline at end of file diff --git a/toid_behaviors/toid_behaviors.xml b/toid_behaviors/toid_behaviors.xml new file mode 100644 index 0000000..a8ad4b3 --- /dev/null +++ b/toid_behaviors/toid_behaviors.xml @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/toid_msgs/CMakeLists.txt b/toid_msgs/CMakeLists.txt index f35ea38..4a27bd4 100644 --- a/toid_msgs/CMakeLists.txt +++ b/toid_msgs/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "srv/SendDouble.srv" + "action/SimpleRotate.action" ) ament_package() \ No newline at end of file diff --git a/toid_msgs/action/SimpleRotate.action b/toid_msgs/action/SimpleRotate.action new file mode 100644 index 0000000..0d6a1db --- /dev/null +++ b/toid_msgs/action/SimpleRotate.action @@ -0,0 +1,13 @@ +uint8 IGNORE_OBSTACLES=1 + +float64 angle +float64 min_angle 0 +uint8 mode 0 +--- +uint16 NONE=0 +uint16 TF_ERROR=1 + +builtin_interfaces/Duration total_elapsed_time +uint16 error_code +string error_msg +---