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 01/86] 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 +--- -- 2.49.1 From 29516efd0d221324ba8c05312494d7490dbc9f41 Mon Sep 17 00:00:00 2001 From: pimpest <82343504+pimpest@users.noreply.github.com> Date: Fri, 27 Feb 2026 14:24:15 +0100 Subject: [PATCH 02/86] Completed implementation of initial rotate behavior --- .../include/toid_behaviors/simple_move.hpp | 1 + .../include/toid_behaviors/simple_rotate.hpp | 7 +++++++ toid_behaviors/src/simple_rotate.cpp | 17 +++++++++++++---- toid_navigation/params/toid_general_params.yaml | 10 ++++++---- 4 files changed, 27 insertions(+), 8 deletions(-) diff --git a/toid_behaviors/include/toid_behaviors/simple_move.hpp b/toid_behaviors/include/toid_behaviors/simple_move.hpp index a32f110..546c437 100644 --- a/toid_behaviors/include/toid_behaviors/simple_move.hpp +++ b/toid_behaviors/include/toid_behaviors/simple_move.hpp @@ -41,6 +41,7 @@ public: current_vel_ = msg.twist.twist; }); control_duration_ = 1.0 / this->cycle_frequency_; + configureCB(); } void onCleanup() override diff --git a/toid_behaviors/include/toid_behaviors/simple_rotate.hpp b/toid_behaviors/include/toid_behaviors/simple_rotate.hpp index bc3d12d..def6994 100644 --- a/toid_behaviors/include/toid_behaviors/simple_rotate.hpp +++ b/toid_behaviors/include/toid_behaviors/simple_rotate.hpp @@ -12,6 +12,7 @@ class SimpleRotateBehavior : public SimpleMove { public: SimpleRotateBehavior(); + ~SimpleRotateBehavior(); void configureCB() override; @@ -22,6 +23,10 @@ public: 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() { + return nav2_core::CostmapInfoType::NONE; + } protected: @@ -33,11 +38,13 @@ protected: //State double angular_speed_; double last_angle_; + double last_time_; //Config double max_angular_speed_; double min_angular_speed_; double max_angular_accel_; + double max_angular_decel_; }; } // namespace toid diff --git a/toid_behaviors/src/simple_rotate.cpp b/toid_behaviors/src/simple_rotate.cpp index 0d9692a..a084b4b 100644 --- a/toid_behaviors/src/simple_rotate.cpp +++ b/toid_behaviors/src/simple_rotate.cpp @@ -8,7 +8,8 @@ namespace toid { -SimpleRotateBehavior::SimpleRotateBehavior() {} +SimpleRotateBehavior::SimpleRotateBehavior() : SimpleMove() {} +SimpleRotateBehavior::~SimpleRotateBehavior() {} void SimpleRotateBehavior::configureCB() { @@ -25,6 +26,10 @@ void SimpleRotateBehavior::configureCB() 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_); + + 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_); } ResultStatus SimpleRotateBehavior::onStart( @@ -38,6 +43,7 @@ ResultStatus SimpleRotateBehavior::onStart( last_angle_ = tf2::getYaw(pose.orientation); angular_speed_ = vel.angular.z; + last_time_ = clock_->now().seconds(); return ResultStatus{Status::SUCCEEDED}; } @@ -70,7 +76,7 @@ ResultStatus SimpleRotateBehavior::updateVel( 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)); + 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_); @@ -79,9 +85,12 @@ ResultStatus SimpleRotateBehavior::updateVel( return ResultStatus{Status::SUCCEEDED}; } + angular_speed_ = speed; out_vel.angular.z = speed; - return ResultStatus{Status::RUNNING}; } -} // namespace toid \ No newline at end of file +} // namespace toid + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(toid::SimpleRotateBehavior, nav2_core::Behavior); \ No newline at end of file diff --git a/toid_navigation/params/toid_general_params.yaml b/toid_navigation/params/toid_general_params.yaml index 7e91c2c..66d5205 100644 --- a/toid_navigation/params/toid_general_params.yaml +++ b/toid_navigation/params/toid_general_params.yaml @@ -21,20 +21,22 @@ behavior_server: global_costmap_topic: global_costmap/costmap_raw local_footprint_topic: local_costmap/published_footprint global_footprint_topic: global_costmap/published_footprint - cycle_frequency: 10.0 - behavior_plugins: ["backup", "spin", "drive_on_heading", "assisted_teleop", "wait"] + cycle_frequency: 50.0 + behavior_plugins: ["backup", "spin", "drive_on_heading", "assisted_teleop", "wait", "rotate"] spin: plugin: "nav2_behaviors::Spin" backup: plugin: "nav2_behaviors::BackUp" - enable_stamped_cmd_vel: true - simulate_ahead_time: 0.2 drive_on_heading: plugin: "nav2_behaviors::DriveOnHeading" wait: plugin: "nav2_behaviors::Wait" assisted_teleop: plugin: "nav2_behaviors::AssistedTeleop" + rotate: + plugin: "toid::SimpleRotateBehavior" + max_angular_accel: 4.0 + max_angular_decel: 1.0 local_frame: map global_frame: map robot_base_frame: base_footprint -- 2.49.1 From 1fd0e1b3120b24399558d9f9382d230bede6884b Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Sat, 28 Feb 2026 15:27:16 +0100 Subject: [PATCH 03/86] fixed docker file --- Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Dockerfile b/Dockerfile index 111d950..eecbf04 100644 --- a/Dockerfile +++ b/Dockerfile @@ -34,7 +34,7 @@ RUN rm -rf ./* RUN cat <> /root/.bashrc source /opt/ros/jazzy/setup.bash -if [[-f ./install/setup.bash ]] +if [[-f ./install/setup.bash ]] then; source ./install/setup.bash if EOF -- 2.49.1 From 123c2f37a881f0c9d7f217bcc4e4657d0f206f8c Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Sun, 1 Mar 2026 18:36:59 +0100 Subject: [PATCH 04/86] Fixed docker build and dependencies --- Dockerfile | 15 ++++++++------- toid_behaviors/package.xml | 2 +- toid_navigation/package.xml | 1 - 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/Dockerfile b/Dockerfile index eecbf04..c980ba2 100644 --- a/Dockerfile +++ b/Dockerfile @@ -18,12 +18,13 @@ RUN rosdep update WORKDIR /ros_ws # ---------- Copy package.xml files ---------- -COPY toid_bot_description/package.xml toid_bot_description/package.xml -COPY toid_control/package.xml toid_control/package.xml -COPY toid_msgs/package.xml toid_msgs/package.xml -COPY toid_odometry/package.xml toid_odometry/package.xml +COPY toid_bot_description/package.xml toid_bot_description/package.xml +COPY toid_control/package.xml toid_control/package.xml +COPY toid_msgs/package.xml toid_msgs/package.xml +COPY toid_odometry/package.xml toid_odometry/package.xml COPY toid_spinner_controller/package.xml toid_spinner_controller/package.xml -#COPY toid_navigation/package.xml toid_navigation/package.xml +COPY toid_behaviors/package.xml toid_behaviors/package.xml +COPY toid_navigation/package.xml toid_navigation/package.xml # ---------- Install dependencies ---------- RUN . /opt/ros/jazzy/setup.sh && \ @@ -34,9 +35,9 @@ RUN rm -rf ./* RUN cat <> /root/.bashrc source /opt/ros/jazzy/setup.bash -if [[-f ./install/setup.bash ]] then; +if [[ -f ./install/setup.bash ]]; then source ./install/setup.bash -if +fi EOF diff --git a/toid_behaviors/package.xml b/toid_behaviors/package.xml index 270e4cc..4fafaa1 100644 --- a/toid_behaviors/package.xml +++ b/toid_behaviors/package.xml @@ -15,7 +15,7 @@ nav2_costmap_2d nav2_util nav2_core - nav2_controller + nav2_behaviors pluginlib tf2 tf2_geometry_msgs diff --git a/toid_navigation/package.xml b/toid_navigation/package.xml index 23e46aa..bbc8b60 100644 --- a/toid_navigation/package.xml +++ b/toid_navigation/package.xml @@ -14,7 +14,6 @@ nav2_controller nav2_bt_navigator nav2_lifecycle_manager - nav2_bringup ros2_control rsl -- 2.49.1 From eaad7015a8b3f336aea9eeb45219db8bfd792dba Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Mon, 2 Mar 2026 16:54:01 +0100 Subject: [PATCH 05/86] toid_control: Launch odom node when running without visualize --- toid_control/launch/toid.launch.py | 1 - 1 file changed, 1 deletion(-) diff --git a/toid_control/launch/toid.launch.py b/toid_control/launch/toid.launch.py index 1aa3cb9..93fa682 100644 --- a/toid_control/launch/toid.launch.py +++ b/toid_control/launch/toid.launch.py @@ -41,7 +41,6 @@ def generate_launch_description(): name='map_to_odom_broadcaster', emulate_tty=True, parameters=[{'mock_odom': use_mock}], - condition=IfCondition(LaunchConfiguration('visualize')) ) -- 2.49.1 From d16a9523b9ce7afa03b29468b78b5ac1e7e70237 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Mon, 2 Mar 2026 16:56:44 +0100 Subject: [PATCH 06/86] Added launch configuration for behavior server --- toid_navigation/launch/main.py | 129 ++++++++++++++++++++++++++++ toid_navigation/maps/mapb2_5cm.yaml | 4 +- 2 files changed, 131 insertions(+), 2 deletions(-) create mode 100644 toid_navigation/launch/main.py diff --git a/toid_navigation/launch/main.py b/toid_navigation/launch/main.py new file mode 100644 index 0000000..9c02541 --- /dev/null +++ b/toid_navigation/launch/main.py @@ -0,0 +1,129 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import os + +def generate_launch_description(): + nav_pkg_share = FindPackageShare("").find('toid_navigation') + control_pkg_share = FindPackageShare("").find('toid_control') + params = os.path.join(nav_pkg_share, 'params', 'toid_general_params.yaml') + map = os.path.join(nav_pkg_share, 'maps', 'mapb2_5cm.yaml') + ctrl_launch_dir = os.path.join(control_pkg_share, 'launch') + default_rviz_config_path = os.path.join(nav_pkg_share, 'rviz', 'nav2.rviz') + bt_path = os.path.join(nav_pkg_share, "behaviors", "navigate_to_pose_w_backtracking.xml") + lattice_path = os.path.join(nav_pkg_share, "params", "output.json") + + visualize = LaunchConfiguration("visualize") + visualize_arg = DeclareLaunchArgument( + 'visualize', + default_value='False', + description="Whether to launch rviz2" + ) + + use_mock = LaunchConfiguration("use_mock") + use_mock_arg = DeclareLaunchArgument( + 'use_mock', + default_value='True', + description="Whether to launch rviz2" + ) + + run_nodes = LaunchConfiguration("run_nodes") + run_nodes_arg = DeclareLaunchArgument( + 'run_nodes', + default_value='True', + description="Whether to launch rviz2" + ) + + toid_control = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ctrl_launch_dir, 'toid.launch.py') + ), + launch_arguments={ + 'visualize': 'False', + 'use_mock': use_mock + }.items(), + condition=IfCondition(run_nodes), + ) + + map_server = Node( + package='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', + parameters=[{'yaml_filename': map}], + condition=IfCondition(run_nodes), + ) + + planner_server = Node( + package='nav2_planner', + executable='planner_server', + name='planner_server', + output='screen', + parameters=[{'GridBased':{'lattice_filepath': lattice_path}}, params], + condition=IfCondition(run_nodes), + ) + + controller_server = Node( + package='nav2_controller', + executable='controller_server', + name='controller_server', + output='screen', + parameters=[params], + condition=IfCondition(run_nodes), + ) + + bt_navigator = Node( + package='nav2_bt_navigator', + executable='bt_navigator', + name='bt_navigator', + output='screen', + parameters=[{'default_nav_to_pose_bt_xml': bt_path}, params], + condition=IfCondition(run_nodes), + ) + +# default_nav_to_pose_bt_xml: "$(find-pkg-share toid_navigation)/behaviors/navigate_to_pose_w_backtracking.xml" + + behavior_server = Node( + package='nav2_behaviors', + executable='behavior_server', + name='behavior_server', + output='screen', + parameters=[params], + condition=IfCondition(run_nodes), + ) + + lifecycle_manager_node = Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager', + output='screen', + parameters=[params], + condition=IfCondition(run_nodes), + ) + + rviz_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', default_rviz_config_path], + condition=IfCondition(visualize) + ) + + return LaunchDescription([ + visualize_arg, + use_mock_arg, + run_nodes_arg, + rviz_node, + map_server, + bt_navigator, + behavior_server, + planner_server, + controller_server, + lifecycle_manager_node, + toid_control + ]) \ No newline at end of file diff --git a/toid_navigation/maps/mapb2_5cm.yaml b/toid_navigation/maps/mapb2_5cm.yaml index ee47b45..e77f172 100644 --- a/toid_navigation/maps/mapb2_5cm.yaml +++ b/toid_navigation/maps/mapb2_5cm.yaml @@ -1,6 +1,6 @@ image: mapb2_5cm.png resolution: 0.025 origin: [-1.55, -1.05, 0.0] -occupied_thresh: 0.10 -free_thresh: 0.06 +occupied_thresh: 0.89 +free_thresh: 0.88 negate: 0 \ No newline at end of file -- 2.49.1 From a35ac1cfcf3515df747335fd18b2ac278ee8c52f Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Mon, 2 Mar 2026 17:02:31 +0100 Subject: [PATCH 07/86] Added nav2_map_server as required package --- toid_navigation/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/toid_navigation/package.xml b/toid_navigation/package.xml index bbc8b60..a82b538 100644 --- a/toid_navigation/package.xml +++ b/toid_navigation/package.xml @@ -11,6 +11,7 @@ robot_state_publisher nav2_behaviors nav2_planner + nav2_map_server nav2_controller nav2_bt_navigator nav2_lifecycle_manager -- 2.49.1 From 5f39a2622f962e2f39b388e6ecebbe26559a1e91 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Wed, 4 Mar 2026 16:11:51 +0100 Subject: [PATCH 08/86] Use pose relative to map form simple_move --- .../include/toid_behaviors/simple_move.hpp | 25 +++++++++++++------ 1 file changed, 18 insertions(+), 7 deletions(-) diff --git a/toid_behaviors/include/toid_behaviors/simple_move.hpp b/toid_behaviors/include/toid_behaviors/simple_move.hpp index 546c437..0c26e5d 100644 --- a/toid_behaviors/include/toid_behaviors/simple_move.hpp +++ b/toid_behaviors/include/toid_behaviors/simple_move.hpp @@ -37,7 +37,6 @@ public: 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_; @@ -65,28 +64,40 @@ public: nav2_behaviors::ResultStatus onRun( const std::shared_ptr command) override { - geometry_msgs::msg::Pose pose; + geometry_msgs::msg::PoseStamped pose; geometry_msgs::msg::Twist vel; { std::lock_guard lock(mut_); - pose = current_pose_; vel = current_vel_; } - return onStart(command, pose, vel); + + if (!nav2_util::getCurrentPose( + pose, *this->tf_, this->local_frame_, this->robot_base_frame_, + this->transform_tolerance_)) { + return nav2_behaviors::ResultStatus{nav2_behaviors::Status::FAILED}; + } + + return onStart(command, pose.pose, vel); } nav2_behaviors::ResultStatus onCycleUpdate() override { - geometry_msgs::msg::Pose pose; + geometry_msgs::msg::PoseStamped 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); + + if (!nav2_util::getCurrentPose( + pose, *this->tf_, this->local_frame_, this->robot_base_frame_, + this->transform_tolerance_)) { + return nav2_behaviors::ResultStatus{nav2_behaviors::Status::FAILED}; + } + + nav2_behaviors::ResultStatus r = updateVel(pose.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_; -- 2.49.1 From aaaf8cf495c4b1c34877c53dc1e7367085e7ba74 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Wed, 4 Mar 2026 16:12:41 +0100 Subject: [PATCH 09/86] Added override to overriden function --- toid_behaviors/include/toid_behaviors/simple_rotate.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/toid_behaviors/include/toid_behaviors/simple_rotate.hpp b/toid_behaviors/include/toid_behaviors/simple_rotate.hpp index def6994..cec55e2 100644 --- a/toid_behaviors/include/toid_behaviors/simple_rotate.hpp +++ b/toid_behaviors/include/toid_behaviors/simple_rotate.hpp @@ -24,7 +24,7 @@ public: const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & vel, geometry_msgs::msg::Twist & out_vel) override; - virtual nav2_core::CostmapInfoType getResourceInfo() { + virtual nav2_core::CostmapInfoType getResourceInfo() override { return nav2_core::CostmapInfoType::NONE; } -- 2.49.1 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 10/86] 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 -- 2.49.1 From f5e08389e2431ffbd9c5e02f0cead7871eeaf803 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Wed, 4 Mar 2026 20:35:46 +0100 Subject: [PATCH 11/86] Revized simple rotate for easier redability --- toid_behaviors/src/simple_rotate.cpp | 25 ++++++++++--------------- 1 file changed, 10 insertions(+), 15 deletions(-) diff --git a/toid_behaviors/src/simple_rotate.cpp b/toid_behaviors/src/simple_rotate.cpp index a084b4b..42a9bb2 100644 --- a/toid_behaviors/src/simple_rotate.cpp +++ b/toid_behaviors/src/simple_rotate.cpp @@ -53,30 +53,25 @@ ResultStatus SimpleRotateBehavior::updateVel( 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_); + const double angle_change = angles::shortest_angular_distance(last_angle_ , current_yaw); last_angle_ = current_yaw; + double err = angles::shortest_angular_distance(current_yaw, target_angle_); + double sign = (err < 0)? -1.0 : 1.0; + err = std::abs(err); + if (min_turn_angle_ > 0.0) { - min_turn_angle_ = fmax(0.0, min_turn_angle_ - initial_direction_ * angle_change); + 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_; } 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 = - std::sqrt(2.0 * max_angular_accel_ * std::fabs(angular_distance_to_heading)); + 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_); -- 2.49.1 From e496e193d1714a4ff1e883feb3308af343ecc10e Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Thu, 5 Mar 2026 00:17:49 +0100 Subject: [PATCH 12/86] Added move coords behavior --- toid_behaviors/CMakeLists.txt | 2 + .../include/toid_behaviors/move_coords.hpp | 64 +++++++++ toid_behaviors/include/toid_behaviors/scl.hpp | 31 ++++ toid_behaviors/src/move_coords.cpp | 136 ++++++++++++++++++ toid_behaviors/src/scl.cpp | 62 ++++++++ toid_behaviors/toid_behaviors.xml | 3 + toid_msgs/CMakeLists.txt | 1 + toid_msgs/action/SimpleMoveCoords.action | 15 ++ .../params/toid_general_params.yaml | 4 +- 9 files changed, 317 insertions(+), 1 deletion(-) create mode 100644 toid_behaviors/include/toid_behaviors/move_coords.hpp create mode 100644 toid_behaviors/include/toid_behaviors/scl.hpp create mode 100644 toid_behaviors/src/move_coords.cpp create mode 100644 toid_behaviors/src/scl.cpp create mode 100644 toid_msgs/action/SimpleMoveCoords.action diff --git a/toid_behaviors/CMakeLists.txt b/toid_behaviors/CMakeLists.txt index dec5493..b1652f5 100644 --- a/toid_behaviors/CMakeLists.txt +++ b/toid_behaviors/CMakeLists.txt @@ -27,6 +27,8 @@ set( set( SOURCES + src/scl.cpp + src/move_coords.cpp src/simple_move.cpp src/simple_rotate.cpp src/simple_translate_x.cpp diff --git a/toid_behaviors/include/toid_behaviors/move_coords.hpp b/toid_behaviors/include/toid_behaviors/move_coords.hpp new file mode 100644 index 0000000..0bf0170 --- /dev/null +++ b/toid_behaviors/include/toid_behaviors/move_coords.hpp @@ -0,0 +1,64 @@ +#pragma once + +#include "toid_behaviors/simple_move.hpp" +#include "toid_behaviors/scl.hpp" +#include "toid_msgs/action/simple_move_coords.hpp" + +#include "geometry_msgs/msg/pose.hpp" + +namespace toid +{ +using MoveAction = toid_msgs::action::SimpleMoveCoords; +using namespace nav2_behaviors; + +class MoveCoords : public SimpleMove +{ +public: + MoveCoords(); + ~MoveCoords(); + + 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: + + SmoothControlLaw scl; + + //Goal + geometry_msgs::msg::Pose target_pose_; + double target_angle_; + double target_sign_; + bool backwards_; + + //State + double last_speed_; + + //Config + double max_vel_accel_; + double max_vel_decel_; + double max_vel_speed_; + double min_vel_speed_; + + double max_angular_speed_; + + double kp_; + double k_phi_; + double k_delta_; + double beta_; + double lambda_; + double slowdown_radius_; +}; + +} // namespace toid + diff --git a/toid_behaviors/include/toid_behaviors/scl.hpp b/toid_behaviors/include/toid_behaviors/scl.hpp new file mode 100644 index 0000000..af8194f --- /dev/null +++ b/toid_behaviors/include/toid_behaviors/scl.hpp @@ -0,0 +1,31 @@ +#pragma once + +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/twist.hpp" + +namespace toid +{ +class SmoothControlLaw +{ +public: + double k_phi = 5.0; + double k_delta = 2.0; + double bbeta = 0.4; + double lam = 2.0; + + double slowdown_radius = 0.20; + double v_linear_min = 0.05; + double v_linear_max = 0.1; + double v_angular_max = 2.0; + + void egocentric_polar( + const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current, bool backwards, + double & r, double & phi, double & delta); + + double curvature(double r, double phi, double delta); + + void calculate_vel( + const geometry_msgs::msg::Pose & target,const geometry_msgs::msg::Pose & current, + geometry_msgs::msg::Twist & out_speed, bool backwards = false); +}; +} // namespace toid \ No newline at end of file diff --git a/toid_behaviors/src/move_coords.cpp b/toid_behaviors/src/move_coords.cpp new file mode 100644 index 0000000..4a2ebef --- /dev/null +++ b/toid_behaviors/src/move_coords.cpp @@ -0,0 +1,136 @@ +#include "toid_behaviors/move_coords.hpp" + +#include "angles/angles.h" + +namespace toid +{ + +MoveCoords::MoveCoords() : SimpleMove() {} +MoveCoords::~MoveCoords() {} + +void MoveCoords::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.10)); + 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_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".kphi", rclcpp::ParameterValue(5.0)); + node->get_parameter(behavior_name_ + ".kphi", k_phi_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".kdelta", rclcpp::ParameterValue(2.0)); + node->get_parameter(behavior_name_ + ".kdelta", k_delta_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".beta", rclcpp::ParameterValue(0.4)); + node->get_parameter(behavior_name_ + ".beta", beta_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".lambda", rclcpp::ParameterValue(2.0)); + node->get_parameter(behavior_name_ + ".lambda", lambda_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".slow_down_radius", rclcpp::ParameterValue(0.20)); + node->get_parameter(behavior_name_ + ".slow_down_radius", slowdown_radius_); +} + +ResultStatus MoveCoords::onStart( + const std::shared_ptr command, const geometry_msgs::msg::Pose &, + const geometry_msgs::msg::Twist & vel) +{ + target_pose_.position.x = command->x; + target_pose_.position.y = command->y; + backwards_ = command->backwards; + + target_pose_.orientation = tf2::toMsg(tf2::Quaternion::createFromRPY(0, 0, command->theta)); + target_angle_ = command->theta; + target_sign_ = backwards_ ? -1.0 : 1.0; + + 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}; +} + +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_); + + if (backwards_) { + angle_dist = angles::two_pi_complement(angle_dist); + } + + 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_; + + if (dist_left <= 0.001) { + out_vel.linear.x = 0; + out_vel.angular.z = 0; + 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); + + + if (dist_left <= 0.02) { + out_vel.linear.x = std::clamp(target_sign_ * vel, lower_bound, upper_bound); + out_vel.angular.z = std::clamp(target_sign_ * 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_); + last_speed_ = out_vel.linear.x; + + RCLCPP_INFO_THROTTLE(logger_, *clock_, 1000, "Distance Left: %lf", dist_left); + RCLCPP_INFO_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); \ No newline at end of file diff --git a/toid_behaviors/src/scl.cpp b/toid_behaviors/src/scl.cpp new file mode 100644 index 0000000..12ad53f --- /dev/null +++ b/toid_behaviors/src/scl.cpp @@ -0,0 +1,62 @@ +#include "toid_behaviors/scl.hpp" + +#include + +#include "angles/angles.h" +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + + +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) +{ + double r, phi, delta; + egocentric_polar(target, current, backwards, r, phi, delta); + + double curvature = this->curvature(r, phi, delta); + curvature = backwards? -curvature : curvature; + + double v = v_linear_max / (1.0 + bbeta * std::pow(fabs(curvature), lam)); + + v = std::min(v_linear_max * (r / slowdown_radius), v); + v = std::clamp(v, v_linear_min, v_linear_max); + v = backwards ? -v : v; + + const double w = curvature * v; + const double w_bound = std::clamp(w, -v_angular_max, v_angular_max); + + v = (curvature != 0.0) ? (w_bound / curvature) : v; + out_speed.linear.x = v; + out_speed.angular.z = w_bound; +} + +double SmoothControlLaw::curvature(double r, double phi, double delta) +{ + if (r < 1e-6) { + return 0.0; + } + const float prop = k_delta * (delta - std::atan(-k_phi * phi)); + const float feedback = (1 + k_phi / (1 + (k_phi * phi) * (k_phi * phi))) * std::sin(delta); + + return -1.0 / r * (prop + feedback); +} + +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 double dx = target.position.x - current.position.x; + const double dy = target.position.y - current.position.y; + const double los = backwards ? (std::atan2(-dy, dx) + M_PI) : std::atan2(-dy, dx); + + const double ttheta = tf2::getYaw(target.orientation); + const double ctheta = tf2::getYaw(current.orientation); + + r = std::hypot(dx, dy); + phi = angles::normalize_angle(ttheta + los); + delta = angles::normalize_angle(ctheta + los); +} + +} \ No newline at end of file diff --git a/toid_behaviors/toid_behaviors.xml b/toid_behaviors/toid_behaviors.xml index 48af7a1..10cc958 100644 --- a/toid_behaviors/toid_behaviors.xml +++ b/toid_behaviors/toid_behaviors.xml @@ -6,5 +6,8 @@ + + + \ No newline at end of file diff --git a/toid_msgs/CMakeLists.txt b/toid_msgs/CMakeLists.txt index 1b60254..a0a4761 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/SimpleMoveCoords.action" "action/SimpleRotate.action" "action/SimpleTranslateX.action" ) diff --git a/toid_msgs/action/SimpleMoveCoords.action b/toid_msgs/action/SimpleMoveCoords.action new file mode 100644 index 0000000..65df8ac --- /dev/null +++ b/toid_msgs/action/SimpleMoveCoords.action @@ -0,0 +1,15 @@ +uint8 IGNORE_OBSTACLES=1 + +float64 x +float64 y +float64 theta +uint8 backwards 0 +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 54152e7..54dd3bf 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", "translateX"] + behavior_plugins: ["backup", "spin", "drive_on_heading", "assisted_teleop", "wait", "rotate", "translateX", "moveCoords"] spin: plugin: "nav2_behaviors::Spin" backup: @@ -39,6 +39,8 @@ behavior_server: max_angular_decel: 1.0 translateX: plugin: "toid::SimpleTranslateXBehavior" + moveCoords: + plugin: "toid::MoveCoords" local_frame: map global_frame: map robot_base_frame: base_footprint -- 2.49.1 From 7e4a1ea7fe878fa546155e7a25d68e47a46f114c Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Thu, 5 Mar 2026 14:11:31 +0100 Subject: [PATCH 13/86] Fixed 0a getting converted to 0d 0a --- firmware/base/src/msgs.h | 16 ++++++++++++---- firmware/base/tusb/tusb_stdio_driver.c | 2 +- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/firmware/base/src/msgs.h b/firmware/base/src/msgs.h index 36624c2..fa71ac1 100644 --- a/firmware/base/src/msgs.h +++ b/firmware/base/src/msgs.h @@ -1,5 +1,6 @@ #pragma once +#include #include "crc.h" #include "stdio.h" @@ -58,15 +59,22 @@ enum { TSTATE_BUILD_MSG, }; -inline void send_tRespState(tRespState *resp) { +static inline void write_raw(void* data, size_t n) { + const uint8_t* a = data; + for(size_t i = 0; i < n; i++){ + putchar_raw(a[i]); + } +} + +static inline void send_tRespState(tRespState *resp) { resp->crc = crcFooter(*resp); - fwrite(resp, 1, sizeof(tRespState), stdout); + write_raw(resp, sizeof(tRespState)); fflush(stdout); } -inline void send_tRespEndCalib(tRespEndCalib *state) { +static inline void send_tRespEndCalib(tRespEndCalib *state) { state->crc = crcFooter(*state); - fwrite(state, 1, sizeof(tRespEndCalib), stdout); + write_raw(state, sizeof(tRespEndCalib)); fflush(stdout); } diff --git a/firmware/base/tusb/tusb_stdio_driver.c b/firmware/base/tusb/tusb_stdio_driver.c index 58c51f4..726b882 100644 --- a/firmware/base/tusb/tusb_stdio_driver.c +++ b/firmware/base/tusb/tusb_stdio_driver.c @@ -51,7 +51,7 @@ static void stdio_dual_usb_out_chars(const char *buf, int length) { tud_task(); tud_cdc_n_write_flush(itf); if (!stdio_dual_usb_connected() || - (!tud_cdc_write_available() && + (!tud_cdc_n_write_available(itf) && time_us_64() > last_avail_time[itf] + STDIO_DUAL_USB_STDOUT_TIMEOUT_US)) { break; -- 2.49.1 From c261ab1a808cb1846be5da57afdc5e27cc640ff5 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Fri, 6 Mar 2026 11:35:50 +0100 Subject: [PATCH 14/86] Disabled some logging --- scripts/idi.sh | 42 ++++++++++++++++++++++++++++++ toid_behaviors/src/move_coords.cpp | 4 +-- 2 files changed, 44 insertions(+), 2 deletions(-) create mode 100755 scripts/idi.sh diff --git a/scripts/idi.sh b/scripts/idi.sh new file mode 100755 index 0000000..5544f36 --- /dev/null +++ b/scripts/idi.sh @@ -0,0 +1,42 @@ +#!/bin/bash + +TARGET="$1" + +if [[ "$TARGET" == "-h" || "$TARGET" == "--help" ]]; then + exit 0 +fi + +if [[ $# -lt 1 ]]; then + echo "Error: Expected at least 1 arg" + echo "Available commands: napred, rotiraj, na" + exit 1 +fi + +if [[ $# -ge 2 ]]; then + SMER="$2" +fi + +source install/setup.bash + +case $TARGET in + "na") + ros2 action send_goal /moveCoords toid_msgs/action/SimpleMoveCoords " + x: 0.0 + y: 0.0 + theta: 0.0 + backwards: ${SMER:-0}" + ;; + "pravo") + ros2 action send_goal /translateX toid_msgs/action/SimpleTranslateX "distance: ${SMER:-0.3}" + ;; + "stani") + ros2 service call /moveCoords/_action/cancel_goal action_msgs/srv/CancelGoal & + ros2 service call /translateX/_action/cancel_goal action_msgs/srv/CancelGoal & + wait + ;; + *) + echo "Target not defined" + exit 1 + ;; +esac +exit 0 diff --git a/toid_behaviors/src/move_coords.cpp b/toid_behaviors/src/move_coords.cpp index 4a2ebef..7a6a259 100644 --- a/toid_behaviors/src/move_coords.cpp +++ b/toid_behaviors/src/move_coords.cpp @@ -123,8 +123,8 @@ ResultStatus MoveCoords::updateVel( scl.calculate_vel(target_pose_, pose, out_vel, backwards_); last_speed_ = out_vel.linear.x; - RCLCPP_INFO_THROTTLE(logger_, *clock_, 1000, "Distance Left: %lf", dist_left); - RCLCPP_INFO_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}; } -- 2.49.1 From a51b237b0cef3d20454013aed9be565b3ed8e158 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Fri, 6 Mar 2026 13:42:45 +0100 Subject: [PATCH 15/86] Added aditionaly necessary dependencies for toid_navigation --- toid_navigation/package.xml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/toid_navigation/package.xml b/toid_navigation/package.xml index a82b538..4b1a3c1 100644 --- a/toid_navigation/package.xml +++ b/toid_navigation/package.xml @@ -10,8 +10,10 @@ ament_cmake robot_state_publisher nav2_behaviors - nav2_planner nav2_map_server + nav2_planner + nav2_regulated_pure_pursuit_controller + nav2_smac_planner nav2_controller nav2_bt_navigator nav2_lifecycle_manager -- 2.49.1 From 83011ba187e697e503ae48d6d46404a021d369fc Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Fri, 6 Mar 2026 13:43:22 +0100 Subject: [PATCH 16/86] Added small script for running move commands --- scripts/idi.sh | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/scripts/idi.sh b/scripts/idi.sh index 5544f36..33274bb 100755 --- a/scripts/idi.sh +++ b/scripts/idi.sh @@ -16,6 +16,10 @@ if [[ $# -ge 2 ]]; then SMER="$2" fi +if [[ $# -ge 3 ]]; then + MIN_ROT="$(echo "$3*a(1)/45" | bc -l)" +fi + source install/setup.bash case $TARGET in @@ -29,9 +33,17 @@ case $TARGET in "pravo") ros2 action send_goal /translateX toid_msgs/action/SimpleTranslateX "distance: ${SMER:-0.3}" ;; + "rotiraj") + SMER="$(echo "$SMER*a(1)/45" | bc -l)" + echo $SMER + ros2 action send_goal /rotate toid_msgs/action/SimpleRotate " + angle: ${SMER:-0.3} + min_angle: ${MIN_ROT:-0.0}" + ;; "stani") ros2 service call /moveCoords/_action/cancel_goal action_msgs/srv/CancelGoal & ros2 service call /translateX/_action/cancel_goal action_msgs/srv/CancelGoal & + ros2 service call /rotate/_action/cancel_goal action_msgs/srv/CancelGoal & wait ;; *) -- 2.49.1 From ae3d0550cdf16f344f1de88a297d03851671d4c4 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Fri, 6 Mar 2026 13:45:44 +0100 Subject: [PATCH 17/86] Normalized angle passed as simple rotate goal --- toid_behaviors/src/simple_rotate.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/toid_behaviors/src/simple_rotate.cpp b/toid_behaviors/src/simple_rotate.cpp index 42a9bb2..f31e36d 100644 --- a/toid_behaviors/src/simple_rotate.cpp +++ b/toid_behaviors/src/simple_rotate.cpp @@ -36,7 +36,7 @@ ResultStatus SimpleRotateBehavior::onStart( const std::shared_ptr command, const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & vel) { - target_angle_ = command->angle; + target_angle_ = angles::normalize_angle(command->angle); min_turn_angle_ = abs(command->min_angle); initial_direction_ = (command->min_angle >= 0.0) ? 1.0 : -1.0; -- 2.49.1 From 1a2661fa775573d08ecdcf64ab4ab62869b14919 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Fri, 6 Mar 2026 14:22:42 +0100 Subject: [PATCH 18/86] Added BehaviorTree.ROS2 as submodule --- .gitmodules | 3 +++ ext/BehaviorTree.ROS2 | 1 + 2 files changed, 4 insertions(+) create mode 100644 .gitmodules create mode 160000 ext/BehaviorTree.ROS2 diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..4c994e1 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "ext/BehaviorTree.ROS2"] + path = ext/BehaviorTree.ROS2 + url = https://github.com/BehaviorTree/BehaviorTree.ROS2.git diff --git a/ext/BehaviorTree.ROS2 b/ext/BehaviorTree.ROS2 new file mode 160000 index 0000000..6c6aa07 --- /dev/null +++ b/ext/BehaviorTree.ROS2 @@ -0,0 +1 @@ +Subproject commit 6c6aa078ee7bc52fec98984bed4964556abf5beb -- 2.49.1 From 5c8ed91ccfafc5eecf456f8b409c40c15619af53 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Fri, 6 Mar 2026 20:52:01 +0100 Subject: [PATCH 19/86] Added initial batch of behaviors --- toid_bt/CMakeLists.txt | 62 ++++++ toid_bt/LICENSE | 202 ++++++++++++++++++ toid_bt/behavior_trees/behavior1.xml | 33 +++ .../behavior_trees/calibration_routines.xml | 53 +++++ toid_bt/behavior_trees/toid_behaviors.btproj | 39 ++++ toid_bt/include/toid_bt/bt_executor.hpp | 34 +++ toid_bt/include/toid_bt/plugin.hpp | 8 + .../toid_bt/plugins/empty_srv_action.hpp | 30 +++ .../toid_bt/plugins/end_calib_action.hpp | 56 +++++ .../toid_bt/plugins/move_coords_action.hpp | 65 ++++++ .../include/toid_bt/plugins/rotate_action.hpp | 63 ++++++ .../toid_bt/plugins/rotate_towards_action.hpp | 70 ++++++ toid_bt/launch/bt.launch.py | 41 ++++ toid_bt/package.xml | 31 +++ toid_bt/params/bt_params.yaml | 9 + toid_bt/src/bt_executor.cpp | 103 +++++++++ 16 files changed, 899 insertions(+) create mode 100644 toid_bt/CMakeLists.txt create mode 100644 toid_bt/LICENSE create mode 100644 toid_bt/behavior_trees/behavior1.xml create mode 100644 toid_bt/behavior_trees/calibration_routines.xml create mode 100644 toid_bt/behavior_trees/toid_behaviors.btproj create mode 100644 toid_bt/include/toid_bt/bt_executor.hpp create mode 100644 toid_bt/include/toid_bt/plugin.hpp create mode 100644 toid_bt/include/toid_bt/plugins/empty_srv_action.hpp create mode 100644 toid_bt/include/toid_bt/plugins/end_calib_action.hpp create mode 100644 toid_bt/include/toid_bt/plugins/move_coords_action.hpp create mode 100644 toid_bt/include/toid_bt/plugins/rotate_action.hpp create mode 100644 toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp create mode 100644 toid_bt/launch/bt.launch.py create mode 100644 toid_bt/package.xml create mode 100644 toid_bt/params/bt_params.yaml create mode 100644 toid_bt/src/bt_executor.cpp diff --git a/toid_bt/CMakeLists.txt b/toid_bt/CMakeLists.txt new file mode 100644 index 0000000..e0a8cf9 --- /dev/null +++ b/toid_bt/CMakeLists.txt @@ -0,0 +1,62 @@ +cmake_minimum_required(VERSION 3.8) +project(toid_bt) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) + +set(PACKAGE_DEPS + rclcpp + angles + behaviortree_cpp + behaviortree_ros2 + btcpp_ros2_interfaces + nav2_util + tf2 + tf2_ros + tf2_geometry_msgs + geometry_msgs + toid_msgs + std_msgs + std_srvs +) + +foreach(PACKAGE ${PACKAGE_DEPS}) + find_package(${PACKAGE} REQUIRED) +endforeach() + +set(SOURCES + src/bt_executor.cpp +) + +add_executable(toid_bt ${SOURCES}) + +target_include_directories( + toid_bt + PRIVATE + include +) + +ament_target_dependencies(toid_bt ${PACKAGE_DEPS}) + +install(TARGETS toid_bt DESTINATION lib/${PROJECT_NAME}) + +install( + DIRECTORY + launch + params + behavior_trees + DESTINATION share/${PROJECT_NAME}/ +) + +target_compile_features( + toid_bt PUBLIC + c_std_99 + cxx_std_17 +) + + +ament_package() + diff --git a/toid_bt/LICENSE b/toid_bt/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/toid_bt/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_bt/behavior_trees/behavior1.xml b/toid_bt/behavior_trees/behavior1.xml new file mode 100644 index 0000000..e3023e1 --- /dev/null +++ b/toid_bt/behavior_trees/behavior1.xml @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + Action server name + + + + diff --git a/toid_bt/behavior_trees/calibration_routines.xml b/toid_bt/behavior_trees/calibration_routines.xml new file mode 100644 index 0000000..2a8d556 --- /dev/null +++ b/toid_bt/behavior_trees/calibration_routines.xml @@ -0,0 +1,53 @@ + + + + + + + + + + + + + + + + + + + + Action server name + + + + + + Action server name + + + Service name + + + + diff --git a/toid_bt/behavior_trees/toid_behaviors.btproj b/toid_bt/behavior_trees/toid_behaviors.btproj new file mode 100644 index 0000000..e8854a1 --- /dev/null +++ b/toid_bt/behavior_trees/toid_behaviors.btproj @@ -0,0 +1,39 @@ + + + + + + + + Service name + + + + + + Action server name + + + + + + Action server name + + + + + + + Action server name + + + + + + Service name + + + Service name + + + diff --git a/toid_bt/include/toid_bt/bt_executor.hpp b/toid_bt/include/toid_bt/bt_executor.hpp new file mode 100644 index 0000000..7e95045 --- /dev/null +++ b/toid_bt/include/toid_bt/bt_executor.hpp @@ -0,0 +1,34 @@ +#pragma once +#include "rclcpp/rclcpp.hpp" + +#include "behaviortree_ros2/tree_execution_server.hpp" +#include "behaviortree_cpp/loggers/bt_cout_logger.h" + +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" + +#include "geometry_msgs/msg/pose_stamped.hpp" + +namespace toid { + class TreeExecutor : public BT::TreeExecutionServer { + public: + TreeExecutor(const rclcpp::NodeOptions opts); + void onTreeCreated(BT::Tree& tree) override; + + void registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) override; + + std::optional onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) override; + + void position(geometry_msgs::msg::PoseStamped &pose); + + std::string describeCustomNodes(); + + private: + std::shared_ptr logger_cout_; + tf2_ros::Buffer::SharedPtr tf_buffer_; + std::shared_ptr tf_listener_; + + std::string base_frame_; + std::string global_frame_; + }; +} \ No newline at end of file diff --git a/toid_bt/include/toid_bt/plugin.hpp b/toid_bt/include/toid_bt/plugin.hpp new file mode 100644 index 0000000..6a7fc85 --- /dev/null +++ b/toid_bt/include/toid_bt/plugin.hpp @@ -0,0 +1,8 @@ +#pragma once +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" + +namespace toid { + using PoseFunc = std::function; +} \ No newline at end of file diff --git a/toid_bt/include/toid_bt/plugins/empty_srv_action.hpp b/toid_bt/include/toid_bt/plugins/empty_srv_action.hpp new file mode 100644 index 0000000..0719f96 --- /dev/null +++ b/toid_bt/include/toid_bt/plugins/empty_srv_action.hpp @@ -0,0 +1,30 @@ +#pragma once + +#include "angles/angles.h" +#include "behaviortree_ros2/bt_service_node.hpp" +#include "std_srvs/srv/empty.hpp" +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "toid_bt/plugin.hpp" + +namespace toid +{ + +class EmptySrvNode : public BT::RosServiceNode +{ +public: + EmptySrvNode( + const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params) + : BT::RosServiceNode(name, conf, params) + { + } + + bool setRequest(typename Request::SharedPtr &) override { return true; } + + BT::NodeStatus onResponseReceived(const typename Response::SharedPtr &) override + { + return BT::NodeStatus::SUCCESS; + } +}; + +} // namespace toid \ No newline at end of file diff --git a/toid_bt/include/toid_bt/plugins/end_calib_action.hpp b/toid_bt/include/toid_bt/plugins/end_calib_action.hpp new file mode 100644 index 0000000..35127fc --- /dev/null +++ b/toid_bt/include/toid_bt/plugins/end_calib_action.hpp @@ -0,0 +1,56 @@ +#pragma once + +#include "angles/angles.h" +#include "behaviortree_ros2/bt_service_node.hpp" +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "toid_bt/plugin.hpp" +#include "toid_msgs/srv/send_double.hpp" + +namespace toid +{ + +class EndCalibNode : public BT::RosServiceNode +{ +public: + EndCalibNode( + const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params, + PoseFunc pose) + : BT::RosServiceNode(name, conf, params), get_pose(pose) + { + } + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ + BT::InputPort("width"), BT::InputPort("count", 1, {}), + BT::OutputPort("new_width"), + //BT::InputPort("options"), + }); + } + + bool setRequest(typename Request::SharedPtr & request) override + { + auto width = getInput("width").value(); + auto count = getInput("count").value(); + + geometry_msgs::msg::PoseStamped pose; + get_pose(pose); + + double theta = tf2::getYaw(pose.pose.orientation); + double new_width = width * (1 + (theta / (2 * M_PI * count))); + request->data = new_width; + + setOutput("new_width", new_width); + return true; + } + + BT::NodeStatus onResponseReceived(const typename Response::SharedPtr &) override + { + return BT::NodeStatus::SUCCESS; + } + + PoseFunc get_pose; +}; + +} // namespace toid \ No newline at end of file diff --git a/toid_bt/include/toid_bt/plugins/move_coords_action.hpp b/toid_bt/include/toid_bt/plugins/move_coords_action.hpp new file mode 100644 index 0000000..6bc5b6c --- /dev/null +++ b/toid_bt/include/toid_bt/plugins/move_coords_action.hpp @@ -0,0 +1,65 @@ +#pragma once + +#include "angles/angles.h" +#include "behaviortree_ros2/bt_action_node.hpp" +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "toid_bt/plugin.hpp" +#include "toid_msgs/action/simple_move_coords.hpp" + +namespace toid +{ + +class MovePointNode : public BT::RosActionNode +{ +public: + MovePointNode( + const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params, + PoseFunc pose) + : BT::RosActionNode(name, conf, params), get_pose(pose) + { + std::cout << "wtf? " << params.default_port_value << std::endl; + } + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ + BT::InputPort("x"), BT::InputPort("y"), BT::InputPort("theta"), + //BT::InputPort("options"), + }); + } + + bool setGoal(Goal & goal) override + { + auto x_goal = getInput("x"); + auto y_goal = getInput("y"); + auto theta = getInput("theta"); + goal.x = x_goal.value(); + goal.y = y_goal.value(); + if (!theta.has_value()) { + geometry_msgs::msg::PoseStamped pose; + get_pose(pose); + goal.theta = tf2::getYaw(pose.pose.orientation); + } else { + goal.theta = angles::from_degrees(theta.value_or(0)); + } + return true; + } + + void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); } + + BT::NodeStatus onResultReceived(const WrappedResult & wr) override + { + return (wr.result->error_code == 0) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; + } + + BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override + { + RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error)); + return BT::NodeStatus::FAILURE; + } + + PoseFunc get_pose; +}; + +} // namespace toid \ No newline at end of file diff --git a/toid_bt/include/toid_bt/plugins/rotate_action.hpp b/toid_bt/include/toid_bt/plugins/rotate_action.hpp new file mode 100644 index 0000000..be76e20 --- /dev/null +++ b/toid_bt/include/toid_bt/plugins/rotate_action.hpp @@ -0,0 +1,63 @@ +#pragma once + +#include "angles/angles.h" +#include "behaviortree_ros2/bt_action_node.hpp" +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "toid_bt/plugin.hpp" +#include "toid_msgs/action/simple_rotate.hpp" + +namespace toid +{ + +class RotateNode : public BT::RosActionNode +{ +public: + RotateNode( + const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params, + PoseFunc pose) + : BT::RosActionNode(name, conf, params), get_pose(pose) + { + } + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ + BT::InputPort("angle", {}), + BT::InputPort("min_angle", 0, {}), + BT::InputPort("radians", false, {}), + //BT::InputPort("options"), + }); + } + + bool setGoal(Goal & goal) override + { + auto angle = getInput("angle"); + auto min_angle = getInput("min_angle"); + auto radians = getInput("radians"); + goal.angle = angle.value(); + goal.min_angle = min_angle.value(); + if (!radians.value()) { + goal.angle = angles::from_degrees(goal.angle); + goal.min_angle = angles::from_degrees(goal.min_angle); + } + return true; + } + + void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); } + + BT::NodeStatus onResultReceived(const WrappedResult & wr) override + { + return (wr.result->error_code == 0) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; + } + + BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override + { + RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error)); + return BT::NodeStatus::FAILURE; + } + + PoseFunc get_pose; +}; + +} // namespace toid \ No newline at end of file diff --git a/toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp b/toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp new file mode 100644 index 0000000..fb39f24 --- /dev/null +++ b/toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp @@ -0,0 +1,70 @@ +#pragma once + +#include "angles/angles.h" +#include "behaviortree_ros2/bt_action_node.hpp" +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "toid_bt/plugin.hpp" +#include "toid_msgs/action/simple_rotate.hpp" + +namespace toid +{ + +class RotateTowardsNode : public BT::RosActionNode +{ +public: + RotateTowardsNode( + const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params, + PoseFunc pose) + : BT::RosActionNode(name, conf, params), get_pose(pose) + { + } + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ + BT::InputPort("x", {}), + BT::InputPort("y", {}), + BT::InputPort("min_angle", 0, {}), + BT::InputPort("radians", false, {}), + //BT::InputPort("options"), + }); + } + + bool setGoal(Goal & goal) override + { + auto x = getInput("x").value(); + auto y = getInput("y").value(); + auto min_angle = getInput("min_angle"); + auto radians = getInput("radians"); + + geometry_msgs::msg::PoseStamped pose; + get_pose(pose); + + goal.angle = std::atan2(y - pose.pose.position.y, x - pose.pose.position.x); + goal.min_angle = min_angle.value(); + + if (!radians.value()) { + goal.min_angle = angles::from_degrees(goal.min_angle); + } + + return true; + } + + void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); } + + BT::NodeStatus onResultReceived(const WrappedResult & wr) override + { + return (wr.result->error_code == 0) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; + } + + BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override + { + RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error)); + return BT::NodeStatus::FAILURE; + } + + PoseFunc get_pose; +}; + +} // namespace toid \ No newline at end of file diff --git a/toid_bt/launch/bt.launch.py b/toid_bt/launch/bt.launch.py new file mode 100644 index 0000000..ab6f55d --- /dev/null +++ b/toid_bt/launch/bt.launch.py @@ -0,0 +1,41 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import Command, LaunchConfiguration, IfElseSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import os + +def generate_launch_description(): + + pkg_share = FindPackageShare("").find('toid_bt') + bt_params = os.path.join(pkg_share, 'params', 'bt_params.yaml') + + visualize = LaunchConfiguration("visualize") + visualize_arg = DeclareLaunchArgument( + 'visualize', + default_value='False', + description="Whether to launch rviz2" + ) + + use_mock = LaunchConfiguration("use_mock") + use_mock_arg = DeclareLaunchArgument( + 'use_mock', + default_value='True', + description="Whether to use mock controller" + ) + + bt_executor = Node( + package='toid_bt', + executable='toid_bt', + output='screen', + emulate_tty=True, + parameters=[bt_params], + ) + + return LaunchDescription([ + visualize_arg, + use_mock_arg, + bt_executor, + ]) + diff --git a/toid_bt/package.xml b/toid_bt/package.xml new file mode 100644 index 0000000..146dca5 --- /dev/null +++ b/toid_bt/package.xml @@ -0,0 +1,31 @@ + + + + toid_bt + 0.0.0 + TODO: Package description + pimpest + Apache-2.0 + + ament_cmake + + rclcpp + angles + behaviortree_ros2 + btcpp_ros2_interfaces + nav2_util + tf2 + tf2_ros + tf2_geometry_msgs + geometry_msgs + toid_msgs + std_msgs + std_srvs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/toid_bt/params/bt_params.yaml b/toid_bt/params/bt_params.yaml new file mode 100644 index 0000000..5ac5c62 --- /dev/null +++ b/toid_bt/params/bt_params.yaml @@ -0,0 +1,9 @@ +toid_bt: + ros__parameters: + action_name: "bt_run" + tick_frequency: 100 + groot2_port: 8420 + ros_plugins_timeout: 1000 + + behavior_trees: + - toid_bt/behavior_trees \ No newline at end of file diff --git a/toid_bt/src/bt_executor.cpp b/toid_bt/src/bt_executor.cpp new file mode 100644 index 0000000..8679486 --- /dev/null +++ b/toid_bt/src/bt_executor.cpp @@ -0,0 +1,103 @@ +#include "toid_bt/bt_executor.hpp" + +#include "behaviortree_cpp/xml_parsing.h" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/robot_utils.hpp" +#include "rclcpp/rclcpp.hpp" +#include "toid_bt/plugins/empty_srv_action.hpp" +#include "toid_bt/plugins/end_calib_action.hpp" +#include "toid_bt/plugins/move_coords_action.hpp" +#include "toid_bt/plugins/rotate_action.hpp" +#include "toid_bt/plugins/rotate_towards_action.hpp" + +namespace toid +{ +TreeExecutor::TreeExecutor(const rclcpp::NodeOptions opts) +: BT::TreeExecutionServer(std::make_shared("toid_bt", opts)) +{ + /* + executeRegistration(); + std::cout << BT::writeTreeNodesModelXML(factory()) << std::endl; + tf_buffer_ = std::make_shared(node()->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + */ + + nav2_util::declare_parameter_if_not_declared( + node(), "base_frame", rclcpp::ParameterValue("base_footprint")); + node()->get_parameter("base_frame", base_frame_); + + nav2_util::declare_parameter_if_not_declared( + node(), "global_frame", rclcpp::ParameterValue("map")); + node()->get_parameter("global_frame", global_frame_); +} + +void TreeExecutor::onTreeCreated(BT::Tree & tree) +{ + logger_cout_ = std::make_shared(tree); +} + +void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory) +{ + PoseFunc get_pose = [this](geometry_msgs::msg::PoseStamped & pose) { this->position(pose); }; + rclcpp::Node::SharedPtr nh = node(); + factory.registerNodeType( + "MovePointSimple", BT::RosNodeParams(nh, "/moveCoords"), get_pose); + + factory.registerNodeType( + "RotateSimple", BT::RosNodeParams(nh, "/rotate"), get_pose); + + factory.registerNodeType( + "RotateTowards", BT::RosNodeParams(nh, "/rotate"), get_pose); + + factory.registerNodeType( + "SetWidth", BT::RosNodeParams(nh, "/set_width"), get_pose); + + factory.registerNodeType( + "ZeroOdom", BT::RosNodeParams(nh, "/zero")); + + factory.registerNodeType( + "EndCalib", BT::RosNodeParams(nh, "/end_calib")); + + std::cout << describeCustomNodes() << std::endl; + +} + +void TreeExecutor::position(geometry_msgs::msg::PoseStamped & pose) +{ + nav2_util::getCurrentPose(pose, *tf_buffer_, global_frame_, base_frame_); +} + +std::optional TreeExecutor::onTreeExecutionCompleted( + BT::NodeStatus status, bool was_cancelled) +{ + (void)status; + logger_cout_.reset(); + + if (was_cancelled) { + std::cout << "Behavior tree was cancled" << std::endl; + } + + return std::nullopt; +} + +std::string TreeExecutor::describeCustomNodes() { return BT::writeTreeNodesModelXML(factory()); } + +} // namespace toid + +int main(const int argc, const char * const * argv) +{ + rclcpp::init(argc, argv); + + rclcpp::NodeOptions options; + auto bt_exec_server = std::make_shared(options); + + rclcpp::executors::MultiThreadedExecutor executor( + rclcpp::ExecutorOptions(), 0, false, std::chrono::milliseconds(250)); + + executor.add_node(bt_exec_server->node()); + executor.spin(); + executor.remove_node(bt_exec_server->node()); + + rclcpp::shutdown(); + return 0; +} \ No newline at end of file -- 2.49.1 From 5777a0e804abf1c951c2267ec60f6c0c18f20f53 Mon Sep 17 00:00:00 2001 From: pimpest <82343504+pimpest@users.noreply.github.com> Date: Sat, 7 Mar 2026 12:31:42 +0100 Subject: [PATCH 20/86] cleanup params and launch files --- toid_navigation/launch/mock_control_launch.py | 93 -------------- toid_navigation/launch/navigation_launch.py | 116 ------------------ .../params/toid_general_params.yaml | 44 +------ 3 files changed, 2 insertions(+), 251 deletions(-) delete mode 100644 toid_navigation/launch/mock_control_launch.py delete mode 100644 toid_navigation/launch/navigation_launch.py diff --git a/toid_navigation/launch/mock_control_launch.py b/toid_navigation/launch/mock_control_launch.py deleted file mode 100644 index 5d4ccfd..0000000 --- a/toid_navigation/launch/mock_control_launch.py +++ /dev/null @@ -1,93 +0,0 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.conditions import IfCondition, UnlessCondition -from launch.substitutions import Command, LaunchConfiguration -from launch_ros.actions import Node, LifecycleNode -from launch_ros.substitutions import FindPackageShare -import os - -def generate_launch_description(): - pkg_share = FindPackageShare("").find('toid_navigation') - params = os.path.join(pkg_share, 'params', 'toid_general_params.yaml') - map = os.path.join(pkg_share, 'maps', 'mapb2_5cm.yaml') - default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'ros_control.rviz') - - - description_pkg_share = FindPackageShare("").find('toid_bot_description') - default_model_path = os.path.join( - description_pkg_share, - 'src', - 'toid_bot_description.urdf' - ) - - visualize = LaunchConfiguration("visualize") - - visualize_arg = DeclareLaunchArgument( - 'visualize', - default_value='True', - description="Whether to launch rviz2" - ) - - odom_broadcast = Node( - package='tf2_ros', - executable='static_transform_publisher', - name='map_to_odom_broadcaster', - arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'], - condition=IfCondition(LaunchConfiguration('visualize')) - ) - - robot_state_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - name='robot_state_publisher', - output='screen', - parameters=[{'robot_description': Command(['xacro ', default_model_path])}] - ) - - controller_manager = Node( - package='controller_manager', - executable='ros2_control_node', - output='screen', - parameters=[params] - ) - - joint_state_broadcaster = Node( - package='controller_manager', - executable='spawner', - output='screen', - arguments=["joint_state_broadcaster"] - ) - - diffbot_base_controller = Node( - package='controller_manager', - executable='spawner', - output='both', - arguments=[ - "diffbot_base_controller", - "-p", - params, - "--controller-ros-args", - "-r diffbot_base_controller/cmd_vel:=/cmd_vel", - "--controller-ros-args", - "-r diffbot_base_controller/odom:=/odom" - ] - ) - - rviz_node = Node( - package='rviz2', - executable='rviz2', - name='rviz2', - output='screen', - arguments=['-d', default_rviz_config_path], - condition=IfCondition(visualize) - ) - - return LaunchDescription([ - visualize_arg, - odom_broadcast, - robot_state_publisher, - controller_manager, - joint_state_broadcaster, - diffbot_base_controller, - rviz_node - ]) \ No newline at end of file diff --git a/toid_navigation/launch/navigation_launch.py b/toid_navigation/launch/navigation_launch.py deleted file mode 100644 index 8193379..0000000 --- a/toid_navigation/launch/navigation_launch.py +++ /dev/null @@ -1,116 +0,0 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.conditions import IfCondition, UnlessCondition -from launch.substitutions import LaunchConfiguration -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare -import os - -def generate_launch_description(): - pkg_share = FindPackageShare("").find('toid_navigation') - params = os.path.join(pkg_share, 'params', 'toid_general_params.yaml') - map = os.path.join(pkg_share, 'maps', 'mapb2_5cm.yaml') - launch_dir = os.path.join(pkg_share, 'launch') - default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'nav2.rviz') - bt_path = os.path.join(pkg_share, "behaviors", "navigate_to_pose_w_backtracking.xml") - lattice_path = os.path.join(pkg_share, "params", "output.json") - - position = "0.756 0.225 0 0 0 0".split(' ') - #position = "0 0 0 0 0 0".split(' ') - - visualize = LaunchConfiguration("visualize") - visualize_arg = DeclareLaunchArgument( - 'visualize', - default_value='True', - description="Whether to launch rviz2" - ) - - toid_control = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_dir, 'mock_control_launch.py') - ), - launch_arguments={ - 'visualize': 'False' - }.items() - ) - - set_position = Node( - package='tf2_ros', - executable='static_transform_publisher', - name='map_to_odom_broadcaster', - arguments=position + ['map', 'odom'], - condition=IfCondition(visualize) - ) - - map_server = Node( - package='nav2_map_server', - executable='map_server', - name='map_server', - output='screen', - parameters=[{'yaml_filename': map}] - ) - - planner_server = Node( - package='nav2_planner', - executable='planner_server', - name='planner_server', - output='screen', - parameters=[{'GridBased':{'lattice_filepath': lattice_path}}, params] - ) - - controller_server = Node( - package='nav2_controller', - executable='controller_server', - name='controller_server', - output='screen', - parameters=[params] - ) - - bt_navigator = Node( - package='nav2_bt_navigator', - executable='bt_navigator', - name='bt_navigator', - output='screen', - parameters=[{'default_nav_to_pose_bt_xml': bt_path}, params] - ) - -# default_nav_to_pose_bt_xml: "$(find-pkg-share toid_navigation)/behaviors/navigate_to_pose_w_backtracking.xml" - - behavior_server = Node( - package='nav2_behaviors', - executable='behavior_server', - name='behavior_server', - output='screen', - parameters=[params] - ) - - lifecycle_manager_node = Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager', - output='screen', - parameters=[params] - ) - - rviz_node = Node( - package='rviz2', - executable='rviz2', - name='rviz2', - output='screen', - arguments=['-d', default_rviz_config_path], - condition=IfCondition(visualize) - ) - - return LaunchDescription([ - visualize_arg, - set_position, - rviz_node, - map_server, - bt_navigator, - behavior_server, - planner_server, - controller_server, - lifecycle_manager_node, - toid_control - ]) \ No newline at end of file diff --git a/toid_navigation/params/toid_general_params.yaml b/toid_navigation/params/toid_general_params.yaml index 54dd3bf..fc0776b 100644 --- a/toid_navigation/params/toid_general_params.yaml +++ b/toid_navigation/params/toid_general_params.yaml @@ -22,17 +22,11 @@ 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", "translateX", "moveCoords"] + behavior_plugins: ["backup", "spin", "rotate", "translateX", "moveCoords"] spin: plugin: "nav2_behaviors::Spin" backup: plugin: "nav2_behaviors::BackUp" - drive_on_heading: - plugin: "nav2_behaviors::DriveOnHeading" - wait: - plugin: "nav2_behaviors::Wait" - assisted_teleop: - plugin: "nav2_behaviors::AssistedTeleop" rotate: plugin: "toid::SimpleRotateBehavior" max_angular_accel: 4.0 @@ -194,38 +188,4 @@ controller_server: lifecycle_manager: ros__parameters: autostart: true - node_names: ['controller_server','planner_server','behavior_server','map_server','bt_navigator' ] - - -controller_manager: - ros__parameters: - update_rate: 20 # Hz - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - -diffbot_base_controller: - ros__parameters: - type: diff_drive_controller/DiffDriveController - - left_wheel_names: ["drivewhl_l_joint"] - right_wheel_names: ["drivewhl_r_joint"] - - wheel_separation: 0.29 - - wheel_radius: 0.04 - use_stamped_vel: false - - wheel_separation_multiplier: 1.0 - left_wheel_radius_multiplier: 1.0 - right_wheel_radius_multiplier: 1.0 - - publish_rate: 50.0 - odom_frame_id: odom - base_frame_id: base_footprint - - open_loop: true - enable_odom_tf: true - publish_limited_velocity: true - - cmd_vel_timeout: 1.0 \ No newline at end of file + node_names: ['controller_server','planner_server','behavior_server','map_server','bt_navigator' ] \ No newline at end of file -- 2.49.1 From 77cb2c5573dd6dee933eb1cc94d1550692fd5149 Mon Sep 17 00:00:00 2001 From: pimpest <82343504+pimpest@users.noreply.github.com> Date: Sat, 7 Mar 2026 12:32:34 +0100 Subject: [PATCH 21/86] Added support for setting max velocities on send_goal --- toid_behaviors/src/move_coords.cpp | 6 ++++++ toid_behaviors/src/simple_rotate.cpp | 7 ++++++- toid_behaviors/src/simple_translate_x.cpp | 6 ++++++ toid_msgs/action/SimpleMoveCoords.action | 2 ++ toid_msgs/action/SimpleRotate.action | 2 ++ toid_msgs/action/SimpleTranslateX.action | 2 ++ 6 files changed, 24 insertions(+), 1 deletion(-) diff --git a/toid_behaviors/src/move_coords.cpp b/toid_behaviors/src/move_coords.cpp index 7a6a259..2cd985c 100644 --- a/toid_behaviors/src/move_coords.cpp +++ b/toid_behaviors/src/move_coords.cpp @@ -67,6 +67,12 @@ ResultStatus MoveCoords::onStart( target_pose_.orientation = tf2::toMsg(tf2::Quaternion::createFromRPY(0, 0, command->theta)); target_angle_ = command->theta; target_sign_ = backwards_ ? -1.0 : 1.0; + max_vel_speed_ = command->max_speed; + + 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_; diff --git a/toid_behaviors/src/simple_rotate.cpp b/toid_behaviors/src/simple_rotate.cpp index f31e36d..4c75f60 100644 --- a/toid_behaviors/src/simple_rotate.cpp +++ b/toid_behaviors/src/simple_rotate.cpp @@ -39,12 +39,17 @@ ResultStatus SimpleRotateBehavior::onStart( target_angle_ = angles::normalize_angle(command->angle); min_turn_angle_ = abs(command->min_angle); initial_direction_ = (command->min_angle >= 0.0) ? 1.0 : -1.0; + max_angular_speed_ = command->max_speed; + + if(command->max_speed == 0) { + auto node = node_.lock(); + node->get_parameter(behavior_name_ + ".max_angular_vel", max_angular_speed_); + } last_angle_ = tf2::getYaw(pose.orientation); angular_speed_ = vel.angular.z; last_time_ = clock_->now().seconds(); - return ResultStatus{Status::SUCCEEDED}; } diff --git a/toid_behaviors/src/simple_translate_x.cpp b/toid_behaviors/src/simple_translate_x.cpp index 1dcd53f..e38a431 100644 --- a/toid_behaviors/src/simple_translate_x.cpp +++ b/toid_behaviors/src/simple_translate_x.cpp @@ -46,6 +46,12 @@ ResultStatus SimpleTranslateXBehavior::onStart( target_distance_ = command->distance; target_angle_ = tf2::getYaw(pose.orientation); target_sign_ = (target_distance_ < 0) ? -1.0 : 1.0; + max_vel_speed_ = command->max_speed; + + if(command->max_speed == 0) { + auto node = node_.lock(); + node->get_parameter(behavior_name_ + ".max_vel_speed", max_vel_speed_); + } target_distance_ *= target_sign_; diff --git a/toid_msgs/action/SimpleMoveCoords.action b/toid_msgs/action/SimpleMoveCoords.action index 65df8ac..f3b3913 100644 --- a/toid_msgs/action/SimpleMoveCoords.action +++ b/toid_msgs/action/SimpleMoveCoords.action @@ -5,6 +5,8 @@ float64 y float64 theta uint8 backwards 0 uint8 mode 0 + +float64 max_speed --- uint16 NONE=0 uint16 TF_ERROR=1 diff --git a/toid_msgs/action/SimpleRotate.action b/toid_msgs/action/SimpleRotate.action index 0d6a1db..e403b9c 100644 --- a/toid_msgs/action/SimpleRotate.action +++ b/toid_msgs/action/SimpleRotate.action @@ -3,6 +3,8 @@ uint8 IGNORE_OBSTACLES=1 float64 angle float64 min_angle 0 uint8 mode 0 + +float64 max_speed 0 --- uint16 NONE=0 uint16 TF_ERROR=1 diff --git a/toid_msgs/action/SimpleTranslateX.action b/toid_msgs/action/SimpleTranslateX.action index 59545bb..5469ae8 100644 --- a/toid_msgs/action/SimpleTranslateX.action +++ b/toid_msgs/action/SimpleTranslateX.action @@ -2,6 +2,8 @@ uint8 IGNORE_OBSTACLES=1 float64 distance uint8 mode 0 + +float64 max_speed 0 --- uint16 NONE=0 uint16 TF_ERROR=1 -- 2.49.1 From 8ae7e4befd39d0e2127d2c5b7d943f653144accb Mon Sep 17 00:00:00 2001 From: pimpest <82343504+pimpest@users.noreply.github.com> Date: Sat, 7 Mar 2026 15:40:41 +0100 Subject: [PATCH 22/86] Modified behavior tree nodes to include max_speed --- toid_bt/behavior_trees/behavior1.xml | 16 ----- .../behavior_trees/calibration_routines.xml | 62 +++++++++++++------ toid_bt/behavior_trees/toid_behaviors.btproj | 10 ++- .../toid_bt/plugins/end_calib_action.hpp | 3 +- .../toid_bt/plugins/move_coords_action.hpp | 10 ++- .../include/toid_bt/plugins/rotate_action.hpp | 11 ++-- .../toid_bt/plugins/rotate_towards_action.hpp | 13 ++-- .../toid_bt/plugins/translate_x_action.hpp | 59 ++++++++++++++++++ toid_bt/src/bt_executor.cpp | 11 ++-- 9 files changed, 138 insertions(+), 57 deletions(-) create mode 100644 toid_bt/include/toid_bt/plugins/translate_x_action.hpp diff --git a/toid_bt/behavior_trees/behavior1.xml b/toid_bt/behavior_trees/behavior1.xml index e3023e1..b6becc0 100644 --- a/toid_bt/behavior_trees/behavior1.xml +++ b/toid_bt/behavior_trees/behavior1.xml @@ -3,31 +3,15 @@ - - - - - - Action server name - - diff --git a/toid_bt/behavior_trees/calibration_routines.xml b/toid_bt/behavior_trees/calibration_routines.xml index 2a8d556..2f04d27 100644 --- a/toid_bt/behavior_trees/calibration_routines.xml +++ b/toid_bt/behavior_trees/calibration_routines.xml @@ -3,20 +3,38 @@ - + - - + + + + + + + - @@ -29,24 +47,32 @@ type="double"/> + Action server name - + Action server name - - Service name + + + + Action server name diff --git a/toid_bt/behavior_trees/toid_behaviors.btproj b/toid_bt/behavior_trees/toid_behaviors.btproj index e8854a1..65d9521 100644 --- a/toid_bt/behavior_trees/toid_behaviors.btproj +++ b/toid_bt/behavior_trees/toid_behaviors.btproj @@ -11,18 +11,19 @@ + Action server name - + Action server name - + Action server name @@ -32,6 +33,11 @@ Service name + + + + Action server name + Service name diff --git a/toid_bt/include/toid_bt/plugins/end_calib_action.hpp b/toid_bt/include/toid_bt/plugins/end_calib_action.hpp index 35127fc..bfb4fb2 100644 --- a/toid_bt/include/toid_bt/plugins/end_calib_action.hpp +++ b/toid_bt/include/toid_bt/plugins/end_calib_action.hpp @@ -23,7 +23,8 @@ public: static BT::PortsList providedPorts() { return providedBasicPorts({ - BT::InputPort("width"), BT::InputPort("count", 1, {}), + BT::InputPort("width"), + BT::InputPort("count", 1, {}), BT::OutputPort("new_width"), //BT::InputPort("options"), }); diff --git a/toid_bt/include/toid_bt/plugins/move_coords_action.hpp b/toid_bt/include/toid_bt/plugins/move_coords_action.hpp index 6bc5b6c..4403516 100644 --- a/toid_bt/include/toid_bt/plugins/move_coords_action.hpp +++ b/toid_bt/include/toid_bt/plugins/move_coords_action.hpp @@ -24,7 +24,10 @@ public: static BT::PortsList providedPorts() { return providedBasicPorts({ - BT::InputPort("x"), BT::InputPort("y"), BT::InputPort("theta"), + BT::InputPort("x"), + BT::InputPort("y"), + BT::InputPort("theta"), + BT::InputPort("max_speed", 0, {}), //BT::InputPort("options"), }); } @@ -34,6 +37,8 @@ public: auto x_goal = getInput("x"); auto y_goal = getInput("y"); auto theta = getInput("theta"); + auto max_speed = getInput("max_speed").value(); + goal.x = x_goal.value(); goal.y = y_goal.value(); if (!theta.has_value()) { @@ -43,6 +48,9 @@ public: } else { goal.theta = angles::from_degrees(theta.value_or(0)); } + + goal.max_speed = max_speed; + return true; } diff --git a/toid_bt/include/toid_bt/plugins/rotate_action.hpp b/toid_bt/include/toid_bt/plugins/rotate_action.hpp index be76e20..14ef835 100644 --- a/toid_bt/include/toid_bt/plugins/rotate_action.hpp +++ b/toid_bt/include/toid_bt/plugins/rotate_action.hpp @@ -25,7 +25,7 @@ public: return providedBasicPorts({ BT::InputPort("angle", {}), BT::InputPort("min_angle", 0, {}), - BT::InputPort("radians", false, {}), + BT::InputPort("max_speed", 0, {}), //BT::InputPort("options"), }); } @@ -34,13 +34,12 @@ public: { auto angle = getInput("angle"); auto min_angle = getInput("min_angle"); - auto radians = getInput("radians"); + auto max_speed = getInput("max_speed"); goal.angle = angle.value(); goal.min_angle = min_angle.value(); - if (!radians.value()) { - goal.angle = angles::from_degrees(goal.angle); - goal.min_angle = angles::from_degrees(goal.min_angle); - } + goal.max_speed = max_speed.value(); + goal.angle = angles::from_degrees(goal.angle); + goal.min_angle = angles::from_degrees(goal.min_angle); return true; } diff --git a/toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp b/toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp index fb39f24..b2733f9 100644 --- a/toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp +++ b/toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp @@ -26,7 +26,7 @@ public: BT::InputPort("x", {}), BT::InputPort("y", {}), BT::InputPort("min_angle", 0, {}), - BT::InputPort("radians", false, {}), + BT::InputPort("max_speed", 0, {}), //BT::InputPort("options"), }); } @@ -35,18 +35,15 @@ public: { auto x = getInput("x").value(); auto y = getInput("y").value(); - auto min_angle = getInput("min_angle"); - auto radians = getInput("radians"); + auto min_angle = getInput("min_angle").value(); + auto max_speed = getInput("max_speed").value(); geometry_msgs::msg::PoseStamped pose; get_pose(pose); goal.angle = std::atan2(y - pose.pose.position.y, x - pose.pose.position.x); - goal.min_angle = min_angle.value(); - - if (!radians.value()) { - goal.min_angle = angles::from_degrees(goal.min_angle); - } + goal.min_angle = angles::from_degrees(min_angle); + goal.max_speed = max_speed; return true; } diff --git a/toid_bt/include/toid_bt/plugins/translate_x_action.hpp b/toid_bt/include/toid_bt/plugins/translate_x_action.hpp new file mode 100644 index 0000000..8ec9f6c --- /dev/null +++ b/toid_bt/include/toid_bt/plugins/translate_x_action.hpp @@ -0,0 +1,59 @@ +#pragma once + +#include "angles/angles.h" +#include "behaviortree_ros2/bt_action_node.hpp" +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "toid_bt/plugin.hpp" +#include "toid_msgs/action/simple_translate_x.hpp" + +namespace toid +{ + +class TranslateXNode : public BT::RosActionNode +{ +public: + TranslateXNode( + const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params, + PoseFunc pose) + : BT::RosActionNode(name, conf, params), get_pose(pose) + { + } + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ + BT::InputPort("x"), + BT::InputPort("max_speed", 0, {}), + //BT::InputPort("options"), + }); + } + + bool setGoal(Goal & goal) override + { + auto x_goal = getInput("x").value(); + auto max_speed = getInput("max_speed").value(); + + goal.distance = x_goal; + goal.max_speed = max_speed; + + return true; + } + + void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); } + + BT::NodeStatus onResultReceived(const WrappedResult & wr) override + { + return (wr.result->error_code == 0) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; + } + + BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override + { + RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error)); + return BT::NodeStatus::FAILURE; + } + + PoseFunc get_pose; +}; + +} // namespace toid \ No newline at end of file diff --git a/toid_bt/src/bt_executor.cpp b/toid_bt/src/bt_executor.cpp index 8679486..f4ef107 100644 --- a/toid_bt/src/bt_executor.cpp +++ b/toid_bt/src/bt_executor.cpp @@ -9,6 +9,7 @@ #include "toid_bt/plugins/move_coords_action.hpp" #include "toid_bt/plugins/rotate_action.hpp" #include "toid_bt/plugins/rotate_towards_action.hpp" +#include "toid_bt/plugins/translate_x_action.hpp" namespace toid { @@ -49,17 +50,17 @@ void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory) factory.registerNodeType( "RotateTowards", BT::RosNodeParams(nh, "/rotate"), get_pose); + factory.registerNodeType( + "TranslateX", BT::RosNodeParams(nh, "/translateX"), get_pose); + factory.registerNodeType( "SetWidth", BT::RosNodeParams(nh, "/set_width"), get_pose); - factory.registerNodeType( - "ZeroOdom", BT::RosNodeParams(nh, "/zero")); + factory.registerNodeType("ZeroOdom", BT::RosNodeParams(nh, "/zero")); - factory.registerNodeType( - "EndCalib", BT::RosNodeParams(nh, "/end_calib")); + factory.registerNodeType("EndCalib", BT::RosNodeParams(nh, "/end_calib")); std::cout << describeCustomNodes() << std::endl; - } void TreeExecutor::position(geometry_msgs::msg::PoseStamped & pose) -- 2.49.1 From 6d3370c4e45bf99af1e83f325fd2eb2e5c2ad6e7 Mon Sep 17 00:00:00 2001 From: pimpest <82343504+pimpest@users.noreply.github.com> Date: Sat, 7 Mar 2026 16:25:12 +0100 Subject: [PATCH 23/86] Added default value to goal --- toid_msgs/action/SimpleMoveCoords.action | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/toid_msgs/action/SimpleMoveCoords.action b/toid_msgs/action/SimpleMoveCoords.action index f3b3913..f178d8a 100644 --- a/toid_msgs/action/SimpleMoveCoords.action +++ b/toid_msgs/action/SimpleMoveCoords.action @@ -6,7 +6,7 @@ float64 theta uint8 backwards 0 uint8 mode 0 -float64 max_speed +float64 max_speed 0 --- uint16 NONE=0 uint16 TF_ERROR=1 -- 2.49.1 From 9010bac9a0ad65e863246cf37a89f875ed20dfd4 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Sun, 8 Mar 2026 13:28:22 +0100 Subject: [PATCH 24/86] Added small helper script to build navigation packages --- scripts/toid_base_build.sh | 5 +++++ 1 file changed, 5 insertions(+) create mode 100755 scripts/toid_base_build.sh diff --git a/scripts/toid_base_build.sh b/scripts/toid_base_build.sh new file mode 100755 index 0000000..fff53f7 --- /dev/null +++ b/scripts/toid_base_build.sh @@ -0,0 +1,5 @@ +#!/bin/env bash + +source /opt/ros/jazzy/setup.bash + +colcon build --packages-up-to-regex "toid_(?!bt)" \ No newline at end of file -- 2.49.1 From 8f4f7dba8b6ba5b41ce064549b9f37e9dc56fd13 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Fri, 20 Mar 2026 17:05:30 +0100 Subject: [PATCH 25/86] add backwards to behavior tree action --- toid_behaviors/src/move_coords.cpp | 6 +- .../behavior_trees/calibration_routines.xml | 114 ++++++++++++++---- toid_bt/behavior_trees/toid_behaviors.btproj | 1 + .../toid_bt/plugins/move_coords_action.hpp | 3 + 4 files changed, 94 insertions(+), 30 deletions(-) diff --git a/toid_behaviors/src/move_coords.cpp b/toid_behaviors/src/move_coords.cpp index 2cd985c..d4a5320 100644 --- a/toid_behaviors/src/move_coords.cpp +++ b/toid_behaviors/src/move_coords.cpp @@ -95,10 +95,6 @@ ResultStatus MoveCoords::updateVel( const double current_yaw = tf2::getYaw(pose.orientation); double angle_dist = angles::shortest_angular_distance(current_yaw, target_angle_); - if (backwards_) { - angle_dist = angles::two_pi_complement(angle_dist); - } - const double dx = target_pose_.position.x - pose.position.x; const double dy = target_pose_.position.y - pose.position.y; @@ -120,7 +116,7 @@ ResultStatus MoveCoords::updateVel( if (dist_left <= 0.02) { out_vel.linear.x = std::clamp(target_sign_ * vel, lower_bound, upper_bound); - out_vel.angular.z = std::clamp(target_sign_ * kp_ * angle_dist, -max_angular_speed_, max_angular_speed_); + 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}; } diff --git a/toid_bt/behavior_trees/calibration_routines.xml b/toid_bt/behavior_trees/calibration_routines.xml index 2f04d27..35c10c9 100644 --- a/toid_bt/behavior_trees/calibration_routines.xml +++ b/toid_bt/behavior_trees/calibration_routines.xml @@ -1,40 +1,95 @@ - + - - - + + + - + - + + + + + + + + + + + + + + + + + + + + - + - - + + + + + + + + + + + + + + @@ -50,6 +105,9 @@ + Action server name @@ -65,14 +123,20 @@ Action server name - - + - - Action server name + + + Service name + + + Service name diff --git a/toid_bt/behavior_trees/toid_behaviors.btproj b/toid_bt/behavior_trees/toid_behaviors.btproj index 65d9521..52ae4ff 100644 --- a/toid_bt/behavior_trees/toid_behaviors.btproj +++ b/toid_bt/behavior_trees/toid_behaviors.btproj @@ -12,6 +12,7 @@ + Action server name diff --git a/toid_bt/include/toid_bt/plugins/move_coords_action.hpp b/toid_bt/include/toid_bt/plugins/move_coords_action.hpp index 4403516..6e40dd7 100644 --- a/toid_bt/include/toid_bt/plugins/move_coords_action.hpp +++ b/toid_bt/include/toid_bt/plugins/move_coords_action.hpp @@ -28,6 +28,7 @@ public: BT::InputPort("y"), BT::InputPort("theta"), BT::InputPort("max_speed", 0, {}), + BT::InputPort("backwards", false, {}), //BT::InputPort("options"), }); } @@ -38,6 +39,7 @@ public: auto y_goal = getInput("y"); auto theta = getInput("theta"); auto max_speed = getInput("max_speed").value(); + auto backwards = getInput("backwards").value(); goal.x = x_goal.value(); goal.y = y_goal.value(); @@ -50,6 +52,7 @@ public: } goal.max_speed = max_speed; + goal.backwards = backwards; return true; } -- 2.49.1 From 01059844586f60924b0a3ebb413e721fb0b4efe0 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Tue, 24 Mar 2026 20:39:18 +0100 Subject: [PATCH 26/86] Fixed tf_buffer_ not being initialized --- docker-compose.yaml | 1 + firmware/base/src/config.h | 5 +++-- firmware/base/src/main.c | 4 ++-- toid_bt/src/bt_executor.cpp | 3 ++- 4 files changed, 8 insertions(+), 5 deletions(-) diff --git a/docker-compose.yaml b/docker-compose.yaml index 2623755..97435e1 100644 --- a/docker-compose.yaml +++ b/docker-compose.yaml @@ -1,6 +1,7 @@ services: toid: image: localhost:5000/toid + build: . container_name: toid privileged: true diff --git a/firmware/base/src/config.h b/firmware/base/src/config.h index bd77eb3..b873097 100644 --- a/firmware/base/src/config.h +++ b/firmware/base/src/config.h @@ -24,7 +24,8 @@ #define ENCODER_LEFT_PIN_B 13 #define ENCODER_CPR 3840 -#define WHEEL_RADIUS 0.0300 -#define WHEEL_SEPARATION 0.264 +//#define WHEEL_RADIUS (0.0300 * 1.01483541) +#define WHEEL_RADIUS 0.028 +#define WHEEL_SEPARATION 0.271 #define TIMER_CYCLE_US 1000 //====================================================== diff --git a/firmware/base/src/main.c b/firmware/base/src/main.c index cdf5b9b..8fa9b21 100644 --- a/firmware/base/src/main.c +++ b/firmware/base/src/main.c @@ -62,8 +62,8 @@ typedef struct calib_diff_t { } calib_diff_t; static calib_diff_t calib_enc = { - .left_gain = 1.000, - .right_gain = 1.0000 + .left_gain = 1.0, + .right_gain = 1.0, }; void update_pos_cb() { diff --git a/toid_bt/src/bt_executor.cpp b/toid_bt/src/bt_executor.cpp index f4ef107..b9eff34 100644 --- a/toid_bt/src/bt_executor.cpp +++ b/toid_bt/src/bt_executor.cpp @@ -19,9 +19,10 @@ TreeExecutor::TreeExecutor(const rclcpp::NodeOptions opts) /* executeRegistration(); std::cout << BT::writeTreeNodesModelXML(factory()) << std::endl; + */ tf_buffer_ = std::make_shared(node()->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); - */ + nav2_util::declare_parameter_if_not_declared( node(), "base_frame", rclcpp::ParameterValue("base_footprint")); -- 2.49.1 From c7caa69bfa03aa0c179815b676ba9d4e6c8aabc9 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Tue, 24 Mar 2026 20:40:14 +0100 Subject: [PATCH 27/86] Added print to print out new calculated width --- toid_bt/behavior_trees/behavior1.xml | 78 +++++++++++++++++-- .../behavior_trees/calibration_routines.xml | 39 ++++++++-- .../toid_bt/plugins/end_calib_action.hpp | 4 +- 3 files changed, 106 insertions(+), 15 deletions(-) diff --git a/toid_bt/behavior_trees/behavior1.xml b/toid_bt/behavior_trees/behavior1.xml index b6becc0..f4ed8bd 100644 --- a/toid_bt/behavior_trees/behavior1.xml +++ b/toid_bt/behavior_trees/behavior1.xml @@ -1,17 +1,81 @@ + + + + + + + + + - - - + + + + + + + + + Action server name + + + + + + + Action server name + + + + + Action server name + + + Service name + + diff --git a/toid_bt/behavior_trees/calibration_routines.xml b/toid_bt/behavior_trees/calibration_routines.xml index 35c10c9..e19211a 100644 --- a/toid_bt/behavior_trees/calibration_routines.xml +++ b/toid_bt/behavior_trees/calibration_routines.xml @@ -2,17 +2,22 @@ - + - - @@ -76,6 +81,8 @@ action_name=""/> + + @@ -84,17 +91,21 @@ - + + Service name + @@ -123,6 +134,20 @@ Action server name + + + + + + Action server name + diff --git a/toid_bt/include/toid_bt/plugins/end_calib_action.hpp b/toid_bt/include/toid_bt/plugins/end_calib_action.hpp index bfb4fb2..2512afc 100644 --- a/toid_bt/include/toid_bt/plugins/end_calib_action.hpp +++ b/toid_bt/include/toid_bt/plugins/end_calib_action.hpp @@ -33,7 +33,7 @@ public: bool setRequest(typename Request::SharedPtr & request) override { auto width = getInput("width").value(); - auto count = getInput("count").value(); + double count = getInput("count").value(); geometry_msgs::msg::PoseStamped pose; get_pose(pose); @@ -42,6 +42,8 @@ public: double new_width = width * (1 + (theta / (2 * M_PI * count))); request->data = new_width; + RCLCPP_INFO(logger(), "width is %lf", new_width); + setOutput("new_width", new_width); return true; } -- 2.49.1 From 9a967e7e1ad29d11a0cddc7d3b9cd5952efe18ee Mon Sep 17 00:00:00 2001 From: pimpest <82343504+pimpest@users.noreply.github.com> Date: Wed, 25 Mar 2026 00:00:43 +0100 Subject: [PATCH 28/86] Created toid_lidar node --- toid_lidar/CMakeLists.txt | 51 +++++ toid_lidar/LICENSE | 202 ++++++++++++++++++ .../include/toid_lidar/toid_lidar_node.hpp | 47 ++++ toid_lidar/package.xml | 30 +++ toid_lidar/src/toid_lidar.cpp | 9 + toid_lidar/src/toid_lidar_node.cpp | 42 ++++ 6 files changed, 381 insertions(+) create mode 100644 toid_lidar/CMakeLists.txt create mode 100644 toid_lidar/LICENSE create mode 100644 toid_lidar/include/toid_lidar/toid_lidar_node.hpp create mode 100644 toid_lidar/package.xml create mode 100644 toid_lidar/src/toid_lidar.cpp create mode 100644 toid_lidar/src/toid_lidar_node.cpp diff --git a/toid_lidar/CMakeLists.txt b/toid_lidar/CMakeLists.txt new file mode 100644 index 0000000..61ae729 --- /dev/null +++ b/toid_lidar/CMakeLists.txt @@ -0,0 +1,51 @@ +cmake_minimum_required(VERSION 3.8) +project(toid_lidar) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) + +# set aditional dependencies +set(PACKAGE_DEPS + rclcpp + Eigen3 + nav2_util + sensor_msgs + tf2 + tf2_ros + tf2_eigen + tf2_geometry_msgs + toid_msgs +) + +foreach(PACKAGE ${PACKAGE_DEPS}) + find_package(${PACKAGE} REQUIRED) +endforeach() + +set(SOURCES + src/toid_lidar.cpp + src/toid_lidar_node.cpp +) + +add_executable(toid_lidar ${SOURCES}) + +target_include_directories( + toid_lidar + PRIVATE + include +) + +ament_target_dependencies(toid_lidar ${PACKAGE_DEPS}) + +install(TARGETS toid_lidar DESTINATION lib/${PROJECT_NAME}) + +target_compile_features( + toid_lidar PUBLIC + c_std_99 + cxx_std_17 +) + +ament_package() diff --git a/toid_lidar/LICENSE b/toid_lidar/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/toid_lidar/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_lidar/include/toid_lidar/toid_lidar_node.hpp b/toid_lidar/include/toid_lidar/toid_lidar_node.hpp new file mode 100644 index 0000000..084b63a --- /dev/null +++ b/toid_lidar/include/toid_lidar/toid_lidar_node.hpp @@ -0,0 +1,47 @@ +#pragma once + +#include "Eigen/Geometry" +#include "geometry_msgs/msg/point_stamped.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "toid_msgs/msg/rival.hpp" +#include "vector" + +namespace toid +{ + +class ToidRivalDetect : public rclcpp::Node +{ + using LaserScan = sensor_msgs::msg::LaserScan; + using Rival = toid_msgs::msg::Rival; + using PointStamped = geometry_msgs::msg::PointStamped; + +public: + static void generate_rotation_matricies(std::vector &) {} + + ToidRivalDetect(); + + static std::vector rotations; +private: + + void process_scan(LaserScan::ConstSharedPtr msg); + + rclcpp::Subscription::SharedPtr scan_sub_; + rclcpp::Publisher::SharedPtr rival_pub_; + rclcpp::Publisher::SharedPtr point_pub_; + + std::shared_ptr tf_buf_; + std::shared_ptr tf_listener_; + + std::string lidar_frame_ = "lidar"; + std::string base_frame_ = "base_footprint"; + std::string global_frame_ = "map"; + + rclcpp::Logger logger_ = get_logger(); + rclcpp::Clock::SharedPtr clock_; + +}; + +} // namespace toid \ No newline at end of file diff --git a/toid_lidar/package.xml b/toid_lidar/package.xml new file mode 100644 index 0000000..b5004ec --- /dev/null +++ b/toid_lidar/package.xml @@ -0,0 +1,30 @@ + + + + toid_lidar + 0.0.0 + TODO: Package description + pimpest + Apache-2.0 + + ament_cmake + + rclcpp + eigen + nav2_util + sensor_msgs + tf2 + tf2_eigen + tf2_ros + tf2_geometry_msgs + toid_msgs + + + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/toid_lidar/src/toid_lidar.cpp b/toid_lidar/src/toid_lidar.cpp new file mode 100644 index 0000000..450a812 --- /dev/null +++ b/toid_lidar/src/toid_lidar.cpp @@ -0,0 +1,9 @@ +#include "rclcpp/rclcpp.hpp" +#include "toid_lidar/toid_lidar_node.hpp" + +int main(const int argc,const char** argv) { + rclcpp::init(argc,argv); + + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/toid_lidar/src/toid_lidar_node.cpp b/toid_lidar/src/toid_lidar_node.cpp new file mode 100644 index 0000000..901dc6a --- /dev/null +++ b/toid_lidar/src/toid_lidar_node.cpp @@ -0,0 +1,42 @@ +#include "toid_lidar/toid_lidar_node.hpp" + +#include "Eigen/Eigen" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/robot_utils.hpp" +#include "tf2/convert.hpp" +#include "tf2_eigen/tf2_eigen.hpp" + +namespace toid +{ + +ToidRivalDetect::ToidRivalDetect() : rclcpp::Node("RivalDetect") +{ + scan_sub_ = create_subscription( + "scan", rclcpp::QoS(1), [this](LaserScan::ConstSharedPtr msg) { this->process_scan(msg); }); + rival_pub_ = create_publisher("/dynamicObstacle", rclcpp::QoS(1)); + + tf_buf_ = std::make_shared(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buf_); + + clock_ = this->get_clock(); + logger_ = this->get_logger(); + + nav2_util::declare_parameter_if_not_declared( + this, "lidar_frame", rclcpp::ParameterValue("lidar")); + this->get_parameter("lidar_frame", lidar_frame_); + + nav2_util::declare_parameter_if_not_declared(this, "global_frame", rclcpp::ParameterValue("map")); + this->get_parameter("global_frame", global_frame_); +} + +void ToidRivalDetect::process_scan(LaserScan::ConstSharedPtr msg) +{ + geometry_msgs::msg::PoseStamped curr_pose; + Eigen::Isometry3d pose; + if (!nav2_util::getCurrentPose(curr_pose, *tf_buf_, global_frame_, lidar_frame_)) { + RCLCPP_ERROR(this->logger_, "Could not get current transform"); + } +} + +} // namespace toid \ No newline at end of file -- 2.49.1 From 97f08804d80f174f210b5f1f96dab6fc788510c5 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Wed, 25 Mar 2026 14:02:10 +0100 Subject: [PATCH 29/86] Finised lidar node --- toid_lidar/CMakeLists.txt | 1 + .../include/toid_lidar/toid_lidar_node.hpp | 13 +- toid_lidar/package.xml | 2 + toid_lidar/src/toid_lidar.cpp | 2 +- toid_lidar/src/toid_lidar_node.cpp | 121 +++++++++++++++++- toid_msgs/CMakeLists.txt | 4 + toid_msgs/msg/Rival.msg | 2 + toid_msgs/package.xml | 2 + 8 files changed, 133 insertions(+), 14 deletions(-) create mode 100644 toid_msgs/msg/Rival.msg diff --git a/toid_lidar/CMakeLists.txt b/toid_lidar/CMakeLists.txt index 61ae729..41afecf 100644 --- a/toid_lidar/CMakeLists.txt +++ b/toid_lidar/CMakeLists.txt @@ -19,6 +19,7 @@ set(PACKAGE_DEPS tf2_eigen tf2_geometry_msgs toid_msgs + visualization_msgs ) foreach(PACKAGE ${PACKAGE_DEPS}) diff --git a/toid_lidar/include/toid_lidar/toid_lidar_node.hpp b/toid_lidar/include/toid_lidar/toid_lidar_node.hpp index 084b63a..c86f0aa 100644 --- a/toid_lidar/include/toid_lidar/toid_lidar_node.hpp +++ b/toid_lidar/include/toid_lidar/toid_lidar_node.hpp @@ -7,6 +7,7 @@ #include "tf2_ros/buffer.hpp" #include "tf2_ros/transform_listener.hpp" #include "toid_msgs/msg/rival.hpp" +#include "visualization_msgs/msg/marker.hpp" #include "vector" namespace toid @@ -17,27 +18,27 @@ class ToidRivalDetect : public rclcpp::Node using LaserScan = sensor_msgs::msg::LaserScan; using Rival = toid_msgs::msg::Rival; using PointStamped = geometry_msgs::msg::PointStamped; + using Marker = visualization_msgs::msg::Marker; public: - static void generate_rotation_matricies(std::vector &) {} - ToidRivalDetect(); - - static std::vector rotations; private: void process_scan(LaserScan::ConstSharedPtr msg); rclcpp::Subscription::SharedPtr scan_sub_; rclcpp::Publisher::SharedPtr rival_pub_; - rclcpp::Publisher::SharedPtr point_pub_; + rclcpp::Publisher::SharedPtr marker_pub_; std::shared_ptr tf_buf_; std::shared_ptr tf_listener_; - std::string lidar_frame_ = "lidar"; std::string base_frame_ = "base_footprint"; std::string global_frame_ = "map"; + double map_width_ = 3.0; + double map_height_ = 2.0; + bool visualize_ = false; + bool closest_ = false; rclcpp::Logger logger_ = get_logger(); rclcpp::Clock::SharedPtr clock_; diff --git a/toid_lidar/package.xml b/toid_lidar/package.xml index b5004ec..0b47a33 100644 --- a/toid_lidar/package.xml +++ b/toid_lidar/package.xml @@ -18,6 +18,8 @@ tf2_ros tf2_geometry_msgs toid_msgs + visualization_msgs + rplidar diff --git a/toid_lidar/src/toid_lidar.cpp b/toid_lidar/src/toid_lidar.cpp index 450a812..5cf2fe2 100644 --- a/toid_lidar/src/toid_lidar.cpp +++ b/toid_lidar/src/toid_lidar.cpp @@ -3,7 +3,7 @@ int main(const int argc,const char** argv) { rclcpp::init(argc,argv); - + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } \ No newline at end of file diff --git a/toid_lidar/src/toid_lidar_node.cpp b/toid_lidar/src/toid_lidar_node.cpp index 901dc6a..52062cf 100644 --- a/toid_lidar/src/toid_lidar_node.cpp +++ b/toid_lidar/src/toid_lidar_node.cpp @@ -1,6 +1,7 @@ #include "toid_lidar/toid_lidar_node.hpp" #include "Eigen/Eigen" +#include "Eigen/Geometry" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/robot_utils.hpp" @@ -15,6 +16,7 @@ ToidRivalDetect::ToidRivalDetect() : rclcpp::Node("RivalDetect") scan_sub_ = create_subscription( "scan", rclcpp::QoS(1), [this](LaserScan::ConstSharedPtr msg) { this->process_scan(msg); }); rival_pub_ = create_publisher("/dynamicObstacle", rclcpp::QoS(1)); + marker_pub_ = create_publisher("/rivalMaker", rclcpp::QoS(1)); tf_buf_ = std::make_shared(this->get_clock()); tf_listener_ = std::make_shared(*tf_buf_); @@ -22,21 +24,126 @@ ToidRivalDetect::ToidRivalDetect() : rclcpp::Node("RivalDetect") clock_ = this->get_clock(); logger_ = this->get_logger(); - nav2_util::declare_parameter_if_not_declared( - this, "lidar_frame", rclcpp::ParameterValue("lidar")); - this->get_parameter("lidar_frame", lidar_frame_); - nav2_util::declare_parameter_if_not_declared(this, "global_frame", rclcpp::ParameterValue("map")); this->get_parameter("global_frame", global_frame_); + + nav2_util::declare_parameter_if_not_declared(this, "map_width", rclcpp::ParameterValue(3.0)); + this->get_parameter("map_width", map_width_); + + nav2_util::declare_parameter_if_not_declared(this, "map_height", rclcpp::ParameterValue(2.0)); + this->get_parameter("map_height", map_height_); + + nav2_util::declare_parameter_if_not_declared(this, "draw_markers", rclcpp::ParameterValue(false)); + this->get_parameter("draw_markers", visualize_); + + nav2_util::declare_parameter_if_not_declared(this, "closest", rclcpp::ParameterValue(false)); + this->get_parameter("closest", closest_); } void ToidRivalDetect::process_scan(LaserScan::ConstSharedPtr msg) { - geometry_msgs::msg::PoseStamped curr_pose; Eigen::Isometry3d pose; - if (!nav2_util::getCurrentPose(curr_pose, *tf_buf_, global_frame_, lidar_frame_)) { - RCLCPP_ERROR(this->logger_, "Could not get current transform"); + + geometry_msgs::msg::TransformStamped transform; + + try { + transform = tf_buf_->lookupTransform(global_frame_, msg->header.frame_id, msg->header.stamp); + } catch (const tf2::TransformException & e) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, "TF Link missing: %s", e.what()); + return; } + + Eigen::Isometry3d iso = tf2::transformToEigen(transform); + + double min_dist = INFINITY; + + toid_msgs::msg::Rival rival; + rival.header.frame_id = global_frame_; + rival.header.stamp = msg->header.stamp; + + auto marker_loan = marker_pub_->borrow_loaned_message(); + Marker & marker = marker_loan.get(); + if(visualize_) { + marker.header = rival.header; + marker.scale.x = 0.1; + marker.scale.y = 0.1; + marker.scale.z = 0.1; + marker.type = Marker::SPHERE_LIST; + marker.frame_locked = true; + marker.lifetime.sec = 1; + marker.color.a = 1.0; + marker.color.g = 1.0; + marker.id = 1; + } + + Eigen::Vector3d accum(0, 0, 0); + Eigen::Vector3d start_point(0, 0, 0); + + int points = 0; + + for (size_t i = 0; i < msg->ranges.size(); ++i) { + double range = msg->ranges[i]; + double intensity = msg->intensities[i]; + + if (range < msg->range_min || range > msg->range_max) continue; + if (intensity < 35.0) continue; + + double angle = msg->angle_min + (i * msg->angle_increment); + + Eigen::Vector3d local_point(range * std::cos(angle), range * std::sin(angle), 0.0); + Eigen::Vector3d map_point = iso * local_point; + + if ( + std::fabs(map_point.x()) < map_width_ / 2.0 && std::fabs(map_point.y()) < map_height_ / 2.0) { + if ((start_point - map_point).dot(start_point - map_point) < 0.025) { + accum += map_point; + points++; + } else { + if (points != 0 && !closest_) { + geometry_msgs::msg::Point p; + accum /= points; + tf2::convert(accum, p); + if(visualize_){ + marker.points.push_back(p); + } + rival.point.push_back(p); + } + points = 1; + accum = map_point; + start_point = map_point; + } + + if(range < min_dist && closest_) { + min_dist = range; + if(rival.point.empty()) { + if(visualize_){ + marker.points.emplace_back(); + } + rival.point.emplace_back(); + } + tf2::convert(map_point, rival.point.front()); + if(visualize_) { + tf2::convert(map_point, marker.points.front()); + } + } + } + } + + if (points != 0 && !closest_) { + geometry_msgs::msg::Point p; + accum /= points; + tf2::convert(accum, p); + if(visualize_){ + marker.points.push_back(p); + } + rival.point.push_back(p); + } + + if(visualize_) { + marker_pub_->publish(std::move(marker_loan)); + } + rival_pub_->publish(rival); } } // namespace toid \ No newline at end of file diff --git a/toid_msgs/CMakeLists.txt b/toid_msgs/CMakeLists.txt index a0a4761..a505d52 100644 --- a/toid_msgs/CMakeLists.txt +++ b/toid_msgs/CMakeLists.txt @@ -9,11 +9,15 @@ endif() find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) +find_package(geometry_msgs REQUIRED) + rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Rival.msg" "srv/SendDouble.srv" "action/SimpleMoveCoords.action" "action/SimpleRotate.action" "action/SimpleTranslateX.action" + DEPENDENCIES geometry_msgs ) ament_package() \ No newline at end of file diff --git a/toid_msgs/msg/Rival.msg b/toid_msgs/msg/Rival.msg new file mode 100644 index 0000000..06f6a29 --- /dev/null +++ b/toid_msgs/msg/Rival.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +geometry_msgs/Point[] point \ No newline at end of file diff --git a/toid_msgs/package.xml b/toid_msgs/package.xml index dc19523..2f16062 100644 --- a/toid_msgs/package.xml +++ b/toid_msgs/package.xml @@ -10,6 +10,8 @@ ament_cmake rosidl_default_generators + geometry_msgs + geometry_msgs rosidl_default_runtime rosidl_interface_packages -- 2.49.1 From 7a2d74edd18a5d8f776f73db5c7343fb6bc13968 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Wed, 25 Mar 2026 14:10:49 +0100 Subject: [PATCH 30/86] Removed unecessary attribute --- toid_lidar/include/toid_lidar/toid_lidar_node.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/toid_lidar/include/toid_lidar/toid_lidar_node.hpp b/toid_lidar/include/toid_lidar/toid_lidar_node.hpp index c86f0aa..7a22732 100644 --- a/toid_lidar/include/toid_lidar/toid_lidar_node.hpp +++ b/toid_lidar/include/toid_lidar/toid_lidar_node.hpp @@ -33,7 +33,6 @@ private: std::shared_ptr tf_buf_; std::shared_ptr tf_listener_; - std::string base_frame_ = "base_footprint"; std::string global_frame_ = "map"; double map_width_ = 3.0; double map_height_ = 2.0; -- 2.49.1 From 8ba8585a2983d48f1500a813f668f6161b1ebbc8 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Wed, 25 Mar 2026 19:25:10 +0100 Subject: [PATCH 31/86] GameElementLayer prototype --- toid_costmaps/CMakeLists.txt | 69 ++++++ toid_costmaps/LICENSE | 202 ++++++++++++++++++ .../toid_costmaps/game_elements_layer.hpp | 39 ++++ toid_costmaps/package.xml | 30 +++ toid_costmaps/src/game_elements_layer.cpp | 38 ++++ toid_costmaps/toid_costmaps.xml | 7 + toid_lidar/CMakeLists.txt | 8 + toid_lidar/launch/launch.py | 68 ++++++ toid_lidar/params/lidar.yaml | 11 + toid_lidar/rviz/conf.rviz | 175 +++++++++++++++ 10 files changed, 647 insertions(+) create mode 100644 toid_costmaps/CMakeLists.txt create mode 100644 toid_costmaps/LICENSE create mode 100644 toid_costmaps/include/toid_costmaps/game_elements_layer.hpp create mode 100644 toid_costmaps/package.xml create mode 100644 toid_costmaps/src/game_elements_layer.cpp create mode 100644 toid_costmaps/toid_costmaps.xml create mode 100644 toid_lidar/launch/launch.py create mode 100644 toid_lidar/params/lidar.yaml create mode 100644 toid_lidar/rviz/conf.rviz diff --git a/toid_costmaps/CMakeLists.txt b/toid_costmaps/CMakeLists.txt new file mode 100644 index 0000000..9f31edc --- /dev/null +++ b/toid_costmaps/CMakeLists.txt @@ -0,0 +1,69 @@ +cmake_minimum_required(VERSION 3.8) +project(toid_costmaps) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(library_name toid_costmaps) + +set( + PACKAGE_DEPS + + rclcpp + angles + geometry_msgs + pluginlib + nav_msgs + nav2_core + nav2_costmap_2d + nav2_util + tf2 + tf2_geometry_msgs + tf2_ros + Eigen3 + toid_msgs +) + +set( + SOURCES + src/game_elements_layer.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_costmap_2d toid_costmaps.xml) + +ament_package() diff --git a/toid_costmaps/LICENSE b/toid_costmaps/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/toid_costmaps/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_costmaps/include/toid_costmaps/game_elements_layer.hpp b/toid_costmaps/include/toid_costmaps/game_elements_layer.hpp new file mode 100644 index 0000000..de71151 --- /dev/null +++ b/toid_costmaps/include/toid_costmaps/game_elements_layer.hpp @@ -0,0 +1,39 @@ +#pragma once + +#include "nav2_costmap_2d/costmap_layer.hpp" +#include "nav2_costmap_2d/layer.hpp" +#include "nav2_costmap_2d/layered_costmap.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace toid +{ + +class GameElementLayer : public nav2_costmap_2d::CostmapLayer +{ +public: + GameElementLayer(){ + costmap_ = NULL; + } + ~GameElementLayer(){} + + virtual void onInitialize(); + virtual void updateBounds( + double robot_x, double robot_y, double robot_yaw, double * min_x, double * min_y, + double * max_x, double * max_y); + + virtual void updateCosts( + nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j); + + virtual void reset() { return; } + + virtual void onFootprintChanged() {} + + virtual bool isClearable() { return false; } + +private: + double last_min_x_, last_min_y_, last_max_x_, last_max_y_; + + bool need_recalculation_; +}; + +} // namespace toid \ No newline at end of file diff --git a/toid_costmaps/package.xml b/toid_costmaps/package.xml new file mode 100644 index 0000000..98952d0 --- /dev/null +++ b/toid_costmaps/package.xml @@ -0,0 +1,30 @@ + + + + toid_costmaps + 0.0.0 + TODO: Package description + Pimpest + Apache-2.0 + + ament_cmake + + angles + geometry_msgs + nav_msgs + nav2_costmap_2d + nav2_util + nav2_core + pluginlib + tf2 + tf2_geometry_msgs + tf2_ros + toid_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/toid_costmaps/src/game_elements_layer.cpp b/toid_costmaps/src/game_elements_layer.cpp new file mode 100644 index 0000000..aecba67 --- /dev/null +++ b/toid_costmaps/src/game_elements_layer.cpp @@ -0,0 +1,38 @@ +#include "toid_costmaps/game_elements_layer.hpp" + +#include "nav2_util/node_utils.hpp" + +namespace toid +{ + +void GameElementLayer::onInitialize() {} + +void GameElementLayer::updateBounds( + double, double, double, double * min_x, double * min_y, double * max_x, double * max_y) +{ + touch(1.0 - 0.1, 0.5 - 0.1, min_x, min_y, max_x, max_y); + touch(1.0 + 0.1, 0.5 + 0.1, min_x, min_y, max_x, max_y); +} + +void GameElementLayer::updateCosts( + nav2_costmap_2d::Costmap2D & grid, int min_i, int min_j, int max_i, int max_j) +{ + bool in_bounds = true; + uint mx, my; + in_bounds &= worldToMap(1.0 - 0.1, 0.5 - 0.1, mx, my); + uint mmx, mmy; + in_bounds &= worldToMap(1.0 + 0.1, 0.5 + 0.1, mmx, mmy); + + if (in_bounds) { + for (uint j = my; j < mmy; j++) { + for (uint i = mx; i < mmx; i++) { + grid.setCost(i, j, nav2_costmap_2d::LETHAL_OBSTACLE); + } + } + } +} + +} // namespace toid + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(toid::GameElementLayer, nav2_costmap_2d::Layer); \ No newline at end of file diff --git a/toid_costmaps/toid_costmaps.xml b/toid_costmaps/toid_costmaps.xml new file mode 100644 index 0000000..bead28e --- /dev/null +++ b/toid_costmaps/toid_costmaps.xml @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/toid_lidar/CMakeLists.txt b/toid_lidar/CMakeLists.txt index 41afecf..1e83fe0 100644 --- a/toid_lidar/CMakeLists.txt +++ b/toid_lidar/CMakeLists.txt @@ -39,6 +39,14 @@ target_include_directories( include ) +install( + DIRECTORY + launch + params + rviz + DESTINATION share/${PROJECT_NAME}/ +) + ament_target_dependencies(toid_lidar ${PACKAGE_DEPS}) install(TARGETS toid_lidar DESTINATION lib/${PROJECT_NAME}) diff --git a/toid_lidar/launch/launch.py b/toid_lidar/launch/launch.py new file mode 100644 index 0000000..a78e77e --- /dev/null +++ b/toid_lidar/launch/launch.py @@ -0,0 +1,68 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import PathJoinSubstitution, LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +from pathlib import Path + +def generate_launch_description(): + + basedir = FindPackageShare("").find("toid_lidar") + + lidar_config = PathJoinSubstitution([basedir, "params/lidar.yaml"]) + rviz_config = PathJoinSubstitution([basedir, "rviz/conf.rviz"]) + + visualize = LaunchConfiguration("visualize") + draw_markers = LaunchConfiguration("draw_markers") + use_closest = LaunchConfiguration("use_closest") + lidar_frame = LaunchConfiguration("lidar_frame") + + return LaunchDescription([ + DeclareLaunchArgument( + 'visualize', + default_value='False', + description="Whether to launch rviz2" + ), + DeclareLaunchArgument( + 'draw_markers', + default_value='False', + description="Draw markers" + ), + DeclareLaunchArgument( + 'use_closest', + default_value='True', + description="Use closest point for calibration" + ), + DeclareLaunchArgument( + 'lidar_frame', + default_value='lidar_frame', + description="TF frame of the lidar" + ), + Node( + package='toid_lidar', + executable='toid_lidar', + output="screen", + parameters=[ + lidar_config, + { + 'closest': use_closest, + 'draw_markers': draw_markers + }] + ), + Node( + package='rplidar_ros', + executable='rplidar_composition', + output="screen", + parameters=[lidar_config, {'frame_id': lidar_frame}] + ), + Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config], + condition=IfCondition(visualize) + ) + ]) diff --git a/toid_lidar/params/lidar.yaml b/toid_lidar/params/lidar.yaml new file mode 100644 index 0000000..e6db752 --- /dev/null +++ b/toid_lidar/params/lidar.yaml @@ -0,0 +1,11 @@ +rplidar_node: + ros__parameters: + frame_id: "lidar_frame" + +toid_lidar: + ros__parameters: + map_width: 3.0 + map_height: 2.0 + draw_makers: false + closest: true + \ No newline at end of file diff --git a/toid_lidar/rviz/conf.rviz b/toid_lidar/rviz/conf.rviz new file mode 100644 index 0000000..2c0ce6e --- /dev/null +++ b/toid_lidar/rviz/conf.rviz @@ -0,0 +1,175 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.71659916639328 + Tree Height: 1144 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LaserScan +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Marker + Namespaces: + "": true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rivalMaker + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 47 + Min Color: 0; 0; 0 + Min Intensity: 47 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: -0.24500009417533875 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 152.6725311279297 + Target Frame: + Value: TopDownOrtho (rviz_default_plugins) + X: 0 + Y: 0 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1662 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001f000000574fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000070000005740000018600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015b00000574fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000070000005740000013800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000b400000005efc0100000002fb0000000800540069006d0065010000000000000b400000048700fffffffb0000000800540069006d00650100000000000004500000000000000000000007dd0000057400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2880 + X: 0 + Y: 64 -- 2.49.1 From 3d8dd3127deb67c1008c5f1a80eae2f2b2fa1804 Mon Sep 17 00:00:00 2001 From: pimpest <82343504+pimpest@users.noreply.github.com> Date: Thu, 26 Mar 2026 18:33:32 +0100 Subject: [PATCH 32/86] Added RivalLayer and finished GameElementLayer --- toid_costmaps/CMakeLists.txt | 18 ++- toid_costmaps/elements/elements.json | 46 +++++++ .../include/toid_costmaps/element_info.hpp | 46 +++++++ .../toid_costmaps/game_elements_layer.hpp | 43 +++++-- .../include/toid_costmaps/rival_layer.hpp | 55 +++++++++ toid_costmaps/package.xml | 2 + toid_costmaps/src/game_elements_layer.cpp | 114 ++++++++++++++++-- toid_costmaps/src/rival_layer.cpp | 108 +++++++++++++++++ toid_costmaps/toid_costmaps.xml | 3 + toid_msgs/CMakeLists.txt | 1 + toid_msgs/msg/ActiveElements.msg | 2 + .../params/toid_costmap_params.yaml | 12 +- 12 files changed, 426 insertions(+), 24 deletions(-) create mode 100644 toid_costmaps/elements/elements.json create mode 100644 toid_costmaps/include/toid_costmaps/element_info.hpp create mode 100644 toid_costmaps/include/toid_costmaps/rival_layer.hpp create mode 100644 toid_costmaps/src/rival_layer.cpp create mode 100644 toid_msgs/msg/ActiveElements.msg diff --git a/toid_costmaps/CMakeLists.txt b/toid_costmaps/CMakeLists.txt index 9f31edc..2aae4e9 100644 --- a/toid_costmaps/CMakeLists.txt +++ b/toid_costmaps/CMakeLists.txt @@ -9,9 +9,9 @@ set(library_name toid_costmaps) set( PACKAGE_DEPS - rclcpp - angles + ament_index_cpp + Boost geometry_msgs pluginlib nav_msgs @@ -28,6 +28,7 @@ set( set( SOURCES src/game_elements_layer.cpp + src/rival_layer.cpp ) find_package(ament_cmake REQUIRED) @@ -35,8 +36,16 @@ foreach(PACKAGE ${PACKAGE_DEPS}) find_package(${PACKAGE} REQUIRED) endforeach() +find_package(Boost REQUIRED COMPONENTS json) + + add_library(${library_name} SHARED ${SOURCES}) +target_link_libraries( + ${library_name} + Boost::json +) + target_include_directories( ${library_name} PRIVATE @@ -60,6 +69,11 @@ install( DESTINATION include/ ) +install( + DIRECTORY elements + DESTINATION share/${PROJECT_NAME}/ +) + ament_export_include_directories(include) ament_export_libraries(${library_name}) ament_export_dependencies(${PACKAGE_DEPS}) diff --git a/toid_costmaps/elements/elements.json b/toid_costmaps/elements/elements.json new file mode 100644 index 0000000..2d3e5b2 --- /dev/null +++ b/toid_costmaps/elements/elements.json @@ -0,0 +1,46 @@ +{ + "game_elements": [ + { + "x": 0.0, + "y": 0.7, + "width": 0.2, + "height": 0.2 + }, + { + "x": 0.05, + "y": 0.3, + "width": 0.2, + "height": 0.2 + }, + { + "x": 0.6, + "y": 0.0, + "width": 0.2, + "height": 0.2 + }, + { + "x": 0.05, + "y": 1.1, + "width": 0.2, + "height": 0.2 + } + ], + + "blue": [ + { + "x": 0.00, + "y": 1.55, + "width": 0.6, + "height": 0.45 + } + ], + + "yellow": [ + { + "x": 2.40, + "y": 1.55, + "width": 0.6, + "height": 0.45 + } + ] +} diff --git a/toid_costmaps/include/toid_costmaps/element_info.hpp b/toid_costmaps/include/toid_costmaps/element_info.hpp new file mode 100644 index 0000000..a021b18 --- /dev/null +++ b/toid_costmaps/include/toid_costmaps/element_info.hpp @@ -0,0 +1,46 @@ +#pragma once + +#include "boost/json.hpp" + +namespace toid +{ +class GameElement +{ +public: + GameElement(double x, double y, double width, double height) + : x_(x), y_(y), width_(width), height_(height) + { + } + + void start(double & x, double & y) const + { + x = this->x_; + y = this->y_; + } + + void end(double & x, double & y) const + { + x = this->x_ + this->width_; + y = this->y_ + this->height_; + } + +private: + double x_; + double y_; + double width_; + double height_; +}; + +inline GameElement tag_invoke( + boost::json::value_to_tag, boost::json::value const & jv) +{ + auto const& obj = jv.as_object(); + return GameElement{ + boost::json::value_to(obj.at("x")) - 1.5, + boost::json::value_to(obj.at("y")) - 1.0, + boost::json::value_to(obj.at("width")), + boost::json::value_to(obj.at("height")) + }; +} + +} // namespace toid \ No newline at end of file diff --git a/toid_costmaps/include/toid_costmaps/game_elements_layer.hpp b/toid_costmaps/include/toid_costmaps/game_elements_layer.hpp index de71151..d550e3a 100644 --- a/toid_costmaps/include/toid_costmaps/game_elements_layer.hpp +++ b/toid_costmaps/include/toid_costmaps/game_elements_layer.hpp @@ -1,39 +1,58 @@ #pragma once +#include "jsoncpp/json/json.h" #include "nav2_costmap_2d/costmap_layer.hpp" #include "nav2_costmap_2d/layer.hpp" #include "nav2_costmap_2d/layered_costmap.hpp" #include "rclcpp/rclcpp.hpp" +#include "toid_costmaps/element_info.hpp" +#include "toid_msgs/msg/active_elements.hpp" namespace toid { class GameElementLayer : public nav2_costmap_2d::CostmapLayer { + using ActiveElements = toid_msgs::msg::ActiveElements; + public: - GameElementLayer(){ - costmap_ = NULL; - } - ~GameElementLayer(){} + GameElementLayer() { costmap_ = NULL; } + ~GameElementLayer() {} - virtual void onInitialize(); - virtual void updateBounds( + void onInitialize() override; + void activate() override; + void deactivate() override; + + void updateBounds( double robot_x, double robot_y, double robot_yaw, double * min_x, double * min_y, - double * max_x, double * max_y); + double * max_x, double * max_y) override; - virtual void updateCosts( - nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j); + void updateCosts( + nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j) override; - virtual void reset() { return; } + void reset() override { return; } - virtual void onFootprintChanged() {} + void onFootprintChanged() override {} - virtual bool isClearable() { return false; } + void placeElement(nav2_costmap_2d::Costmap2D & grid, const GameElement & element); + + bool isClearable() override { return true; } + + void initGameElements(); + + void active_elements_cb(ActiveElements::UniquePtr msg); private: double last_min_x_, last_min_y_, last_max_x_, last_max_y_; bool need_recalculation_; + + std::vector game_elements_; + std::vector static_elements_; + std::vector extra_elements_; + ActiveElements::UniquePtr active_elements_; + + rclcpp::Subscription::SharedPtr active_elements_sub_; }; } // namespace toid \ No newline at end of file diff --git a/toid_costmaps/include/toid_costmaps/rival_layer.hpp b/toid_costmaps/include/toid_costmaps/rival_layer.hpp new file mode 100644 index 0000000..d63177e --- /dev/null +++ b/toid_costmaps/include/toid_costmaps/rival_layer.hpp @@ -0,0 +1,55 @@ +#pragma once + +#include "nav2_costmap_2d/costmap_layer.hpp" +#include "nav2_costmap_2d/layered_costmap.hpp" +#include "rclcpp/rclcpp.hpp" +#include "toid_msgs/msg/rival.hpp" + + +namespace toid +{ + +class RivalLayer : public nav2_costmap_2d::CostmapLayer +{ + using Rivals = toid_msgs::msg::Rival; + +public: + RivalLayer() { costmap_ = NULL; } + ~RivalLayer() {} + + void onInitialize() override; + void activate() override; + void deactivate() override; + + void updateBounds( + double robot_x, double robot_y, double robot_yaw, double * min_x, double * min_y, + double * max_x, double * max_y) override; + + void updateCosts( + nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j) override; + + void reset() override { return; } + + void onFootprintChanged() override {} + + void placeRival(nav2_costmap_2d::Costmap2D & grid, double x, double y); + + bool isClearable() override { return true; } + + void rival_cb(Rivals::UniquePtr msg); + +private: + double last_min_x_, last_min_y_, last_max_x_, last_max_y_; + + bool need_recalculation_; + + uint debounce_ = 0; + + Rivals::UniquePtr rivals_; + + rclcpp::Subscription::SharedPtr rival_sub_; + + double rival_size_ = 0.15; +}; + +} // namespace toid \ No newline at end of file diff --git a/toid_costmaps/package.xml b/toid_costmaps/package.xml index 98952d0..80080d8 100644 --- a/toid_costmaps/package.xml +++ b/toid_costmaps/package.xml @@ -10,6 +10,8 @@ ament_cmake angles + ament_index_cpp + boost geometry_msgs nav_msgs nav2_costmap_2d diff --git a/toid_costmaps/src/game_elements_layer.cpp b/toid_costmaps/src/game_elements_layer.cpp index aecba67..05f0357 100644 --- a/toid_costmaps/src/game_elements_layer.cpp +++ b/toid_costmaps/src/game_elements_layer.cpp @@ -1,27 +1,86 @@ #include "toid_costmaps/game_elements_layer.hpp" +#include + +#include "ament_index_cpp/get_package_share_directory.hpp" #include "nav2_util/node_utils.hpp" namespace toid { -void GameElementLayer::onInitialize() {} +void GameElementLayer::onInitialize() +{ + auto node = node_.lock(); + if (!node) { + return; + } +} + +void GameElementLayer::activate() +{ + auto node = node_.lock(); + + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".extra_elements", rclcpp::ParameterValue(std::vector{})); + node->get_parameter(name_ + ".extra_elements", extra_elements_); + + initGameElements(); + + active_elements_ = std::make_unique(); + active_elements_->active = std::vector(game_elements_.size(), true); + + using namespace std::placeholders; + active_elements_sub_ = node->create_subscription( + "/active_elements", 1, std::bind(&GameElementLayer::active_elements_cb, this, _1)); +} + +void GameElementLayer::deactivate() +{ + auto node = node_.lock(); + active_elements_sub_.reset(); + active_elements_.reset(); + game_elements_.clear(); + static_elements_.clear(); + extra_elements_.clear(); +} + +void GameElementLayer::active_elements_cb(ActiveElements::UniquePtr msg) +{ + active_elements_ = std::move(msg); +} void GameElementLayer::updateBounds( double, double, double, double * min_x, double * min_y, double * max_x, double * max_y) { - touch(1.0 - 0.1, 0.5 - 0.1, min_x, min_y, max_x, max_y); - touch(1.0 + 0.1, 0.5 + 0.1, min_x, min_y, max_x, max_y); + touch(1.50, 1.0, min_x, min_y, max_x, max_y); + touch(-1.50, -1.0, min_x, min_y, max_x, max_y); } -void GameElementLayer::updateCosts( - nav2_costmap_2d::Costmap2D & grid, int min_i, int min_j, int max_i, int max_j) +void GameElementLayer::updateCosts(nav2_costmap_2d::Costmap2D & grid, int, int, int, int) +{ + ulong idx = 0; + for (const auto & elem : game_elements_) { + if (active_elements_->active.size() > idx && active_elements_->active.at(idx)) { + placeElement(grid, elem); + } + idx++; + } + + for (const auto & elem : static_elements_) { + placeElement(grid, elem); + } +} + +void GameElementLayer::placeElement(nav2_costmap_2d::Costmap2D & grid, const GameElement & element) { bool in_bounds = true; + double ex, ey; + element.start(ex, ey); uint mx, my; - in_bounds &= worldToMap(1.0 - 0.1, 0.5 - 0.1, mx, my); + in_bounds &= worldToMap(ex + 0.001, ey + 0.001, mx, my); + element.end(ex, ey); uint mmx, mmy; - in_bounds &= worldToMap(1.0 + 0.1, 0.5 + 0.1, mmx, mmy); + in_bounds &= worldToMap(ex + 0.001, ey + 0.001, mmx, mmy); if (in_bounds) { for (uint j = my; j < mmy; j++) { @@ -32,6 +91,47 @@ void GameElementLayer::updateCosts( } } +void GameElementLayer::initGameElements() +{ + std::string file_path = "/elements/elements.json"; + std::string base_path = ament_index_cpp::get_package_share_directory("toid_costmaps"); + + RCLCPP_INFO(rclcpp::get_logger("toid_costmap"), "Bro hear me out"); + + std::ifstream fs(base_path + file_path); + if (!fs.is_open()) { + RCLCPP_ERROR( + rclcpp::get_logger("toid_costmap"), "Failed to open elements file: %s%s", base_path.c_str(), + file_path.c_str()); + throw std::runtime_error("File not found"); + } + + boost::json::stream_parser p; + boost::json::error_code ec; + + char buffer[4096]; + while (fs.read(buffer, sizeof(buffer)) || fs.gcount() > 0) { + p.write(buffer, fs.gcount(), ec); + if (ec) { + RCLCPP_FATAL(rclcpp::get_logger("toid_costmap"), "JsonError: %s", ec.message().c_str()); + throw std::runtime_error("JsonError"); + } + } + p.finish(ec); + if (ec) { + RCLCPP_FATAL(rclcpp::get_logger("toid_costmap"), "JsonError: %s", ec.message().c_str()); + throw std::runtime_error("JsonError"); + } + + boost::json::value jv = p.release(); + + game_elements_ = boost::json::value_to>(jv.at("game_elements")); + for (const auto & t : extra_elements_) { + std::vector elements = boost::json::value_to>(jv.at(t)); + static_elements_.insert(static_elements_.end(), elements.begin(), elements.end()); + } +} + } // namespace toid #include "pluginlib/class_list_macros.hpp" diff --git a/toid_costmaps/src/rival_layer.cpp b/toid_costmaps/src/rival_layer.cpp new file mode 100644 index 0000000..4ffbb0b --- /dev/null +++ b/toid_costmaps/src/rival_layer.cpp @@ -0,0 +1,108 @@ + +#include "toid_costmaps/rival_layer.hpp" + +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "nav2_util/node_utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace toid +{ + +void RivalLayer::onInitialize() +{ + auto node = node_.lock(); + if (!node) { + return; + } +} + +void RivalLayer::activate() +{ + auto node = node_.lock(); + + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".rival_size", rclcpp::ParameterValue(0.15)); + node->get_parameter(name_ + ".rival_size", rival_size_); + + using namespace std::placeholders; + rival_sub_ = node->create_subscription( + "/dynamicObstacle", 1, std::bind(&RivalLayer::rival_cb, this, _1)); +} + +void RivalLayer::deactivate() +{ + auto node = node_.lock(); + rival_sub_.reset(); + rivals_.reset(); +} + +void RivalLayer::rival_cb(Rivals::UniquePtr msg) +{ + if (msg->point.size() != 0 || debounce_++ > 10) { + rivals_ = std::move(msg); + debounce_ = 0; + } +} + +void RivalLayer::updateBounds( + double, double, double, double * min_x, double * min_y, double * max_x, double * max_y) +{ + touch(1.50, 1.0, min_x, min_y, max_x, max_y); + touch(-1.50, -1.0, min_x, min_y, max_x, max_y); +} + +void RivalLayer::updateCosts(nav2_costmap_2d::Costmap2D & grid, int, int, int, int) +{ + geometry_msgs::msg::TransformStamped tf_msg; + if(!rivals_) { + return; + } + + try { + tf_msg = tf_->lookupTransform( + layered_costmap_->getGlobalFrameID(), rivals_->header.frame_id, rivals_->header.stamp); + } catch (const tf2::TransformException & e) { + RCLCPP_WARN_THROTTLE(logger_, *clock_, 1000, "Failed to transform rival message to costmap"); + return; + } + + for (const auto & rival : rivals_->point) { + geometry_msgs::msg::Point point; + tf2::doTransform(rival, point, tf_msg); + placeRival(grid, point.x, point.y); + } +} + +void RivalLayer::placeRival(nav2_costmap_2d::Costmap2D & grid, const double x, const double y) +{ + unsigned int mx, my; + if (!grid.worldToMap(x, y, mx, my)) { + return; // Center is outside the map bounds + } + + double res = grid.getResolution(); + int cell_radius = static_cast(rival_size_ / res); + + int min_i = std::max(0, static_cast(mx) - cell_radius); + int max_i = + std::min(static_cast(grid.getSizeInCellsX()) - 1, static_cast(mx) + cell_radius); + int min_j = std::max(0, static_cast(my) - cell_radius); + int max_j = + std::min(static_cast(grid.getSizeInCellsY()) - 1, static_cast(my) + cell_radius); + + for (int i = min_i; i <= max_i; ++i) { + for (int j = min_j; j <= max_j; ++j) { + double di = static_cast(i) - static_cast(mx); + double dj = static_cast(j) - static_cast(my); + double distance_sq = di * di + dj * dj; + if (distance_sq <= static_cast(cell_radius * cell_radius)) { + grid.setCost(i, j, nav2_costmap_2d::LETHAL_OBSTACLE); + } + } + } +} + +} // namespace toid + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(toid::RivalLayer, nav2_costmap_2d::Layer); \ No newline at end of file diff --git a/toid_costmaps/toid_costmaps.xml b/toid_costmaps/toid_costmaps.xml index bead28e..3211914 100644 --- a/toid_costmaps/toid_costmaps.xml +++ b/toid_costmaps/toid_costmaps.xml @@ -3,5 +3,8 @@ + + + \ No newline at end of file diff --git a/toid_msgs/CMakeLists.txt b/toid_msgs/CMakeLists.txt index a505d52..1142ee6 100644 --- a/toid_msgs/CMakeLists.txt +++ b/toid_msgs/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(rosidl_default_generators REQUIRED) find_package(geometry_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} + "msg/ActiveElements.msg" "msg/Rival.msg" "srv/SendDouble.srv" "action/SimpleMoveCoords.action" diff --git a/toid_msgs/msg/ActiveElements.msg b/toid_msgs/msg/ActiveElements.msg new file mode 100644 index 0000000..689d2c1 --- /dev/null +++ b/toid_msgs/msg/ActiveElements.msg @@ -0,0 +1,2 @@ +# Activate/deactivate game elements +bool[] active \ No newline at end of file diff --git a/toid_navigation/params/toid_costmap_params.yaml b/toid_navigation/params/toid_costmap_params.yaml index 56cb21c..aa0781a 100644 --- a/toid_navigation/params/toid_costmap_params.yaml +++ b/toid_navigation/params/toid_costmap_params.yaml @@ -1,14 +1,14 @@ global_costmap: ros__parameters: - update_frequency: 1.0 - publish_frequency: 1.0 + update_frequency: 10.0 + publish_frequency: 10.0 global_frame: map robot_base_frame: base_link robot_radius: 0.17 resolution: 0.01 track_unknown_space: false rolling_window: false - plugins: ["static_layer", "inflation_layer"] + plugins: ["static_layer", "game_element_layer", "rival_layer", "inflation_layer"] static_layer: plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True @@ -16,6 +16,12 @@ global_costmap: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 inflation_radius: 0.23 + game_element_layer: + plugin: "toid::GameElementLayer" + extra_elements: ["blue"] + rival_layer: + plugin: "toid::RivalLayer" + rival_size: 0.15 always_send_full_costmap: True lifecycle_manager: -- 2.49.1 From 089b630538bb17e5313bb78a2cda5c1eccfe4b0c Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Mon, 30 Mar 2026 12:18:41 +0200 Subject: [PATCH 33/86] Switch to shared pointer to hopefully avoid crash --- toid_costmaps/include/toid_costmaps/game_elements_layer.hpp | 4 ++-- toid_costmaps/include/toid_costmaps/rival_layer.hpp | 4 ++-- toid_costmaps/src/game_elements_layer.cpp | 4 ++-- toid_costmaps/src/rival_layer.cpp | 4 ++-- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/toid_costmaps/include/toid_costmaps/game_elements_layer.hpp b/toid_costmaps/include/toid_costmaps/game_elements_layer.hpp index d550e3a..f589d50 100644 --- a/toid_costmaps/include/toid_costmaps/game_elements_layer.hpp +++ b/toid_costmaps/include/toid_costmaps/game_elements_layer.hpp @@ -40,7 +40,7 @@ public: void initGameElements(); - void active_elements_cb(ActiveElements::UniquePtr msg); + void active_elements_cb(ActiveElements::SharedPtr msg); private: double last_min_x_, last_min_y_, last_max_x_, last_max_y_; @@ -50,7 +50,7 @@ private: std::vector game_elements_; std::vector static_elements_; std::vector extra_elements_; - ActiveElements::UniquePtr active_elements_; + ActiveElements::SharedPtr active_elements_; rclcpp::Subscription::SharedPtr active_elements_sub_; }; diff --git a/toid_costmaps/include/toid_costmaps/rival_layer.hpp b/toid_costmaps/include/toid_costmaps/rival_layer.hpp index d63177e..87c6914 100644 --- a/toid_costmaps/include/toid_costmaps/rival_layer.hpp +++ b/toid_costmaps/include/toid_costmaps/rival_layer.hpp @@ -36,7 +36,7 @@ public: bool isClearable() override { return true; } - void rival_cb(Rivals::UniquePtr msg); + void rival_cb(Rivals::SharedPtr msg); private: double last_min_x_, last_min_y_, last_max_x_, last_max_y_; @@ -45,7 +45,7 @@ private: uint debounce_ = 0; - Rivals::UniquePtr rivals_; + Rivals::SharedPtr rivals_; rclcpp::Subscription::SharedPtr rival_sub_; diff --git a/toid_costmaps/src/game_elements_layer.cpp b/toid_costmaps/src/game_elements_layer.cpp index 05f0357..fb11c92 100644 --- a/toid_costmaps/src/game_elements_layer.cpp +++ b/toid_costmaps/src/game_elements_layer.cpp @@ -44,9 +44,9 @@ void GameElementLayer::deactivate() extra_elements_.clear(); } -void GameElementLayer::active_elements_cb(ActiveElements::UniquePtr msg) +void GameElementLayer::active_elements_cb(ActiveElements::SharedPtr msg) { - active_elements_ = std::move(msg); + active_elements_ = msg; } void GameElementLayer::updateBounds( diff --git a/toid_costmaps/src/rival_layer.cpp b/toid_costmaps/src/rival_layer.cpp index 4ffbb0b..7ae1fbf 100644 --- a/toid_costmaps/src/rival_layer.cpp +++ b/toid_costmaps/src/rival_layer.cpp @@ -36,10 +36,10 @@ void RivalLayer::deactivate() rivals_.reset(); } -void RivalLayer::rival_cb(Rivals::UniquePtr msg) +void RivalLayer::rival_cb(Rivals::SharedPtr msg) { if (msg->point.size() != 0 || debounce_++ > 10) { - rivals_ = std::move(msg); + rivals_ = msg; debounce_ = 0; } } -- 2.49.1 From 88ebe9fb6829aa7361560ef796a966958ef9e4ea Mon Sep 17 00:00:00 2001 From: pimpest <82343504+pimpest@users.noreply.github.com> Date: Wed, 1 Apr 2026 17:46:49 +0200 Subject: [PATCH 34/86] added collision detection --- .../include/toid_behaviors/move_coords.hpp | 11 +- toid_behaviors/include/toid_behaviors/scl.hpp | 10 +- .../include/toid_behaviors/simple_move.hpp | 59 +++++++++ .../include/toid_behaviors/simple_rotate.hpp | 18 ++- toid_behaviors/src/move_coords.cpp | 120 ++++++++++++++---- toid_behaviors/src/scl.cpp | 31 ++++- toid_behaviors/src/simple_rotate.cpp | 113 ++++++++++++++--- 7 files changed, 301 insertions(+), 61 deletions(-) diff --git a/toid_behaviors/include/toid_behaviors/move_coords.hpp b/toid_behaviors/include/toid_behaviors/move_coords.hpp index 0bf0170..1296af2 100644 --- a/toid_behaviors/include/toid_behaviors/move_coords.hpp +++ b/toid_behaviors/include/toid_behaviors/move_coords.hpp @@ -23,17 +23,24 @@ public: const std::shared_ptr command, const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & vel) override; + double distanceToTarget( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & target_point, const double target_theta, bool backwards); + + double velocityTarget(const double dist_left); + + bool collisionDetection(const geometry_msgs::msg::Pose &pose, geometry_msgs::msg::Pose &last_ok_pose); + 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; + return nav2_core::CostmapInfoType::LOCAL; } protected: - SmoothControlLaw scl; + SmoothControlLaw scl_; //Goal geometry_msgs::msg::Pose target_pose_; diff --git a/toid_behaviors/include/toid_behaviors/scl.hpp b/toid_behaviors/include/toid_behaviors/scl.hpp index af8194f..d80528f 100644 --- a/toid_behaviors/include/toid_behaviors/scl.hpp +++ b/toid_behaviors/include/toid_behaviors/scl.hpp @@ -19,13 +19,17 @@ public: double v_angular_max = 2.0; void 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); double curvature(double r, double phi, double delta); void calculate_vel( - const geometry_msgs::msg::Pose & target,const geometry_msgs::msg::Pose & current, + const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current, geometry_msgs::msg::Twist & out_speed, bool backwards = false); + + void step( + const geometry_msgs::msg::Pose & target, geometry_msgs::msg::Pose & current, double dt, + bool backwards = false); }; } // namespace toid \ No newline at end of file diff --git a/toid_behaviors/include/toid_behaviors/simple_move.hpp b/toid_behaviors/include/toid_behaviors/simple_move.hpp index 0c26e5d..fefd9f5 100644 --- a/toid_behaviors/include/toid_behaviors/simple_move.hpp +++ b/toid_behaviors/include/toid_behaviors/simple_move.hpp @@ -6,6 +6,7 @@ #include "nav_msgs/msg/odometry.hpp" #include "rclcpp/rclcpp.hpp" #include "toid_msgs/action/simple_rotate.hpp" +#include "toid_msgs/msg/rival.hpp" namespace toid { @@ -14,6 +15,8 @@ template class SimpleMove : public nav2_behaviors::TimedBehavior { public: + using Rival = toid_msgs::msg::Rival; + virtual void configureCB() {} virtual void cleanupCB() {} @@ -34,6 +37,18 @@ public: 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(); + + nav2_util::declare_parameter_if_not_declared(node, "robot_width", rclcpp::ParameterValue(0.30)); + node->get_parameter("robot_width", robot_width_); + + nav2_util::declare_parameter_if_not_declared( + node, "robot_length", rclcpp::ParameterValue(0.30)); + node->get_parameter("robot_length", robot_length_); + + nav2_util::declare_parameter_if_not_declared( + node, "rival_radius", rclcpp::ParameterValue(0.30)); + node->get_parameter("rival_radius", rival_radius_); + odom_sub_ = node->create_subscription( odom_topic_name, 1, [&](nav_msgs::msg::Odometry msg) { std::lock_guard lock(mut_); @@ -52,12 +67,17 @@ public: void activate() override { nav2_behaviors::TimedBehavior::activate(); + rclcpp_lifecycle::LifecycleNode::SharedPtr node = this->node_.lock(); + using namespace std::placeholders; + rivals_sub_ = node->create_subscription( + "/dynamicObstacle", 1, std::bind(&SimpleMove::rival_cb, this, _1)); activateCB(); } void deactivate() override { nav2_behaviors::TimedBehavior::deactivate(); + rivals_sub_.reset(); deactivateCB(); } @@ -105,12 +125,51 @@ public: return r; } + bool check_rival_collision(geometry_msgs::msg::Pose2D & pose) + { + if (!rivals_) { + return false; + } + const double cosp = std::cos(pose.theta); + const double sinp = std::sin(pose.theta); + for (const auto & rival : rivals_->point) { + geometry_msgs::msg::Point local_rival; + const double dx = rival.x - pose.x; + const double dy = rival.y - pose.y; + local_rival.x = dx * cosp + dy * sinp; + local_rival.y = -dx * sinp + dy * cosp; + + const double qx = std::abs(local_rival.x) - robot_length_ / 2.0; + const double qy = std::abs(local_rival.y) - robot_width_ / 2.0; + + const double mqx = std::max(qx, 0.0); + const double mqy = std::max(qy, 0.0); + + double length = std::sqrt(mqx * mqx + mqy * mqy); + + double sdf = length + std::min(std::max(qx, qy), 0.0); + if (sdf < rival_radius_) { + return true; + } + } + return false; + } + + void rival_cb(Rival::SharedPtr msg) { rivals_ = msg; } + 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_; + + double robot_width_ = 0.30; + double robot_length_ = 0.30; + double rival_radius_ = 0.30; + + Rival::SharedPtr rivals_; + rclcpp::Subscription::SharedPtr rivals_sub_; }; } // 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 index cec55e2..758eb68 100644 --- a/toid_behaviors/include/toid_behaviors/simple_rotate.hpp +++ b/toid_behaviors/include/toid_behaviors/simple_rotate.hpp @@ -2,6 +2,7 @@ #include "toid_behaviors/simple_move.hpp" #include "toid_msgs/action/simple_rotate.h" +#include "toid_msgs/msg/rival.hpp" namespace toid { @@ -23,17 +24,25 @@ public: 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; + + virtual nav2_core::CostmapInfoType getResourceInfo() override + { + return nav2_core::CostmapInfoType::LOCAL; } -protected: + void calc_err_and_sign( + double last_angle, double current_yaw, double & min_turn_angle, double & err, double & sign); + double check_space(const geometry_msgs::msg::Pose pose, const double e, const double sign); + + double calc_speed(const double err, const double sign, const double angular_speed); + +protected: //Goal double target_angle_; double min_turn_angle_; double initial_direction_; + unsigned char mode_; //State double angular_speed_; @@ -45,6 +54,7 @@ protected: double min_angular_speed_; double max_angular_accel_; double max_angular_decel_; + double lookahead_; }; } // namespace toid diff --git a/toid_behaviors/src/move_coords.cpp b/toid_behaviors/src/move_coords.cpp index d4a5320..b80ed1d 100644 --- a/toid_behaviors/src/move_coords.cpp +++ b/toid_behaviors/src/move_coords.cpp @@ -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(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); \ No newline at end of file diff --git a/toid_behaviors/src/scl.cpp b/toid_behaviors/src/scl.cpp index 12ad53f..fe22bdc 100644 --- a/toid_behaviors/src/scl.cpp +++ b/toid_behaviors/src/scl.cpp @@ -3,20 +3,21 @@ #include #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); } -} \ No newline at end of file +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 \ No newline at end of file diff --git a/toid_behaviors/src/simple_rotate.cpp b/toid_behaviors/src/simple_rotate.cpp index 4c75f60..9fdde0f 100644 --- a/toid_behaviors/src/simple_rotate.cpp +++ b/toid_behaviors/src/simple_rotate.cpp @@ -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" -- 2.49.1 From ff1fbf5abeb59a11781ce37fd0e1134dc5eced46 Mon Sep 17 00:00:00 2001 From: pimpest <82343504+pimpest@users.noreply.github.com> Date: Wed, 1 Apr 2026 17:51:29 +0200 Subject: [PATCH 35/86] Changed behavior tree actions to ignore costmap --- .../include/toid_behaviors/move_coords.hpp | 1 + toid_behaviors/src/move_coords.cpp | 40 +++++++++---------- .../toid_bt/plugins/move_coords_action.hpp | 1 + .../include/toid_bt/plugins/rotate_action.hpp | 1 + .../toid_bt/plugins/rotate_towards_action.hpp | 1 + 5 files changed, 24 insertions(+), 20 deletions(-) diff --git a/toid_behaviors/include/toid_behaviors/move_coords.hpp b/toid_behaviors/include/toid_behaviors/move_coords.hpp index 1296af2..9b84d4b 100644 --- a/toid_behaviors/include/toid_behaviors/move_coords.hpp +++ b/toid_behaviors/include/toid_behaviors/move_coords.hpp @@ -47,6 +47,7 @@ protected: double target_angle_; double target_sign_; bool backwards_; + unsigned char mode_; //State double last_speed_; diff --git a/toid_behaviors/src/move_coords.cpp b/toid_behaviors/src/move_coords.cpp index b80ed1d..c71336e 100644 --- a/toid_behaviors/src/move_coords.cpp +++ b/toid_behaviors/src/move_coords.cpp @@ -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(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(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); diff --git a/toid_bt/include/toid_bt/plugins/move_coords_action.hpp b/toid_bt/include/toid_bt/plugins/move_coords_action.hpp index 6e40dd7..2b759c0 100644 --- a/toid_bt/include/toid_bt/plugins/move_coords_action.hpp +++ b/toid_bt/include/toid_bt/plugins/move_coords_action.hpp @@ -53,6 +53,7 @@ public: goal.max_speed = max_speed; goal.backwards = backwards; + goal.mode = 1; return true; } diff --git a/toid_bt/include/toid_bt/plugins/rotate_action.hpp b/toid_bt/include/toid_bt/plugins/rotate_action.hpp index 14ef835..7597dc9 100644 --- a/toid_bt/include/toid_bt/plugins/rotate_action.hpp +++ b/toid_bt/include/toid_bt/plugins/rotate_action.hpp @@ -40,6 +40,7 @@ public: goal.max_speed = max_speed.value(); goal.angle = angles::from_degrees(goal.angle); goal.min_angle = angles::from_degrees(goal.min_angle); + goal.mode = 1; return true; } diff --git a/toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp b/toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp index b2733f9..9649a8f 100644 --- a/toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp +++ b/toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp @@ -44,6 +44,7 @@ public: goal.angle = std::atan2(y - pose.pose.position.y, x - pose.pose.position.x); goal.min_angle = angles::from_degrees(min_angle); goal.max_speed = max_speed; + goal.mode = 1; return true; } -- 2.49.1 From 080676384d9f51fff09ddc4589d8eeb05e2c9a9c Mon Sep 17 00:00:00 2001 From: pimpest <82343504+pimpest@users.noreply.github.com> Date: Wed, 1 Apr 2026 21:33:53 +0200 Subject: [PATCH 36/86] Added decorator node for checking stuck state --- toid_bt/behavior_trees/behavior1.xml | 17 +++-- toid_bt/behavior_trees/toid_behaviors.btproj | 33 +++++---- .../plugins/stuck_detector_decorator.hpp | 69 +++++++++++++++++++ toid_bt/src/bt_executor.cpp | 4 +- 4 files changed, 102 insertions(+), 21 deletions(-) create mode 100644 toid_bt/include/toid_bt/plugins/stuck_detector_decorator.hpp diff --git a/toid_bt/behavior_trees/behavior1.xml b/toid_bt/behavior_trees/behavior1.xml index f4ed8bd..38e29f7 100644 --- a/toid_bt/behavior_trees/behavior1.xml +++ b/toid_bt/behavior_trees/behavior1.xml @@ -23,16 +23,23 @@ - + + + + + + diff --git a/toid_bt/behavior_trees/toid_behaviors.btproj b/toid_bt/behavior_trees/toid_behaviors.btproj index 52ae4ff..501f4ea 100644 --- a/toid_bt/behavior_trees/toid_behaviors.btproj +++ b/toid_bt/behavior_trees/toid_behaviors.btproj @@ -4,43 +4,46 @@ + + + - Service name + Service name - - - Action server name + + + Action server name - - - Action server name + + + Action server name - - - Action server name + + + Action server name - + - Service name + Service name - - Action server name + + Action server name - Service name + Service name diff --git a/toid_bt/include/toid_bt/plugins/stuck_detector_decorator.hpp b/toid_bt/include/toid_bt/plugins/stuck_detector_decorator.hpp new file mode 100644 index 0000000..8430f31 --- /dev/null +++ b/toid_bt/include/toid_bt/plugins/stuck_detector_decorator.hpp @@ -0,0 +1,69 @@ +#pragma once + +#include "angles/angles.h" +#include "behaviortree_ros2/bt_service_node.hpp" +#include "std_srvs/srv/empty.hpp" +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "toid_bt/plugin.hpp" + +namespace toid +{ + +class StuckDetectorNode : public BT::DecoratorNode +{ +public: + StuckDetectorNode( + const std::string & name, const BT::NodeConfig & conf, PoseFunc get_pose) + : BT::DecoratorNode(name, conf), get_pose(get_pose) + { + } + + static BT::PortsList providedPorts() { + return { + BT::InputPort("timeout", 1, {}) + }; + } + + private: + bool checkIfPosesAreClose(geometry_msgs::msg::PoseStamped &poseA, geometry_msgs::msg::PoseStamped &poseB) { + const double dx = poseA.pose.position.x - poseB.pose.position.x; + const double dy = poseA.pose.position.y - poseB.pose.position.y; + const double dth = abs(tf2::getYaw(poseA.pose.orientation) - tf2::getYaw(poseB.pose.orientation)); + return dx*dx + dy*dy < 0.004*0.004 && dth < 0.05; + } + + BT::NodeStatus tick() override + { + if(status() == BT::NodeStatus::IDLE) { + setStatus(BT::NodeStatus::RUNNING); + last_pos_change = clock.now(); + } + + double timeout = getInput("timeout").value_or(1.0); + + + geometry_msgs::msg::PoseStamped pose; + get_pose(pose); + + if(checkIfPosesAreClose(last_pos, pose)) { + if(clock.now() - last_pos_change > rclcpp::Duration::from_seconds(timeout)) { + haltChild(); + return BT::NodeStatus::FAILURE; + } + } else { + last_pos = pose; + last_pos_change = clock.now(); + } + + const BT::NodeStatus child_status = child_node_->executeTick(); + return child_status; + } + + geometry_msgs::msg::PoseStamped last_pos; + PoseFunc get_pose; + rclcpp::Time last_pos_change; + rclcpp::Clock clock; +}; + +} // namespace toid \ No newline at end of file diff --git a/toid_bt/src/bt_executor.cpp b/toid_bt/src/bt_executor.cpp index b9eff34..5db62cf 100644 --- a/toid_bt/src/bt_executor.cpp +++ b/toid_bt/src/bt_executor.cpp @@ -9,6 +9,7 @@ #include "toid_bt/plugins/move_coords_action.hpp" #include "toid_bt/plugins/rotate_action.hpp" #include "toid_bt/plugins/rotate_towards_action.hpp" +#include "toid_bt/plugins/stuck_detector_decorator.hpp" #include "toid_bt/plugins/translate_x_action.hpp" namespace toid @@ -22,7 +23,6 @@ TreeExecutor::TreeExecutor(const rclcpp::NodeOptions opts) */ tf_buffer_ = std::make_shared(node()->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); - nav2_util::declare_parameter_if_not_declared( node(), "base_frame", rclcpp::ParameterValue("base_footprint")); @@ -61,6 +61,8 @@ void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory) factory.registerNodeType("EndCalib", BT::RosNodeParams(nh, "/end_calib")); + factory.registerNodeType("DetectStuck", get_pose); + std::cout << describeCustomNodes() << std::endl; } -- 2.49.1 From d5ed611c20533993b83fb629b3a9baa9daa6657f Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Fri, 3 Apr 2026 21:08:27 +0200 Subject: [PATCH 37/86] Created mechanism node --- .gitignore | 1 + toid_bt/behavior_trees/behavior1.xml | 23 + .../behavior_trees/calibration_routines.xml | 13 +- toid_bt/behavior_trees/toid_behaviors.btproj | 42 +- .../toid_bt/plugins/send_text_action.hpp | 43 ++ toid_bt/src/bt_executor.cpp | 9 + toid_interaction/package.xml | 20 + toid_interaction/resource/toid_interaction | 0 toid_interaction/setup.cfg | 4 + toid_interaction/setup.py | 31 + toid_interaction/test/test_copyright.py | 25 + toid_interaction/test/test_flake8.py | 25 + toid_interaction/test/test_pep257.py | 23 + toid_interaction/toid_interaction/__init__.py | 0 .../toid_interaction/interaction_node.py | 97 ++++ .../mechanism/STservo_sdk/__init__.py | 8 + .../mechanism/STservo_sdk/group_sync_read.py | 151 +++++ .../mechanism/STservo_sdk/group_sync_write.py | 73 +++ .../mechanism/STservo_sdk/port_handler.py | 115 ++++ .../STservo_sdk/protocol_packet_handler.py | 530 ++++++++++++++++++ .../mechanism/STservo_sdk/scscl.py | 104 ++++ .../mechanism/STservo_sdk/setup.py | 7 + .../mechanism/STservo_sdk/sts.py | 140 +++++ .../mechanism/STservo_sdk/stservo_def.py | 25 + .../toid_interaction/mechanism/__init__.py | 0 .../mechanism/sekvenca_2026.py | 67 +++ .../toid_interaction/mechanism/zidovi_load.py | 366 ++++++++++++ .../toid_interaction/mechanism/zupcanik.py | 130 +++++ toid_lidar/package.xml | 2 +- toid_msgs/CMakeLists.txt | 1 + toid_msgs/srv/SendString.srv | 2 + 31 files changed, 2056 insertions(+), 21 deletions(-) create mode 100644 toid_bt/include/toid_bt/plugins/send_text_action.hpp create mode 100644 toid_interaction/package.xml create mode 100644 toid_interaction/resource/toid_interaction create mode 100644 toid_interaction/setup.cfg create mode 100644 toid_interaction/setup.py create mode 100644 toid_interaction/test/test_copyright.py create mode 100644 toid_interaction/test/test_flake8.py create mode 100644 toid_interaction/test/test_pep257.py create mode 100644 toid_interaction/toid_interaction/__init__.py create mode 100644 toid_interaction/toid_interaction/interaction_node.py create mode 100644 toid_interaction/toid_interaction/mechanism/STservo_sdk/__init__.py create mode 100644 toid_interaction/toid_interaction/mechanism/STservo_sdk/group_sync_read.py create mode 100644 toid_interaction/toid_interaction/mechanism/STservo_sdk/group_sync_write.py create mode 100644 toid_interaction/toid_interaction/mechanism/STservo_sdk/port_handler.py create mode 100644 toid_interaction/toid_interaction/mechanism/STservo_sdk/protocol_packet_handler.py create mode 100644 toid_interaction/toid_interaction/mechanism/STservo_sdk/scscl.py create mode 100644 toid_interaction/toid_interaction/mechanism/STservo_sdk/setup.py create mode 100644 toid_interaction/toid_interaction/mechanism/STservo_sdk/sts.py create mode 100644 toid_interaction/toid_interaction/mechanism/STservo_sdk/stservo_def.py create mode 100644 toid_interaction/toid_interaction/mechanism/__init__.py create mode 100644 toid_interaction/toid_interaction/mechanism/sekvenca_2026.py create mode 100644 toid_interaction/toid_interaction/mechanism/zidovi_load.py create mode 100644 toid_interaction/toid_interaction/mechanism/zupcanik.py create mode 100644 toid_msgs/srv/SendString.srv diff --git a/.gitignore b/.gitignore index fe1e202..a8ed90f 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ +__pycache__ .cache build install diff --git a/toid_bt/behavior_trees/behavior1.xml b/toid_bt/behavior_trees/behavior1.xml index 38e29f7..e2a1bdb 100644 --- a/toid_bt/behavior_trees/behavior1.xml +++ b/toid_bt/behavior_trees/behavior1.xml @@ -1,5 +1,14 @@ + + + + + + + + @@ -70,6 +79,20 @@ Action server name + + Service name + + + + Service name + + + Service name + diff --git a/toid_bt/behavior_trees/calibration_routines.xml b/toid_bt/behavior_trees/calibration_routines.xml index e19211a..19b8c42 100644 --- a/toid_bt/behavior_trees/calibration_routines.xml +++ b/toid_bt/behavior_trees/calibration_routines.xml @@ -26,14 +26,14 @@ action_name=""/> - + - + - + - + @@ -102,6 +102,11 @@ + + + Service name diff --git a/toid_bt/behavior_trees/toid_behaviors.btproj b/toid_bt/behavior_trees/toid_behaviors.btproj index 501f4ea..054a172 100644 --- a/toid_bt/behavior_trees/toid_behaviors.btproj +++ b/toid_bt/behavior_trees/toid_behaviors.btproj @@ -5,45 +5,55 @@ - + - Service name + Service name - - - Action server name + + + Action server name - - - Action server name + + + Action server name - - - Action server name + + + Action server name + + + Service name + + + + Service name + + + Service name - + - Service name + Service name - - Action server name + + Action server name - Service name + Service name diff --git a/toid_bt/include/toid_bt/plugins/send_text_action.hpp b/toid_bt/include/toid_bt/plugins/send_text_action.hpp new file mode 100644 index 0000000..bcb18fd --- /dev/null +++ b/toid_bt/include/toid_bt/plugins/send_text_action.hpp @@ -0,0 +1,43 @@ +#pragma once + +#include "angles/angles.h" +#include "behaviortree_ros2/bt_service_node.hpp" +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "toid_bt/plugin.hpp" + +#include "toid_msgs/srv/send_string.hpp" + +namespace toid +{ + +class SendTextNode : public BT::RosServiceNode +{ +public: + SendTextNode( + const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params) + : BT::RosServiceNode(name, conf, params) + { + } + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ + BT::InputPort("text"), + }); + } + + bool setRequest(typename Request::SharedPtr &req) override { + std::string text = getInput("text").value_or(""); + req->text = text; + + return true; + } + + BT::NodeStatus onResponseReceived(const typename Response::SharedPtr &) override + { + return BT::NodeStatus::SUCCESS; + } +}; + +} // namespace toid \ No newline at end of file diff --git a/toid_bt/src/bt_executor.cpp b/toid_bt/src/bt_executor.cpp index 5db62cf..889aede 100644 --- a/toid_bt/src/bt_executor.cpp +++ b/toid_bt/src/bt_executor.cpp @@ -9,6 +9,7 @@ #include "toid_bt/plugins/move_coords_action.hpp" #include "toid_bt/plugins/rotate_action.hpp" #include "toid_bt/plugins/rotate_towards_action.hpp" +#include "toid_bt/plugins/send_text_action.hpp" #include "toid_bt/plugins/stuck_detector_decorator.hpp" #include "toid_bt/plugins/translate_x_action.hpp" @@ -61,6 +62,14 @@ void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory) factory.registerNodeType("EndCalib", BT::RosNodeParams(nh, "/end_calib")); + factory.registerNodeType("Seq1", BT::RosNodeParams(nh, "/sequence1")); + + BT::RosNodeParams service_params(nh, "/sequence2"); + service_params.server_timeout = std::chrono::seconds(15); + factory.registerNodeType("Seq2", service_params); + + factory.registerNodeType("Seq3", BT::RosNodeParams(nh, "/sequence3")); + factory.registerNodeType("DetectStuck", get_pose); std::cout << describeCustomNodes() << std::endl; diff --git a/toid_interaction/package.xml b/toid_interaction/package.xml new file mode 100644 index 0000000..2959e79 --- /dev/null +++ b/toid_interaction/package.xml @@ -0,0 +1,20 @@ + + + + toid_interaction + 0.0.0 + TODO: Package description + pimpest + MIT + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + python3-serial + + + ament_python + + diff --git a/toid_interaction/resource/toid_interaction b/toid_interaction/resource/toid_interaction new file mode 100644 index 0000000..e69de29 diff --git a/toid_interaction/setup.cfg b/toid_interaction/setup.cfg new file mode 100644 index 0000000..eb87452 --- /dev/null +++ b/toid_interaction/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/toid_interaction +[install] +install_scripts=$base/lib/toid_interaction diff --git a/toid_interaction/setup.py b/toid_interaction/setup.py new file mode 100644 index 0000000..6473e97 --- /dev/null +++ b/toid_interaction/setup.py @@ -0,0 +1,31 @@ +from setuptools import find_packages, setup + +package_name = 'toid_interaction' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='pimpest', + maintainer_email='82343504+pimpest@users.noreply.github.com', + description='TODO: Package description', + license='MIT', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + 'sequence = toid_interaction.mechanism.sekvenca_2026:main', + 'node = toid_interaction.interaction_node:main' + ], + }, +) diff --git a/toid_interaction/test/test_copyright.py b/toid_interaction/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/toid_interaction/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/toid_interaction/test/test_flake8.py b/toid_interaction/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/toid_interaction/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/toid_interaction/test/test_pep257.py b/toid_interaction/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/toid_interaction/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/toid_interaction/toid_interaction/__init__.py b/toid_interaction/toid_interaction/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/toid_interaction/toid_interaction/interaction_node.py b/toid_interaction/toid_interaction/interaction_node.py new file mode 100644 index 0000000..1a2bdcc --- /dev/null +++ b/toid_interaction/toid_interaction/interaction_node.py @@ -0,0 +1,97 @@ +import rclpy +from rclpy.node import Node +from std_srvs.srv import Empty + +from serial.tools import list_ports + +from toid_interaction.mechanism.sekvenca_2026 import okreni, okreni_niz +from toid_interaction.mechanism.zidovi_load import ZidoviAction +from toid_interaction.mechanism.zupcanik import ZupcanikAction +from toid_msgs.srv import SendString + +class InteracitionNode(Node): + step: int = 0 + def __init__(self): + super().__init__('ToidInteractionNode') + + self.find_sigma() + + self.srv = self.create_service( + Empty, + '/sequence1', + self.sequence1_cb + ) + + self.srv = self.create_service( + SendString, + '/sequence2', + self.sequence2_cb + ) + + self.srv = self.create_service( + Empty, + '/sequence3', + self.sequence3_cb + ) + + + + self.get_logger().info("Service 'sequence1' ready.") + + def find_sigma(self): + for port_info in list_ports.comports(): + if port_info.vid == 0x18a6 and port_info.pid == 0x55d3: + break + + print(port_info.device) + self.st_motor_device_name = port_info.device + + def sequence1_cb(self, request, response): + if(self.step != 0): + return Empty.Response() + okreni(5) + zupcanik = ZupcanikAction(self.st_motor_device_name) + zupcanik.zupcanik(1, -1010, 25) + self.step = 1 + return response + + def sequence2_cb(self, request : SendString.Request, response : SendString.Response): + if(self.step != 1): + return Empty.Response() + zidovi = ZidoviAction(self.st_motor_device_name) + zidovi.beli_zid(1) + zidovi.plavi_zid(1) + + okreni_niz(request.text) + + zidovi.plavi_zid(0, TargetPos=150) + zidovi.beli_zid(0, TargetPos=450) + self.step = 2 + return response + + def sequence3_cb(self, request, response): + if(self.step != 2): + return Empty.Response() + + zupcanik = ZupcanikAction(self.st_motor_device_name) + zidovi = ZidoviAction(self.st_motor_device_name) + + zupcanik.zupcanik(1, 1010, 25) + zidovi.plavi_zid(0, TargetPos=150) + zidovi.beli_zid(0, TargetPos=150) + okreni(5) + self.step = 0 + return response + + +def main(args=None): + rclpy.init(args=args) + + node = InteracitionNode() + rclpy.spin(node) + + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/__init__.py b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__init__.py new file mode 100644 index 0000000..58551ab --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__init__.py @@ -0,0 +1,8 @@ +#!/usr/bin/env python + +from .port_handler import * +from .protocol_packet_handler import * +from .group_sync_write import * +from .group_sync_read import * +from .sts import * +from .scscl import * diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/group_sync_read.py b/toid_interaction/toid_interaction/mechanism/STservo_sdk/group_sync_read.py new file mode 100644 index 0000000..dad8d33 --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/group_sync_read.py @@ -0,0 +1,151 @@ +#!/usr/bin/env python + +from .stservo_def import * + +class GroupSyncRead: + def __init__(self, ph, start_address, data_length): + self.ph = ph + self.start_address = start_address + self.data_length = data_length + + self.last_result = False + self.is_param_changed = False + self.param = [] + self.data_dict = {} + + self.clearParam() + + def makeParam(self): + if not self.data_dict: # len(self.data_dict.keys()) == 0: + return + + self.param = [] + + for scs_id in self.data_dict: + self.param.append(scs_id) + + def addParam(self, sts_id): + if sts_id in self.data_dict: # sts_id already exist + return False + + self.data_dict[sts_id] = [] # [0] * self.data_length + + self.is_param_changed = True + return True + + def removeParam(self, sts_id): + if sts_id not in self.data_dict: # NOT exist + return + + del self.data_dict[sts_id] + + self.is_param_changed = True + + def clearParam(self): + self.data_dict.clear() + + def txPacket(self): + if len(self.data_dict.keys()) == 0: + + return COMM_NOT_AVAILABLE + + if self.is_param_changed is True or not self.param: + self.makeParam() + + return self.ph.syncReadTx(self.start_address, self.data_length, self.param, len(self.data_dict.keys())) + + def rxPacket(self): + self.last_result = True + + result = COMM_RX_FAIL + + if len(self.data_dict.keys()) == 0: + return COMM_NOT_AVAILABLE + + result, rxpacket = self.ph.syncReadRx(self.data_length, len(self.data_dict.keys())) + # print(rxpacket) + if len(rxpacket) >= (self.data_length+6): + for sts_id in self.data_dict: + self.data_dict[sts_id], result = self.readRx(rxpacket, sts_id, self.data_length) + if result != COMM_SUCCESS: + self.last_result = False + # print(sts_id) + else: + self.last_result = False + # print(self.last_result) + return result + + def txRxPacket(self): + result = self.txPacket() + if result != COMM_SUCCESS: + return result + + return self.rxPacket() + + def readRx(self, rxpacket, sts_id, data_length): + # print(sts_id) + # print(rxpacket) + data = [] + rx_length = len(rxpacket) + # print(rx_length) + rx_index = 0; + while (rx_index+6+data_length) <= rx_length: + headpacket = [0x00, 0x00, 0x00] + while rx_index < rx_length: + headpacket[2] = headpacket[1]; + headpacket[1] = headpacket[0]; + headpacket[0] = rxpacket[rx_index]; + rx_index += 1 + if (headpacket[2] == 0xFF) and (headpacket[1] == 0xFF) and headpacket[0] == sts_id: + # print(rx_index) + break + # print(rx_index+3+data_length) + if (rx_index+3+data_length) > rx_length: + break; + if rxpacket[rx_index] != (data_length+2): + rx_index += 1 + # print(rx_index) + continue + rx_index += 1 + Error = rxpacket[rx_index] + rx_index += 1 + calSum = sts_id + (data_length+2) + Error + data = [Error] + data.extend(rxpacket[rx_index : rx_index+data_length]) + for i in range(0, data_length): + calSum += rxpacket[rx_index] + rx_index += 1 + calSum = ~calSum & 0xFF + # print(calSum) + if calSum != rxpacket[rx_index]: + return None, COMM_RX_CORRUPT + return data, COMM_SUCCESS + # print(rx_index) + return None, COMM_RX_CORRUPT + + def isAvailable(self, sts_id, address, data_length): + #if self.last_result is False or sts_id not in self.data_dict: + if sts_id not in self.data_dict: + return False, 0 + + if (address < self.start_address) or (self.start_address + self.data_length - data_length < address): + return False, 0 + if not self.data_dict[sts_id]: + return False, 0 + if len(self.data_dict[sts_id])<(data_length+1): + return False, 0 + return True, self.data_dict[sts_id][0] + + def getData(self, sts_id, address, data_length): + if data_length == 1: + return self.data_dict[sts_id][address-self.start_address+1] + elif data_length == 2: + return self.ph.scs_makeword(self.data_dict[sts_id][address-self.start_address+1], + self.data_dict[sts_id][address-self.start_address+2]) + elif data_length == 4: + return self.ph.scs_makedword(self.ph.scs_makeword(self.data_dict[sts_id][address-self.start_address+1], + self.data_dict[sts_id][address-self.start_address+2]), + self.ph.scs_makeword(self.data_dict[sts_id][address-self.start_address+3], + self.data_dict[sts_id][address-self.start_address+4])) + else: + return 0 diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/group_sync_write.py b/toid_interaction/toid_interaction/mechanism/STservo_sdk/group_sync_write.py new file mode 100644 index 0000000..b2ce36d --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/group_sync_write.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python + +from .stservo_def import * + +class GroupSyncWrite: + def __init__(self, ph, start_address, data_length): + self.ph = ph + self.start_address = start_address + self.data_length = data_length + + self.is_param_changed = False + self.param = [] + self.data_dict = {} + + self.clearParam() + + def makeParam(self): + if not self.data_dict: + return + + self.param = [] + + for sts_id in self.data_dict: + if not self.data_dict[sts_id]: + return + + self.param.append(sts_id) + self.param.extend(self.data_dict[sts_id]) + + def addParam(self, sts_id, data): + if sts_id in self.data_dict: # sts_id already exist + return False + + if len(data) > self.data_length: # input data is longer than set + return False + + self.data_dict[sts_id] = data + + self.is_param_changed = True + return True + + def removeParam(self, sts_id): + if sts_id not in self.data_dict: # NOT exist + return + + del self.data_dict[sts_id] + + self.is_param_changed = True + + def changeParam(self, sts_id, data): + if sts_id not in self.data_dict: # NOT exist + return False + + if len(data) > self.data_length: # input data is longer than set + return False + + self.data_dict[sts_id] = data + + self.is_param_changed = True + return True + + def clearParam(self): + self.data_dict.clear() + + def txPacket(self): + if len(self.data_dict.keys()) == 0: + return COMM_NOT_AVAILABLE + + if self.is_param_changed is True or not self.param: + self.makeParam() + + return self.ph.syncWriteTxOnly(self.start_address, self.data_length, self.param, + len(self.data_dict.keys()) * (1 + self.data_length)) diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/port_handler.py b/toid_interaction/toid_interaction/mechanism/STservo_sdk/port_handler.py new file mode 100644 index 0000000..63f0943 --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/port_handler.py @@ -0,0 +1,115 @@ +#!/usr/bin/env python + +import time +import serial +import sys +import platform + +DEFAULT_BAUDRATE = 1000000 +LATENCY_TIMER = 50 + +class PortHandler(object): + def __init__(self, port_name): + self.is_open = False + self.baudrate = DEFAULT_BAUDRATE + self.packet_start_time = 0.0 + self.packet_timeout = 0.0 + self.tx_time_per_byte = 0.0 + + self.is_using = False + self.port_name = port_name + self.ser = None + + def openPort(self): + return self.setBaudRate(self.baudrate) + + def closePort(self): + self.ser.close() + self.is_open = False + + def clearPort(self): + self.ser.flush() + + def setPortName(self, port_name): + self.port_name = port_name + + def getPortName(self): + return self.port_name + + def setBaudRate(self, baudrate): + baud = self.getCFlagBaud(baudrate) + + if baud <= 0: + # self.setupPort(38400) + # self.baudrate = baudrate + return False # TODO: setCustomBaudrate(baudrate) + else: + self.baudrate = baudrate + return self.setupPort(baud) + + def getBaudRate(self): + return self.baudrate + + def getBytesAvailable(self): + return self.ser.in_waiting + + def readPort(self, length): + if (sys.version_info > (3, 0)): + return self.ser.read(length) + else: + return [ord(ch) for ch in self.ser.read(length)] + + def writePort(self, packet): + return self.ser.write(packet) + + def setPacketTimeout(self, packet_length): + self.packet_start_time = self.getCurrentTime() + self.packet_timeout = (self.tx_time_per_byte * packet_length) + (self.tx_time_per_byte * 3.0) + LATENCY_TIMER + + def setPacketTimeoutMillis(self, msec): + self.packet_start_time = self.getCurrentTime() + self.packet_timeout = msec + + def isPacketTimeout(self): + if self.getTimeSinceStart() > self.packet_timeout: + self.packet_timeout = 0 + return True + + return False + + def getCurrentTime(self): + return round(time.time() * 1000000000) / 1000000.0 + + def getTimeSinceStart(self): + time_since = self.getCurrentTime() - self.packet_start_time + if time_since < 0.0: + self.packet_start_time = self.getCurrentTime() + + return time_since + + def setupPort(self, cflag_baud): + if self.is_open: + self.closePort() + + self.ser = serial.Serial( + port=self.port_name, + baudrate=self.baudrate, + # parity = serial.PARITY_ODD, + # stopbits = serial.STOPBITS_TWO, + bytesize=serial.EIGHTBITS, + timeout=0 + ) + + self.is_open = True + + self.ser.reset_input_buffer() + + self.tx_time_per_byte = (1000.0 / self.baudrate) * 10.0 + + return True + + def getCFlagBaud(self, baudrate): + if baudrate in [4800, 9600, 14400, 19200, 38400, 57600, 115200, 128000, 250000, 500000, 1000000]: + return baudrate + else: + return -1 \ No newline at end of file diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/protocol_packet_handler.py b/toid_interaction/toid_interaction/mechanism/STservo_sdk/protocol_packet_handler.py new file mode 100644 index 0000000..3e8412c --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/protocol_packet_handler.py @@ -0,0 +1,530 @@ +#!/usr/bin/env python + +from .stservo_def import * + +TXPACKET_MAX_LEN = 250 +RXPACKET_MAX_LEN = 250 + +# for Protocol Packet +PKT_HEADER0 = 0 +PKT_HEADER1 = 1 +PKT_ID = 2 +PKT_LENGTH = 3 +PKT_INSTRUCTION = 4 +PKT_ERROR = 4 +PKT_PARAMETER0 = 5 + +# Protocol Error bit +ERRBIT_VOLTAGE = 1 +ERRBIT_ANGLE = 2 +ERRBIT_OVERHEAT = 4 +ERRBIT_OVERELE = 8 +ERRBIT_OVERLOAD = 32 + + +class protocol_packet_handler(object): + def __init__(self, portHandler, protocol_end): + #self.sts_setend(protocol_end)# STServo bit end(STS/SMS=0, SCS=1) + self.portHandler = portHandler + self.sts_end = protocol_end + + def sts_getend(self): + return self.sts_end + + def sts_setend(self, e): + self.sts_end = e + + def sts_tohost(self, a, b): + if (a & (1<> 16) & 0xFFFF + + def sts_lobyte(self, w): + if self.sts_end==0: + return w & 0xFF + else: + return (w >> 8) & 0xFF + + def sts_hibyte(self, w): + if self.sts_end==0: + return (w >> 8) & 0xFF + else: + return w & 0xFF + + def getProtocolVersion(self): + return 1.0 + + def getTxRxResult(self, result): + if result == COMM_SUCCESS: + return "[TxRxResult] Communication success!" + elif result == COMM_PORT_BUSY: + return "[TxRxResult] Port is in use!" + elif result == COMM_TX_FAIL: + return "[TxRxResult] Failed transmit instruction packet!" + elif result == COMM_RX_FAIL: + return "[TxRxResult] Failed get status packet from device!" + elif result == COMM_TX_ERROR: + return "[TxRxResult] Incorrect instruction packet!" + elif result == COMM_RX_WAITING: + return "[TxRxResult] Now receiving status packet!" + elif result == COMM_RX_TIMEOUT: + return "[TxRxResult] There is no status packet!" + elif result == COMM_RX_CORRUPT: + return "[TxRxResult] Incorrect status packet!" + elif result == COMM_NOT_AVAILABLE: + return "[TxRxResult] Protocol does not support this function!" + else: + return "" + + def getRxPacketError(self, error): + if error & ERRBIT_VOLTAGE: + return "[ServoStatus] Input voltage error!" + + if error & ERRBIT_ANGLE: + return "[ServoStatus] Angle sen error!" + + if error & ERRBIT_OVERHEAT: + return "[ServoStatus] Overheat error!" + + if error & ERRBIT_OVERELE: + return "[ServoStatus] OverEle error!" + + if error & ERRBIT_OVERLOAD: + return "[ServoStatus] Overload error!" + + return "" + + def txPacket(self, txpacket): + checksum = 0 + total_packet_length = txpacket[PKT_LENGTH] + 4 # 4: HEADER0 HEADER1 ID LENGTH + + if self.portHandler.is_using: + return COMM_PORT_BUSY + self.portHandler.is_using = True + + # check max packet length + if total_packet_length > TXPACKET_MAX_LEN: + self.portHandler.is_using = False + return COMM_TX_ERROR + + # make packet header + txpacket[PKT_HEADER0] = 0xFF + txpacket[PKT_HEADER1] = 0xFF + + # add a checksum to the packet + for idx in range(2, total_packet_length - 1): # except header, checksum + checksum += txpacket[idx] + + txpacket[total_packet_length - 1] = ~checksum & 0xFF + + #print "[TxPacket] %r" % txpacket + + # tx packet + self.portHandler.clearPort() + written_packet_length = self.portHandler.writePort(txpacket) + if total_packet_length != written_packet_length: + self.portHandler.is_using = False + return COMM_TX_FAIL + + return COMM_SUCCESS + + def rxPacket(self): + rxpacket = [] + + result = COMM_TX_FAIL + checksum = 0 + rx_length = 0 + wait_length = 6 # minimum length (HEADER0 HEADER1 ID LENGTH ERROR CHKSUM) + + while True: + rxpacket.extend(self.portHandler.readPort(wait_length - rx_length)) + rx_length = len(rxpacket) + if rx_length >= wait_length: + # find packet header + for idx in range(0, (rx_length - 1)): + if (rxpacket[idx] == 0xFF) and (rxpacket[idx + 1] == 0xFF): + break + + if idx == 0: # found at the beginning of the packet + if (rxpacket[PKT_ID] > 0xFD) or (rxpacket[PKT_LENGTH] > RXPACKET_MAX_LEN) or ( + rxpacket[PKT_ERROR] > 0x7F): + # unavailable ID or unavailable Length or unavailable Error + # remove the first byte in the packet + del rxpacket[0] + rx_length -= 1 + continue + + # re-calculate the exact length of the rx packet + if wait_length != (rxpacket[PKT_LENGTH] + PKT_LENGTH + 1): + wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1 + continue + + if rx_length < wait_length: + # check timeout + if self.portHandler.isPacketTimeout(): + if rx_length == 0: + result = COMM_RX_TIMEOUT + else: + result = COMM_RX_CORRUPT + break + else: + continue + + # calculate checksum + for i in range(2, wait_length - 1): # except header, checksum + checksum += rxpacket[i] + checksum = ~checksum & 0xFF + + # verify checksum + if rxpacket[wait_length - 1] == checksum: + result = COMM_SUCCESS + else: + result = COMM_RX_CORRUPT + break + + else: + # remove unnecessary packets + del rxpacket[0: idx] + rx_length -= idx + + else: + # check timeout + if self.portHandler.isPacketTimeout(): + if rx_length == 0: + result = COMM_RX_TIMEOUT + else: + result = COMM_RX_CORRUPT + break + + self.portHandler.is_using = False + return rxpacket, result + + def txRxPacket(self, txpacket): + rxpacket = None + error = 0 + + # tx packet + result = self.txPacket(txpacket) + if result != COMM_SUCCESS: + return rxpacket, result, error + + # (ID == Broadcast ID) == no need to wait for status packet or not available + if (txpacket[PKT_ID] == BROADCAST_ID): + self.portHandler.is_using = False + return rxpacket, result, error + + # set packet timeout + if txpacket[PKT_INSTRUCTION] == INST_READ: + self.portHandler.setPacketTimeout(txpacket[PKT_PARAMETER0 + 1] + 6) + else: + self.portHandler.setPacketTimeout(6) # HEADER0 HEADER1 ID LENGTH ERROR CHECKSUM + + # rx packet + while True: + rxpacket, result = self.rxPacket() + if result != COMM_SUCCESS or txpacket[PKT_ID] == rxpacket[PKT_ID]: + break + + if result == COMM_SUCCESS and txpacket[PKT_ID] == rxpacket[PKT_ID]: + error = rxpacket[PKT_ERROR] + + return rxpacket, result, error + + def ping(self, sts_id): + model_number = 0 + error = 0 + + txpacket = [0] * 6 + + if sts_id >= BROADCAST_ID: + return model_number, COMM_NOT_AVAILABLE, error + + txpacket[PKT_ID] = sts_id + txpacket[PKT_LENGTH] = 2 + txpacket[PKT_INSTRUCTION] = INST_PING + + rxpacket, result, error = self.txRxPacket(txpacket) + + if result == COMM_SUCCESS: + data_read, result, error = self.readTxRx(sts_id, 3, 2) # Address 3 : Model Number + if result == COMM_SUCCESS: + model_number = self.sts_makeword(data_read[0], data_read[1]) + + return model_number, result, error + + def action(self, sts_id): + txpacket = [0] * 6 + + txpacket[PKT_ID] = sts_id + txpacket[PKT_LENGTH] = 2 + txpacket[PKT_INSTRUCTION] = INST_ACTION + + _, result, _ = self.txRxPacket(txpacket) + + return result + + def readTx(self, sts_id, address, length): + + txpacket = [0] * 8 + + if sts_id >= BROADCAST_ID: + return COMM_NOT_AVAILABLE + + txpacket[PKT_ID] = sts_id + txpacket[PKT_LENGTH] = 4 + txpacket[PKT_INSTRUCTION] = INST_READ + txpacket[PKT_PARAMETER0 + 0] = address + txpacket[PKT_PARAMETER0 + 1] = length + + result = self.txPacket(txpacket) + + # set packet timeout + if result == COMM_SUCCESS: + self.portHandler.setPacketTimeout(length + 6) + + return result + + def readRx(self, sts_id, length): + result = COMM_TX_FAIL + error = 0 + + rxpacket = None + data = [] + + while True: + rxpacket, result = self.rxPacket() + + if result != COMM_SUCCESS or rxpacket[PKT_ID] == sts_id: + break + + if result == COMM_SUCCESS and rxpacket[PKT_ID] == sts_id: + error = rxpacket[PKT_ERROR] + + data.extend(rxpacket[PKT_PARAMETER0 : PKT_PARAMETER0+length]) + + return data, result, error + + def readTxRx(self, sts_id, address, length): + txpacket = [0] * 8 + data = [] + + if sts_id >= BROADCAST_ID: + return data, COMM_NOT_AVAILABLE, 0 + + txpacket[PKT_ID] = sts_id + txpacket[PKT_LENGTH] = 4 + txpacket[PKT_INSTRUCTION] = INST_READ + txpacket[PKT_PARAMETER0 + 0] = address + txpacket[PKT_PARAMETER0 + 1] = length + + rxpacket, result, error = self.txRxPacket(txpacket) + if result == COMM_SUCCESS: + error = rxpacket[PKT_ERROR] + + data.extend(rxpacket[PKT_PARAMETER0 : PKT_PARAMETER0+length]) + + return data, result, error + + def read1ByteTx(self, sts_id, address): + return self.readTx(sts_id, address, 1) + + def read1ByteRx(self, sts_id): + data, result, error = self.readRx(sts_id, 1) + data_read = data[0] if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read1ByteTxRx(self, sts_id, address): + data, result, error = self.readTxRx(sts_id, address, 1) + data_read = data[0] if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read2ByteTx(self, sts_id, address): + return self.readTx(sts_id, address, 2) + + def read2ByteRx(self, sts_id): + data, result, error = self.readRx(sts_id, 2) + data_read = self.sts_makeword(data[0], data[1]) if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read2ByteTxRx(self, sts_id, address): + data, result, error = self.readTxRx(sts_id, address, 2) + data_read = self.sts_makeword(data[0], data[1]) if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read4ByteTx(self, sts_id, address): + return self.readTx(sts_id, address, 4) + + def read4ByteRx(self, sts_id): + data, result, error = self.readRx(sts_id, 4) + data_read = self.sts_makedword(self.sts_makeword(data[0], data[1]), + self.sts_makeword(data[2], data[3])) if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read4ByteTxRx(self, sts_id, address): + data, result, error = self.readTxRx(sts_id, address, 4) + data_read = self.sts_makedword(self.sts_makeword(data[0], data[1]), + self.sts_makeword(data[2], data[3])) if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def writeTxOnly(self, sts_id, address, length, data): + txpacket = [0] * (length + 7) + + txpacket[PKT_ID] = sts_id + txpacket[PKT_LENGTH] = length + 3 + txpacket[PKT_INSTRUCTION] = INST_WRITE + txpacket[PKT_PARAMETER0] = address + + txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length] + + result = self.txPacket(txpacket) + self.portHandler.is_using = False + + return result + + def writeTxRx(self, sts_id, address, length, data): + txpacket = [0] * (length + 7) + + txpacket[PKT_ID] = sts_id + txpacket[PKT_LENGTH] = length + 3 + txpacket[PKT_INSTRUCTION] = INST_WRITE + txpacket[PKT_PARAMETER0] = address + + txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length] + rxpacket, result, error = self.txRxPacket(txpacket) + + return result, error + + def write1ByteTxOnly(self, sts_id, address, data): + data_write = [data] + return self.writeTxOnly(sts_id, address, 1, data_write) + + def write1ByteTxRx(self, sts_id, address, data): + data_write = [data] + return self.writeTxRx(sts_id, address, 1, data_write) + + def write2ByteTxOnly(self, sts_id, address, data): + data_write = [self.sts_lobyte(data), self.sts_hibyte(data)] + return self.writeTxOnly(sts_id, address, 2, data_write) + + def write2ByteTxRx(self, sts_id, address, data): + data_write = [self.sts_lobyte(data), self.sts_hibyte(data)] + return self.writeTxRx(sts_id, address, 2, data_write) + + def write4ByteTxOnly(self, sts_id, address, data): + data_write = [self.sts_lobyte(self.sts_loword(data)), + self.sts_hibyte(self.sts_loword(data)), + self.sts_lobyte(self.sts_hiword(data)), + self.sts_hibyte(self.sts_hiword(data))] + return self.writeTxOnly(sts_id, address, 4, data_write) + + def write4ByteTxRx(self, sts_id, address, data): + data_write = [self.sts_lobyte(self.sts_loword(data)), + self.sts_hibyte(self.sts_loword(data)), + self.sts_lobyte(self.sts_hiword(data)), + self.sts_hibyte(self.sts_hiword(data))] + return self.writeTxRx(sts_id, address, 4, data_write) + + def regWriteTxOnly(self, sts_id, address, length, data): + txpacket = [0] * (length + 7) + + txpacket[PKT_ID] = sts_id + txpacket[PKT_LENGTH] = length + 3 + txpacket[PKT_INSTRUCTION] = INST_REG_WRITE + txpacket[PKT_PARAMETER0] = address + + txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length] + + result = self.txPacket(txpacket) + self.portHandler.is_using = False + + return result + + def regWriteTxRx(self, sts_id, address, length, data): + txpacket = [0] * (length + 7) + + txpacket[PKT_ID] = sts_id + txpacket[PKT_LENGTH] = length + 3 + txpacket[PKT_INSTRUCTION] = INST_REG_WRITE + txpacket[PKT_PARAMETER0] = address + + txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length] + + _, result, error = self.txRxPacket(txpacket) + + return result, error + + def syncReadTx(self, start_address, data_length, param, param_length): + txpacket = [0] * (param_length + 8) + # 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN CHKSUM + + txpacket[PKT_ID] = BROADCAST_ID + txpacket[PKT_LENGTH] = param_length + 4 # 7: INST START_ADDR DATA_LEN CHKSUM + txpacket[PKT_INSTRUCTION] = INST_SYNC_READ + txpacket[PKT_PARAMETER0 + 0] = start_address + txpacket[PKT_PARAMETER0 + 1] = data_length + + txpacket[PKT_PARAMETER0 + 2: PKT_PARAMETER0 + 2 + param_length] = param[0: param_length] + + # print(txpacket) + result = self.txPacket(txpacket) + return result + + def syncReadRx(self, data_length, param_length): + wait_length = (6 + data_length) * param_length + self.portHandler.setPacketTimeout(wait_length) + rxpacket = [] + rx_length = 0 + while True: + rxpacket.extend(self.portHandler.readPort(wait_length - rx_length)) + rx_length = len(rxpacket) + if rx_length >= wait_length: + result = COMM_SUCCESS + break + else: + # check timeout + if self.portHandler.isPacketTimeout(): + if rx_length == 0: + result = COMM_RX_TIMEOUT + else: + result = COMM_RX_CORRUPT + break + self.portHandler.is_using = False + return result, rxpacket + + def syncWriteTxOnly(self, start_address, data_length, param, param_length): + txpacket = [0] * (param_length + 8) + # 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM + + txpacket[PKT_ID] = BROADCAST_ID + txpacket[PKT_LENGTH] = param_length + 4 # 4: INST START_ADDR DATA_LEN ... CHKSUM + txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE + txpacket[PKT_PARAMETER0 + 0] = start_address + txpacket[PKT_PARAMETER0 + 1] = data_length + + txpacket[PKT_PARAMETER0 + 2: PKT_PARAMETER0 + 2 + param_length] = param[0: param_length] + + _, result, _ = self.txRxPacket(txpacket) + + return result diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/scscl.py b/toid_interaction/toid_interaction/mechanism/STservo_sdk/scscl.py new file mode 100644 index 0000000..0b64349 --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/scscl.py @@ -0,0 +1,104 @@ +#!/usr/bin/env python + +from .stservo_def import * +from .protocol_packet_handler import * +from .group_sync_write import * + +#波特率定义 +SCSCL_1M = 0 +SCSCL_0_5M = 1 +SCSCL_250K = 2 +SCSCL_128K = 3 +SCSCL_115200 = 4 +SCSCL_76800 = 5 +SCSCL_57600 = 6 +SCSCL_38400 = 7 + +#内存表定义 +#-------EPROM(只读)-------- +SCSCL_MODEL_L = 3 +SCSCL_MODEL_H = 4 + +#-------EPROM(读写)-------- +scs_id = 5 +SCSCL_BAUD_RATE = 6 +SCSCL_MIN_ANGLE_LIMIT_L = 9 +SCSCL_MIN_ANGLE_LIMIT_H = 10 +SCSCL_MAX_ANGLE_LIMIT_L = 11 +SCSCL_MAX_ANGLE_LIMIT_H = 12 +SCSCL_CW_DEAD = 26 +SCSCL_CCW_DEAD = 27 + +#-------SRAM(读写)-------- +SCSCL_TORQUE_ENABLE = 40 +SCSCL_GOAL_POSITION_L = 42 +SCSCL_GOAL_POSITION_H = 43 +SCSCL_GOAL_TIME_L = 44 +SCSCL_GOAL_TIME_H = 45 +SCSCL_GOAL_SPEED_L = 46 +SCSCL_GOAL_SPEED_H = 47 +SCSCL_LOCK = 48 + +#-------SRAM(只读)-------- +SCSCL_PRESENT_POSITION_L = 56 +SCSCL_PRESENT_POSITION_H = 57 +SCSCL_PRESENT_SPEED_L = 58 +SCSCL_PRESENT_SPEED_H = 59 +SCSCL_PRESENT_LOAD_L = 60 +SCSCL_PRESENT_LOAD_H = 61 +SCSCL_PRESENT_VOLTAGE = 62 +SCSCL_PRESENT_TEMPERATURE = 63 +SCSCL_MOVING = 66 +SCSCL_PRESENT_CURRENT_L = 69 +SCSCL_PRESENT_CURRENT_H = 70 + +class scscl(protocol_packet_handler): + def __init__(self, portHandler): + protocol_packet_handler.__init__(self, portHandler, 1) + self.groupSyncWrite = GroupSyncWrite(self, SCSCL_GOAL_POSITION_L, 6) + + def WritePos(self, scs_id, position, time, speed): + txpacket = [self.scs_lobyte(position), self.scs_hibyte(position), self.scs_lobyte(time), self.scs_hibyte(time), self.scs_lobyte(speed), self.scs_hibyte(speed)] + return self.writeTxRx(scs_id, SCSCL_GOAL_POSITION_L, len(txpacket), txpacket) + + def ReadPos(self, scs_id): + scs_present_position, scs_comm_result, scs_error = self.read2ByteTxRx(scs_id, SCSCL_PRESENT_POSITION_L) + return scs_present_position, scs_comm_result, scs_error + + def ReadSpeed(self, scs_id): + scs_present_speed, scs_comm_result, scs_error = self.read2ByteTxRx(scs_id, SCSCL_PRESENT_SPEED_L) + return self.scs_tohost(scs_present_speed, 15), scs_comm_result, scs_error + + def ReadPosSpeed(self, scs_id): + scs_present_position_speed, scs_comm_result, scs_error = self.read4ByteTxRx(scs_id, SCSCL_PRESENT_POSITION_L) + scs_present_position = self.scs_loword(scs_present_position_speed) + scs_present_speed = self.scs_hiword(scs_present_position_speed) + return scs_present_position, self.scs_tohost(scs_present_speed, 15), scs_comm_result, scs_error + + def ReadMoving(self, scs_id): + moving, scs_comm_result, scs_error = self.read1ByteTxRx(scs_id, SCSCL_MOVING) + return moving, scs_comm_result, scs_error + + def SyncWritePos(self, scs_id, position, time, speed): + txpacket = [self.scs_lobyte(position), self.scs_hibyte(position), self.scs_lobyte(time), self.scs_hibyte(time), self.scs_lobyte(speed), self.scs_hibyte(speed)] + return self.groupSyncWrite.addParam(scs_id, txpacket) + + def RegWritePos(self, scs_id, position, time, speed): + txpacket = [self.scs_lobyte(position), self.scs_hibyte(position), self.scs_lobyte(time), self.scs_hibyte(time), self.scs_lobyte(speed), self.scs_hibyte(speed)] + return self.regWriteTxRx(scs_id, SCSCL_GOAL_POSITION_L, len(txpacket), txpacket) + + def RegAction(self): + return self.action(BROADCAST_ID) + + def PWMMode(self, scs_id): + txpacket = [0, 0, 0, 0] + return self.writeTxRx(scs_id, SCSCL_MIN_ANGLE_LIMIT_L, len(txpacket), txpacket) + + def WritePWM(self, scs_id, time): + return self.write2ByteTxRx(scs_id, SCSCL_GOAL_TIME_L, self.scs_toscs(time, 10)) + + def LockEprom(self, scs_id): + return self.write1ByteTxRx(scs_id, SCSCL_LOCK, 1) + + def unLockEprom(self, scs_id): + return self.write1ByteTxRx(scs_id, SCSCL_LOCK, 0) diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/setup.py b/toid_interaction/toid_interaction/mechanism/STservo_sdk/setup.py new file mode 100644 index 0000000..f9bc198 --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/setup.py @@ -0,0 +1,7 @@ +from setuptools import setup, find_packages + +setup( + name='STservo_sdk', + version='0.1', + packages=find_packages(), +) \ No newline at end of file diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/sts.py b/toid_interaction/toid_interaction/mechanism/STservo_sdk/sts.py new file mode 100644 index 0000000..3123996 --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/sts.py @@ -0,0 +1,140 @@ +#!/usr/bin/env python + +from .stservo_def import * +from .protocol_packet_handler import * +from .group_sync_read import * +from .group_sync_write import * + +#波特率定义 +STS_1M = 0 +STS_0_5M = 1 +STS_250K = 2 +STS_128K = 3 +STS_115200 = 4 +STS_76800 = 5 +STS_57600 = 6 +STS_38400 = 7 + +#内存表定义 +#-------EPROM(只读)-------- +STS_MODEL_L = 3 +STS_MODEL_H = 4 + +#-------EPROM(读写)-------- +STS_ID = 5 +STS_BAUD_RATE = 6 +STS_MIN_ANGLE_LIMIT_L = 9 +STS_MIN_ANGLE_LIMIT_H = 10 +STS_MAX_ANGLE_LIMIT_L = 11 +STS_MAX_ANGLE_LIMIT_H = 12 +STS_CW_DEAD = 26 +STS_CCW_DEAD = 27 +STS_OFS_L = 31 +STS_OFS_H = 32 +STS_MODE = 33 + +#-------SRAM(读写)-------- +MAX_POSITION_L = 42 +STS_GOAL_POSITION_H = 43 +STS_GOAL_TIME_L = 44 +STS_GOAL_TIME_H = 45 +STS_LOAD_LIMIT = 36 +STS_TORQUE_ENABLE = 40 +STS_ACC = 41 +STS_GOAL_SPEED_L = 46 +STS_GOAL_SPEED_H = 47 +STS_MAX_LOAD = 0x30 +STS_LOCK = 55 + +#-------SRAM(只读)-------- +STS_PRESENT_POSITION_L = 56 +STS_PRESENT_POSITION_H = 57 +STS_PRESENT_SPEED_L = 58 +STS_PRESENT_SPEED_H = 59 +STS_PRESENT_LOAD_L = 60 +STS_PRESENT_LOAD_H = 61 +STS_PRESENT_VOLTAGE = 62 +STS_PRESENT_TEMPERATURE = 63 +STS_MOVING = 66 +STS_PRESENT_CURRENT_L = 69 +STS_PRESENT_CURRENT_H = 70 + +class sts(protocol_packet_handler): + def __init__(self, portHandler): + protocol_packet_handler.__init__(self, portHandler, 0) + self.groupSyncWrite = GroupSyncWrite(self, 40, 7) + + def WritePosEx(self, sts_id, position, speed, acc): + txpacket = [acc, self.sts_lobyte(position), self.sts_hibyte(position), 0, 0, self.sts_lobyte(speed), self.sts_hibyte(speed)] + return self.writeTxRx(sts_id, STS_ACC, len(txpacket), txpacket) + + def ReadPos(self, sts_id): + sts_present_position, sts_comm_result, sts_error = self.read2ByteTxRx(sts_id, 60) + return self.sts_tohost(sts_present_position, 15), sts_comm_result, sts_error + + def ReadLoad(self, sts_id): + sts_present_load, sts_comm_result,sts_error = self.read2ByteTxRx(sts_id, STS_PRESENT_LOAD_L) # 60 + return self.sts_tohost(sts_present_load, 15), sts_comm_result, sts_error + + def ReadMaxLoad(self, sts_id): + sts_max_load, sts_comm_result,sts_error = self.read2ByteTxRx(sts_id, STS_LOAD_LIMIT) # 0x36 + return sts_max_load, sts_comm_result, sts_error + + def ReadSpeed(self, sts_id): + sts_present_speed, sts_comm_result, sts_error = self.read2ByteTxRx(sts_id, STS_PRESENT_SPEED_L) + return self.sts_tohost(sts_present_speed, 15), sts_comm_result, sts_error + + def ReadPosSpeed(self, sts_id): + sts_present_position_speed, sts_comm_result, sts_error = self.read4ByteTxRx(sts_id, STS_PRESENT_POSITION_L) + sts_present_position = self.sts_loword(sts_present_position_speed) + sts_present_speed = self.sts_hiword(sts_present_position_speed) + return self.sts_tohost(sts_present_position, 15), self.sts_tohost(sts_present_speed, 15), sts_comm_result, sts_error + + def ReadPosLoad(self, sts_id): + sts_present_position_speed, sts_comm_result, sts_error = self.read4ByteTxRx(sts_id, 60) + sts_present_position = self.sts_loword(sts_present_position_speed) + sts_present_speed = self.sts_hiword(sts_present_position_speed) + return self.sts_tohost(sts_present_position, 15), self.sts_tohost(sts_present_speed, 15), sts_comm_result, sts_error + + def ReadPosLoad1(self, sts_id): + sts_present_position_speed, sts_comm_result, sts_error = self.read4ByteTxRx(sts_id, 61) + sts_present_position = self.sts_loword(sts_present_position_speed) + sts_present_speed = self.sts_hiword(sts_present_position_speed) + return self.sts_tohost(sts_present_position, 15), self.sts_tohost(sts_present_speed, 15), sts_comm_result, sts_error + + def ReadMoving(self, sts_id): + moving, sts_comm_result, sts_error = self.read1ByteTxRx(sts_id, STS_MOVING) + return moving, sts_comm_result, sts_error + + def MaxLoad(self, sts_id): + max_load, sts_comm_result, sts_error = self.read1ByteTxRx(sts_id, STS_LOAD_LIMIT) + return max_load, sts_comm_result, sts_error + + def SyncWritePosEx(self, sts_id, position, speed, acc): + txpacket = [acc, self.sts_lobyte(position), self.sts_hibyte(position), 0, 0, self.sts_lobyte(speed), self.sts_hibyte(speed)] + return self.groupSyncWrite.addParam(sts_id, txpacket) + + def RegWritePosEx(self, sts_id, position, speed, acc): + txpacket = [acc, self.sts_lobyte(position), self.sts_hibyte(position), 0, 0, self.sts_lobyte(speed), self.sts_hibyte(speed)] + return self.regWriteTxRx(sts_id, STS_ACC, len(txpacket), txpacket) + + def RegAction(self): + return self.action(BROADCAST_ID) + + def WheelMode(self, sts_id): + return self.write1ByteTxRx(sts_id, STS_MODE, 1) + + def SetMaxLoad(self, sts_id, load): + return self.write2ByteTxRx(sts_id, STS_MAX_LOAD, int(load*10)) + + def WriteSpec(self, sts_id, speed, acc): + speed = self.sts_toscs(speed, 15) + txpacket = [acc, 0, 0, 0, 0, self.sts_lobyte(speed), self.sts_hibyte(speed)] + return self.writeTxRx(sts_id, STS_ACC, len(txpacket), txpacket) + + def LockEprom(self, sts_id): + return self.write1ByteTxRx(sts_id, STS_LOCK, 1) + + def unLockEprom(self, sts_id): + return self.write1ByteTxRx(sts_id, STS_LOCK, 0) + diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/stservo_def.py b/toid_interaction/toid_interaction/mechanism/STservo_sdk/stservo_def.py new file mode 100644 index 0000000..a5fb1f5 --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/stservo_def.py @@ -0,0 +1,25 @@ +#!/usr/bin/env python + +BROADCAST_ID = 0xFE # 254 +MAX_ID = 0xFC # 252 +STS_END = 0 + +# Instruction for STS Protocol +INST_PING = 1 +INST_READ = 2 +INST_WRITE = 3 +INST_REG_WRITE = 4 +INST_ACTION = 5 +INST_SYNC_WRITE = 131 # 0x83 +INST_SYNC_READ = 130 # 0x82 + +# Communication Result +COMM_SUCCESS = 0 # tx or rx packet communication success +COMM_PORT_BUSY = -1 # Port is busy (in use) +COMM_TX_FAIL = -2 # Failed transmit instruction packet +COMM_RX_FAIL = -3 # Failed get status packet +COMM_TX_ERROR = -4 # Incorrect instruction packet +COMM_RX_WAITING = -5 # Now recieving status packet +COMM_RX_TIMEOUT = -6 # There is no status packet +COMM_RX_CORRUPT = -7 # Incorrect status packet +COMM_NOT_AVAILABLE = -9 # diff --git a/toid_interaction/toid_interaction/mechanism/__init__.py b/toid_interaction/toid_interaction/mechanism/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/toid_interaction/toid_interaction/mechanism/sekvenca_2026.py b/toid_interaction/toid_interaction/mechanism/sekvenca_2026.py new file mode 100644 index 0000000..442f370 --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/sekvenca_2026.py @@ -0,0 +1,67 @@ +# u ovom fajlu sam napisala celu sekvencu koju obuhvata hvatanje zirova podizanje, okretanje, spustanje i odlaganje +# dole cu napisaci koje funkicje su koja sekvenca ono sto smo pricali , nisam htela da pravim jos jedan fajl sa sekvencama da +# ne bude da ima opet previse fajlova +# sekvecncu 4 nije jos zavrsila poslacu ti to naknadno +from .zidovi_load import ZidoviAction +from .zupcanik import ZupcanikAction +import time +import serial +import serial.tools.list_ports as list_ports + +SERIAL_ID = "50443405C8C3B21C" + + +def okreni(i): + for port_info in list_ports.comports(): + if port_info.serial_number == SERIAL_ID: + break + + ser = serial.Serial(port_info.device, 115200, timeout=1) + ser.write(str(i).encode()) + + +def okreni_niz(broj): + """ + Funkcija koja prima string od 4 karaktera (0 ili 1) + """ + if len(broj) != 4: + raise ValueError("Binarni niz mora imati tačno 4 karaktera") + + okreni(6) + for i, char in enumerate(broj): + if char == "1": + okreni(i + 1) + + +def main(): + + zidovi = ZidoviAction('/dev/ttyACM1') + zupcanik = ZupcanikAction('/dev/ttyACM1') + + + # sekvenca 1 + okreni(5) + zupcanik.zupcanik(1, -1010, 25) + + # sekvenca 2 + zidovi.beli_zid(1) + zidovi.plavi_zid(1) + + okreni_niz("1010") + + zidovi.plavi_zid(0, TargetPos=150) + zidovi.beli_zid(0, TargetPos=450) + + + # sekvenca 3 + + zupcanik.zupcanik(1, 1010, 25) + + zidovi.plavi_zid(0, TargetPos=150) + zidovi.beli_zid(0, TargetPos=150) + + time.sleep(1) + + +if __name__ == "__main__": + main() diff --git a/toid_interaction/toid_interaction/mechanism/zidovi_load.py b/toid_interaction/toid_interaction/mechanism/zidovi_load.py new file mode 100644 index 0000000..b623e2b --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/zidovi_load.py @@ -0,0 +1,366 @@ +from .STservo_sdk import PortHandler, sts, COMM_SUCCESS, STS_ID +# da li ovo dole treba da importujem ako je nalazi u STservo_sdk? +# from stservo_def import COMM_SUCCESS + +# Default settings + + +class ZidoviAction: + BAUDRATE : int + DEVICENAME : str + STS_MOVING_ACC: int + def __init__( + self, + devicename, # Check which port is being used on your controller + baudrate=1000000, # STServo default baudrate + sts_moving_acc=0, # STServo moving acceleration + ): + self.BAUDRATE = baudrate + self.DEVICENAME = devicename + self.STS_MOVING_ACC = sts_moving_acc + + + def plavi_zid(self, smer, brzina=1500, opterecenje_threshold=75, TargetPos=300): + # TargetPos = 300 # Target position in degrees + TargetPos = TargetPos * 4096 / 360 # Convert to motor ticks + if smer == 1: + Speed_ID2 = brzina # Speed for motor ID 2 + Speed_ID4 = -brzina # Speed for motor ID 4 (opposite sign) + else: + Speed_ID2 = -brzina # Speed for motor ID 2 + Speed_ID4 = brzina # Speed for motor ID 4 (opposite sign) + + # Initialize PortHandler instance + portHandler = PortHandler(self.DEVICENAME) + + # Initialize PacketHandler instance + packetHandler = sts(portHandler) + + # Open port + if portHandler.openPort(): + print("Succeeded to open the port") + else: + print("Failed to open the port") + print("Press any key to terminate...") + + quit() + + # Set port baudrate + if portHandler.setBaudRate(self.BAUDRATE): + print("Succeeded to change the baudrate") + else: + print("Failed to change the baudrate") + print("Press any key to terminate...") + + quit() + + # Set motors to Wheel Mode + for motor_id in [2, 4]: + sts_comm_result, sts_error = packetHandler.WheelMode(motor_id) + if sts_comm_result != COMM_SUCCESS: + print( + "[ID:%01d] %s" + % (motor_id, packetHandler.getTxRxResult(sts_comm_result)) + ) + elif sts_error != 0: + print( + "[ID:%01d] %s" + % (motor_id, packetHandler.getRxPacketError(sts_error)) + ) + + # Read initial positions for both motors + sts_present_position_ID2, sts_present_speed_ID2, sts_comm_result, sts_error = ( + packetHandler.ReadPosSpeed(2) + ) + sts_present_position_ID4, sts_present_speed_ID4, sts_comm_result, sts_error = ( + packetHandler.ReadPosSpeed(4) + ) + + print("Initial positions:") + print( + "[ID:002] PresPos:%d PresSpd:%d" + % (sts_present_position_ID2, sts_present_speed_ID2) + ) + print( + "[ID:004] PresPos:%d PresSpd:%d" + % (sts_present_position_ID4, sts_present_speed_ID4) + ) + + # Initialize variables for tracking movement + TrenutnaPos_ID2 = sts_present_position_ID2 + ProslaPos_ID2 = sts_present_position_ID2 + PredjeniPut_ID2 = 0 + + TrenutnaPos_ID4 = sts_present_position_ID4 + ProslaPos_ID4 = sts_present_position_ID4 + PredjeniPut_ID4 = 0 + packetHandler.SetMaxLoad(STS_ID, 80) + + while True: + # Write goal position, speed, and acceleration for both motors + sts_comm_result, sts_error = packetHandler.WriteSpec( + 2, Speed_ID2, self.STS_MOVING_ACC + ) + if sts_comm_result != COMM_SUCCESS: + print("[ID:002] %s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("[ID:002] %s" % packetHandler.getRxPacketError(sts_error)) + + sts_comm_result, sts_error = packetHandler.WriteSpec( + 4, Speed_ID4, self.STS_MOVING_ACC + ) + if sts_comm_result != COMM_SUCCESS: + print("[ID:004] %s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("[ID:004] %s" % packetHandler.getRxPacketError(sts_error)) + + # Update positions for both motors + ProslaPos_ID2 = TrenutnaPos_ID2 + ProslaPos_ID4 = TrenutnaPos_ID4 + + ( + sts_present_position_ID2, + sts_present_speed_ID2, + sts_comm_result, + sts_error, + ) = packetHandler.ReadPosSpeed(2) + if sts_comm_result != COMM_SUCCESS: + print("[ID:002] %s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("[ID:002] %s" % packetHandler.getRxPacketError(sts_error)) + + ( + sts_present_position_ID4, + sts_present_speed_ID4, + sts_comm_result, + sts_error, + ) = packetHandler.ReadPosSpeed(4) + if sts_comm_result != COMM_SUCCESS: + print("[ID:004] %s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("[ID:004] %s" % packetHandler.getRxPacketError(sts_error)) + sts_present_load2, sts_comm_result, sts_error = packetHandler.ReadLoad(2) + sts_present_load4, sts_comm_result, sts_error = packetHandler.ReadLoad(4) + opterecenje2 = (sts_present_load2 & ((1 << 10) - 1)) * 0.1 + opterecenje4 = (sts_present_load4 & ((1 << 10) - 1)) * 0.1 + print("Current positions:") + print( + "[ID:002] PresPos:%d PresSpd:%d Load:%d" + % (sts_present_position_ID2, sts_present_speed_ID2, opterecenje2) + ) + print( + "[ID:004] PresPos:%d PresSpd:%d Load:%d" + % (sts_present_position_ID4, sts_present_speed_ID4, opterecenje4) + ) + + TrenutnaPos_ID2 = sts_present_position_ID2 + TrenutnaPos_ID4 = sts_present_position_ID4 + + # Check if the positions have changed + if ( + opterecenje2 > opterecenje_threshold + or opterecenje4 > opterecenje_threshold + ): + print( + "High load detected: [ID:002] PresPos:%d PresSpd:%d Load:%d" + % (sts_present_position_ID2, sts_present_speed_ID2, opterecenje2) + ) + print( + "High load detected: [ID:004] PresPos:%d PresSpd:%d Load:%d" + % (sts_present_position_ID4, sts_present_speed_ID4, opterecenje4) + ) + packetHandler.WriteSpec(2, 0, 0) # Stop motor ID 2 + packetHandler.WriteSpec(4, 0, 0) # Stop motor ID 4 + break + if ( + abs(TrenutnaPos_ID2 - ProslaPos_ID2) > 3000 + or abs(TrenutnaPos_ID4 - ProslaPos_ID4) > 3000 + ): + PredjeniPut_ID2 = PredjeniPut_ID2 + PredjeniPut_ID4 = PredjeniPut_ID4 + else: + PredjeniPut_ID2 += abs(TrenutnaPos_ID2 - ProslaPos_ID2) + PredjeniPut_ID4 += abs(TrenutnaPos_ID4 - ProslaPos_ID4) + + print("PredjeniPut ID2:%d deg " % (PredjeniPut_ID2 * 360 / 4096)) + print("PredjeniPut ID4:%d deg " % (PredjeniPut_ID4 * 360 / 4096)) + + # Stop if both motors reach their target positions + if PredjeniPut_ID2 >= TargetPos and PredjeniPut_ID4 >= TargetPos + 10: + print("Target positions reached.") + packetHandler.WriteSpec(2, 0, 0) # Stop motor ID 2 + packetHandler.WriteSpec(4, 0, 0) # Stop motor ID 4 + break + + # Close port + portHandler.closePort() + + def beli_zid(self, smer, brzina=1500, opterecenje_threshold=75, TargetPos=600): + # TargetPos = 600 # Target position in degrees + TargetPos = TargetPos * 4096 / 360 # Convert to motor ticks + if smer == 1: + Speed_ID3 = -brzina # Speed for motor ID 3 + Speed_ID5 = brzina # Speed for motor ID 5 (opposite sign) + + else: + Speed_ID3 = brzina # Speed for motor ID 3 + Speed_ID5 = -brzina # Speed for motor ID 5 (opposite sign) + + # Initialize PortHandler instance + portHandler = PortHandler(self.DEVICENAME) + + # Initialize PacketHandler instance + packetHandler = sts(portHandler) + + # Open port + if portHandler.openPort(): + print("Succeeded to open the port") + else: + print("Failed to open the port") + print("Press any key to terminate...") + + quit() + + # Set port baudrate + if portHandler.setBaudRate(self.BAUDRATE): + print("Succeeded to change the baudrate") + else: + print("Failed to change the baudrate") + print("Press any key to terminate...") + + quit() + + # Set motors to Wheel Mode + for motor_id in [3, 5]: + sts_comm_result, sts_error = packetHandler.WheelMode(motor_id) + if sts_comm_result != COMM_SUCCESS: + print( + "[ID:%01d] %s" + % (motor_id, packetHandler.getTxRxResult(sts_comm_result)) + ) + elif sts_error != 0: + print( + "[ID:%01d] %s" + % (motor_id, packetHandler.getRxPacketError(sts_error)) + ) + + # Read initial positions for both motors + sts_present_position_ID3, sts_present_speed_ID3, sts_comm_result, sts_error = ( + packetHandler.ReadPosSpeed(3) + ) + sts_present_position_ID5, sts_present_speed_ID5, sts_comm_result, sts_error = ( + packetHandler.ReadPosSpeed(5) + ) + + print("Initial positions:") + # print("[ID:003] PresPos:%d PresSpd:%d" % (sts_present_position_ID3, sts_present_speed_ID3)) + # print("[ID:005] PresPos:%d PresSpd:%d" % (sts_present_position_ID5, sts_present_speed_ID5)) + + # Initialize variables for tracking movement + TrenutnaPos_ID3 = sts_present_position_ID3 + ProslaPos_ID3 = sts_present_position_ID3 + PredjeniPut_ID3 = 0 + + TrenutnaPos_ID5 = sts_present_position_ID5 + ProslaPos_ID5 = sts_present_position_ID5 + PredjeniPut_ID5 = 0 + packetHandler.SetMaxLoad(STS_ID, 80) + + while True: + # Write goal position, speed, and acceleration for both motors + sts_comm_result, sts_error = packetHandler.WriteSpec( + 3, Speed_ID3, self.STS_MOVING_ACC + ) + if sts_comm_result != COMM_SUCCESS: + print("[ID:003] %s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("[ID:003] %s" % packetHandler.getRxPacketError(sts_error)) + + sts_comm_result, sts_error = packetHandler.WriteSpec( + 5, Speed_ID5, self.STS_MOVING_ACC + ) + if sts_comm_result != COMM_SUCCESS: + print("[ID:005] %s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("[ID:005] %s" % packetHandler.getRxPacketError(sts_error)) + + # Update positions for both motors + ProslaPos_ID3 = TrenutnaPos_ID3 + ProslaPos_ID5 = TrenutnaPos_ID5 + + ( + sts_present_position_ID3, + sts_present_speed_ID3, + sts_comm_result, + sts_error, + ) = packetHandler.ReadPosSpeed(3) + if sts_comm_result != COMM_SUCCESS: + print("[ID:003] %s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("[ID:003] %s" % packetHandler.getRxPacketError(sts_error)) + + ( + sts_present_position_ID5, + sts_present_speed_ID5, + sts_comm_result, + sts_error, + ) = packetHandler.ReadPosSpeed(5) + if sts_comm_result != COMM_SUCCESS: + print("[ID:005] %s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("[ID:005] %s" % packetHandler.getRxPacketError(sts_error)) + sts_present_load3, sts_comm_result, sts_error = packetHandler.ReadLoad(3) + sts_present_load5, sts_comm_result, sts_error = packetHandler.ReadLoad(5) + opterecenje3 = (sts_present_load3 & ((1 << 10) - 1)) * 0.1 + opterecenje5 = (sts_present_load5 & ((1 << 10) - 1)) * 0.1 + print("Current positions:") + print( + "[ID:003] PresPos:%d PresSpd:%d Load:%d" + % (sts_present_position_ID3, sts_present_speed_ID3, opterecenje3) + ) + print( + "[ID:005] PresPos:%d PresSpd:%d Load:%d" + % (sts_present_position_ID5, sts_present_speed_ID5, opterecenje5) + ) + + TrenutnaPos_ID3 = sts_present_position_ID3 + TrenutnaPos_ID5 = sts_present_position_ID5 + + # Check if the positions have changed + if ( + opterecenje3 > opterecenje_threshold + or opterecenje5 > opterecenje_threshold + ): + print( + "High load detected: [ID:003] PresPos:%d PresSpd:%d Load:%d" + % (sts_present_position_ID3, sts_present_speed_ID3, opterecenje3) + ) + print( + "High load detected: [ID:005] PresPos:%d PresSpd:%d Load:%d" + % (sts_present_position_ID5, sts_present_speed_ID5, opterecenje5) + ) + packetHandler.WriteSpec(3, 0, 0) # Stop motor ID 3 + packetHandler.WriteSpec(5, 0, 0) # Stop motor ID 5 + break + if ( + abs(TrenutnaPos_ID3 - ProslaPos_ID3) > 3000 + or abs(TrenutnaPos_ID5 - ProslaPos_ID5) > 3000 + ): + PredjeniPut_ID3 = PredjeniPut_ID3 + PredjeniPut_ID5 = PredjeniPut_ID5 + else: + PredjeniPut_ID3 += abs(TrenutnaPos_ID3 - ProslaPos_ID3) + PredjeniPut_ID5 += abs(TrenutnaPos_ID5 - ProslaPos_ID5) + + print("PredjeniPut ID3:%d deg " % (PredjeniPut_ID3 * 360 / 4096)) + print("PredjeniPut ID5:%d deg " % (PredjeniPut_ID5 * 360 / 4096)) + + # Stop if both motors reach their target positions + if PredjeniPut_ID3 >= TargetPos and PredjeniPut_ID5 >= TargetPos + 10: + print("Target positions reached.") + packetHandler.WriteSpec(3, 0, 0) # Stop motor ID 3 + packetHandler.WriteSpec(5, 0, 0) # Stop motor ID 5 + break + + # Close port + portHandler.closePort() diff --git a/toid_interaction/toid_interaction/mechanism/zupcanik.py b/toid_interaction/toid_interaction/mechanism/zupcanik.py new file mode 100644 index 0000000..4eec341 --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/zupcanik.py @@ -0,0 +1,130 @@ +from .STservo_sdk import PortHandler, sts, COMM_SUCCESS + +class ZupcanikAction: + BAUDRATE: int + DEVICENAME: str + STS_MOVING_ACC: int + + def __init__(self, devicename, baudrate=1000000, sts_moving_acc=50): + self.BAUDRATE = baudrate + self.DEVICENAME = devicename + self.STS_MOVING_ACC = sts_moving_acc + + def zupcanik(self, STS_ID, STS_MOVING_SPEED, TargetPos): + + # Initialize PortHandler instance + portHandler = PortHandler(self.DEVICENAME) + + # Initialize PacketHandler instance + packetHandler = sts(portHandler) + + # Open port + if not portHandler.openPort(): + print("Failed to open the port") + return + + # Set port baudrate + if not portHandler.setBaudRate(self.BAUDRATE): + print("Failed to change the baudrate") + return + + sts_comm_result, sts_error = packetHandler.WheelMode(STS_ID) + if sts_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(sts_comm_result)) + return + elif sts_error != 0: + print("%s" % packetHandler.getRxPacketError(sts_error)) + return + # Read STServo present position + sts_present_position, sts_present_speed, sts_comm_result, sts_error = ( + packetHandler.ReadPosSpeed(STS_ID) + ) + if sts_comm_result != COMM_SUCCESS: + print(packetHandler.getTxRxResult(sts_comm_result)) + else: + print( + "[ID:%03d] PresPos:%d PresSpd:%d" + % (STS_ID, sts_present_position, sts_present_speed) + ) + if sts_error != 0: + print(packetHandler.getRxPacketError(sts_error)) + + TargetPos = TargetPos * 4096 / 360 # Convert to motor ticks + sts_present_position, sts_present_speed, sts_comm_result, sts_error = ( + packetHandler.ReadPosSpeed(STS_ID) + ) + + print( + "Initial position: [ID:%03d] PresPos:%d PresSpd:%d" + % (STS_ID, sts_present_position, sts_present_speed) + ) + + TrenutnaPos = sts_present_position + ProslaPos = sts_present_position + PredjeniPut = TrenutnaPos - ProslaPos + opterecenje = 0 + packetHandler.SetMaxLoad(STS_ID, 80) + + while True: + # Write STServo goal position/moving speed/moving acc + sts_comm_result, sts_error = packetHandler.WriteSpec( + STS_ID, STS_MOVING_SPEED, self.STS_MOVING_ACC + ) + if sts_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("%s" % packetHandler.getRxPacketError(sts_error)) + + ProslaPos = sts_present_position + sts_present_position, sts_present_speed, sts_comm_result, sts_error = ( + packetHandler.ReadPosSpeed(STS_ID) + ) + sts_present_load, sts_comm_result, sts_error = packetHandler.ReadLoad( + STS_ID + ) + sts_max_load, sts_comm_result, sts_error = packetHandler.MaxLoad(STS_ID) + opterecenje = (sts_present_load & ((1 << 10) - 1)) * 0.1 + max_opterecenje = sts_max_load & 255 + print( + "Current position: [ID:%03d] PresPos:%d PresSpd:%d Load:%d Max:%d" + % ( + STS_ID, + sts_present_position, + sts_present_speed, + opterecenje, + max_opterecenje, + ) + ) + + if opterecenje > 75: + print( + "High load detected: [ID:%03d] PresPos:%d PresSpd:%d Load:%d" + % (STS_ID, sts_present_position, sts_present_speed, opterecenje) + ) + sts_comm_result, sts_error = packetHandler.WriteSpec(STS_ID, 0, 0) + if sts_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("%s" % packetHandler.getRxPacketError(sts_error)) + break + TrenutnaPos = sts_present_position + if abs(TrenutnaPos - ProslaPos) > 3000: + PredjeniPut = PredjeniPut + else: + PredjeniPut = PredjeniPut + abs(TrenutnaPos - ProslaPos) + + print("Distance traveled: %d deg " % (PredjeniPut * 360 / 4096)) + if PredjeniPut >= TargetPos: + print( + "Target position reached: [ID:%03d] PresPos:%d PresSpd:%d" + % (STS_ID, sts_present_position, sts_present_speed) + ) + sts_comm_result, sts_error = packetHandler.WriteSpec(STS_ID, 0, 0) + if sts_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("%s" % packetHandler.getRxPacketError(sts_error)) + break + + # Close port + portHandler.closePort() diff --git a/toid_lidar/package.xml b/toid_lidar/package.xml index 0b47a33..da1b7c3 100644 --- a/toid_lidar/package.xml +++ b/toid_lidar/package.xml @@ -19,7 +19,7 @@ tf2_geometry_msgs toid_msgs visualization_msgs - rplidar + rplidar_ros diff --git a/toid_msgs/CMakeLists.txt b/toid_msgs/CMakeLists.txt index 1142ee6..6c0b3fd 100644 --- a/toid_msgs/CMakeLists.txt +++ b/toid_msgs/CMakeLists.txt @@ -15,6 +15,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/ActiveElements.msg" "msg/Rival.msg" "srv/SendDouble.srv" + "srv/SendString.srv" "action/SimpleMoveCoords.action" "action/SimpleRotate.action" "action/SimpleTranslateX.action" diff --git a/toid_msgs/srv/SendString.srv b/toid_msgs/srv/SendString.srv new file mode 100644 index 0000000..70a257c --- /dev/null +++ b/toid_msgs/srv/SendString.srv @@ -0,0 +1,2 @@ +string text +--- \ No newline at end of file -- 2.49.1 From 2c0aae45d64927449402ff6bae8398fb39c25bda Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Mon, 6 Apr 2026 19:20:36 +0200 Subject: [PATCH 38/86] Updated docker file to use custom libcamera fork --- .gitmodules | 3 +++ Dockerfile | 28 ++++++++++++++++++++++++++-- ext/camera_ros | 1 + scripts/toid_base_build.sh | 3 ++- 4 files changed, 32 insertions(+), 3 deletions(-) create mode 160000 ext/camera_ros diff --git a/.gitmodules b/.gitmodules index 4c994e1..9963ab5 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,6 @@ [submodule "ext/BehaviorTree.ROS2"] path = ext/BehaviorTree.ROS2 url = https://github.com/BehaviorTree/BehaviorTree.ROS2.git +[submodule "ext/camera_ros"] + path = ext/camera_ros + url = https://github.com/christianrauch/camera_ros.git diff --git a/Dockerfile b/Dockerfile index c980ba2..d007a35 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,4 +1,4 @@ -FROM ros:jazzy-ros-base +FROM ros:jazzy-perception ENV DEBIAN_FRONTEND=noninteractive @@ -6,10 +6,28 @@ ENV DEBIAN_FRONTEND=noninteractive RUN apt-get update && apt-get install -y \ python3-colcon-common-extensions \ python3-rosdep \ + meson cmake \ build-essential \ udev \ git +# ---------- Libcamera ---------- + +WORKDIR /extras + +RUN apt-get install -y libboost-dev \ + libgnutls28-dev openssl libtiff5-dev \ + pybind11-dev \ + qtbase5-dev libqt5core5a libqt5gui5 libqt5widgets5 \ + python3-yaml python3-ply python3-jinja2 python3-pip\ + libglib2.0-dev libgstreamer-plugins-base1.0-dev \ + build-essential + +RUN git clone https://github.com/raspberrypi/libcamera.git \ + && cd libcamera \ + && meson setup build --buildtype=release -Dpipelines=rpi/vc4,rpi/pisp -Dipas=rpi/vc4,rpi/pisp -Dv4l2=true -Dgstreamer=enabled -Dtest=false -Dlc-compliance=disabled -Dcam=disabled -Dqcam=disabled -Ddocumentation=disabled -Dpycamera=enabled \ + && ninja -C build install + # ---------- Initialize rosdep ---------- RUN rosdep init || true RUN rosdep update @@ -22,13 +40,19 @@ COPY toid_bot_description/package.xml toid_bot_description/package.xml COPY toid_control/package.xml toid_control/package.xml COPY toid_msgs/package.xml toid_msgs/package.xml COPY toid_odometry/package.xml toid_odometry/package.xml +COPY toid_lidar/package.xml toid_lidar/package.xml +COPY toid_interaction/package.xml toid_interaction/package.xml +COPY toid_costmaps/package.xml toid_costmaps/package.xml +COPY toid_bt/package.xml toid_bt/package.xml COPY toid_spinner_controller/package.xml toid_spinner_controller/package.xml COPY toid_behaviors/package.xml toid_behaviors/package.xml COPY toid_navigation/package.xml toid_navigation/package.xml +COPY ext/camera_ros/package.xml ext/camera_ros/package.xml +COPY ext/BehaviorTree.ROS2/ ext/BehaviorTree.ROS2/ # ---------- Install dependencies ---------- RUN . /opt/ros/jazzy/setup.sh && \ - rosdep install --from-paths ./ --ignore-src -r -y \ + rosdep install --from-paths ./ --ignore-src -r -y --skip-keys=libcamera \ && rm -rf /var/lib/apt/lists/* RUN rm -rf ./* diff --git a/ext/camera_ros b/ext/camera_ros new file mode 160000 index 0000000..121c98a --- /dev/null +++ b/ext/camera_ros @@ -0,0 +1 @@ +Subproject commit 121c98a7fef9ea745ad000ff28bf48da6dee994a diff --git a/scripts/toid_base_build.sh b/scripts/toid_base_build.sh index fff53f7..0c1190f 100755 --- a/scripts/toid_base_build.sh +++ b/scripts/toid_base_build.sh @@ -2,4 +2,5 @@ source /opt/ros/jazzy/setup.bash -colcon build --packages-up-to-regex "toid_(?!bt)" \ No newline at end of file +colcon build --packages-up-to-regex "toid_(?!bt)" +colcon build --packages-up-to-regex "camera_ros" \ No newline at end of file -- 2.49.1 From dd30e776c1b26b7f13d50cb9bd45a3df7c89e47d Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Fri, 10 Apr 2026 18:57:32 +0200 Subject: [PATCH 39/86] Created vision node --- toid_vision/CMakeLists.txt | 60 ++++++ toid_vision/LICENSE | 202 ++++++++++++++++++ .../include/toid_vision/toid_vision.hpp | 67 ++++++ toid_vision/package.xml | 18 ++ toid_vision/src/toid_vision.cpp | 177 +++++++++++++++ toid_vision/src/vision.cpp | 10 + 6 files changed, 534 insertions(+) create mode 100644 toid_vision/CMakeLists.txt create mode 100644 toid_vision/LICENSE create mode 100644 toid_vision/include/toid_vision/toid_vision.hpp create mode 100644 toid_vision/package.xml create mode 100644 toid_vision/src/toid_vision.cpp create mode 100644 toid_vision/src/vision.cpp diff --git a/toid_vision/CMakeLists.txt b/toid_vision/CMakeLists.txt new file mode 100644 index 0000000..c0e59f6 --- /dev/null +++ b/toid_vision/CMakeLists.txt @@ -0,0 +1,60 @@ + +cmake_minimum_required(VERSION 3.8) +project(toid_vision) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) + +set(PACKAGE_DEPS + rclcpp + sensor_msgs + message_filters + cv_bridge + OpenCV + + tf2 + tf2_ros + tf2_geometry_msgs +) + +foreach(PACKAGE ${PACKAGE_DEPS}) + find_package(${PACKAGE} REQUIRED) +endforeach() + +find_package(OpenCV 4 REQUIRED + COMPONENTS + opencv_core + opencv_aruco + opencv_imgproc + opencv_imgcodecs +) + + +set(SOURCES + src/toid_vision.cpp + src/vision.cpp +) + +add_executable(toid_vision ${SOURCES}) + +target_include_directories( + toid_vision + PRIVATE + include +) + +ament_target_dependencies(toid_vision ${PACKAGE_DEPS}) + +install(TARGETS toid_vision DESTINATION lib/${PROJECT_NAME}) + +target_compile_features( + toid_vision PUBLIC + c_std_99 + cxx_std_17 +) + + +ament_package() diff --git a/toid_vision/LICENSE b/toid_vision/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/toid_vision/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_vision/include/toid_vision/toid_vision.hpp b/toid_vision/include/toid_vision/toid_vision.hpp new file mode 100644 index 0000000..85072b4 --- /dev/null +++ b/toid_vision/include/toid_vision/toid_vision.hpp @@ -0,0 +1,67 @@ +#pragma once + +#include +#include + +#include "message_filters/subscriber.hpp" +#include "message_filters/sync_policies/approximate_time.hpp" +#include "message_filters/time_synchronizer.hpp" +#include "opencv2/aruco.hpp" +#include "opencv2/objdetect.hpp" +#include "opencv2/opencv.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/camera_info.hpp" +#include "sensor_msgs/msg/compressed_image.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "std_msgs/msg/string.hpp" +#include "tf2/LinearMath/Quaternion.hpp" +#include "tf2/LinearMath/Transform.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace toid +{ + +class NutDetector : public rclcpp::Node +{ + using SyncPolicy = message_filters::sync_policies::ApproximateTime< + sensor_msgs::msg::CompressedImage, sensor_msgs::msg::CameraInfo>; + +public: + NutDetector(rclcpp::NodeOptions &options); + +public: + void compressed_topic_callback( + const sensor_msgs::msg::CompressedImage::ConstSharedPtr & msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & cam_info); + + void process_image( + cv::Mat & decodedImage, const std_msgs::msg::Header & header, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & cam_info); + + void extract_markers(cv::Mat & image); + + void get_pose(cv::Vec3d & tvec, cv::Vec3d & rvec, geometry_msgs::msg::Pose & out_pose); + + message_filters::Subscriber compressed_img_sub_; + message_filters::Subscriber cam_info_sub_; + std::shared_ptr> sync_; + + + rclcpp::Publisher::SharedPtr pose_pub_; + rclcpp::Publisher::SharedPtr flip_pub_; + + std::vector> markerCorners_; + std::vector markerIds_; + std::vector, int>> markers_; + + std::string camera_frame_id_ = "camera"; + bool is_blue_; + + cv::Ptr detectorParams_; + cv::Ptr dictionary_; + + + message_filters::Subscriber img_sub_; +}; + +} // namespace toid \ No newline at end of file diff --git a/toid_vision/package.xml b/toid_vision/package.xml new file mode 100644 index 0000000..4ef3ce6 --- /dev/null +++ b/toid_vision/package.xml @@ -0,0 +1,18 @@ + + + + toid_vision + 0.0.0 + TODO: Package description + pimpest + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/toid_vision/src/toid_vision.cpp b/toid_vision/src/toid_vision.cpp new file mode 100644 index 0000000..398faee --- /dev/null +++ b/toid_vision/src/toid_vision.cpp @@ -0,0 +1,177 @@ +#include "toid_vision/toid_vision.hpp" + +#include +#include + +namespace toid +{ + +NutDetector::NutDetector(rclcpp::NodeOptions & options) +: Node("compressed_image_subscriber", options) +{ + detectorParams_ = cv::aruco::DetectorParameters::create(); + dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_100); + + bool use_compressed = true; + this->declare_parameter("use_compressed", rclcpp::ParameterValue(true)); + this->get_parameter("use_compressed", use_compressed); + + this->declare_parameter("camera_frame_id", rclcpp::ParameterValue("camera")); + this->get_parameter("camera_frame_id", camera_frame_id_); + + this->declare_parameter("is_blue", rclcpp::ParameterValue(true)); + this->get_parameter("is_blue", is_blue_); + + compressed_img_sub_.subscribe(this, "/camera/image_raw/compressed"); + cam_info_sub_.subscribe(this, "/camera/camera_info"); + + sync_ = std::make_shared>( + SyncPolicy(2), compressed_img_sub_, cam_info_sub_); + + using namespace std::placeholders; + sync_->registerCallback(std::bind(&NutDetector::compressed_topic_callback, this, _1, _2)); + RCLCPP_INFO(this->get_logger(), "NutDetector activated... scanning for DEEZ NUTZ"); + + pose_pub_ = + this->create_publisher("/closest_acorn", rclcpp::QoS(1)); +flip_pub_ = + this->create_publisher("/to_flip", rclcpp::QoS(1)); +} + +void NutDetector::compressed_topic_callback( + const sensor_msgs::msg::CompressedImage::ConstSharedPtr & msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & cam_info) +{ + try { + cv::Mat rawData(1, msg->data.size(), CV_8UC1, (void *)msg->data.data()); + cv::Mat decodedImage = cv::imdecode(rawData, cv::IMREAD_COLOR); + + if (decodedImage.empty()) { + RCLCPP_ERROR(this->get_logger(), "Could not decode image!"); + return; + } + + process_image(decodedImage, msg->header, cam_info); + + } catch (const std::exception & e) { + RCLCPP_ERROR(this->get_logger(), "Error processing image: %s", e.what()); + } +} + +void NutDetector::process_image( + cv::Mat & decodedImage, const std_msgs::msg::Header & header, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & cam_info) +{ + markers_.clear(); + + extract_markers(decodedImage); + + if (markers_.empty()) { + return; + } + + std::vector rvecs, tvecs; + + cv::Mat k = cv::Mat(3, 3, CV_64F, (void *)cam_info->k.data()); + cv::aruco::estimatePoseSingleMarkers(markerCorners_, 0.03, k, cam_info->d, rvecs, tvecs); + + size_t prev = 0; + std::string colors = ""; + + for (size_t i = 0; i < markerIds_.size(); i++) { + const int idx = markers_[i].second; + if (markerIds_[idx] != 47 && markerIds_[idx] != 36) { + continue; + } + + RCLCPP_DEBUG( + this->get_logger(), "Marker %ld at %lf %lf", i, markerCorners_[idx][0].x, + markerCorners_[idx][0].y); + + RCLCPP_DEBUG( + this->get_logger(), "World coords: %lf %lf %lf", tvecs[idx][0], tvecs[idx][1], tvecs[idx][2]); + + float dx = tvecs[idx][0] - tvecs[prev][0]; + float dy = tvecs[idx][1] - tvecs[prev][1]; + float dz = tvecs[idx][2] - tvecs[prev][2]; + + float dist = std::sqrt(dx * dx + dy * dy + dz * dz); + + RCLCPP_DEBUG(this->get_logger(), "Dist from prev: %lf", dist); + + if (dist > 0.07) { + continue; + } + + prev = idx; + + colors += (markerIds_[idx] == 36) ? "b" : "y"; + + if (colors.size() > 3) { + break; + } + } + + if (colors.size() == 4) { + RCLCPP_DEBUG(this->get_logger(), "Last seen orientation: %s", colors.c_str()); + for(int i = 0; i<4; i++) { + if(colors[i] == 'b') { + colors[i] = (is_blue_)? '0' : '1'; + } else { + colors[i] = (is_blue_)? '1' : '0'; + } + } + + RCLCPP_DEBUG(this->get_logger(), "Last seen orientation: %s", colors.c_str()); + std_msgs::msg::String m; + m.data = colors; + flip_pub_->publish(m); + } + + geometry_msgs::msg::PoseStamped pose; + get_pose(tvecs[markers_[0].second], rvecs[markers_[0].second], pose.pose); + pose.header.frame_id = camera_frame_id_; + pose.header.stamp = header.stamp; + pose_pub_->publish(pose); +} + +void NutDetector::extract_markers(cv::Mat & image) +{ + cv::aruco::detectMarkers(image, dictionary_, markerCorners_, markerIds_, detectorParams_); + + // cv::aruco::drawDetectedMarkers(image, markerCorners_, markerIds_); + + markers_.reserve(markerIds_.size()); + markers_.resize(markerIds_.size()); + + for (size_t i = 0; i < markerIds_.size(); i++) { + markers_[i].first = markerCorners_[i]; + markers_[i].second = i; + } + + std::sort( + markers_.begin(), markers_.end(), + [](std::pair, int> & a, std::pair, int> & b) { + return a.first[0].y > b.first[0].y; + }); +} + +void NutDetector::get_pose(cv::Vec3d & tvec, cv::Vec3d & rvec, geometry_msgs::msg::Pose & out_pose) +{ + cv::Mat R; + cv::Rodrigues(rvec, R); + tf2::Matrix3x3 tf2_rot{R.at(0, 0), R.at(0, 1), R.at(0, 2), + R.at(1, 0), R.at(1, 1), R.at(1, 2), + R.at(2, 0), R.at(2, 1), R.at(2, 2)}; + + tf2::Quaternion q; + tf2_rot.getRotation(q); + + tf2::convert(q, out_pose.orientation); + + out_pose.position.x = tvec[0]; + out_pose.position.y = tvec[1]; + out_pose.position.z = tvec[2]; +} + +} // namespace toid \ No newline at end of file diff --git a/toid_vision/src/vision.cpp b/toid_vision/src/vision.cpp new file mode 100644 index 0000000..fe41f4f --- /dev/null +++ b/toid_vision/src/vision.cpp @@ -0,0 +1,10 @@ +#include "toid_vision/toid_vision.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + rclcpp::spin(std::make_shared(options)); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file -- 2.49.1 From 85353d06b2e3ec07f355183a864e89cc66c38404 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Fri, 10 Apr 2026 20:02:35 +0200 Subject: [PATCH 40/86] Made toid_vision into composable node --- toid_vision/CMakeLists.txt | 39 ++++++++++++++++- toid_vision/config/camera_info.yaml | 27 ++++++++++++ .../include/toid_vision/toid_vision.hpp | 2 +- toid_vision/launch/launch.py | 43 +++++++++++++++++++ toid_vision/package.xml | 12 +++++- toid_vision/src/toid_vision.cpp | 7 ++- 6 files changed, 124 insertions(+), 6 deletions(-) create mode 100644 toid_vision/config/camera_info.yaml create mode 100644 toid_vision/launch/launch.py diff --git a/toid_vision/CMakeLists.txt b/toid_vision/CMakeLists.txt index c0e59f6..d09cf28 100644 --- a/toid_vision/CMakeLists.txt +++ b/toid_vision/CMakeLists.txt @@ -14,7 +14,7 @@ set(PACKAGE_DEPS message_filters cv_bridge OpenCV - + rclcpp_components tf2 tf2_ros tf2_geometry_msgs @@ -38,6 +38,10 @@ set(SOURCES src/vision.cpp ) +set(SOURCES_COMPOSABLE + src/toid_vision.cpp +) + add_executable(toid_vision ${SOURCES}) target_include_directories( @@ -50,6 +54,39 @@ ament_target_dependencies(toid_vision ${PACKAGE_DEPS}) install(TARGETS toid_vision DESTINATION lib/${PROJECT_NAME}) +# TOID_VISION COMPOSABLE NODE + +add_library(toid_vision_component SHARED ${SOURCES_COMPOSABLE}) + +rclcpp_components_register_node( + toid_vision_component + PLUGIN "toid::NutDetector" + EXECUTABLE nut_detector +) + +target_include_directories( + toid_vision_component + PRIVATE + include +) + +ament_target_dependencies(toid_vision_component ${PACKAGE_DEPS}) + +ament_export_targets(export_toid_vision_component) +install(TARGETS toid_vision_component + EXPORT export_toid_vision_component + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME}/ +) + target_compile_features( toid_vision PUBLIC c_std_99 diff --git a/toid_vision/config/camera_info.yaml b/toid_vision/config/camera_info.yaml new file mode 100644 index 0000000..bc031db --- /dev/null +++ b/toid_vision/config/camera_info.yaml @@ -0,0 +1,27 @@ +image_width: 1640 +image_height: 1232 +camera_name: imx219__base_axi_pcie_1000120000_rp1_i2c_88000_imx219_10_1640x1232 +camera_matrix: + rows: 3 + cols: 3 + data: [1281.9678810697528, 0.0, 792.9357401565763, + 0.0, 1283.1760537785433, 615.1915708814056, + 0.0, 0.0, 1.0] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [0.19247275241900028, -0.5098314224324741, 6.710711247494001e-05, + -0.00018449989636263572, 0.40356075592228546] +rectification_matrix: + rows: 3 + cols: 3 + data: [1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0] +projection_matrix: + rows: 3 + cols: 4 + data: [1281.9678810697528, 0.0, 792.9357401565763, 0.0, + 0.0, 1283.1760537785433, 615.1915708814056, 0.0, + 0.0, 0.0, 1.0, 0.0] \ No newline at end of file diff --git a/toid_vision/include/toid_vision/toid_vision.hpp b/toid_vision/include/toid_vision/toid_vision.hpp index 85072b4..ade3112 100644 --- a/toid_vision/include/toid_vision/toid_vision.hpp +++ b/toid_vision/include/toid_vision/toid_vision.hpp @@ -27,7 +27,7 @@ class NutDetector : public rclcpp::Node sensor_msgs::msg::CompressedImage, sensor_msgs::msg::CameraInfo>; public: - NutDetector(rclcpp::NodeOptions &options); + NutDetector(const rclcpp::NodeOptions &options); public: void compressed_topic_callback( diff --git a/toid_vision/launch/launch.py b/toid_vision/launch/launch.py new file mode 100644 index 0000000..843c0a7 --- /dev/null +++ b/toid_vision/launch/launch.py @@ -0,0 +1,43 @@ +import launch +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare +import os + +def generate_launch_description(): + """Generate launch description with multiple components.""" + + vision_share = FindPackageShare("").find("toid_vision") + camera_info = os.path.join(vision_share, 'config/camera_info.yaml') + + + container = ComposableNodeContainer( + name='vision_container', + namespace='', + package='rclcpp_components', + executable='component_container_mt', + composable_node_descriptions=[ + ComposableNode( + package='toid_vision', + plugin='toid::NutDetector', + name='nut_detector', + parameters= [{ + 'is_blue': True, + }]), + ComposableNode( + package='camera_ros', + plugin='camera::CameraNode', + name='camera_driver', + parameters= [{ + 'width': 1640, + 'height': 1242, + 'orientation': 180, + 'camera': 0, + 'frame_id': 'camera', + 'camera_info_url': "file://" + camera_info + }]) + ], + output='screen', + ) + + return launch.LaunchDescription([container]) \ No newline at end of file diff --git a/toid_vision/package.xml b/toid_vision/package.xml index 4ef3ce6..4893019 100644 --- a/toid_vision/package.xml +++ b/toid_vision/package.xml @@ -9,9 +9,17 @@ ament_cmake - ament_lint_auto - ament_lint_common + rclcpp + sensor_msgs + message_filters + cv_bridge + opencv + rclcpp_components + tf2 + tf2_ros + tf2_geometry_msgs + ament_cmake diff --git a/toid_vision/src/toid_vision.cpp b/toid_vision/src/toid_vision.cpp index 398faee..5092944 100644 --- a/toid_vision/src/toid_vision.cpp +++ b/toid_vision/src/toid_vision.cpp @@ -6,7 +6,7 @@ namespace toid { -NutDetector::NutDetector(rclcpp::NodeOptions & options) +NutDetector::NutDetector(const rclcpp::NodeOptions & options) : Node("compressed_image_subscriber", options) { detectorParams_ = cv::aruco::DetectorParameters::create(); @@ -174,4 +174,7 @@ void NutDetector::get_pose(cv::Vec3d & tvec, cv::Vec3d & rvec, geometry_msgs::ms out_pose.position.z = tvec[2]; } -} // namespace toid \ No newline at end of file +} // namespace toid + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(toid::NutDetector) \ No newline at end of file -- 2.49.1 From a10271a87f8ee5c4a41cac3dcf020be3f01f04d7 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Fri, 10 Apr 2026 20:12:25 +0200 Subject: [PATCH 41/86] fixed camera topic name --- toid_vision/launch/launch.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/toid_vision/launch/launch.py b/toid_vision/launch/launch.py index 843c0a7..bbd0d8b 100644 --- a/toid_vision/launch/launch.py +++ b/toid_vision/launch/launch.py @@ -27,7 +27,7 @@ def generate_launch_description(): ComposableNode( package='camera_ros', plugin='camera::CameraNode', - name='camera_driver', + name='camera', parameters= [{ 'width': 1640, 'height': 1242, @@ -35,7 +35,8 @@ def generate_launch_description(): 'camera': 0, 'frame_id': 'camera', 'camera_info_url': "file://" + camera_info - }]) + }], + ) ], output='screen', ) -- 2.49.1 From 5c474e741bc17c49797b046aa06bd933f56677cb Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Sat, 11 Apr 2026 10:59:45 +0200 Subject: [PATCH 42/86] Corrected vid for tty servos --- toid_interaction/toid_interaction/interaction_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/toid_interaction/toid_interaction/interaction_node.py b/toid_interaction/toid_interaction/interaction_node.py index 1a2bdcc..1ccdd38 100644 --- a/toid_interaction/toid_interaction/interaction_node.py +++ b/toid_interaction/toid_interaction/interaction_node.py @@ -40,7 +40,7 @@ class InteracitionNode(Node): def find_sigma(self): for port_info in list_ports.comports(): - if port_info.vid == 0x18a6 and port_info.pid == 0x55d3: + if port_info.vid == 0x1a86 and port_info.pid == 0x55d3: break print(port_info.device) -- 2.49.1 From 7edb62ef34d2a4ea4e70edbd52081e6b9bbb5bd9 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Sat, 11 Apr 2026 11:12:23 +0200 Subject: [PATCH 43/86] Fixed camera pose --- toid_bot_description/rviz/default.rviz | 79 +++++++++++++------ .../src/toid_bot_description.urdf | 13 +++ 2 files changed, 70 insertions(+), 22 deletions(-) diff --git a/toid_bot_description/rviz/default.rviz b/toid_bot_description/rviz/default.rviz index 5c953cf..34836b4 100644 --- a/toid_bot_description/rviz/default.rviz +++ b/toid_bot_description/rviz/default.rviz @@ -1,13 +1,14 @@ Panels: - Class: rviz_common/Displays - Help Height: 78 + Help Height: 138 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 + - /Pose1 Splitter Ratio: 0.5 - Tree Height: 542 + Tree Height: 732 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -74,6 +75,14 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + camera: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_base: + Alpha: 1 + Show Axes: false + Show Trail: false drivewhl_l_link: Alpha: 1 Show Axes: false @@ -118,6 +127,10 @@ Visualization Manager: Value: true base_link: Value: true + camera: + Value: true + camera_base: + Value: true drivewhl_l_link: Value: true drivewhl_r_link: @@ -134,21 +147,43 @@ Visualization Manager: Show Axes: true Show Names: true Tree: - base_link: - base_footprint: - {} - drivewhl_l_link: - {} - drivewhl_r_link: - {} - left_caster: - {} - lidar: - {} - right_caster: - {} + base_footprint: + base_link: + drivewhl_l_link: + {} + drivewhl_r_link: + {} + left_caster: + {} + lidar: + {} + right_caster: + {} + camera_base: + camera: + {} Update Interval: 0 Value: true + - Alpha: 1 + Axes Length: 0.10000000149011612 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.10000000149011612 + Head Radius: 0.029999999329447746 + Name: Pose + Shaft Length: 0.10000000149011612 + Shaft Radius: 0.009999999776482582 + Shape: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /closest_acorn + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -195,7 +230,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/XYOrbit - Distance: 1.59040105342865 + Distance: 1.2598285675048828 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -210,18 +245,18 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.6053992509841919 + Pitch: 0.015398791059851646 Target Frame: Value: XYOrbit (rviz_default_plugins) - Yaw: 1.088563323020935 + Yaw: 0.09356015175580978 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 846 + Height: 1250 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016c000002acfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000002ac000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002acfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f000002ac000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002de00fffffffb0000000800540069006d0065010000000000000450000000000000000000000229000002ac00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000027a000003d8fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000070000003d80000018600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015b000003d8fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000070000003d80000013800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007620000005efc0100000002fb0000000800540069006d00650100000000000007620000048700fffffffb0000000800540069006d0065010000000000000450000000000000000000000375000003d800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -230,6 +265,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1200 + Width: 1890 X: 60 - Y: 60 + Y: 64 diff --git a/toid_bot_description/src/toid_bot_description.urdf b/toid_bot_description/src/toid_bot_description.urdf index 83de634..c9ce36a 100644 --- a/toid_bot_description/src/toid_bot_description.urdf +++ b/toid_bot_description/src/toid_bot_description.urdf @@ -24,6 +24,10 @@ + + + + @@ -39,6 +43,9 @@ + + + @@ -56,6 +63,12 @@ + + + + + + -- 2.49.1 From 94e7626fdafd3ad01fe63f6089553fdd193f312b Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Sat, 11 Apr 2026 11:13:26 +0200 Subject: [PATCH 44/86] Created node for checking if in position --- toid_bt/include/toid_bt/bt_executor.hpp | 13 ++++ toid_bt/include/toid_bt/plugin.hpp | 1 + .../toid_bt/plugins/in_position_decorator.hpp | 74 +++++++++++++++++++ .../toid_bt/plugins/send_text_action.hpp | 9 ++- toid_bt/src/bt_executor.cpp | 46 ++++++++++-- 5 files changed, 136 insertions(+), 7 deletions(-) create mode 100644 toid_bt/include/toid_bt/plugins/in_position_decorator.hpp diff --git a/toid_bt/include/toid_bt/bt_executor.hpp b/toid_bt/include/toid_bt/bt_executor.hpp index 7e95045..f93fd43 100644 --- a/toid_bt/include/toid_bt/bt_executor.hpp +++ b/toid_bt/include/toid_bt/bt_executor.hpp @@ -8,6 +8,7 @@ #include "tf2_ros/transform_listener.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" +#include "std_msgs/msg/string.hpp" namespace toid { class TreeExecutor : public BT::TreeExecutionServer { @@ -21,6 +22,12 @@ namespace toid { void position(geometry_msgs::msg::PoseStamped &pose); + void acorn_position(geometry_msgs::msg::PoseStamped &msg); + + void acorn_pose_cb(geometry_msgs::msg::PoseStamped::ConstSharedPtr msg); + + void to_flip_cb(std_msgs::msg::String::ConstSharedPtr msg); + std::string describeCustomNodes(); private: @@ -28,6 +35,12 @@ namespace toid { tf2_ros::Buffer::SharedPtr tf_buffer_; std::shared_ptr tf_listener_; + rclcpp::Subscription::SharedPtr acorn_pose_sub_; + rclcpp::Subscription::SharedPtr on_rotate_sub_; + + geometry_msgs::msg::PoseStamped acorn_pose_; + std::string to_flip_ = "0000"; + std::string base_frame_; std::string global_frame_; }; diff --git a/toid_bt/include/toid_bt/plugin.hpp b/toid_bt/include/toid_bt/plugin.hpp index 6a7fc85..303551d 100644 --- a/toid_bt/include/toid_bt/plugin.hpp +++ b/toid_bt/include/toid_bt/plugin.hpp @@ -5,4 +5,5 @@ namespace toid { using PoseFunc = std::function; + using FlipFunc = std::function; } \ No newline at end of file diff --git a/toid_bt/include/toid_bt/plugins/in_position_decorator.hpp b/toid_bt/include/toid_bt/plugins/in_position_decorator.hpp new file mode 100644 index 0000000..ea95d7f --- /dev/null +++ b/toid_bt/include/toid_bt/plugins/in_position_decorator.hpp @@ -0,0 +1,74 @@ +#pragma once + +#include "angles/angles.h" +#include "behaviortree_ros2/bt_service_node.hpp" +#include "std_srvs/srv/empty.hpp" +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "toid_bt/plugin.hpp" + +namespace toid +{ + +class InPositionNode : public BT::DecoratorNode +{ +public: + InPositionNode( + const std::string & name, const BT::NodeConfig & conf, PoseFunc get_pose) + : BT::DecoratorNode(name, conf), get_pose(get_pose) + { + } + + static BT::PortsList providedPorts() { + return { + BT::InputPort("timeout", 1, {}) + }; + } + + private: + bool checkIfPosesAreClose(geometry_msgs::msg::PoseStamped &poseA, geometry_msgs::msg::PoseStamped &poseB) { + const double dx = abs(poseA.pose.position.x - poseB.pose.position.x); + return dx < 0.005; + } + + bool checkIfInPose(geometry_msgs::msg::PoseStamped &pose) { + const double dx = abs(0.2575 - pose.pose.position.x); + return dx < 0.005; + } + + BT::NodeStatus tick() override + { + if(status() == BT::NodeStatus::IDLE) { + setStatus(BT::NodeStatus::RUNNING); + last_pos_change = clock.now(); + } + + double timeout = getInput("timeout").value_or(1.0); + + + geometry_msgs::msg::PoseStamped pose; + get_pose(pose); + + if(checkIfInPose(pose)) { + if(checkIfPosesAreClose(last_pos, pose)) { + if(clock.now() - last_pos_change > rclcpp::Duration::from_seconds(timeout)) { + haltChild(); + return BT::NodeStatus::SUCCESS; + } + } else { + last_pos = pose; + last_pos_change = clock.now(); + } + } + + const BT::NodeStatus child_status = child_node_->executeTick(); + return child_status; + } + + geometry_msgs::msg::PoseStamped last_pos; + PoseFunc get_pose; + rclcpp::Time last_pos_change; + rclcpp::Clock clock; +}; + +} // namespace toid \ No newline at end of file diff --git a/toid_bt/include/toid_bt/plugins/send_text_action.hpp b/toid_bt/include/toid_bt/plugins/send_text_action.hpp index bcb18fd..27a75d1 100644 --- a/toid_bt/include/toid_bt/plugins/send_text_action.hpp +++ b/toid_bt/include/toid_bt/plugins/send_text_action.hpp @@ -15,8 +15,8 @@ class SendTextNode : public BT::RosServiceNode { public: SendTextNode( - const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params) - : BT::RosServiceNode(name, conf, params) + const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params, FlipFunc get_text) + : BT::RosServiceNode(name, conf, params), get_text(get_text) { } @@ -29,6 +29,9 @@ public: bool setRequest(typename Request::SharedPtr &req) override { std::string text = getInput("text").value_or(""); + if(text=="") { + get_text(text); + } req->text = text; return true; @@ -38,6 +41,8 @@ public: { return BT::NodeStatus::SUCCESS; } +protected: + FlipFunc get_text; }; } // namespace toid \ No newline at end of file diff --git a/toid_bt/src/bt_executor.cpp b/toid_bt/src/bt_executor.cpp index 889aede..124e6ef 100644 --- a/toid_bt/src/bt_executor.cpp +++ b/toid_bt/src/bt_executor.cpp @@ -6,6 +6,7 @@ #include "rclcpp/rclcpp.hpp" #include "toid_bt/plugins/empty_srv_action.hpp" #include "toid_bt/plugins/end_calib_action.hpp" +#include "toid_bt/plugins/in_position_decorator.hpp" #include "toid_bt/plugins/move_coords_action.hpp" #include "toid_bt/plugins/rotate_action.hpp" #include "toid_bt/plugins/rotate_towards_action.hpp" @@ -32,6 +33,13 @@ TreeExecutor::TreeExecutor(const rclcpp::NodeOptions opts) nav2_util::declare_parameter_if_not_declared( node(), "global_frame", rclcpp::ParameterValue("map")); node()->get_parameter("global_frame", global_frame_); + using namespace std::placeholders; + + acorn_pose_sub_ = node()->create_subscription( + "/closest_acorn", rclcpp::QoS(1), std::bind(&TreeExecutor::acorn_pose_cb, this, _1)); + + on_rotate_sub_ = node()->create_subscription( + "/to_flip", rclcpp::QoS(1), std::bind(&TreeExecutor::to_flip_cb, this, _1)); } void TreeExecutor::onTreeCreated(BT::Tree & tree) @@ -41,7 +49,12 @@ void TreeExecutor::onTreeCreated(BT::Tree & tree) void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory) { + FlipFunc get_to_flip = [this](std::string & pose) { pose = this->to_flip_; }; PoseFunc get_pose = [this](geometry_msgs::msg::PoseStamped & pose) { this->position(pose); }; + PoseFunc get_acorn_pose = [this](geometry_msgs::msg::PoseStamped & pose) { + this->acorn_position(pose); + }; + rclcpp::Node::SharedPtr nh = node(); factory.registerNodeType( "MovePointSimple", BT::RosNodeParams(nh, "/moveCoords"), get_pose); @@ -62,16 +75,22 @@ void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory) factory.registerNodeType("EndCalib", BT::RosNodeParams(nh, "/end_calib")); - factory.registerNodeType("Seq1", BT::RosNodeParams(nh, "/sequence1")); + BT::RosNodeParams service_params1(nh, "/sequence1"); + service_params1.server_timeout = std::chrono::seconds(15); + factory.registerNodeType("Seq1", service_params1); - BT::RosNodeParams service_params(nh, "/sequence2"); - service_params.server_timeout = std::chrono::seconds(15); - factory.registerNodeType("Seq2", service_params); + BT::RosNodeParams service_params2(nh, "/sequence2"); + service_params2.server_timeout = std::chrono::seconds(20); + factory.registerNodeType("Seq2", service_params2, get_to_flip); - factory.registerNodeType("Seq3", BT::RosNodeParams(nh, "/sequence3")); + BT::RosNodeParams service_params3(nh, "/sequence3"); + service_params3.server_timeout = std::chrono::seconds(15); + factory.registerNodeType("Seq3", service_params3); factory.registerNodeType("DetectStuck", get_pose); + factory.registerNodeType("InPose", get_acorn_pose); + std::cout << describeCustomNodes() << std::endl; } @@ -80,6 +99,23 @@ void TreeExecutor::position(geometry_msgs::msg::PoseStamped & pose) nav2_util::getCurrentPose(pose, *tf_buffer_, global_frame_, base_frame_); } +void TreeExecutor::acorn_position(geometry_msgs::msg::PoseStamped & pose) { pose = acorn_pose_; } + +void TreeExecutor::acorn_pose_cb(geometry_msgs::msg::PoseStamped::ConstSharedPtr msg) +{ + auto node = this->node(); + try { + acorn_pose_ = tf_buffer_->transform(*msg, "base_footprint"); + } catch (const tf2::TransformException & e) { + RCLCPP_ERROR_THROTTLE(node->get_logger(), *node->get_clock(), 5000, "Transform timeout"); + } +} + +void TreeExecutor::to_flip_cb(std_msgs::msg::String::ConstSharedPtr msg) +{ + this->to_flip_ = msg->data; +} + std::optional TreeExecutor::onTreeExecutionCompleted( BT::NodeStatus status, bool was_cancelled) { -- 2.49.1 From c48e483d6064d25d44eeea5791922d116015c262 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Sat, 11 Apr 2026 13:38:13 +0200 Subject: [PATCH 45/86] Use header time instead of actual time for determining time passed --- toid_bt/include/toid_bt/plugins/in_position_decorator.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/toid_bt/include/toid_bt/plugins/in_position_decorator.hpp b/toid_bt/include/toid_bt/plugins/in_position_decorator.hpp index ea95d7f..dc5e81f 100644 --- a/toid_bt/include/toid_bt/plugins/in_position_decorator.hpp +++ b/toid_bt/include/toid_bt/plugins/in_position_decorator.hpp @@ -51,13 +51,13 @@ public: if(checkIfInPose(pose)) { if(checkIfPosesAreClose(last_pos, pose)) { - if(clock.now() - last_pos_change > rclcpp::Duration::from_seconds(timeout)) { + if(rclcpp::Time(pose.header.stamp) - last_pos_change > rclcpp::Duration::from_seconds(timeout)) { haltChild(); return BT::NodeStatus::SUCCESS; } } else { last_pos = pose; - last_pos_change = clock.now(); + last_pos_change = pose.header.stamp; } } -- 2.49.1 From 85071f92d54b89e6ec0e9e2f76da36798e486803 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Sat, 11 Apr 2026 13:47:00 +0200 Subject: [PATCH 46/86] Formating + debug output --- .../include/toid_vision/toid_vision.hpp | 10 ++++-- toid_vision/src/toid_vision.cpp | 31 ++++++++++++++----- 2 files changed, 30 insertions(+), 11 deletions(-) diff --git a/toid_vision/include/toid_vision/toid_vision.hpp b/toid_vision/include/toid_vision/toid_vision.hpp index ade3112..e1232a9 100644 --- a/toid_vision/include/toid_vision/toid_vision.hpp +++ b/toid_vision/include/toid_vision/toid_vision.hpp @@ -18,6 +18,9 @@ #include "tf2/LinearMath/Transform.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/buffer.hpp" + namespace toid { @@ -27,7 +30,7 @@ class NutDetector : public rclcpp::Node sensor_msgs::msg::CompressedImage, sensor_msgs::msg::CameraInfo>; public: - NutDetector(const rclcpp::NodeOptions &options); + NutDetector(const rclcpp::NodeOptions & options); public: void compressed_topic_callback( @@ -46,7 +49,6 @@ public: message_filters::Subscriber cam_info_sub_; std::shared_ptr> sync_; - rclcpp::Publisher::SharedPtr pose_pub_; rclcpp::Publisher::SharedPtr flip_pub_; @@ -59,9 +61,11 @@ public: cv::Ptr detectorParams_; cv::Ptr dictionary_; - message_filters::Subscriber img_sub_; + + tf2_ros::Buffer::SharedPtr tf_; + std::shared_ptr tf_listener_; }; } // namespace toid \ No newline at end of file diff --git a/toid_vision/src/toid_vision.cpp b/toid_vision/src/toid_vision.cpp index 5092944..c59f2fd 100644 --- a/toid_vision/src/toid_vision.cpp +++ b/toid_vision/src/toid_vision.cpp @@ -2,6 +2,7 @@ #include #include +#include "tf2/utils.hpp" namespace toid { @@ -12,6 +13,9 @@ NutDetector::NutDetector(const rclcpp::NodeOptions & options) detectorParams_ = cv::aruco::DetectorParameters::create(); dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_100); + tf_ = std::make_shared(this->get_clock()); + tf_listener_ = std::make_shared(*tf_); + bool use_compressed = true; this->declare_parameter("use_compressed", rclcpp::ParameterValue(true)); this->get_parameter("use_compressed", use_compressed); @@ -34,8 +38,7 @@ NutDetector::NutDetector(const rclcpp::NodeOptions & options) pose_pub_ = this->create_publisher("/closest_acorn", rclcpp::QoS(1)); -flip_pub_ = - this->create_publisher("/to_flip", rclcpp::QoS(1)); + flip_pub_ = this->create_publisher("/to_flip", rclcpp::QoS(1)); } void NutDetector::compressed_topic_callback( @@ -114,12 +117,12 @@ void NutDetector::process_image( if (colors.size() == 4) { RCLCPP_DEBUG(this->get_logger(), "Last seen orientation: %s", colors.c_str()); - for(int i = 0; i<4; i++) { - if(colors[i] == 'b') { - colors[i] = (is_blue_)? '0' : '1'; - } else { - colors[i] = (is_blue_)? '1' : '0'; - } + for (int i = 0; i < 4; i++) { + if (colors[i] == 'b') { + colors[i] = (is_blue_) ? '0' : '1'; + } else { + colors[i] = (is_blue_) ? '1' : '0'; + } } RCLCPP_DEBUG(this->get_logger(), "Last seen orientation: %s", colors.c_str()); @@ -133,6 +136,18 @@ void NutDetector::process_image( pose.header.frame_id = camera_frame_id_; pose.header.stamp = header.stamp; pose_pub_->publish(pose); + + try { + geometry_msgs::msg::PoseStamped out_pose; + out_pose = tf_->transform(pose, "base_footprint"); + RCLCPP_DEBUG_THROTTLE( + this->get_logger(), *this->get_clock(), 2000, "Closest guy is at: %lf %lf %lf", + out_pose.pose.position.x, out_pose.pose.position.y, tf2::getYaw(out_pose.pose.orientation) + ); + } catch (const tf2::TransformException & e) { + RCLCPP_ERROR_THROTTLE( + this->get_logger(), *this->get_clock(), 2000, "Failed to transfrom point"); + } } void NutDetector::extract_markers(cv::Mat & image) -- 2.49.1 From 53f3c073b8b3796c3fd0abd85e01ef0aa9e2c17a Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Sat, 11 Apr 2026 15:50:20 +0200 Subject: [PATCH 47/86] Added approach acorn behavior --- toid_behaviors/CMakeLists.txt | 1 + .../toid_behaviors/approach_acorns.hpp | 88 ++++++ toid_behaviors/src/approach_acorns.cpp | 286 ++++++++++++++++++ toid_behaviors/toid_behaviors.xml | 3 + .../params/toid_general_params.yaml | 4 +- 5 files changed, 381 insertions(+), 1 deletion(-) create mode 100644 toid_behaviors/include/toid_behaviors/approach_acorns.hpp create mode 100644 toid_behaviors/src/approach_acorns.cpp diff --git a/toid_behaviors/CMakeLists.txt b/toid_behaviors/CMakeLists.txt index b1652f5..2498146 100644 --- a/toid_behaviors/CMakeLists.txt +++ b/toid_behaviors/CMakeLists.txt @@ -32,6 +32,7 @@ set( src/simple_move.cpp src/simple_rotate.cpp src/simple_translate_x.cpp + src/approach_acorns.cpp ) find_package(ament_cmake REQUIRED) diff --git a/toid_behaviors/include/toid_behaviors/approach_acorns.hpp b/toid_behaviors/include/toid_behaviors/approach_acorns.hpp new file mode 100644 index 0000000..0ce1a07 --- /dev/null +++ b/toid_behaviors/include/toid_behaviors/approach_acorns.hpp @@ -0,0 +1,88 @@ +#pragma once + +#include "geometry_msgs/msg/pose.hpp" +#include "toid_behaviors/scl.hpp" +#include "toid_behaviors/simple_move.hpp" +#include "toid_msgs/action/simple_move_coords.hpp" +#include "visualization_msgs/msg/marker.hpp" + +namespace toid +{ +using MoveAction = toid_msgs::action::SimpleMoveCoords; +using PoseStamped = geometry_msgs::msg::PoseStamped; +using namespace nav2_behaviors; + +class ApproachAcorns : public SimpleMove +{ +public: + ApproachAcorns(); + ~ApproachAcorns(); + + void configureCB() override; + + void activateCB() override; + void deactivateCB() override; + + ResultStatus onStart( + const std::shared_ptr command, const geometry_msgs::msg::Pose & pose, + const geometry_msgs::msg::Twist & vel) override; + + double distanceToTarget( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & target_point, + const double target_theta, bool backwards); + + double velocityTarget(const double dist_left); + + bool collisionDetection( + const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Pose & last_ok_pose); + + void acorn_position_cb(PoseStamped::ConstSharedPtr msg); + + 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::LOCAL; + } + +protected: + SmoothControlLaw scl_; + rclcpp::Subscription::SharedPtr acorn_pose_sub_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr target_pose_pub_; + std::shared_mutex mutex_; + + //Goal + geometry_msgs::msg::Pose new_target_pose_; + double new_target_angle_; + + geometry_msgs::msg::Pose target_pose_; + double target_angle_; + double target_sign_; + bool backwards_; + unsigned char mode_; + + //State + double last_speed_; + rclcpp::Time last_seen_; + double distance_; + + //Config + double max_vel_accel_; + double max_vel_decel_; + double max_vel_speed_; + double min_vel_speed_; + + double max_angular_speed_; + + double kp_; + double k_phi_; + double k_delta_; + double beta_; + double lambda_; + double slowdown_radius_; + bool debug_marker_; +}; + +} // namespace toid diff --git a/toid_behaviors/src/approach_acorns.cpp b/toid_behaviors/src/approach_acorns.cpp new file mode 100644 index 0000000..c791d8c --- /dev/null +++ b/toid_behaviors/src/approach_acorns.cpp @@ -0,0 +1,286 @@ +#include "toid_behaviors/approach_acorns.hpp" + +#include "angles/angles.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace toid +{ + +ApproachAcorns::ApproachAcorns() : SimpleMove() {} +ApproachAcorns::~ApproachAcorns() {} + +void ApproachAcorns::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.10)); + 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_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".kphi", rclcpp::ParameterValue(5.0)); + node->get_parameter(behavior_name_ + ".kphi", k_phi_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".kdelta", rclcpp::ParameterValue(2.0)); + node->get_parameter(behavior_name_ + ".kdelta", k_delta_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".beta", rclcpp::ParameterValue(0.4)); + node->get_parameter(behavior_name_ + ".beta", beta_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".lambda", rclcpp::ParameterValue(2.0)); + node->get_parameter(behavior_name_ + ".lambda", lambda_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".slow_down_radius", rclcpp::ParameterValue(0.20)); + node->get_parameter(behavior_name_ + ".slow_down_radius", slowdown_radius_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".debug_marker", rclcpp::ParameterValue(false)); + node->get_parameter(behavior_name_ + ".debug_marker", debug_marker_); + + target_pose_pub_ = node->create_publisher("marker", 1); +} + +void ApproachAcorns::activateCB() +{ + auto node = node_.lock(); + using namespace std::placeholders; + acorn_pose_sub_ = node->create_subscription( + "closest_acorn", rclcpp::QoS(1), std::bind(&ApproachAcorns::acorn_position_cb, this, _1)); + target_pose_pub_->on_activate(); + distance_ = 1.0; +} + +void ApproachAcorns::deactivateCB() { + acorn_pose_sub_.reset(); + target_pose_pub_->on_deactivate(); +} + +void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg) +{ + geometry_msgs::msg::PoseStamped pose_local; + geometry_msgs::msg::PoseStamped pose_global; + try { + pose_local = tf_->transform(*msg, robot_base_frame_); + pose_global = tf_->transform(*msg, global_frame_); + } catch (const tf2::TransformException & e) { + RCLCPP_ERROR_ONCE(logger_, "Transform timeout [showing once]..."); + return; + } + double x = pose_local.pose.position.x; + double y = pose_local.pose.position.y; + if (x * x + y * y > distance_ + 0.01) { + return; + } + + + double yaw = tf2::getYaw(pose_global.pose.orientation); + + if (yaw < 0) { + angles::normalize_angle(yaw + M_PI / 2); + } + + pose_global.pose.position.x += std::cos(yaw) * 0.01 + std::sin(yaw) * -0.20; + pose_global.pose.position.y += -std::cos(yaw) * 0.01 + std::cos(yaw) * -0.20; + tf2::convert(tf2::Quaternion::createFromRPY(0,0,yaw), pose_global.pose.orientation); + + if(debug_marker_) { + visualization_msgs::msg::Marker marker; + marker.lifetime.sec = 1.0; + marker.header = pose_global.header; + marker.pose = pose_global.pose; + marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.scale.x = 0.02; + marker.scale.y = 0.02; + marker.scale.z = 0.02; + marker.color.a = 1.0; + marker.color.r = 1.0; + marker.color.g = 0; + marker.color.b = 0; + target_pose_pub_->publish(marker); + } + + std::lock_guard _lock(mutex_); + distance_ = x * x + y * y; + new_target_pose_ = pose_global.pose; + new_target_angle_ = yaw; +} + +ResultStatus ApproachAcorns::onStart( + const std::shared_ptr command, const geometry_msgs::msg::Pose &, + const geometry_msgs::msg::Twist & vel) +{ + + std::lock_guard _lock(mutex_); + distance_ = 1.0; + new_target_pose_.position.x = command->x; + new_target_pose_.position.y = command->y; + backwards_ = command->backwards; + + new_target_pose_.orientation = tf2::toMsg(tf2::Quaternion::createFromRPY(0, 0, command->theta)); + new_target_angle_ = command->theta; + target_sign_ = backwards_ ? -1.0 : 1.0; + max_vel_speed_ = command->max_speed; + + if (command->max_speed == 0) { + auto node = node_.lock(); + 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_; + 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 ApproachAcorns::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 ApproachAcorns::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 ApproachAcorns::collisionDetection( + const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Pose & last_ok_pose) +{ + const int samples = static_cast(0.5 / control_duration_); + geometry_msgs::msg::Pose current_pose = pose; + last_ok_pose = pose; + 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; + p.x = current_pose.position.x; + p.y = current_pose.position.y; + p.theta = tf2::getYaw(current_pose.orientation); + + if (check_map && !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 ApproachAcorns::updateVel( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist &, + geometry_msgs::msg::Twist & out_vel) +{ + + { + std::lock_guard _lock(mutex_); + target_pose_ = new_target_pose_; + target_angle_ = new_target_angle_; + } + + double dist_left = distanceToTarget(pose, target_pose_.position, target_angle_, backwards_); + + if (dist_left <= 0.001) { + out_vel.linear.x = 0; + out_vel.angular.z = 0; + return ResultStatus{Status::SUCCEEDED}; + } + + 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 = 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_ * 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); + + return ResultStatus{Status::RUNNING}; +} + +} // namespace toid + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(toid::ApproachAcorns, nav2_core::Behavior); \ No newline at end of file diff --git a/toid_behaviors/toid_behaviors.xml b/toid_behaviors/toid_behaviors.xml index 10cc958..2556c3f 100644 --- a/toid_behaviors/toid_behaviors.xml +++ b/toid_behaviors/toid_behaviors.xml @@ -9,5 +9,8 @@ + + + \ No newline at end of file diff --git a/toid_navigation/params/toid_general_params.yaml b/toid_navigation/params/toid_general_params.yaml index fc0776b..0101127 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", "rotate", "translateX", "moveCoords"] + behavior_plugins: ["backup", "spin", "rotate", "translateX", "moveCoords", "approachAcorns"] spin: plugin: "nav2_behaviors::Spin" backup: @@ -35,6 +35,8 @@ behavior_server: plugin: "toid::SimpleTranslateXBehavior" moveCoords: plugin: "toid::MoveCoords" + approachAcorns: + plugin: "toid::ApproachAcorns" local_frame: map global_frame: map robot_base_frame: base_footprint -- 2.49.1 From f84ca5d3bf1831e75065e415e1522e56aa88de9d Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Sat, 11 Apr 2026 15:56:28 +0200 Subject: [PATCH 48/86] Formating toid_lidar --- toid_lidar/launch/launch.py | 93 ++++++++++++++++++------------------- 1 file changed, 46 insertions(+), 47 deletions(-) diff --git a/toid_lidar/launch/launch.py b/toid_lidar/launch/launch.py index a78e77e..6f03521 100644 --- a/toid_lidar/launch/launch.py +++ b/toid_lidar/launch/launch.py @@ -7,6 +7,7 @@ from launch_ros.substitutions import FindPackageShare from pathlib import Path + def generate_launch_description(): basedir = FindPackageShare("").find("toid_lidar") @@ -19,50 +20,48 @@ def generate_launch_description(): use_closest = LaunchConfiguration("use_closest") lidar_frame = LaunchConfiguration("lidar_frame") - return LaunchDescription([ - DeclareLaunchArgument( - 'visualize', - default_value='False', - description="Whether to launch rviz2" - ), - DeclareLaunchArgument( - 'draw_markers', - default_value='False', - description="Draw markers" - ), - DeclareLaunchArgument( - 'use_closest', - default_value='True', - description="Use closest point for calibration" - ), - DeclareLaunchArgument( - 'lidar_frame', - default_value='lidar_frame', - description="TF frame of the lidar" - ), - Node( - package='toid_lidar', - executable='toid_lidar', - output="screen", - parameters=[ - lidar_config, - { - 'closest': use_closest, - 'draw_markers': draw_markers - }] - ), - Node( - package='rplidar_ros', - executable='rplidar_composition', - output="screen", - parameters=[lidar_config, {'frame_id': lidar_frame}] - ), - Node( - package='rviz2', - executable='rviz2', - name='rviz2', - output='screen', - arguments=['-d', rviz_config], - condition=IfCondition(visualize) - ) - ]) + return LaunchDescription( + [ + DeclareLaunchArgument( + "visualize", + default_value="False", + description="Whether to launch rviz2", + ), + DeclareLaunchArgument( + "draw_markers", default_value="False", description="Draw markers" + ), + DeclareLaunchArgument( + "use_closest", + default_value="True", + description="Use closest point for calibration", + ), + DeclareLaunchArgument( + "lidar_frame", + default_value="lidar_frame", + description="TF frame of the lidar", + ), + Node( + package="toid_lidar", + executable="toid_lidar", + output="screen", + parameters=[ + lidar_config, + {"closest": use_closest, "draw_markers": draw_markers}, + ], + ), + Node( + package="rplidar_ros", + executable="rplidar_composition", + output="screen", + parameters=[lidar_config, {"frame_id": lidar_frame}], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="screen", + arguments=["-d", rviz_config], + condition=IfCondition(visualize), + ), + ] + ) -- 2.49.1 From 7f6cfbeb8788beebfb729d2a81b835237a5f3bb4 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Sat, 11 Apr 2026 16:03:40 +0200 Subject: [PATCH 49/86] Added team configuration to toid_vision launch file --- toid_vision/launch/launch.py | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/toid_vision/launch/launch.py b/toid_vision/launch/launch.py index bbd0d8b..ee848a1 100644 --- a/toid_vision/launch/launch.py +++ b/toid_vision/launch/launch.py @@ -1,4 +1,6 @@ import launch +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode from launch_ros.substitutions import FindPackageShare @@ -10,6 +12,12 @@ def generate_launch_description(): vision_share = FindPackageShare("").find("toid_vision") camera_info = os.path.join(vision_share, 'config/camera_info.yaml') + is_blue_arg = DeclareLaunchArgument( + name= "is_blue", + default_value = "True" + ) + + is_blue = LaunchConfiguration("is_blue") container = ComposableNodeContainer( name='vision_container', @@ -22,7 +30,7 @@ def generate_launch_description(): plugin='toid::NutDetector', name='nut_detector', parameters= [{ - 'is_blue': True, + 'is_blue': is_blue, }]), ComposableNode( package='camera_ros', @@ -41,4 +49,7 @@ def generate_launch_description(): output='screen', ) - return launch.LaunchDescription([container]) \ No newline at end of file + return launch.LaunchDescription([ + is_blue_arg, + container + ]) \ No newline at end of file -- 2.49.1 From 5624c44574abc7e25fc4d12b26b42ebf1e01fadd Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Sat, 11 Apr 2026 16:47:57 +0200 Subject: [PATCH 50/86] fixed approac acorn target offset --- toid_behaviors/src/approach_acorns.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/toid_behaviors/src/approach_acorns.cpp b/toid_behaviors/src/approach_acorns.cpp index c791d8c..c994cbf 100644 --- a/toid_behaviors/src/approach_acorns.cpp +++ b/toid_behaviors/src/approach_acorns.cpp @@ -96,22 +96,24 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg) } - double yaw = tf2::getYaw(pose_global.pose.orientation); + double yaw = angles::normalize_angle(tf2::getYaw(pose_global.pose.orientation)); - if (yaw < 0) { - angles::normalize_angle(yaw + M_PI / 2); + if(tf2::getYaw(pose_local.pose.orientation) > 0) { + yaw += M_PI; } - pose_global.pose.position.x += std::cos(yaw) * 0.01 + std::sin(yaw) * -0.20; - pose_global.pose.position.y += -std::cos(yaw) * 0.01 + std::cos(yaw) * -0.20; - tf2::convert(tf2::Quaternion::createFromRPY(0,0,yaw), pose_global.pose.orientation); + yaw += M_PI/2; + + pose_global.pose.position.x += std::cos(yaw) * -0.35 - std::sin(yaw) * + 0.005; + pose_global.pose.position.y += std::sin(yaw) * -0.35 + std::cos(yaw) * + 0.005; + tf2::convert(tf2::Quaternion::createFromRPY(0,0,yaw ), pose_global.pose.orientation); if(debug_marker_) { visualization_msgs::msg::Marker marker; marker.lifetime.sec = 1.0; marker.header = pose_global.header; marker.pose = pose_global.pose; - marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.type = visualization_msgs::msg::Marker::ARROW; marker.scale.x = 0.02; marker.scale.y = 0.02; marker.scale.z = 0.02; -- 2.49.1 From 6633ef41faf125f1edab331acf63e82dcdde9da2 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Mon, 13 Apr 2026 13:24:52 +0200 Subject: [PATCH 51/86] Added start plug action server --- .../toid_behaviors/approach_acorns.hpp | 2 + .../toid_behaviors/rolling_average.hpp | 67 +++++++++++++++++ toid_behaviors/src/approach_acorns.cpp | 16 +++- toid_interaction/package.xml | 1 + toid_interaction/setup.py | 4 +- .../toid_interaction/interaction_node.py | 74 +++++++++++-------- toid_msgs/CMakeLists.txt | 1 + toid_msgs/action/EmptyAction.action | 2 + 8 files changed, 134 insertions(+), 33 deletions(-) create mode 100644 toid_behaviors/include/toid_behaviors/rolling_average.hpp create mode 100644 toid_msgs/action/EmptyAction.action diff --git a/toid_behaviors/include/toid_behaviors/approach_acorns.hpp b/toid_behaviors/include/toid_behaviors/approach_acorns.hpp index 0ce1a07..ddd0f1c 100644 --- a/toid_behaviors/include/toid_behaviors/approach_acorns.hpp +++ b/toid_behaviors/include/toid_behaviors/approach_acorns.hpp @@ -3,6 +3,7 @@ #include "geometry_msgs/msg/pose.hpp" #include "toid_behaviors/scl.hpp" #include "toid_behaviors/simple_move.hpp" +#include "toid_behaviors/rolling_average.hpp" #include "toid_msgs/action/simple_move_coords.hpp" #include "visualization_msgs/msg/marker.hpp" @@ -52,6 +53,7 @@ protected: rclcpp::Subscription::SharedPtr acorn_pose_sub_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr target_pose_pub_; std::shared_mutex mutex_; + RollingAverage avg_ = RollingAverage(5); //Goal geometry_msgs::msg::Pose new_target_pose_; diff --git a/toid_behaviors/include/toid_behaviors/rolling_average.hpp b/toid_behaviors/include/toid_behaviors/rolling_average.hpp new file mode 100644 index 0000000..9c5b2dd --- /dev/null +++ b/toid_behaviors/include/toid_behaviors/rolling_average.hpp @@ -0,0 +1,67 @@ +#pragma once + +#include +#include + +#include "geometry_msgs/msg/pose.hpp" +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace toid +{ + +class RollingAverage +{ +public: + using Pose2D = std::tuple; + RollingAverage(size_t size = 0) : poses_(size), size_(size) {} + + Pose2D push(geometry_msgs::msg::Pose & pose) + { + if(size_ > 0) { + return {}; + } + if (size_ == data_count_) { + accum_x_ -= poses_[front_idx_].position.x; + accum_y_ -= poses_[front_idx_].position.y; + accum_theta_ -= tf2::getYaw(poses_[front_idx_].orientation); + front_idx_ += 1; + front_idx_ %= size_; + data_count_--; + } + + back_idx_ += 1; + back_idx_ %= size_; + data_count_++; + + poses_[back_idx_] = pose; + + accum_x_ += poses_[back_idx_].position.x; + accum_y_ += poses_[back_idx_].position.y; + accum_theta_ += tf2::getYaw(poses_[back_idx_].orientation); + + return {accum_x_ / data_count_, accum_y_ / data_count_, accum_theta_ / data_count_}; + } + + void reset() { + data_count_ = 0; + front_idx_ = 0; + back_idx_ = 0; + accum_x_ = 0; + accum_y_ = 0; + accum_theta_ = 0; + } + +private: + std::vector poses_; + const size_t size_; + size_t front_idx_ = 0; + size_t back_idx_ = 0; + size_t data_count_ = 0; + + double accum_x_ = 0; + double accum_y_ = 0; + double accum_theta_ = 0; +}; + +} // namespace toid \ No newline at end of file diff --git a/toid_behaviors/src/approach_acorns.cpp b/toid_behaviors/src/approach_acorns.cpp index c994cbf..85c0a5d 100644 --- a/toid_behaviors/src/approach_acorns.cpp +++ b/toid_behaviors/src/approach_acorns.cpp @@ -89,9 +89,10 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg) RCLCPP_ERROR_ONCE(logger_, "Transform timeout [showing once]..."); return; } + double x = pose_local.pose.position.x; double y = pose_local.pose.position.y; - if (x * x + y * y > distance_ + 0.01) { + if (x * x + y * y > distance_ + 0.005) { return; } @@ -104,10 +105,12 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg) yaw += M_PI/2; + pose_global.pose.position.x += std::cos(yaw) * -0.35 - std::sin(yaw) * + 0.005; pose_global.pose.position.y += std::sin(yaw) * -0.35 + std::cos(yaw) * + 0.005; tf2::convert(tf2::Quaternion::createFromRPY(0,0,yaw ), pose_global.pose.orientation); + if(debug_marker_) { visualization_msgs::msg::Marker marker; marker.lifetime.sec = 1.0; @@ -124,10 +127,15 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg) target_pose_pub_->publish(marker); } + yaw = angles::normalize_angle(yaw); + std::lock_guard _lock(mutex_); + auto[a,b,c] = avg_.push(pose_global.pose); distance_ = x * x + y * y; - new_target_pose_ = pose_global.pose; - new_target_angle_ = yaw; + new_target_pose_.position.x = a; + new_target_pose_.position.y = b; + tf2::convert(tf2::Quaternion::createFromRPY(0,0, c), new_target_pose_.orientation); + new_target_angle_ = c; } ResultStatus ApproachAcorns::onStart( @@ -146,6 +154,8 @@ ResultStatus ApproachAcorns::onStart( target_sign_ = backwards_ ? -1.0 : 1.0; max_vel_speed_ = command->max_speed; + avg_.reset(); + if (command->max_speed == 0) { auto node = node_.lock(); node->get_parameter(behavior_name_ + ".max_vel_speed", max_vel_speed_); diff --git a/toid_interaction/package.xml b/toid_interaction/package.xml index 2959e79..6fde46c 100644 --- a/toid_interaction/package.xml +++ b/toid_interaction/package.xml @@ -13,6 +13,7 @@ python3-pytest python3-serial + python3-gpiozero ament_python diff --git a/toid_interaction/setup.py b/toid_interaction/setup.py index 6473e97..aadf802 100644 --- a/toid_interaction/setup.py +++ b/toid_interaction/setup.py @@ -25,7 +25,9 @@ setup( entry_points={ 'console_scripts': [ 'sequence = toid_interaction.mechanism.sekvenca_2026:main', - 'node = toid_interaction.interaction_node:main' + 'node = toid_interaction.interaction_node:main', + 'cam_calib = toid_interaction.camera:main', + 'cam_calib1 = toid_interaction.camera1:main' ], }, ) diff --git a/toid_interaction/toid_interaction/interaction_node.py b/toid_interaction/toid_interaction/interaction_node.py index 1ccdd38..b887c9a 100644 --- a/toid_interaction/toid_interaction/interaction_node.py +++ b/toid_interaction/toid_interaction/interaction_node.py @@ -2,52 +2,60 @@ import rclpy from rclpy.node import Node from std_srvs.srv import Empty +from gpiozero import Button, OutputDevice + from serial.tools import list_ports from toid_interaction.mechanism.sekvenca_2026 import okreni, okreni_niz from toid_interaction.mechanism.zidovi_load import ZidoviAction from toid_interaction.mechanism.zupcanik import ZupcanikAction +from toid_msgs.action import EmptyAction from toid_msgs.srv import SendString +from rclpy.action.server import ServerGoalHandle +from rclpy.action.server import ActionServer +import asyncio + + class InteracitionNode(Node): step: int = 0 + btn_: Button + output_pin_: OutputDevice + start_pin_action_: ActionServer + def __init__(self): - super().__init__('ToidInteractionNode') + super().__init__("ToidInteractionNode") self.find_sigma() - self.srv = self.create_service( - Empty, - '/sequence1', - self.sequence1_cb - ) - - self.srv = self.create_service( - SendString, - '/sequence2', - self.sequence2_cb - ) - - self.srv = self.create_service( - Empty, - '/sequence3', - self.sequence3_cb - ) - - - + self.srv = self.create_service(Empty, "/sequence1", self.sequence1_cb) self.get_logger().info("Service 'sequence1' ready.") + self.srv = self.create_service(SendString, "/sequence2", self.sequence2_cb) + self.get_logger().info("Service 'sequence2' ready.") + + self.srv = self.create_service(Empty, "/sequence3", self.sequence3_cb) + self.get_logger().info("Service 'sequence3' ready.") + + self.btn_ = Button(17) + self.output_pin_ = OutputDevice(27) + + self.start_pin_action_ = ActionServer( + self, EmptyAction, "/start_plug", execute_callback=self.start_plug_action_cb + ) + self.get_logger().info("Action 'start_plug' ready.") + + def find_sigma(self): for port_info in list_ports.comports(): - if port_info.vid == 0x1a86 and port_info.pid == 0x55d3: + if port_info.vid == 0x1A86 and port_info.pid == 0x55D3: break print(port_info.device) self.st_motor_device_name = port_info.device def sequence1_cb(self, request, response): - if(self.step != 0): + if self.step != 0: return Empty.Response() okreni(5) zupcanik = ZupcanikAction(self.st_motor_device_name) @@ -55,8 +63,8 @@ class InteracitionNode(Node): self.step = 1 return response - def sequence2_cb(self, request : SendString.Request, response : SendString.Response): - if(self.step != 1): + def sequence2_cb(self, request: SendString.Request, response: SendString.Response): + if self.step != 1: return Empty.Response() zidovi = ZidoviAction(self.st_motor_device_name) zidovi.beli_zid(1) @@ -70,11 +78,11 @@ class InteracitionNode(Node): return response def sequence3_cb(self, request, response): - if(self.step != 2): + if self.step != 2: return Empty.Response() zupcanik = ZupcanikAction(self.st_motor_device_name) - zidovi = ZidoviAction(self.st_motor_device_name) + zidovi = ZidoviAction(self.st_motor_device_name) zupcanik.zupcanik(1, 1010, 25) zidovi.plavi_zid(0, TargetPos=150) @@ -83,6 +91,14 @@ class InteracitionNode(Node): self.step = 0 return response + async def start_plug_action_cb(self, goal_handle: ServerGoalHandle): + while not self.btn_.is_active: + await asyncio.sleep(0.1) + while self.btn_.is_active: + await asyncio.sleep(0.1) + goal_handle.succeed() + return EmptyAction.Result() + def main(args=None): rclpy.init(args=args) @@ -93,5 +109,5 @@ def main(args=None): rclpy.shutdown() -if __name__ == '__main__': - main() \ No newline at end of file +if __name__ == "__main__": + main() diff --git a/toid_msgs/CMakeLists.txt b/toid_msgs/CMakeLists.txt index 6c0b3fd..2d33e00 100644 --- a/toid_msgs/CMakeLists.txt +++ b/toid_msgs/CMakeLists.txt @@ -19,6 +19,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "action/SimpleMoveCoords.action" "action/SimpleRotate.action" "action/SimpleTranslateX.action" + "action/EmptyAction.action" DEPENDENCIES geometry_msgs ) diff --git a/toid_msgs/action/EmptyAction.action b/toid_msgs/action/EmptyAction.action new file mode 100644 index 0000000..a49ba48 --- /dev/null +++ b/toid_msgs/action/EmptyAction.action @@ -0,0 +1,2 @@ +--- +--- \ No newline at end of file -- 2.49.1 From 4c29baa005b5e6c5ad92d14e18e2fdcaeb310b67 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Mon, 13 Apr 2026 13:32:07 +0200 Subject: [PATCH 52/86] Fixed docker compose --- docker-compose.yaml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/docker-compose.yaml b/docker-compose.yaml index 97435e1..2ff4b42 100644 --- a/docker-compose.yaml +++ b/docker-compose.yaml @@ -8,9 +8,10 @@ services: network_mode: host volumes: - - ./:/ros_ws/src + - ./:/ros_ws - /dev/:/dev/ - /sys/:/sys/ + - /run/udev/:/run/udev/ entrypoint: ["sleep","infinity"] -- 2.49.1 From 6d3794d154a4f5de81a8a0bd4451f3de2b49ce41 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Mon, 13 Apr 2026 14:28:08 +0200 Subject: [PATCH 53/86] Made interaction node use multitreaded executor --- toid_interaction/package.xml | 1 + .../toid_interaction/interaction_node.py | 14 ++++++++------ 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/toid_interaction/package.xml b/toid_interaction/package.xml index 6fde46c..d48ec2b 100644 --- a/toid_interaction/package.xml +++ b/toid_interaction/package.xml @@ -14,6 +14,7 @@ python3-serial python3-gpiozero + toid_msgs ament_python diff --git a/toid_interaction/toid_interaction/interaction_node.py b/toid_interaction/toid_interaction/interaction_node.py index b887c9a..842736e 100644 --- a/toid_interaction/toid_interaction/interaction_node.py +++ b/toid_interaction/toid_interaction/interaction_node.py @@ -1,4 +1,6 @@ import rclpy +import rclpy.callback_groups +import rclpy.executors from rclpy.node import Node from std_srvs.srv import Empty @@ -14,7 +16,6 @@ from toid_msgs.srv import SendString from rclpy.action.server import ServerGoalHandle from rclpy.action.server import ActionServer -import asyncio class InteracitionNode(Node): @@ -41,8 +42,10 @@ class InteracitionNode(Node): self.output_pin_ = OutputDevice(27) self.start_pin_action_ = ActionServer( - self, EmptyAction, "/start_plug", execute_callback=self.start_plug_action_cb + self, EmptyAction, "/start_plug", execute_callback=self.start_plug_action_cb, + callback_group=rclpy.callback_groups.ReentrantCallbackGroup() ) + self.get_logger().info("Action 'start_plug' ready.") @@ -93,9 +96,9 @@ class InteracitionNode(Node): async def start_plug_action_cb(self, goal_handle: ServerGoalHandle): while not self.btn_.is_active: - await asyncio.sleep(0.1) + pass while self.btn_.is_active: - await asyncio.sleep(0.1) + pass goal_handle.succeed() return EmptyAction.Result() @@ -104,8 +107,7 @@ def main(args=None): rclpy.init(args=args) node = InteracitionNode() - rclpy.spin(node) - + rclpy.executors.MultiThreadedExecutor().spin(node) rclpy.shutdown() -- 2.49.1 From 6a8d7176f4760aa75a268f1d928a408dc1c764b5 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Mon, 13 Apr 2026 18:26:17 +0200 Subject: [PATCH 54/86] Fixed Moving average for acorn approach --- .../toid_behaviors/approach_acorns.hpp | 2 +- .../toid_behaviors/rolling_average.hpp | 4 +-- toid_behaviors/src/approach_acorns.cpp | 32 ++++++++++--------- 3 files changed, 20 insertions(+), 18 deletions(-) diff --git a/toid_behaviors/include/toid_behaviors/approach_acorns.hpp b/toid_behaviors/include/toid_behaviors/approach_acorns.hpp index ddd0f1c..ff17434 100644 --- a/toid_behaviors/include/toid_behaviors/approach_acorns.hpp +++ b/toid_behaviors/include/toid_behaviors/approach_acorns.hpp @@ -53,7 +53,7 @@ protected: rclcpp::Subscription::SharedPtr acorn_pose_sub_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr target_pose_pub_; std::shared_mutex mutex_; - RollingAverage avg_ = RollingAverage(5); + RollingAverage avg_ = RollingAverage(10); //Goal geometry_msgs::msg::Pose new_target_pose_; diff --git a/toid_behaviors/include/toid_behaviors/rolling_average.hpp b/toid_behaviors/include/toid_behaviors/rolling_average.hpp index 9c5b2dd..1a9050f 100644 --- a/toid_behaviors/include/toid_behaviors/rolling_average.hpp +++ b/toid_behaviors/include/toid_behaviors/rolling_average.hpp @@ -18,10 +18,10 @@ public: Pose2D push(geometry_msgs::msg::Pose & pose) { - if(size_ > 0) { + if(size_ == 0) { return {}; } - if (size_ == data_count_) { + if (size_-1 == data_count_) { accum_x_ -= poses_[front_idx_].position.x; accum_y_ -= poses_[front_idx_].position.y; accum_theta_ -= tf2::getYaw(poses_[front_idx_].orientation); diff --git a/toid_behaviors/src/approach_acorns.cpp b/toid_behaviors/src/approach_acorns.cpp index 85c0a5d..b3f0f3f 100644 --- a/toid_behaviors/src/approach_acorns.cpp +++ b/toid_behaviors/src/approach_acorns.cpp @@ -111,21 +111,6 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg) tf2::convert(tf2::Quaternion::createFromRPY(0,0,yaw ), pose_global.pose.orientation); - if(debug_marker_) { - visualization_msgs::msg::Marker marker; - marker.lifetime.sec = 1.0; - marker.header = pose_global.header; - marker.pose = pose_global.pose; - marker.type = visualization_msgs::msg::Marker::ARROW; - marker.scale.x = 0.02; - marker.scale.y = 0.02; - marker.scale.z = 0.02; - marker.color.a = 1.0; - marker.color.r = 1.0; - marker.color.g = 0; - marker.color.b = 0; - target_pose_pub_->publish(marker); - } yaw = angles::normalize_angle(yaw); @@ -136,6 +121,23 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg) new_target_pose_.position.y = b; tf2::convert(tf2::Quaternion::createFromRPY(0,0, c), new_target_pose_.orientation); new_target_angle_ = c; + + if(debug_marker_) { + visualization_msgs::msg::Marker marker; + marker.lifetime.sec = 1.0; + marker.header = pose_global.header; + marker.pose = new_target_pose_; + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.scale.x = 0.02; + marker.scale.y = 0.02; + marker.scale.z = 0.02; + marker.color.a = 1.0; + marker.color.r = 1.0; + marker.color.g = 0; + marker.color.b = 0; + target_pose_pub_->publish(marker); + } + } ResultStatus ApproachAcorns::onStart( -- 2.49.1 From 773411951d22cc1297fd4d88c4f5ed0695254f66 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Mon, 13 Apr 2026 18:33:16 +0200 Subject: [PATCH 55/86] approach acorn modifications --- toid_behaviors/src/approach_acorns.cpp | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/toid_behaviors/src/approach_acorns.cpp b/toid_behaviors/src/approach_acorns.cpp index b3f0f3f..e1c15b3 100644 --- a/toid_behaviors/src/approach_acorns.cpp +++ b/toid_behaviors/src/approach_acorns.cpp @@ -96,7 +96,6 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg) return; } - double yaw = angles::normalize_angle(tf2::getYaw(pose_global.pose.orientation)); if(tf2::getYaw(pose_local.pose.orientation) > 0) { @@ -110,8 +109,6 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg) pose_global.pose.position.y += std::sin(yaw) * -0.35 + std::cos(yaw) * + 0.005; tf2::convert(tf2::Quaternion::createFromRPY(0,0,yaw ), pose_global.pose.orientation); - - yaw = angles::normalize_angle(yaw); std::lock_guard _lock(mutex_); @@ -247,15 +244,15 @@ ResultStatus ApproachAcorns::updateVel( double dist_left = distanceToTarget(pose, target_pose_.position, target_angle_, backwards_); - if (dist_left <= 0.001) { + const double current_yaw = tf2::getYaw(pose.orientation); + double angle_dist = angles::shortest_angular_distance(current_yaw, target_angle_); + + if (dist_left <= 0.001 && std::fabs(dist_left) <= 0.005) { out_vel.linear.x = 0; out_vel.angular.z = 0; return ResultStatus{Status::SUCCEEDED}; } - 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( -- 2.49.1 From 1558ca1217ad21709b8e656344cafa2423a4488b Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Wed, 15 Apr 2026 11:17:36 +0200 Subject: [PATCH 56/86] Added rotate acorn and misc changes to approach acorns --- toid_behaviors/CMakeLists.txt | 1 + .../include/toid_behaviors/rotate_acorns.hpp | 80 ++++++ toid_behaviors/src/approach_acorns.cpp | 45 ++-- toid_behaviors/src/rotate_acorns.cpp | 229 ++++++++++++++++++ toid_behaviors/toid_behaviors.xml | 3 + .../params/toid_general_params.yaml | 4 + 6 files changed, 344 insertions(+), 18 deletions(-) create mode 100644 toid_behaviors/include/toid_behaviors/rotate_acorns.hpp create mode 100644 toid_behaviors/src/rotate_acorns.cpp diff --git a/toid_behaviors/CMakeLists.txt b/toid_behaviors/CMakeLists.txt index 2498146..9426654 100644 --- a/toid_behaviors/CMakeLists.txt +++ b/toid_behaviors/CMakeLists.txt @@ -33,6 +33,7 @@ set( src/simple_rotate.cpp src/simple_translate_x.cpp src/approach_acorns.cpp + src/rotate_acorns.cpp ) find_package(ament_cmake REQUIRED) diff --git a/toid_behaviors/include/toid_behaviors/rotate_acorns.hpp b/toid_behaviors/include/toid_behaviors/rotate_acorns.hpp new file mode 100644 index 0000000..411fc06 --- /dev/null +++ b/toid_behaviors/include/toid_behaviors/rotate_acorns.hpp @@ -0,0 +1,80 @@ +#pragma once + +#include "geometry_msgs/msg/pose.hpp" +#include "toid_behaviors/scl.hpp" +#include "toid_behaviors/simple_move.hpp" +#include "toid_behaviors/rolling_average.hpp" +#include "toid_msgs/action/simple_move_coords.hpp" +#include "visualization_msgs/msg/marker.hpp" + +namespace toid +{ +using RotateAction = toid_msgs::action::SimpleRotate; +using PoseStamped = geometry_msgs::msg::PoseStamped; +using namespace nav2_behaviors; + +class RotateAcorns : public SimpleMove +{ +public: + RotateAcorns(); + ~RotateAcorns(); + + void configureCB() override; + + void activateCB() override; + void deactivateCB() override; + + ResultStatus onStart( + const std::shared_ptr command, const geometry_msgs::msg::Pose & pose, + const geometry_msgs::msg::Twist & vel) override; + + bool collisionDetection( + const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Pose & last_ok_pose); + + void acorn_position_cb(PoseStamped::ConstSharedPtr msg); + + 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::LOCAL; + } + + void calc_err_and_sign( + double last_angle, double current_yaw, double & min_turn_angle, double & err, double & sign); + + double check_space(const geometry_msgs::msg::Pose pose, const double e, const double sign); + + double calc_speed(const double err, const double sign, const double angular_speed); + +protected: + SmoothControlLaw scl_; + rclcpp::Subscription::SharedPtr acorn_pose_sub_; + double new_target_angle_; + std::shared_mutex mutex_; + RollingAverage avg_ = RollingAverage(10); + + + //Goal + double target_angle_; + double min_turn_angle_; + double initial_direction_; + unsigned char mode_; + + //State + double angular_speed_; + double last_angle_; + double last_time_; + double distance_ = 1.0; + + //Config + double max_angular_speed_; + double min_angular_speed_; + double max_angular_accel_; + double max_angular_decel_; + double lookahead_; +}; + +} // namespace toid diff --git a/toid_behaviors/src/approach_acorns.cpp b/toid_behaviors/src/approach_acorns.cpp index e1c15b3..270cb9f 100644 --- a/toid_behaviors/src/approach_acorns.cpp +++ b/toid_behaviors/src/approach_acorns.cpp @@ -73,9 +73,10 @@ void ApproachAcorns::activateCB() distance_ = 1.0; } -void ApproachAcorns::deactivateCB() { - acorn_pose_sub_.reset(); - target_pose_pub_->on_deactivate(); +void ApproachAcorns::deactivateCB() +{ + acorn_pose_sub_.reset(); + target_pose_pub_->on_deactivate(); } void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg) @@ -96,30 +97,34 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg) return; } + // Smooth out approach + if (x * x + y * y < 0.42) { + return; + } + double yaw = angles::normalize_angle(tf2::getYaw(pose_global.pose.orientation)); - if(tf2::getYaw(pose_local.pose.orientation) > 0) { + if (tf2::getYaw(pose_local.pose.orientation) > 0) { yaw += M_PI; } - yaw += M_PI/2; + yaw += M_PI / 2; - - pose_global.pose.position.x += std::cos(yaw) * -0.35 - std::sin(yaw) * + 0.005; - pose_global.pose.position.y += std::sin(yaw) * -0.35 + std::cos(yaw) * + 0.005; - tf2::convert(tf2::Quaternion::createFromRPY(0,0,yaw ), pose_global.pose.orientation); + pose_global.pose.position.x += std::cos(yaw) * -0.35 - std::sin(yaw) * -0.005; + pose_global.pose.position.y += std::sin(yaw) * -0.35 + std::cos(yaw) * -0.005; + tf2::convert(tf2::Quaternion::createFromRPY(0, 0, yaw), pose_global.pose.orientation); yaw = angles::normalize_angle(yaw); std::lock_guard _lock(mutex_); - auto[a,b,c] = avg_.push(pose_global.pose); + auto [a, b, c] = avg_.push(pose_global.pose); distance_ = x * x + y * y; - new_target_pose_.position.x = a; - new_target_pose_.position.y = b; - tf2::convert(tf2::Quaternion::createFromRPY(0,0, c), new_target_pose_.orientation); + new_target_pose_.position.x = a; + new_target_pose_.position.y = b; + tf2::convert(tf2::Quaternion::createFromRPY(0, 0, c), new_target_pose_.orientation); new_target_angle_ = c; - if(debug_marker_) { + if (debug_marker_) { visualization_msgs::msg::Marker marker; marker.lifetime.sec = 1.0; marker.header = pose_global.header; @@ -134,14 +139,12 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg) marker.color.b = 0; target_pose_pub_->publish(marker); } - } ResultStatus ApproachAcorns::onStart( const std::shared_ptr command, const geometry_msgs::msg::Pose &, const geometry_msgs::msg::Twist & vel) { - std::lock_guard _lock(mutex_); distance_ = 1.0; new_target_pose_.position.x = command->x; @@ -235,7 +238,6 @@ ResultStatus ApproachAcorns::updateVel( const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist &, geometry_msgs::msg::Twist & out_vel) { - { std::lock_guard _lock(mutex_); target_pose_ = new_target_pose_; @@ -247,7 +249,7 @@ ResultStatus ApproachAcorns::updateVel( const double current_yaw = tf2::getYaw(pose.orientation); double angle_dist = angles::shortest_angular_distance(current_yaw, target_angle_); - if (dist_left <= 0.001 && std::fabs(dist_left) <= 0.005) { + if (dist_left <= 0.001 && std::fabs(angle_dist) <= 0.030) { out_vel.linear.x = 0; out_vel.angular.z = 0; return ResultStatus{Status::SUCCEEDED}; @@ -279,6 +281,13 @@ ResultStatus ApproachAcorns::updateVel( last_speed_ = out_vel.linear.x; return ResultStatus{Status::RUNNING}; } + + if (dist_left <= 0.001) { + out_vel.linear.x = 0; + 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_ * velocityTarget(dist_left); scl_.calculate_vel(target_pose_, pose, out_vel, backwards_); diff --git a/toid_behaviors/src/rotate_acorns.cpp b/toid_behaviors/src/rotate_acorns.cpp new file mode 100644 index 0000000..ffbf905 --- /dev/null +++ b/toid_behaviors/src/rotate_acorns.cpp @@ -0,0 +1,229 @@ +#include "toid_behaviors/rotate_acorns.hpp" + +#include + +#include "angles/angles.h" +#include "tf2/convert.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace toid +{ + +RotateAcorns::RotateAcorns() : SimpleMove() {} +RotateAcorns::~RotateAcorns() {} + +void RotateAcorns::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_); + + 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_); +} + + +void RotateAcorns::activateCB() +{ + auto node = node_.lock(); + using namespace std::placeholders; + acorn_pose_sub_ = node->create_subscription( + "closest_acorn", rclcpp::QoS(1), std::bind(&RotateAcorns::acorn_position_cb, this, _1)); + distance_ = 1.0; +} + +void RotateAcorns::deactivateCB() { + acorn_pose_sub_.reset(); +} + +void RotateAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg) +{ + geometry_msgs::msg::PoseStamped pose_local; + geometry_msgs::msg::PoseStamped pose_global; + try { + pose_local = tf_->transform(*msg, robot_base_frame_); + pose_global = tf_->transform(*msg, global_frame_); + } catch (const tf2::TransformException & e) { + RCLCPP_ERROR_ONCE(logger_, "Transform timeout [showing once]..."); + return; + } + + double x = pose_local.pose.position.x; + double y = pose_local.pose.position.y; + if (x * x + y * y > distance_ + 0.005) { + return; + } + + double yaw = angles::normalize_angle(tf2::getYaw(pose_global.pose.orientation)); + + if(tf2::getYaw(pose_local.pose.orientation) > 0) { + yaw += M_PI; + } + + yaw += M_PI/2; + + + pose_global.pose.position.x += std::cos(yaw) * 0.0 - std::sin(yaw) * + 0.005; + pose_global.pose.position.y += std::sin(yaw) * 0.0 + std::cos(yaw) * + 0.005; + tf2::convert(tf2::Quaternion::createFromRPY(0,0,yaw ), pose_global.pose.orientation); + + yaw = angles::normalize_angle(yaw); + + std::lock_guard _lock(mutex_); + auto[a,b,c] = avg_.push(pose_global.pose); + distance_ = x * x + y * y; + new_target_angle_ = c; + +} + +ResultStatus RotateAcorns::onStart( + const std::shared_ptr command, const geometry_msgs::msg::Pose & pose, + const geometry_msgs::msg::Twist & vel) +{ + std::lock_guard _lock(mutex_); + target_angle_ = new_target_angle_; + 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; + avg_.reset(); + + 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; + last_time_ = clock_->now().seconds(); + return ResultStatus{Status::SUCCEEDED}; +} + +void RotateAcorns::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 RotateAcorns::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 RotateAcorns::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 min_turn_angle = min_turn_angle_; + double angular_speed = angular_speed_; + double err, sign; + + calc_err_and_sign(last_angle_, current_yaw, min_turn_angle, err, sign); + + if (!(mode_ & RotateAction::Goal::IGNORE_OBSTACLES)) { + err = check_space(pose, err, sign); + } + + double speed = 0.0; + + 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 RotateAcorns::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" +PLUGINLIB_EXPORT_CLASS(toid::RotateAcorns, nav2_core::Behavior); \ No newline at end of file diff --git a/toid_behaviors/toid_behaviors.xml b/toid_behaviors/toid_behaviors.xml index 2556c3f..d64afa7 100644 --- a/toid_behaviors/toid_behaviors.xml +++ b/toid_behaviors/toid_behaviors.xml @@ -12,5 +12,8 @@ + + + \ No newline at end of file diff --git a/toid_navigation/params/toid_general_params.yaml b/toid_navigation/params/toid_general_params.yaml index 0101127..aa399fc 100644 --- a/toid_navigation/params/toid_general_params.yaml +++ b/toid_navigation/params/toid_general_params.yaml @@ -37,6 +37,10 @@ behavior_server: plugin: "toid::MoveCoords" approachAcorns: plugin: "toid::ApproachAcorns" + kphi: 2.0 + kdelta: 1.0 + lambda: 2.0 + debug_marker: true local_frame: map global_frame: map robot_base_frame: base_footprint -- 2.49.1 From 2619a9b0b513f900211607e9fb2f196e31065a9b Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Wed, 15 Apr 2026 13:23:31 +0200 Subject: [PATCH 57/86] Fix target update distance --- toid_behaviors/src/approach_acorns.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/toid_behaviors/src/approach_acorns.cpp b/toid_behaviors/src/approach_acorns.cpp index 270cb9f..f8ce5e9 100644 --- a/toid_behaviors/src/approach_acorns.cpp +++ b/toid_behaviors/src/approach_acorns.cpp @@ -98,7 +98,7 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg) } // Smooth out approach - if (x * x + y * y < 0.42) { + if (x * x + y * y < 0.42 * 0.42) { return; } @@ -275,7 +275,7 @@ ResultStatus ApproachAcorns::updateVel( return ResultStatus{Status::RUNNING}; } - if (dist_left <= 0.02) { + if (dist_left >= 0.001 && dist_left <= 0.02) { 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; -- 2.49.1 From b683dc0bb435f89cc20ef4c3e5b27a4df3018ca4 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Wed, 15 Apr 2026 13:33:50 +0200 Subject: [PATCH 58/86] Added rotate acorns --- toid_navigation/launch/main.py | 30 ++++++++- .../params/toid_general_params.yaml | 13 ++-- toid_navigation/rviz/nav2.rviz | 64 +++++++++++++++---- 3 files changed, 89 insertions(+), 18 deletions(-) diff --git a/toid_navigation/launch/main.py b/toid_navigation/launch/main.py index 9c02541..92ee94c 100644 --- a/toid_navigation/launch/main.py +++ b/toid_navigation/launch/main.py @@ -1,7 +1,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.conditions import IfCondition, UnlessCondition -from launch.substitutions import LaunchConfiguration +from launch.substitutions import LaunchConfiguration, AllSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare @@ -10,6 +10,7 @@ import os def generate_launch_description(): nav_pkg_share = FindPackageShare("").find('toid_navigation') control_pkg_share = FindPackageShare("").find('toid_control') + lidar_pkg_share = FindPackageShare("").find('toid_lidar') params = os.path.join(nav_pkg_share, 'params', 'toid_general_params.yaml') map = os.path.join(nav_pkg_share, 'maps', 'mapb2_5cm.yaml') ctrl_launch_dir = os.path.join(control_pkg_share, 'launch') @@ -37,6 +38,20 @@ def generate_launch_description(): default_value='True', description="Whether to launch rviz2" ) + + use_lidar = LaunchConfiguration("use_lidar") + use_lidar_arg = DeclareLaunchArgument( + 'use_lidar', + default_value='True', + description="Whether to launch rviz2" + ) + + is_blue = LaunchConfiguration("is_blue") + is_blue_arg = DeclareLaunchArgument( + 'is_blue', + default_value='True', + description="Whether to launch rviz2" + ) toid_control = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -49,6 +64,17 @@ def generate_launch_description(): condition=IfCondition(run_nodes), ) + toid_lidar = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(lidar_pkg_share, 'launch' , 'launch.py') + ), + launch_arguments={ + 'visualize': 'False', + 'lidar_frame': 'lidar_frame', + }.items(), + condition=IfCondition(AllSubstitution(run_nodes, use_lidar)), + ) + map_server = Node( package='nav2_map_server', executable='map_server', @@ -118,6 +144,8 @@ def generate_launch_description(): visualize_arg, use_mock_arg, run_nodes_arg, + use_lidar_arg, + toid_lidar, rviz_node, map_server, bt_navigator, diff --git a/toid_navigation/params/toid_general_params.yaml b/toid_navigation/params/toid_general_params.yaml index aa399fc..d08280b 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", "rotate", "translateX", "moveCoords", "approachAcorns"] + behavior_plugins: ["backup", "spin", "rotate", "translateX", "moveCoords", "approachAcorns", rotateAcorns] spin: plugin: "nav2_behaviors::Spin" backup: @@ -41,6 +41,8 @@ behavior_server: kdelta: 1.0 lambda: 2.0 debug_marker: true + rotateAcorns: + plugin: "toid::RotateAcorns" local_frame: map global_frame: map robot_base_frame: base_footprint @@ -58,7 +60,7 @@ global_costmap: publish_frequency: 1.0 global_frame: map robot_base_frame: base_footprint - footprint: "[[-0.08, 0.15], [0.22, 0.15], [0.22, -0.15], [-0.08, -0.15]]" + footprint: "[[-0.045, 0.15], [0.255, 0.15], [0.255, -0.15], [-0.045, -0.15]]" footprint_padding: 0.02 track_unknown_space: false rolling_window: false @@ -79,7 +81,7 @@ local_costmap: ros__parameters: update_frequency: 5.0 publish_frequency: 2.0 - footprint: "[[-0.08, 0.15], [0.22, 0.15], [0.22, -0.15], [-0.08, -0.15]]" + footprint: "[[-0.045, 0.15], [0.255, 0.15], [0.255, -0.15], [-0.045, -0.15]]" footprint_padding: 0.01 #robot_radius: 0.18 global_frame: map @@ -89,11 +91,14 @@ local_costmap: height: 1 resolution: 0.01 introspection_mode: "disabled" - plugins: ["static_layer", "inflation_layer"] + plugins: ["static_layer", rival_layer, "inflation_layer"] static_layer: plugin: "nav2_costmap_2d::StaticLayer" footprint_clearing_enabled: true map_subscribe_transient_local: True + rival_layer: + plugin: "toid::RivalLayer" + rival_size: 0.15 inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 diff --git a/toid_navigation/rviz/nav2.rviz b/toid_navigation/rviz/nav2.rviz index 5eaf224..981ff64 100644 --- a/toid_navigation/rviz/nav2.rviz +++ b/toid_navigation/rviz/nav2.rviz @@ -1,13 +1,14 @@ Panels: - Class: rviz_common/Displays - Help Height: 78 + Help Height: 138 Name: Displays Property Tree Widget: Expanded: - /RobotModel1 - /GlobalCostMap1/Topic1 + - /Pose1 Splitter Ratio: 0.5 - Tree Height: 592 + Tree Height: 668 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -74,6 +75,10 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + camera: + Alpha: 1 + Show Axes: false + Show Trail: false drivewhl_l_link: Alpha: 1 Show Axes: false @@ -247,6 +252,39 @@ Visualization Manager: Reliability Policy: Reliable Value: /start_point Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Marker + Namespaces: + "": true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /marker + Value: true + - Alpha: 1 + Axes Length: 0.10000000149011612 + Axes Radius: 0.019999999552965164 + Class: rviz_default_plugins/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /closest_acorn + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -293,33 +331,33 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/XYOrbit - Distance: 3.863811731338501 + Distance: 2.2019217014312744 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.1778966784477234 - Y: -1.1747734546661377 - Z: 1.7285346984863281e-06 + X: 0.3222081661224365 + Y: -0.08033189922571182 + Z: 0 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.3697962760925293 + Pitch: 1.064796805381775 Target Frame: Value: XYOrbit (rviz_default_plugins) - Yaw: 4.628584861755371 + Yaw: 4.925206184387207 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 896 + Height: 1186 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000254000002defc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000002de000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002defc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f000002de000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005d30000003efc0100000002fb0000000800540069006d00650100000000000005d30000027b00fffffffb0000000800540069006d0065010000000000000450000000000000000000000264000002de00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000025400000398fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000070000003980000018600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015b00000398fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000070000003980000013800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000070c0000005efc0100000002fb0000000800540069006d006501000000000000070c0000048700fffffffb0000000800540069006d00650100000000000004500000000000000000000003450000039800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -328,6 +366,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1491 - X: 429 - Y: 75 + Width: 1804 + X: 428 + Y: 74 -- 2.49.1 From 81ebd8b7a276800d03a6b4bfc1a65172e7a45c8c Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Wed, 15 Apr 2026 13:55:16 +0200 Subject: [PATCH 59/86] Modified rotate_acorns precision and decel --- toid_behaviors/src/rotate_acorns.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/toid_behaviors/src/rotate_acorns.cpp b/toid_behaviors/src/rotate_acorns.cpp index ffbf905..044d574 100644 --- a/toid_behaviors/src/rotate_acorns.cpp +++ b/toid_behaviors/src/rotate_acorns.cpp @@ -78,8 +78,8 @@ void RotateAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg) yaw += M_PI/2; - pose_global.pose.position.x += std::cos(yaw) * 0.0 - std::sin(yaw) * + 0.005; - pose_global.pose.position.y += std::sin(yaw) * 0.0 + std::cos(yaw) * + 0.005; + pose_global.pose.position.x += std::cos(yaw) * 0.0 - std::sin(yaw) * - 0.005; + pose_global.pose.position.y += std::sin(yaw) * 0.0 + std::cos(yaw) * - 0.005; tf2::convert(tf2::Quaternion::createFromRPY(0,0,yaw ), pose_global.pose.orientation); yaw = angles::normalize_angle(yaw); @@ -143,7 +143,7 @@ double RotateAcorns::calc_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 speed_until_overshoot = std::sqrt(2.0 * max_angular_decel_ * 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_); @@ -171,7 +171,7 @@ ResultStatus RotateAcorns::updateVel( speed = calc_speed(err, sign, angular_speed); } - if (min_turn_angle_ == 0 && std::fabs(current_yaw - target_angle_) < 0.01) { + if (min_turn_angle_ == 0 && std::fabs(current_yaw - target_angle_) < 0.004) { return ResultStatus{Status::SUCCEEDED}; } -- 2.49.1 From 4d4c598ce99f02f021f6974791bf835601f8652e Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Wed, 15 Apr 2026 13:58:36 +0200 Subject: [PATCH 60/86] extended closest_acorn topic queue and set to keep_last --- toid_behaviors/src/approach_acorns.cpp | 2 +- toid_behaviors/src/rotate_acorns.cpp | 4 ++-- toid_vision/src/toid_vision.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/toid_behaviors/src/approach_acorns.cpp b/toid_behaviors/src/approach_acorns.cpp index f8ce5e9..1a8bad1 100644 --- a/toid_behaviors/src/approach_acorns.cpp +++ b/toid_behaviors/src/approach_acorns.cpp @@ -68,7 +68,7 @@ void ApproachAcorns::activateCB() auto node = node_.lock(); using namespace std::placeholders; acorn_pose_sub_ = node->create_subscription( - "closest_acorn", rclcpp::QoS(1), std::bind(&ApproachAcorns::acorn_position_cb, this, _1)); + "closest_acorn", rclcpp::QoS(2), std::bind(&ApproachAcorns::acorn_position_cb, this, _1)); target_pose_pub_->on_activate(); distance_ = 1.0; } diff --git a/toid_behaviors/src/rotate_acorns.cpp b/toid_behaviors/src/rotate_acorns.cpp index 044d574..c6292b5 100644 --- a/toid_behaviors/src/rotate_acorns.cpp +++ b/toid_behaviors/src/rotate_acorns.cpp @@ -29,7 +29,7 @@ void RotateAcorns::configureCB() node->get_parameter(behavior_name_ + ".max_angular_accel", max_angular_accel_); nav2_util::declare_parameter_if_not_declared( - node, behavior_name_ + ".max_angular_decel", rclcpp::ParameterValue(4.0)); + node, behavior_name_ + ".max_angular_decel", rclcpp::ParameterValue(1.0)); node->get_parameter(behavior_name_ + ".max_angular_decel", max_angular_decel_); nav2_util::declare_parameter_if_not_declared( @@ -43,7 +43,7 @@ void RotateAcorns::activateCB() auto node = node_.lock(); using namespace std::placeholders; acorn_pose_sub_ = node->create_subscription( - "closest_acorn", rclcpp::QoS(1), std::bind(&RotateAcorns::acorn_position_cb, this, _1)); + "closest_acorn", rclcpp::QoS(2), std::bind(&RotateAcorns::acorn_position_cb, this, _1)); distance_ = 1.0; } diff --git a/toid_vision/src/toid_vision.cpp b/toid_vision/src/toid_vision.cpp index c59f2fd..4b0dadc 100644 --- a/toid_vision/src/toid_vision.cpp +++ b/toid_vision/src/toid_vision.cpp @@ -37,7 +37,7 @@ NutDetector::NutDetector(const rclcpp::NodeOptions & options) RCLCPP_INFO(this->get_logger(), "NutDetector activated... scanning for DEEZ NUTZ"); pose_pub_ = - this->create_publisher("/closest_acorn", rclcpp::QoS(1)); + this->create_publisher("/closest_acorn", rclcpp::QoS(5)); flip_pub_ = this->create_publisher("/to_flip", rclcpp::QoS(1)); } -- 2.49.1 From 980598718d60d28952d3cbb6065045496db6b1f4 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Wed, 15 Apr 2026 15:32:28 +0200 Subject: [PATCH 61/86] various fixes --- toid_behaviors/src/approach_acorns.cpp | 6 +- toid_behaviors/src/rotate_acorns.cpp | 3 +- .../src/toid_bot_description.urdf | 4 +- toid_bt/behavior_trees/behavior1.xml | 56 +++++++++----- .../behavior_trees/calibration_routines.xml | 77 ++++++++++++------- toid_bt/behavior_trees/toid_behaviors.btproj | 3 + toid_vision/src/toid_vision.cpp | 10 ++- 7 files changed, 107 insertions(+), 52 deletions(-) diff --git a/toid_behaviors/src/approach_acorns.cpp b/toid_behaviors/src/approach_acorns.cpp index 1a8bad1..8965f1b 100644 --- a/toid_behaviors/src/approach_acorns.cpp +++ b/toid_behaviors/src/approach_acorns.cpp @@ -68,7 +68,7 @@ void ApproachAcorns::activateCB() auto node = node_.lock(); using namespace std::placeholders; acorn_pose_sub_ = node->create_subscription( - "closest_acorn", rclcpp::QoS(2), std::bind(&ApproachAcorns::acorn_position_cb, this, _1)); + "closest_acorn", rclcpp::QoS(2).keep_last(2), std::bind(&ApproachAcorns::acorn_position_cb, this, _1)); target_pose_pub_->on_activate(); distance_ = 1.0; } @@ -110,8 +110,8 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg) yaw += M_PI / 2; - pose_global.pose.position.x += std::cos(yaw) * -0.35 - std::sin(yaw) * -0.005; - pose_global.pose.position.y += std::sin(yaw) * -0.35 + std::cos(yaw) * -0.005; + pose_global.pose.position.x += std::cos(yaw) * -0.35 - std::sin(yaw) * 0.010; + pose_global.pose.position.y += std::sin(yaw) * -0.35 + std::cos(yaw) * 0.010; tf2::convert(tf2::Quaternion::createFromRPY(0, 0, yaw), pose_global.pose.orientation); yaw = angles::normalize_angle(yaw); diff --git a/toid_behaviors/src/rotate_acorns.cpp b/toid_behaviors/src/rotate_acorns.cpp index c6292b5..405ca0a 100644 --- a/toid_behaviors/src/rotate_acorns.cpp +++ b/toid_behaviors/src/rotate_acorns.cpp @@ -43,7 +43,7 @@ void RotateAcorns::activateCB() auto node = node_.lock(); using namespace std::placeholders; acorn_pose_sub_ = node->create_subscription( - "closest_acorn", rclcpp::QoS(2), std::bind(&RotateAcorns::acorn_position_cb, this, _1)); + "closest_acorn", rclcpp::QoS(2).keep_last(2), std::bind(&RotateAcorns::acorn_position_cb, this, _1)); distance_ = 1.0; } @@ -101,6 +101,7 @@ ResultStatus RotateAcorns::onStart( initial_direction_ = (command->min_angle >= 0.0) ? 1.0 : -1.0; max_angular_speed_ = command->max_speed; mode_ = command->mode; + distance_ = 1.0; avg_.reset(); if (command->max_speed == 0) { diff --git a/toid_bot_description/src/toid_bot_description.urdf b/toid_bot_description/src/toid_bot_description.urdf index c9ce36a..55efc49 100644 --- a/toid_bot_description/src/toid_bot_description.urdf +++ b/toid_bot_description/src/toid_bot_description.urdf @@ -11,7 +11,7 @@ - + @@ -22,7 +22,7 @@ - + diff --git a/toid_bt/behavior_trees/behavior1.xml b/toid_bt/behavior_trees/behavior1.xml index e2a1bdb..11880ca 100644 --- a/toid_bt/behavior_trees/behavior1.xml +++ b/toid_bt/behavior_trees/behavior1.xml @@ -2,6 +2,9 @@ + + + @@ -12,21 +15,40 @@ - - + + + + - @@ -49,6 +71,11 @@ default="1.000000" type="double"/> + + + @@ -93,15 +120,6 @@ Service name - - - - Action server name - Service name diff --git a/toid_bt/behavior_trees/calibration_routines.xml b/toid_bt/behavior_trees/calibration_routines.xml index 19b8c42..d7c4d79 100644 --- a/toid_bt/behavior_trees/calibration_routines.xml +++ b/toid_bt/behavior_trees/calibration_routines.xml @@ -22,7 +22,7 @@ @@ -36,7 +36,7 @@ @@ -47,30 +47,55 @@ - - - - - - - - + + + + + + + + + + + + + + + + + Service name + + + diff --git a/toid_vision/src/toid_vision.cpp b/toid_vision/src/toid_vision.cpp index 4b0dadc..369df71 100644 --- a/toid_vision/src/toid_vision.cpp +++ b/toid_vision/src/toid_vision.cpp @@ -37,7 +37,7 @@ NutDetector::NutDetector(const rclcpp::NodeOptions & options) RCLCPP_INFO(this->get_logger(), "NutDetector activated... scanning for DEEZ NUTZ"); pose_pub_ = - this->create_publisher("/closest_acorn", rclcpp::QoS(5)); + this->create_publisher("/closest_acorn", rclcpp::QoS(1).keep_last(1)); flip_pub_ = this->create_publisher("/to_flip", rclcpp::QoS(1)); } @@ -80,12 +80,16 @@ void NutDetector::process_image( size_t prev = 0; std::string colors = ""; + size_t first_ok = -1; for (size_t i = 0; i < markerIds_.size(); i++) { const int idx = markers_[i].second; if (markerIds_[idx] != 47 && markerIds_[idx] != 36) { continue; } + if (first_ok == (size_t)-1) { + first_ok = idx; + } RCLCPP_DEBUG( this->get_logger(), "Marker %ld at %lf %lf", i, markerCorners_[idx][0].x, @@ -131,6 +135,10 @@ void NutDetector::process_image( flip_pub_->publish(m); } + if (first_ok == (size_t)-1) { + return; + } + geometry_msgs::msg::PoseStamped pose; get_pose(tvecs[markers_[0].second], rvecs[markers_[0].second], pose.pose); pose.header.frame_id = camera_frame_id_; -- 2.49.1 From b154b5e719aa1bef05d408c9265cabab06c69036 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Wed, 15 Apr 2026 15:39:30 +0200 Subject: [PATCH 62/86] Changed approach acorn to not use goal angle as target --- .../include/toid_behaviors/approach_acorns.hpp | 1 + toid_behaviors/src/approach_acorns.cpp | 13 ++++++++++--- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/toid_behaviors/include/toid_behaviors/approach_acorns.hpp b/toid_behaviors/include/toid_behaviors/approach_acorns.hpp index ff17434..6cc6397 100644 --- a/toid_behaviors/include/toid_behaviors/approach_acorns.hpp +++ b/toid_behaviors/include/toid_behaviors/approach_acorns.hpp @@ -60,6 +60,7 @@ protected: double new_target_angle_; geometry_msgs::msg::Pose target_pose_; + geometry_msgs::msg::Pose initial_pose_; double target_angle_; double target_sign_; bool backwards_; diff --git a/toid_behaviors/src/approach_acorns.cpp b/toid_behaviors/src/approach_acorns.cpp index 8965f1b..b022e02 100644 --- a/toid_behaviors/src/approach_acorns.cpp +++ b/toid_behaviors/src/approach_acorns.cpp @@ -112,11 +112,16 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg) pose_global.pose.position.x += std::cos(yaw) * -0.35 - std::sin(yaw) * 0.010; pose_global.pose.position.y += std::sin(yaw) * -0.35 + std::cos(yaw) * 0.010; - tf2::convert(tf2::Quaternion::createFromRPY(0, 0, yaw), pose_global.pose.orientation); - yaw = angles::normalize_angle(yaw); std::lock_guard _lock(mutex_); + + const double dx = initial_pose_.position.x - pose_global.pose.position.x; + const double dy = initial_pose_.position.y - pose_global.pose.position.y; + double yaw_to_goal = std::atan2(dy,dx); + + tf2::convert(tf2::Quaternion::createFromRPY(0, 0, yaw_to_goal), pose_global.pose.orientation); + yaw = yaw_to_goal; auto [a, b, c] = avg_.push(pose_global.pose); distance_ = x * x + y * y; new_target_pose_.position.x = a; @@ -142,14 +147,16 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg) } ResultStatus ApproachAcorns::onStart( - const std::shared_ptr command, const geometry_msgs::msg::Pose &, + const std::shared_ptr command, const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & vel) { std::lock_guard _lock(mutex_); distance_ = 1.0; new_target_pose_.position.x = command->x; new_target_pose_.position.y = command->y; + backwards_ = command->backwards; + initial_pose_ = pose; new_target_pose_.orientation = tf2::toMsg(tf2::Quaternion::createFromRPY(0, 0, command->theta)); new_target_angle_ = command->theta; -- 2.49.1 From 9abbd93b1abe788e574091663f56db5495be26f9 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Wed, 15 Apr 2026 15:44:53 +0200 Subject: [PATCH 63/86] parameter changes --- toid_navigation/params/toid_general_params.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/toid_navigation/params/toid_general_params.yaml b/toid_navigation/params/toid_general_params.yaml index d08280b..3069468 100644 --- a/toid_navigation/params/toid_general_params.yaml +++ b/toid_navigation/params/toid_general_params.yaml @@ -37,9 +37,9 @@ behavior_server: plugin: "toid::MoveCoords" approachAcorns: plugin: "toid::ApproachAcorns" - kphi: 2.0 - kdelta: 1.0 - lambda: 2.0 + kphi: 1.2 + kdelta: 2.0 + lambda: 3.0 debug_marker: true rotateAcorns: plugin: "toid::RotateAcorns" -- 2.49.1 From ac402a584c880e7a924a1a31a8451b4590da8ba5 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Wed, 15 Apr 2026 16:05:13 +0200 Subject: [PATCH 64/86] fixed target angle --- toid_behaviors/src/approach_acorns.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/toid_behaviors/src/approach_acorns.cpp b/toid_behaviors/src/approach_acorns.cpp index b022e02..3c7cd8b 100644 --- a/toid_behaviors/src/approach_acorns.cpp +++ b/toid_behaviors/src/approach_acorns.cpp @@ -116,8 +116,8 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg) std::lock_guard _lock(mutex_); - const double dx = initial_pose_.position.x - pose_global.pose.position.x; - const double dy = initial_pose_.position.y - pose_global.pose.position.y; + const double dx = pose_global.pose.position.x - initial_pose_.position.x; + const double dy = pose_global.pose.position.y - initial_pose_.position.y; double yaw_to_goal = std::atan2(dy,dx); tf2::convert(tf2::Quaternion::createFromRPY(0, 0, yaw_to_goal), pose_global.pose.orientation); -- 2.49.1 From 0a7d8525a0f2e7a047cab9b508e5d29d308ae0c9 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Wed, 15 Apr 2026 18:03:10 +0200 Subject: [PATCH 65/86] sync changes from pi --- toid_behaviors/src/rotate_acorns.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/toid_behaviors/src/rotate_acorns.cpp b/toid_behaviors/src/rotate_acorns.cpp index 405ca0a..98c3a15 100644 --- a/toid_behaviors/src/rotate_acorns.cpp +++ b/toid_behaviors/src/rotate_acorns.cpp @@ -21,7 +21,7 @@ void RotateAcorns::configureCB() 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, behavior_name_ + ".min_angular_vel", rclcpp::ParameterValue(0.1)); node->get_parameter(behavior_name_ + ".min_angular_vel", min_angular_speed_); nav2_util::declare_parameter_if_not_declared( @@ -29,7 +29,7 @@ void RotateAcorns::configureCB() node->get_parameter(behavior_name_ + ".max_angular_accel", max_angular_accel_); nav2_util::declare_parameter_if_not_declared( - node, behavior_name_ + ".max_angular_decel", rclcpp::ParameterValue(1.0)); + node, behavior_name_ + ".max_angular_decel", rclcpp::ParameterValue(0.5)); node->get_parameter(behavior_name_ + ".max_angular_decel", max_angular_decel_); nav2_util::declare_parameter_if_not_declared( @@ -155,6 +155,10 @@ ResultStatus RotateAcorns::updateVel( const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist &, geometry_msgs::msg::Twist & out_vel) { + { + std::lock_guard _lock(mutex_); + target_angle_ = new_target_angle_; + } const double current_yaw = tf2::getYaw(pose.orientation); double min_turn_angle = min_turn_angle_; double angular_speed = angular_speed_; -- 2.49.1 From d0ffceebe13ac4de7455fbda3c33138b8d2083a1 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Wed, 15 Apr 2026 21:18:49 +0200 Subject: [PATCH 66/86] Behavior tree glue code --- toid_bt/behavior_trees/toid_behaviors.btproj | 62 +++++++++++++------ .../toid_bt/plugins/pull_pin_action.hpp | 48 ++++++++++++++ .../toid_bt/plugins/set_parameter_action.hpp | 50 +++++++++++++++ toid_bt/src/bt_executor.cpp | 13 ++++ .../toid_interaction/interaction_node.py | 4 +- 5 files changed, 156 insertions(+), 21 deletions(-) create mode 100644 toid_bt/include/toid_bt/plugins/pull_pin_action.hpp create mode 100644 toid_bt/include/toid_bt/plugins/set_parameter_action.hpp diff --git a/toid_bt/behavior_trees/toid_behaviors.btproj b/toid_bt/behavior_trees/toid_behaviors.btproj index 1a2994b..adf08cc 100644 --- a/toid_bt/behavior_trees/toid_behaviors.btproj +++ b/toid_bt/behavior_trees/toid_behaviors.btproj @@ -4,59 +4,81 @@ + + + + + + + Action server name + - + - Service name + Service name - + - - - Action server name + + + Action server name + + + + + + Action server name - - - Action server name + + + Action server name - - - Action server name + + + Action server name - Service name + Service name - Service name + Service name - Service name + Service name + + + + + - + - Service name + Service name - - Action server name + + Action server name + + + Action server name - Service name + Service name diff --git a/toid_bt/include/toid_bt/plugins/pull_pin_action.hpp b/toid_bt/include/toid_bt/plugins/pull_pin_action.hpp new file mode 100644 index 0000000..71561a2 --- /dev/null +++ b/toid_bt/include/toid_bt/plugins/pull_pin_action.hpp @@ -0,0 +1,48 @@ +#pragma once + +#include "angles/angles.h" +#include "behaviortree_ros2/bt_action_node.hpp" +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "toid_bt/plugin.hpp" +#include "toid_msgs/action/empty_action.hpp" + +namespace toid +{ + +class PullPinNode : public BT::RosActionNode +{ +public: + PullPinNode( + const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params) + : BT::RosActionNode(name, conf, params) + { + } + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ + //BT::InputPort("options"), + }); + } + + bool setGoal(Goal &) override + { + return true; + } + + void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); } + + BT::NodeStatus onResultReceived(const WrappedResult &) override + { + return BT::NodeStatus::SUCCESS; + } + + BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override + { + RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error)); + return BT::NodeStatus::FAILURE; + } +}; + +} // namespace toid \ No newline at end of file diff --git a/toid_bt/include/toid_bt/plugins/set_parameter_action.hpp b/toid_bt/include/toid_bt/plugins/set_parameter_action.hpp new file mode 100644 index 0000000..9d973ba --- /dev/null +++ b/toid_bt/include/toid_bt/plugins/set_parameter_action.hpp @@ -0,0 +1,50 @@ +#pragma once + +#include "angles/angles.h" +#include "behaviortree_ros2/bt_service_node.hpp" +#include "std_srvs/srv/empty.hpp" +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "toid_bt/plugin.hpp" +#include "rcl_interfaces/srv/set_parameters.hpp" + +namespace toid +{ + +class SetParameterNode : public BT::RosServiceNode +{ +public: + SetParameterNode( + const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params) + : BT::RosServiceNode(name, conf, params) + { + } + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ + BT::InputPort("node"), + BT::InputPort("parameter"), + BT::InputPort("value") + }); + } + + bool setRequest(typename Request::SharedPtr & request) override { + std::string parameter = getInput("parameter").value(); + std::string value = getInput("value").value(); + std::string node = getInput("node").value(); + + setServiceName("/" + node + "/set_parameters"); + request->parameters.emplace_back(); + request->parameters.front().name = parameter; + request->parameters.front().value.string_value = value; + return true; +} + + BT::NodeStatus onResponseReceived(const typename Response::SharedPtr &) override + { + return BT::NodeStatus::SUCCESS; + } +}; + +} // namespace \ No newline at end of file diff --git a/toid_bt/src/bt_executor.cpp b/toid_bt/src/bt_executor.cpp index 124e6ef..93e7874 100644 --- a/toid_bt/src/bt_executor.cpp +++ b/toid_bt/src/bt_executor.cpp @@ -8,9 +8,11 @@ #include "toid_bt/plugins/end_calib_action.hpp" #include "toid_bt/plugins/in_position_decorator.hpp" #include "toid_bt/plugins/move_coords_action.hpp" +#include "toid_bt/plugins/pull_pin_action.hpp" #include "toid_bt/plugins/rotate_action.hpp" #include "toid_bt/plugins/rotate_towards_action.hpp" #include "toid_bt/plugins/send_text_action.hpp" +#include "toid_bt/plugins/set_parameter_action.hpp" #include "toid_bt/plugins/stuck_detector_decorator.hpp" #include "toid_bt/plugins/translate_x_action.hpp" @@ -59,9 +61,15 @@ void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory) factory.registerNodeType( "MovePointSimple", BT::RosNodeParams(nh, "/moveCoords"), get_pose); + factory.registerNodeType( + "ApproachAcorns", BT::RosNodeParams(nh, "/approachAcorns"), get_pose); + factory.registerNodeType( "RotateSimple", BT::RosNodeParams(nh, "/rotate"), get_pose); + factory.registerNodeType( + "RotateAcorns", BT::RosNodeParams(nh, "/rotateAcorns"), get_pose); + factory.registerNodeType( "RotateTowards", BT::RosNodeParams(nh, "/rotate"), get_pose); @@ -71,6 +79,11 @@ void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory) factory.registerNodeType( "SetWidth", BT::RosNodeParams(nh, "/set_width"), get_pose); + factory.registerNodeType("WaitPullPin", BT::RosNodeParams(nh, "/start_plug")); + + factory.registerNodeType( + "SetParameter", BT::RosNodeParams(nh, "/compressed_image_subscriber/set_parameters")); + factory.registerNodeType("ZeroOdom", BT::RosNodeParams(nh, "/zero")); factory.registerNodeType("EndCalib", BT::RosNodeParams(nh, "/end_calib")); diff --git a/toid_interaction/toid_interaction/interaction_node.py b/toid_interaction/toid_interaction/interaction_node.py index 842736e..f3c225d 100644 --- a/toid_interaction/toid_interaction/interaction_node.py +++ b/toid_interaction/toid_interaction/interaction_node.py @@ -107,7 +107,9 @@ def main(args=None): rclpy.init(args=args) node = InteracitionNode() - rclpy.executors.MultiThreadedExecutor().spin(node) + executor = rclpy.executors.MultiThreadedExecutor() + executor.add_node(node) + executor.spin() rclpy.shutdown() -- 2.49.1 From 18ef55a204787f8aa08250b010a1b05062ebd601 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Thu, 16 Apr 2026 02:51:43 +0200 Subject: [PATCH 67/86] Hodgepodge of fixes --- toid_lidar/src/toid_lidar_node.cpp | 2 +- toid_navigation/launch/main.py | 24 ++++++++++++++++++- .../params/toid_general_params.yaml | 4 ++-- toid_vision/package.xml | 2 +- 4 files changed, 27 insertions(+), 5 deletions(-) diff --git a/toid_lidar/src/toid_lidar_node.cpp b/toid_lidar/src/toid_lidar_node.cpp index 52062cf..47532d1 100644 --- a/toid_lidar/src/toid_lidar_node.cpp +++ b/toid_lidar/src/toid_lidar_node.cpp @@ -47,7 +47,7 @@ void ToidRivalDetect::process_scan(LaserScan::ConstSharedPtr msg) geometry_msgs::msg::TransformStamped transform; try { - transform = tf_buf_->lookupTransform(global_frame_, msg->header.frame_id, msg->header.stamp); + transform = tf_buf_->lookupTransform(global_frame_, msg->header.frame_id, tf2::TimePointZero); } catch (const tf2::TransformException & e) { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), 1000, "TF Link missing: %s", e.what()); diff --git a/toid_navigation/launch/main.py b/toid_navigation/launch/main.py index 92ee94c..e009910 100644 --- a/toid_navigation/launch/main.py +++ b/toid_navigation/launch/main.py @@ -11,6 +11,7 @@ def generate_launch_description(): nav_pkg_share = FindPackageShare("").find('toid_navigation') control_pkg_share = FindPackageShare("").find('toid_control') lidar_pkg_share = FindPackageShare("").find('toid_lidar') + vision_pkg_share = FindPackageShare("").find('toid_vision') params = os.path.join(nav_pkg_share, 'params', 'toid_general_params.yaml') map = os.path.join(nav_pkg_share, 'maps', 'mapb2_5cm.yaml') ctrl_launch_dir = os.path.join(control_pkg_share, 'launch') @@ -70,11 +71,29 @@ def generate_launch_description(): ), launch_arguments={ 'visualize': 'False', - 'lidar_frame': 'lidar_frame', + 'lidar_frame': 'lidar', }.items(), condition=IfCondition(AllSubstitution(run_nodes, use_lidar)), ) + toid_vision = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(vision_pkg_share, 'launch' , 'launch.py') + ), + launch_arguments={ + 'is_blue': is_blue, + }.items(), + condition=IfCondition(AllSubstitution(run_nodes, use_lidar)), + ) + + toid_interaction = Node( + package='toid_interaction', + executable='node', + name='toid_interaction', + output='screen', + condition=IfCondition(run_nodes), + ) + map_server = Node( package='nav2_map_server', executable='map_server', @@ -145,7 +164,10 @@ def generate_launch_description(): use_mock_arg, run_nodes_arg, use_lidar_arg, + is_blue_arg, toid_lidar, + toid_vision, + toid_interaction, rviz_node, map_server, bt_navigator, diff --git a/toid_navigation/params/toid_general_params.yaml b/toid_navigation/params/toid_general_params.yaml index 3069468..48da697 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", "rotate", "translateX", "moveCoords", "approachAcorns", rotateAcorns] + behavior_plugins: ["backup", "spin", "rotate", "translateX", "moveCoords", "approachAcorns", "rotateAcorns"] spin: plugin: "nav2_behaviors::Spin" backup: @@ -91,7 +91,7 @@ local_costmap: height: 1 resolution: 0.01 introspection_mode: "disabled" - plugins: ["static_layer", rival_layer, "inflation_layer"] + plugins: ["static_layer", "rival_layer", "inflation_layer"] static_layer: plugin: "nav2_costmap_2d::StaticLayer" footprint_clearing_enabled: true diff --git a/toid_vision/package.xml b/toid_vision/package.xml index 4893019..4250143 100644 --- a/toid_vision/package.xml +++ b/toid_vision/package.xml @@ -13,7 +13,7 @@ sensor_msgs message_filters cv_bridge - opencv + libopencv-dev rclcpp_components tf2 -- 2.49.1 From 12a83e876a0d54648477cef19ab4eaea98ed1760 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Thu, 16 Apr 2026 03:14:34 +0200 Subject: [PATCH 68/86] Fixed dockerfile to upgrade packages after updating --- Dockerfile | 3 ++- toid_behaviors/include/toid_behaviors/simple_move.hpp | 8 +++++--- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/Dockerfile b/Dockerfile index d007a35..9df8837 100644 --- a/Dockerfile +++ b/Dockerfile @@ -3,7 +3,7 @@ FROM ros:jazzy-perception ENV DEBIAN_FRONTEND=noninteractive # ---------- System dependencies ---------- -RUN apt-get update && apt-get install -y \ +RUN apt-get update && apt-get upgrade && apt-get install -y \ python3-colcon-common-extensions \ python3-rosdep \ meson cmake \ @@ -47,6 +47,7 @@ COPY toid_bt/package.xml toid_bt/package.xml COPY toid_spinner_controller/package.xml toid_spinner_controller/package.xml COPY toid_behaviors/package.xml toid_behaviors/package.xml COPY toid_navigation/package.xml toid_navigation/package.xml +COPY toid_vision/package.xml toid_vision/package.xml COPY ext/camera_ros/package.xml ext/camera_ros/package.xml COPY ext/BehaviorTree.ROS2/ ext/BehaviorTree.ROS2/ diff --git a/toid_behaviors/include/toid_behaviors/simple_move.hpp b/toid_behaviors/include/toid_behaviors/simple_move.hpp index fefd9f5..135e6bf 100644 --- a/toid_behaviors/include/toid_behaviors/simple_move.hpp +++ b/toid_behaviors/include/toid_behaviors/simple_move.hpp @@ -127,17 +127,18 @@ public: bool check_rival_collision(geometry_msgs::msg::Pose2D & pose) { - if (!rivals_) { + if (!rivals_ || (this->clock_->now() - rivals_->header.stamp ) > rclcpp::Duration::from_seconds(1.0)) { return false; } const double cosp = std::cos(pose.theta); const double sinp = std::sin(pose.theta); + int i = 0; for (const auto & rival : rivals_->point) { geometry_msgs::msg::Point local_rival; const double dx = rival.x - pose.x; const double dy = rival.y - pose.y; - local_rival.x = dx * cosp + dy * sinp; - local_rival.y = -dx * sinp + dy * cosp; + local_rival.x = dx * cosp - dy * sinp; + local_rival.y = dx * sinp + dy * cosp; const double qx = std::abs(local_rival.x) - robot_length_ / 2.0; const double qy = std::abs(local_rival.y) - robot_width_ / 2.0; @@ -148,6 +149,7 @@ public: double length = std::sqrt(mqx * mqx + mqy * mqy); double sdf = length + std::min(std::max(qx, qy), 0.0); + RCLCPP_INFO(this->logger_, "Distance to rival %d: %lf", i++, sdf-rival_radius_); if (sdf < rival_radius_) { return true; } -- 2.49.1 From 733a774c37643a8b23503505ef9d48e501f45c05 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Thu, 16 Apr 2026 04:30:21 +0200 Subject: [PATCH 69/86] Switched to using serial ports by id for control --- .../include/toid_behaviors/simple_move.hpp | 6 +- .../src/toid_bot_control.xacro | 3 +- .../src/toid_bot_description.urdf | 2 +- toid_control/launch/toid.launch.py | 155 ++++++++++-------- 4 files changed, 91 insertions(+), 75 deletions(-) diff --git a/toid_behaviors/include/toid_behaviors/simple_move.hpp b/toid_behaviors/include/toid_behaviors/simple_move.hpp index 135e6bf..673dd39 100644 --- a/toid_behaviors/include/toid_behaviors/simple_move.hpp +++ b/toid_behaviors/include/toid_behaviors/simple_move.hpp @@ -140,6 +140,8 @@ public: local_rival.x = dx * cosp - dy * sinp; local_rival.y = dx * sinp + dy * cosp; + local_rival.x -= 0.105; + const double qx = std::abs(local_rival.x) - robot_length_ / 2.0; const double qy = std::abs(local_rival.y) - robot_width_ / 2.0; @@ -149,7 +151,7 @@ public: double length = std::sqrt(mqx * mqx + mqy * mqy); double sdf = length + std::min(std::max(qx, qy), 0.0); - RCLCPP_INFO(this->logger_, "Distance to rival %d: %lf", i++, sdf-rival_radius_); + RCLCPP_DEBUG(this->logger_, "Distance to rival %d: %lf", i++, sdf-rival_radius_); if (sdf < rival_radius_) { return true; } @@ -168,7 +170,7 @@ protected: double robot_width_ = 0.30; double robot_length_ = 0.30; - double rival_radius_ = 0.30; + double rival_radius_ = 0.22; Rival::SharedPtr rivals_; rclcpp::Subscription::SharedPtr rivals_sub_; diff --git a/toid_bot_description/src/toid_bot_control.xacro b/toid_bot_description/src/toid_bot_control.xacro index 4117934..7ab6048 100644 --- a/toid_bot_description/src/toid_bot_control.xacro +++ b/toid_bot_description/src/toid_bot_control.xacro @@ -1,12 +1,13 @@ - + toid_control/StepperInterface + ${serial_port} diff --git a/toid_bot_description/src/toid_bot_description.urdf b/toid_bot_description/src/toid_bot_description.urdf index 55efc49..7c24d29 100644 --- a/toid_bot_description/src/toid_bot_description.urdf +++ b/toid_bot_description/src/toid_bot_description.urdf @@ -125,6 +125,6 @@ - + \ No newline at end of file diff --git a/toid_control/launch/toid.launch.py b/toid_control/launch/toid.launch.py index 93fa682..b2f52d5 100644 --- a/toid_control/launch/toid.launch.py +++ b/toid_control/launch/toid.launch.py @@ -1,97 +1,106 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument -from launch.conditions import IfCondition, UnlessCondition +from launch.conditions import IfCondition, UnlessCondition from launch.substitutions import Command, LaunchConfiguration, IfElseSubstitution from launch_ros.actions import Node, LifecycleNode from launch_ros.substitutions import FindPackageShare import os + def generate_launch_description(): - pkg_share = FindPackageShare("").find('toid_control') - params = os.path.join(pkg_share, 'params', 'toid_general_params.yaml') - default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'rviz.rviz') + pkg_share = FindPackageShare("").find("toid_control") + params = os.path.join(pkg_share, "params", "toid_general_params.yaml") + default_rviz_config_path = os.path.join(pkg_share, "rviz", "rviz.rviz") - description_pkg_share = FindPackageShare("").find('toid_bot_description') + description_pkg_share = FindPackageShare("").find("toid_bot_description") default_model_path = os.path.join( - description_pkg_share, - 'src', - 'toid_bot_description.urdf' + description_pkg_share, "src", "toid_bot_description.urdf" ) visualize = LaunchConfiguration("visualize") visualize_arg = DeclareLaunchArgument( - 'visualize', - default_value='False', - description="Whether to launch rviz2" + "visualize", default_value="False", description="Whether to launch rviz2" ) use_mock = LaunchConfiguration("use_mock") use_mock_arg = DeclareLaunchArgument( - 'use_mock', - default_value='True', - description="Whether to use mock controller" + "use_mock", default_value="True", description="Whether to use mock controller" ) odom_broadcast = Node( - package='toid_odometry', - executable='toid_odometry', - name='map_to_odom_broadcaster', + package="toid_odometry", + executable="toid_odometry", + name="map_to_odom_broadcaster", emulate_tty=True, - parameters=[{'mock_odom': use_mock}], + parameters=[ + { + "mock_odom": use_mock, + "serial_port": "/dev/serial/by-id/usb-Mg_Robotics_Magrob_Odometry_MCU_E66368254F4F4036-if02", + } + ], ) - robot_state_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - name='robot_state_publisher', - output='screen', + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", emulate_tty=True, - parameters=[{'robot_description': Command(['xacro ', default_model_path, ' use_mock:=', use_mock])}] + parameters=[ + { + "robot_description": Command( + [ + "xacro ", + default_model_path, + " use_mock:=", + use_mock, + " serial_port:=/dev/serial/by-id/usb-Mg_Robotics_Magrob_Odometry_MCU_E66368254F4F4036-if02", + ] + ) + } + ], ) controller_manager = Node( - package='controller_manager', - executable='ros2_control_node', - output='screen', + package="controller_manager", + executable="ros2_control_node", + output="screen", emulate_tty=True, parameters=[params], - arguments=['--ros-args', '--log-level', 'warn'] + arguments=["--ros-args", "--log-level", "warn"], ) joint_state_broadcaster = Node( - package='controller_manager', - executable='spawner', - output='screen', + package="controller_manager", + executable="spawner", + output="screen", emulate_tty=True, - arguments=[ - "joint_state_broadcaster", - '--ros-args', '--log-level', 'warn' - ] + arguments=["joint_state_broadcaster", "--ros-args", "--log-level", "warn"], ) velocity_controller = Node( - package='controller_manager', - executable='spawner', - output='screen', + package="controller_manager", + executable="spawner", + output="screen", emulate_tty=True, arguments=[ "velocity_controller", "--inactive", "-p", params, - '--ros-args', '--log-level', 'warn' - ], + "--ros-args", + "--log-level", + "warn", + ], ) - diffbot_base_controller = Node( - package='controller_manager', - executable='spawner', - output='both', + package="controller_manager", + executable="spawner", + output="both", emulate_tty=True, arguments=[ "diffdrive_controller", @@ -102,39 +111,43 @@ def generate_launch_description(): "--controller-ros-args", "-r diffdrive_controller/odom:=/odom", "--controller-ros-args", - IfElseSubstitution(use_mock, - "--param odom_frame_id:=odom", - "--param odom_frame_id:=odom_expected" + IfElseSubstitution( + use_mock, + "--param odom_frame_id:=odom", + "--param odom_frame_id:=odom_expected", ), "--controller-ros-args", - IfElseSubstitution(use_mock, - "--param enable_odom_tf:=true", - "--param enable_odom_tf:=false" + IfElseSubstitution( + use_mock, + "--param enable_odom_tf:=true", + "--param enable_odom_tf:=false", ), - '--ros-args', '--log-level', 'warn' - ] + "--ros-args", + "--log-level", + "warn", + ], ) rviz_node = Node( - package='rviz2', - executable='rviz2', - name='rviz2', - output='screen', + package="rviz2", + executable="rviz2", + name="rviz2", + output="screen", emulate_tty=True, - arguments=['-d', default_rviz_config_path, - '--ros-args', '--log-level', 'warn' - ], - condition=IfCondition(visualize) + arguments=["-d", default_rviz_config_path, "--ros-args", "--log-level", "warn"], + condition=IfCondition(visualize), ) - return LaunchDescription([ - visualize_arg, - use_mock_arg, - odom_broadcast, - robot_state_publisher, - controller_manager, - joint_state_broadcaster, - diffbot_base_controller, - velocity_controller, - rviz_node - ]) + return LaunchDescription( + [ + visualize_arg, + use_mock_arg, + odom_broadcast, + robot_state_publisher, + controller_manager, + joint_state_broadcaster, + diffbot_base_controller, + velocity_controller, + rviz_node, + ] + ) -- 2.49.1 From 405195b9a105d43b05308c0ea787f04ced2fc932 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Thu, 16 Apr 2026 04:55:28 +0200 Subject: [PATCH 70/86] Fixed toid_bot_control device_path parameter --- toid_bot_description/src/toid_bot_control.xacro | 2 +- toid_bot_description/src/toid_bot_description.urdf | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/toid_bot_description/src/toid_bot_control.xacro b/toid_bot_description/src/toid_bot_control.xacro index 7ab6048..9262876 100644 --- a/toid_bot_description/src/toid_bot_control.xacro +++ b/toid_bot_description/src/toid_bot_control.xacro @@ -7,7 +7,7 @@ toid_control/StepperInterface - ${serial_port} + ${serial_port} diff --git a/toid_bot_description/src/toid_bot_description.urdf b/toid_bot_description/src/toid_bot_description.urdf index 7c24d29..520af05 100644 --- a/toid_bot_description/src/toid_bot_description.urdf +++ b/toid_bot_description/src/toid_bot_description.urdf @@ -2,6 +2,7 @@ + -- 2.49.1 From ae77335ef981078796b2aec096756a2280c57c22 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Thu, 16 Apr 2026 06:36:04 +0200 Subject: [PATCH 71/86] New mechanism code for going in same direction --- .../toid_interaction/interaction_node.py | 16 +- .../mechanism/STservo_sdk/sts.py | 4 + .../toid_interaction/mechanism/motori.py | 83 +++++++++ .../toid_interaction/mechanism/zidovi.py | 167 ++++++++++++++++++ 4 files changed, 262 insertions(+), 8 deletions(-) create mode 100644 toid_interaction/toid_interaction/mechanism/motori.py create mode 100644 toid_interaction/toid_interaction/mechanism/zidovi.py diff --git a/toid_interaction/toid_interaction/interaction_node.py b/toid_interaction/toid_interaction/interaction_node.py index f3c225d..c496f07 100644 --- a/toid_interaction/toid_interaction/interaction_node.py +++ b/toid_interaction/toid_interaction/interaction_node.py @@ -10,6 +10,7 @@ from serial.tools import list_ports from toid_interaction.mechanism.sekvenca_2026 import okreni, okreni_niz from toid_interaction.mechanism.zidovi_load import ZidoviAction +from toid_interaction.mechanism.zidovi import Zidovi from toid_interaction.mechanism.zupcanik import ZupcanikAction from toid_msgs.action import EmptyAction from toid_msgs.srv import SendString @@ -69,14 +70,14 @@ class InteracitionNode(Node): def sequence2_cb(self, request: SendString.Request, response: SendString.Response): if self.step != 1: return Empty.Response() - zidovi = ZidoviAction(self.st_motor_device_name) - zidovi.beli_zid(1) - zidovi.plavi_zid(1) + + zidovi = Zidovi(self.st_motor_device_name) + + zidovi.zidovi(0, 1500, 600, 300, 100, 120) okreni_niz(request.text) - zidovi.plavi_zid(0, TargetPos=150) - zidovi.beli_zid(0, TargetPos=450) + zidovi.zidovi(0, 1500, 450, 150, 100, 120) self.step = 2 return response @@ -85,11 +86,10 @@ class InteracitionNode(Node): return Empty.Response() zupcanik = ZupcanikAction(self.st_motor_device_name) - zidovi = ZidoviAction(self.st_motor_device_name) + zidovi = Zidovi(self.st_motor_device_name) zupcanik.zupcanik(1, 1010, 25) - zidovi.plavi_zid(0, TargetPos=150) - zidovi.beli_zid(0, TargetPos=150) + zidovi.zidovi(0, 1500, 150, 150, 100, 120) okreni(5) self.step = 0 return response diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/sts.py b/toid_interaction/toid_interaction/mechanism/STservo_sdk/sts.py index 3123996..4941c79 100644 --- a/toid_interaction/toid_interaction/mechanism/STservo_sdk/sts.py +++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/sts.py @@ -75,6 +75,10 @@ class sts(protocol_packet_handler): def ReadLoad(self, sts_id): sts_present_load, sts_comm_result,sts_error = self.read2ByteTxRx(sts_id, STS_PRESENT_LOAD_L) # 60 return self.sts_tohost(sts_present_load, 15), sts_comm_result, sts_error + + def ReadCurrent(self, sts_id): + sts_present_current, sts_comm_result,sts_error = self.read2ByteTxRx(sts_id, STS_PRESENT_CURRENT_L) # 69 + return self.sts_tohost(sts_present_current, 15), sts_comm_result, sts_error def ReadMaxLoad(self, sts_id): sts_max_load, sts_comm_result,sts_error = self.read2ByteTxRx(sts_id, STS_LOAD_LIMIT) # 0x36 diff --git a/toid_interaction/toid_interaction/mechanism/motori.py b/toid_interaction/toid_interaction/mechanism/motori.py new file mode 100644 index 0000000..2e3f05c --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/motori.py @@ -0,0 +1,83 @@ +from .STservo_sdk.stservo_def import COMM_SUCCESS +from .STservo_sdk import sts + +STS_MOVING_ACC = 0 + +def get_load(packetHandler : sts, id_motora: int) -> float: + sts_present_load, _, _ = packetHandler.ReadLoad(id_motora) + return (sts_present_load & ((1 << 10) - 1)) * 0.1 + +def get_current(packetHandler : sts, id_motora: int) -> float: + sts_present_current, _, _ = packetHandler.ReadCurrent(id_motora) + return (sts_present_current * 6.5) + +def stop_motor(packetHandler : sts, id_motora: int): + packetHandler.WriteSpec(id_motora, 0, 0) + +def print_motor_error(packetHandler : sts, sts_com_result, sts_error, id_motora): + if sts_com_result != COMM_SUCCESS: + print("[ID:%d] %s" % (id_motora, packetHandler.getTxRxResult(sts_com_result))) + if sts_error != 0: + print("[ID:%d] %s" % (id_motora, packetHandler.getRxPacketError(sts_error))) + +def get_motor_pos_speed(packetHandler : sts, id_motora: int) -> tuple[float, float]: + sts_present_position, sts_present_speed, sts_com_result, sts_error = packetHandler.ReadPosSpeed(id_motora) + print_motor_error(packetHandler, sts_com_result, sts_error, id_motora) + + return sts_present_position, sts_present_speed + +def set_motor_speed(packetHandler : sts, id_motora: int, speed: float) -> tuple[float, float]: + sts_com_result, sts_error = packetHandler.WriteSpec(id_motora, speed, STS_MOVING_ACC) + print_motor_error(packetHandler, sts_com_result, sts_error, id_motora) + +def normalize_dpos(curr: float, prev: float) -> float: + if abs(curr - prev) > 3000: + return 4096 - abs(curr - prev) + else: + return abs(curr - prev) + +class CrniMotor: + def __init__(self, packetHandler : sts, id_motora: int): + self.packetHandler = packetHandler + self.id_motora = id_motora + self.targret_reached = False + self.brzina = 0 + + + sts_comm_result, sts_error = packetHandler.WheelMode(id_motora) + if sts_comm_result != COMM_SUCCESS: + print("[ID:%01d] %s" % (id_motora, packetHandler.getTxRxResult(sts_comm_result))) + elif sts_error != 0: + print("[ID:%01d] %s" % (id_motora, packetHandler.getRxPacketError(sts_error))) + + sts_present_position, sts_present_speed = get_motor_pos_speed(packetHandler, id_motora) + self.trenutna_pos = sts_present_position + self.prosla_pos = sts_present_position + self.predjeni_put = 0 + + packetHandler.SetMaxLoad(id_motora, 80) + + + def doCycle(self, target_pos: float, brzina: float) -> bool | None: + if not self.targret_reached: + sts_present_position, sts_present_speed = get_motor_pos_speed(self.packetHandler, self.id_motora) + self.trenutna_pos = sts_present_position + self.predjeni_put += normalize_dpos(self.trenutna_pos, self.prosla_pos) + self.prosla_pos = self.trenutna_pos + + if self.predjeni_put >= target_pos: + self.stop() + self.targret_reached = True + else: + set_motor_speed(self.packetHandler, self.id_motora, brzina) + self.brzina = brzina + + # TODO: if overload stop motor and return false: + + return True + return True + + def stop(self): + stop_motor(self.packetHandler, self.id_motora) + self.brzina = 0 + \ No newline at end of file diff --git a/toid_interaction/toid_interaction/mechanism/zidovi.py b/toid_interaction/toid_interaction/mechanism/zidovi.py new file mode 100644 index 0000000..4ee2140 --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/zidovi.py @@ -0,0 +1,167 @@ +from .STservo_sdk import PortHandler, sts +from .motori import get_load, stop_motor, get_current, CrniMotor +import time + + +BAUDRATE = 1000000 +STS_MOVING_ACC = 0 + + +class Zidovi: + def __init__(self, devicename: str = '/dev/ttyUSB0'): + self.devicename = devicename + self.portHandler = None + self.packetHandler = None + + def _open_port(self): + self.portHandler = PortHandler(self.devicename) + self.packetHandler = sts(self.portHandler) + if self.portHandler.openPort(): + print("Succeeded to open the port") + else: + print("Failed to open the port") + raise RuntimeError(f"Failed to open port: {self.devicename}") + + def _close_port(self): + if self.portHandler is not None: + for motor_id in (3, 5, 2, 4): + stopped = False + while not stopped: + try: + stop_motor(self.packetHandler, motor_id) + stopped = True + except Exception: + self.portHandler.ser.reset_input_buffer() + self.portHandler.ser.reset_output_buffer() + time.sleep(0.01) + self.portHandler.closePort() + + def _stop_all(self): + # Reset serial buffers — a KeyboardInterrupt can fire mid-packet, + # leaving garbage in the buffers that would corrupt subsequent writes. + if self.portHandler is not None and self.portHandler.ser is not None: + self.portHandler.ser.reset_input_buffer() + self.portHandler.ser.reset_output_buffer() + for motor_id in (2, 3, 4, 5): + stopped = False + while not stopped: + try: + stop_motor(self.packetHandler, motor_id) + stopped = True + except Exception: + self.portHandler.ser.reset_input_buffer() + self.portHandler.ser.reset_output_buffer() + time.sleep(0.01) + + def zidovi(self, smer, brzina=1500, + TargetPos_beli=600, TargetPos_plavi=300, + opterecenje_threshold_beli=120, opterecenje_threshold_plavi=150): + TargetPos_beli = TargetPos_beli * 4096 / 360 + TargetPos_plavi = TargetPos_plavi * 4096 / 360 + + if smer == 1: + Speed_ID2 = brzina + Speed_ID4 = -brzina + Speed_ID3 = -brzina + Speed_ID5 = brzina + else: + Speed_ID2 = -brzina + Speed_ID4 = brzina + Speed_ID3 = brzina + Speed_ID5 = -brzina + + self._open_port() + try: + motor2 = CrniMotor(self.packetHandler, 2) + motor3 = CrniMotor(self.packetHandler, 3) + motor4 = CrniMotor(self.packetHandler, 4) + motor5 = CrniMotor(self.packetHandler, 5) + + brojac = 0 + + + load_blue_count = 0 + load_white_count = 0 + + while True: + curr_time = time.time() + + if smer == 1: + motor3.doCycle(TargetPos_beli, Speed_ID3) + motor5.doCycle(TargetPos_beli, Speed_ID5) + if brojac >= 50: + motor2.doCycle(TargetPos_plavi, Speed_ID2) + motor4.doCycle(TargetPos_plavi, Speed_ID4) + else: + brojac += 1 + else: + motor2.doCycle(TargetPos_plavi, Speed_ID2) + motor4.doCycle(TargetPos_plavi, Speed_ID4) + if brojac >= 50: + motor3.doCycle(TargetPos_beli, Speed_ID3) + motor5.doCycle(TargetPos_beli, Speed_ID5) + else: + brojac += 1 + + print("Time elapsed: %.4f seconds" % (time.time() - curr_time)) + + opterecenje2 = get_load(self.packetHandler, 2) + opterecenje4 = get_load(self.packetHandler, 4) + opterecenje3 = get_load(self.packetHandler, 3) + opterecenje5 = get_load(self.packetHandler, 5) + + napon2 = get_current(self.packetHandler, 2) + napon4 = get_current(self.packetHandler, 4) + napon3 = get_current(self.packetHandler, 3) + napon5 = get_current(self.packetHandler, 5) + + print("Current positions:") + print("[ID:002] PresPos:%d Load:%.1f Current:%.1f" % (motor2.prosla_pos, opterecenje2, napon2)) + print("[ID:004] PresPos:%d Load:%.1f Current:%.1f" % (motor4.prosla_pos, opterecenje4, napon4)) + print("[ID:003] PresPos:%d Load:%.1f Current:%.1f" % (motor3.prosla_pos, opterecenje3, napon3)) + print("[ID:005] PresPos:%d Load:%.1f Current:%.1f" % (motor5.prosla_pos, opterecenje5, napon5)) + + if napon2 > opterecenje_threshold_plavi or napon4 > opterecenje_threshold_plavi: + + load_blue_count+=1 + if load_blue_count > 2: + print("High load detected: [ID:002] Load:%.1f [ID:004] Load:%.1f" % (opterecenje2, opterecenje4)) + stop_motor(self.packetHandler, 2) + stop_motor(self.packetHandler, 4) + motor2.targret_reached = True + motor4.targret_reached = True + else: + load_blue_count = max(0, load_white_count - 1) + + if napon3 > opterecenje_threshold_beli or napon5 > opterecenje_threshold_beli: + load_white_count+=1 + if load_white_count > 2: + print("High load detected: [ID:003] Load:%.1f [ID:005] Load:%.1f" % (opterecenje3, opterecenje5)) + stop_motor(self.packetHandler, 3) + stop_motor(self.packetHandler, 5) + motor3.targret_reached = True + motor5.targret_reached = True + else: + load_white_count = max(0, load_white_count - 1) + + print("PredjeniPut ID2: %d deg" % (motor2.predjeni_put * 360 / 4096)) + print("PredjeniPut ID4: %d deg" % (motor4.predjeni_put * 360 / 4096)) + print("PredjeniPut ID3: %d deg" % (motor3.predjeni_put * 360 / 4096)) + print("PredjeniPut ID5: %d deg" % (motor5.predjeni_put * 360 / 4096)) + + targets_reached = ( + motor2.targret_reached + and motor3.targret_reached + and motor4.targret_reached + and motor5.targret_reached + ) + if targets_reached: + print("Svi motori su stigli na cilj!") + break + + return smer, motor2.predjeni_put, motor3.predjeni_put, motor4.predjeni_put, motor5.predjeni_put + except KeyboardInterrupt: + print("\nCtrl+C detected, stopping all motors.") + self._stop_all() + finally: + self._close_port() -- 2.49.1 From c431a7f8d5ccd286e4ae904c7e6ce073c6d19ca9 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Thu, 16 Apr 2026 22:21:47 +0200 Subject: [PATCH 72/86] Changes following pico swap and mechanism changes --- toid_control/launch/toid.launch.py | 4 ++-- .../toid_interaction/interaction_node.py | 12 ++++++------ .../toid_interaction/mechanism/sekvenca_2026.py | 2 +- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/toid_control/launch/toid.launch.py b/toid_control/launch/toid.launch.py index b2f52d5..792383d 100644 --- a/toid_control/launch/toid.launch.py +++ b/toid_control/launch/toid.launch.py @@ -38,7 +38,7 @@ def generate_launch_description(): parameters=[ { "mock_odom": use_mock, - "serial_port": "/dev/serial/by-id/usb-Mg_Robotics_Magrob_Odometry_MCU_E66368254F4F4036-if02", + "serial_port": "/dev/serial/by-id/usb-Mg_Robotics_Magrob_Odometry_MCU_504430456075AF1C-if02", } ], ) @@ -57,7 +57,7 @@ def generate_launch_description(): default_model_path, " use_mock:=", use_mock, - " serial_port:=/dev/serial/by-id/usb-Mg_Robotics_Magrob_Odometry_MCU_E66368254F4F4036-if02", + " serial_port:=/dev/serial/by-id/usb-Mg_Robotics_Magrob_Odometry_MCU_504430456075AF1C-if00", ] ) } diff --git a/toid_interaction/toid_interaction/interaction_node.py b/toid_interaction/toid_interaction/interaction_node.py index c496f07..7ef50d6 100644 --- a/toid_interaction/toid_interaction/interaction_node.py +++ b/toid_interaction/toid_interaction/interaction_node.py @@ -63,21 +63,21 @@ class InteracitionNode(Node): return Empty.Response() okreni(5) zupcanik = ZupcanikAction(self.st_motor_device_name) - zupcanik.zupcanik(1, -1010, 25) + zupcanik.zupcanik(1, -1015, 25) self.step = 1 return response def sequence2_cb(self, request: SendString.Request, response: SendString.Response): if self.step != 1: - return Empty.Response() + return SendString.Response() zidovi = Zidovi(self.st_motor_device_name) - zidovi.zidovi(0, 1500, 600, 300, 100, 120) + zidovi.zidovi(1, 1500, 600, 210, 130, 130) okreni_niz(request.text) - zidovi.zidovi(0, 1500, 450, 150, 100, 120) + zidovi.zidovi(0, 1500, 420, 30, 130, 130) self.step = 2 return response @@ -88,8 +88,8 @@ class InteracitionNode(Node): zupcanik = ZupcanikAction(self.st_motor_device_name) zidovi = Zidovi(self.st_motor_device_name) - zupcanik.zupcanik(1, 1010, 25) - zidovi.zidovi(0, 1500, 150, 150, 100, 120) + zupcanik.zupcanik(1, 1015, 25) + zidovi.zidovi(0, 1500, 180, 180, 130, 130) okreni(5) self.step = 0 return response diff --git a/toid_interaction/toid_interaction/mechanism/sekvenca_2026.py b/toid_interaction/toid_interaction/mechanism/sekvenca_2026.py index 442f370..e813eff 100644 --- a/toid_interaction/toid_interaction/mechanism/sekvenca_2026.py +++ b/toid_interaction/toid_interaction/mechanism/sekvenca_2026.py @@ -8,7 +8,7 @@ import time import serial import serial.tools.list_ports as list_ports -SERIAL_ID = "50443405C8C3B21C" +SERIAL_ID = "259B221729115453" def okreni(i): -- 2.49.1 From e204792cbf642b0d0399aa83718e898f6335812c Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Thu, 16 Apr 2026 22:26:03 +0200 Subject: [PATCH 73/86] Added automatic restart docker compose service --- docker-compose.yaml | 20 +++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/docker-compose.yaml b/docker-compose.yaml index 2ff4b42..d64c7d2 100644 --- a/docker-compose.yaml +++ b/docker-compose.yaml @@ -16,4 +16,22 @@ services: entrypoint: ["sleep","infinity"] profiles: - - base \ No newline at end of file + - base + + toid_forever: + image: localhost:5000/toid + build: . + container_name: toid + + restart: unless-stopped + + privileged: true + network_mode: host + + volumes: + - ./:/ros_ws + - /dev/:/dev/ + - /sys/:/sys/ + - /run/udev/:/run/udev/ + + entrypoint: ["bash", "-c", "ros2 launch toid_navigation main.py use_lidar:=False use_mock:=False"] \ No newline at end of file -- 2.49.1 From e3ea35950897bee0a7fcaae906204d9b542ca685 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Thu, 16 Apr 2026 23:17:15 +0200 Subject: [PATCH 74/86] Sequence fix --- docker-compose.yaml | 2 +- toid_interaction/toid_interaction/interaction_node.py | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/docker-compose.yaml b/docker-compose.yaml index d64c7d2..b2fe803 100644 --- a/docker-compose.yaml +++ b/docker-compose.yaml @@ -34,4 +34,4 @@ services: - /sys/:/sys/ - /run/udev/:/run/udev/ - entrypoint: ["bash", "-c", "ros2 launch toid_navigation main.py use_lidar:=False use_mock:=False"] \ No newline at end of file + entrypoint: ["bash", "-c", "source install/setup.bash && ros2 launch toid_navigation main.py use_lidar:=False use_mock:=False"] \ No newline at end of file diff --git a/toid_interaction/toid_interaction/interaction_node.py b/toid_interaction/toid_interaction/interaction_node.py index 7ef50d6..dc69da4 100644 --- a/toid_interaction/toid_interaction/interaction_node.py +++ b/toid_interaction/toid_interaction/interaction_node.py @@ -73,11 +73,11 @@ class InteracitionNode(Node): zidovi = Zidovi(self.st_motor_device_name) - zidovi.zidovi(1, 1500, 600, 210, 130, 130) + zidovi.zidovi(1, 1500, 600, 210, 120, 120) okreni_niz(request.text) - zidovi.zidovi(0, 1500, 420, 30, 130, 130) + zidovi.zidovi(0, 1500, 420, 50, 120, 120) self.step = 2 return response @@ -89,7 +89,7 @@ class InteracitionNode(Node): zidovi = Zidovi(self.st_motor_device_name) zupcanik.zupcanik(1, 1015, 25) - zidovi.zidovi(0, 1500, 180, 180, 130, 130) + zidovi.zidovi(0, 1500, 180, 160, 120, 120) okreni(5) self.step = 0 return response -- 2.49.1 From 0a0c6da6f081384e1c0eb4beff282254e2aa9687 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Thu, 16 Apr 2026 23:58:32 +0200 Subject: [PATCH 75/86] New method to keep track of servo positions --- .../toid_interaction/interaction_node.py | 33 +++++++++++++++---- .../toid_interaction/mechanism/zidovi.py | 29 ++++++++-------- 2 files changed, 43 insertions(+), 19 deletions(-) diff --git a/toid_interaction/toid_interaction/interaction_node.py b/toid_interaction/toid_interaction/interaction_node.py index dc69da4..036f28d 100644 --- a/toid_interaction/toid_interaction/interaction_node.py +++ b/toid_interaction/toid_interaction/interaction_node.py @@ -25,6 +25,11 @@ class InteracitionNode(Node): output_pin_: OutputDevice start_pin_action_: ActionServer + servo_2_pos: float = 0 + servo_3_pos: float = 0 + servo_4_pos: float = 0 + servo_5_pos: float = 0 + def __init__(self): super().__init__("ToidInteractionNode") @@ -71,13 +76,11 @@ class InteracitionNode(Node): if self.step != 1: return SendString.Response() - zidovi = Zidovi(self.st_motor_device_name) - - zidovi.zidovi(1, 1500, 600, 210, 120, 120) + self.move_wall(600,210) okreni_niz(request.text) - zidovi.zidovi(0, 1500, 420, 50, 120, 120) + self.move_wall(180,160) self.step = 2 return response @@ -86,14 +89,32 @@ class InteracitionNode(Node): return Empty.Response() zupcanik = ZupcanikAction(self.st_motor_device_name) - zidovi = Zidovi(self.st_motor_device_name) zupcanik.zupcanik(1, 1015, 25) - zidovi.zidovi(0, 1500, 180, 160, 120, 120) + + self.move_wall(3,3) + okreni(5) self.step = 0 return response + def move_wall(self, white : float, blue : float, max_current:float = 110): + zidovi = Zidovi(self.st_motor_device_name) + sign = -1 if self.servo_3_pos > white else 1 + targets = ( + abs(self.servo_2_pos - blue), + abs(self.servo_3_pos - white), + abs(self.servo_4_pos - blue), + abs(self.servo_5_pos - white), + ) + + _, dist1, dist2, dist3, dist4 = zidovi.zidovi(max(sign, 0), TargetPos=targets) + + self.servo_2_pos += dist1 * sign + self.servo_3_pos += dist2 * sign + self.servo_4_pos += dist3 * sign + self.servo_5_pos += dist4 * sign + async def start_plug_action_cb(self, goal_handle: ServerGoalHandle): while not self.btn_.is_active: pass diff --git a/toid_interaction/toid_interaction/mechanism/zidovi.py b/toid_interaction/toid_interaction/mechanism/zidovi.py index 4ee2140..cab7695 100644 --- a/toid_interaction/toid_interaction/mechanism/zidovi.py +++ b/toid_interaction/toid_interaction/mechanism/zidovi.py @@ -54,10 +54,13 @@ class Zidovi: time.sleep(0.01) def zidovi(self, smer, brzina=1500, - TargetPos_beli=600, TargetPos_plavi=300, + TargetPos : tuple[float,float,float,float] = (0,0,0,0), opterecenje_threshold_beli=120, opterecenje_threshold_plavi=150): - TargetPos_beli = TargetPos_beli * 4096 / 360 - TargetPos_plavi = TargetPos_plavi * 4096 / 360 + + TargetPos_2 = TargetPos[0] * 4096 / 360 + TargetPos_3 = TargetPos[1] * 4096 / 360 + TargetPos_4 = TargetPos[2] * 4096 / 360 + TargetPos_5 = TargetPos[3] * 4096 / 360 if smer == 1: Speed_ID2 = brzina @@ -87,19 +90,19 @@ class Zidovi: curr_time = time.time() if smer == 1: - motor3.doCycle(TargetPos_beli, Speed_ID3) - motor5.doCycle(TargetPos_beli, Speed_ID5) + motor3.doCycle(TargetPos_3, Speed_ID3) + motor5.doCycle(TargetPos_5, Speed_ID5) if brojac >= 50: - motor2.doCycle(TargetPos_plavi, Speed_ID2) - motor4.doCycle(TargetPos_plavi, Speed_ID4) + motor2.doCycle(TargetPos_2, Speed_ID2) + motor4.doCycle(TargetPos_4, Speed_ID4) else: brojac += 1 else: - motor2.doCycle(TargetPos_plavi, Speed_ID2) - motor4.doCycle(TargetPos_plavi, Speed_ID4) + motor2.doCycle(TargetPos_2, Speed_ID2) + motor4.doCycle(TargetPos_4, Speed_ID4) if brojac >= 50: - motor3.doCycle(TargetPos_beli, Speed_ID3) - motor5.doCycle(TargetPos_beli, Speed_ID5) + motor3.doCycle(TargetPos_3, Speed_ID3) + motor5.doCycle(TargetPos_5, Speed_ID5) else: brojac += 1 @@ -124,7 +127,7 @@ class Zidovi: if napon2 > opterecenje_threshold_plavi or napon4 > opterecenje_threshold_plavi: load_blue_count+=1 - if load_blue_count > 2: + if load_blue_count > 10: print("High load detected: [ID:002] Load:%.1f [ID:004] Load:%.1f" % (opterecenje2, opterecenje4)) stop_motor(self.packetHandler, 2) stop_motor(self.packetHandler, 4) @@ -135,7 +138,7 @@ class Zidovi: if napon3 > opterecenje_threshold_beli or napon5 > opterecenje_threshold_beli: load_white_count+=1 - if load_white_count > 2: + if load_white_count > 10: print("High load detected: [ID:003] Load:%.1f [ID:005] Load:%.1f" % (opterecenje3, opterecenje5)) stop_motor(self.packetHandler, 3) stop_motor(self.packetHandler, 5) -- 2.49.1 From 50ec8e3a8397b72409b3d141fe5124fc2ec57912 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Fri, 17 Apr 2026 01:38:55 +0200 Subject: [PATCH 76/86] added behavior trees --- toid_bt/behavior_trees/behavior1.xml | 2 +- .../behavior_trees/calibration_routines.xml | 2 +- toid_bt/behavior_trees/toid_behaviors.btproj | 61 +++++++++++-------- .../plugins/set_initial_pose_action.hpp | 60 ++++++++++++++++++ toid_bt/src/bt_executor.cpp | 4 ++ 5 files changed, 100 insertions(+), 29 deletions(-) create mode 100644 toid_bt/include/toid_bt/plugins/set_initial_pose_action.hpp diff --git a/toid_bt/behavior_trees/behavior1.xml b/toid_bt/behavior_trees/behavior1.xml index 11880ca..990968a 100644 --- a/toid_bt/behavior_trees/behavior1.xml +++ b/toid_bt/behavior_trees/behavior1.xml @@ -6,7 +6,7 @@ - diff --git a/toid_bt/behavior_trees/calibration_routines.xml b/toid_bt/behavior_trees/calibration_routines.xml index d7c4d79..b2085e1 100644 --- a/toid_bt/behavior_trees/calibration_routines.xml +++ b/toid_bt/behavior_trees/calibration_routines.xml @@ -47,7 +47,7 @@ - + - - - Action server name + + + Action server name - + - Service name + Service name - + - - - Action server name + + + Action server name - - - Action server name + + + Action server name - - - Action server name + + + Action server name - - - Action server name + + + Action server name - Service name + Service name - Service name + Service name - Service name + Service name + + Y position in meters + Heading in degrees + Reference frame + X position in meters + Topic name + @@ -65,20 +72,20 @@ - + - Service name + Service name - - Action server name + + Action server name - Action server name + Action server name - Service name + Service name diff --git a/toid_bt/include/toid_bt/plugins/set_initial_pose_action.hpp b/toid_bt/include/toid_bt/plugins/set_initial_pose_action.hpp new file mode 100644 index 0000000..24618c3 --- /dev/null +++ b/toid_bt/include/toid_bt/plugins/set_initial_pose_action.hpp @@ -0,0 +1,60 @@ +#pragma once + +#include "angles/angles.h" +#include "behaviortree_ros2/bt_topic_pub_node.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "tf2/LinearMath/Quaternion.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace toid +{ + +class SetInitialPoseNode +: public BT::RosTopicPubNode +{ +public: + SetInitialPoseNode( + const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params) + : BT::RosTopicPubNode(name, conf, params) + { + } + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ + BT::InputPort("x", 0.0, "X position in meters"), + BT::InputPort("y", 0.0, "Y position in meters"), + BT::InputPort("theta", 0.0, "Heading in degrees"), + BT::InputPort("frame_id", "map", "Reference frame"), + }); + } + + bool setMessage(geometry_msgs::msg::PoseWithCovarianceStamped & msg) override + { + double x = getInput("x").value_or(0.0); + double y = getInput("y").value_or(0.0); + double theta_deg = getInput("theta").value_or(0.0); + std::string frame_id = getInput("frame_id").value_or("map"); + + msg.header.stamp = node_->get_clock()->now(); + msg.header.frame_id = frame_id; + + msg.pose.pose.position.x = x; + msg.pose.pose.position.y = y; + msg.pose.pose.position.z = 0.0; + + tf2::Quaternion q; + q.setRPY(0.0, 0.0, angles::from_degrees(theta_deg)); + msg.pose.pose.orientation = tf2::toMsg(q); + + // Default covariance: small diagonal for x, y, yaw + msg.pose.covariance = {}; + msg.pose.covariance[0] = 0.25; // x + msg.pose.covariance[7] = 0.25; // y + msg.pose.covariance[35] = 0.068; // yaw + + return true; + } +}; + +} // namespace toid diff --git a/toid_bt/src/bt_executor.cpp b/toid_bt/src/bt_executor.cpp index 93e7874..ff50348 100644 --- a/toid_bt/src/bt_executor.cpp +++ b/toid_bt/src/bt_executor.cpp @@ -14,6 +14,7 @@ #include "toid_bt/plugins/send_text_action.hpp" #include "toid_bt/plugins/set_parameter_action.hpp" #include "toid_bt/plugins/stuck_detector_decorator.hpp" +#include "toid_bt/plugins/set_initial_pose_action.hpp" #include "toid_bt/plugins/translate_x_action.hpp" namespace toid @@ -84,6 +85,9 @@ void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory) factory.registerNodeType( "SetParameter", BT::RosNodeParams(nh, "/compressed_image_subscriber/set_parameters")); + factory.registerNodeType( + "SetInitialPose", BT::RosNodeParams(nh, "/initialpose")); + factory.registerNodeType("ZeroOdom", BT::RosNodeParams(nh, "/zero")); factory.registerNodeType("EndCalib", BT::RosNodeParams(nh, "/end_calib")); -- 2.49.1 From 35081044de1d1862e158d33d1a0db5257cd8d074 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Fri, 17 Apr 2026 05:48:26 +0200 Subject: [PATCH 77/86] Added debug print for robot position --- toid_odometry/src/odometry.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/toid_odometry/src/odometry.cpp b/toid_odometry/src/odometry.cpp index b99d7d8..95b5a50 100644 --- a/toid_odometry/src/odometry.cpp +++ b/toid_odometry/src/odometry.cpp @@ -203,6 +203,7 @@ private: o.child_frame_id = t.child_frame_id; t.transform = make_transform(state.x, state.y, state.theta); + RCLCPP_DEBUG_THROTTLE(this->get_logger(), *this->get_clock(), 500, "Robot position: %lf %lf %lf", state.x, state.y, state.theta); const auto& [x,y,z] = t.transform.translation; o.pose.pose.position.x = x; -- 2.49.1 From a69c0b97e9c056b55631cc441bda862ed5498e3c Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Fri, 17 Apr 2026 07:40:08 +0200 Subject: [PATCH 78/86] Try and fix random disconnects --- toid_control/src/toid_control.cpp | 9 +++++++++ toid_odometry/src/odometry.cpp | 33 +++++++++++++++++++++++++++++++ 2 files changed, 42 insertions(+) diff --git a/toid_control/src/toid_control.cpp b/toid_control/src/toid_control.cpp index af6f261..5534873 100644 --- a/toid_control/src/toid_control.cpp +++ b/toid_control/src/toid_control.cpp @@ -82,6 +82,15 @@ hardware_interface::return_type toid::StepperInterface::write(const rclcpp::Time boost::system::error_code ec; ec = serial_port_.close(ec); ec = serial_port_.open(serial_port_name_, ec); + if(!ec.failed()) { + try { + for(int i = 0; i < 64; i++) { + asio::write(serial_port_, asio::buffer("\x00")); + } + } catch(const std::runtime_error& err) { + + } + } } return hardware_interface::return_type::OK; } diff --git a/toid_odometry/src/odometry.cpp b/toid_odometry/src/odometry.cpp index 95b5a50..891109e 100644 --- a/toid_odometry/src/odometry.cpp +++ b/toid_odometry/src/odometry.cpp @@ -139,6 +139,23 @@ private: tf_static_broadcaster_->sendTransform(t); } + void serial_ec_recovery(boost::system::error_code ec) { + if(ec.failed()) { + boost::system::error_code ec; + ec = this->serial_.close(ec); + ec = this->serial_.open(serial_port_path_, ec); + if(!ec.failed()) { + try { + for(int i = 0; i < 64; i++) { + asio::write(this->serial_, asio::buffer("\x00")); + } + } catch(const std::runtime_error& err) { + + } + } + } + } + void set_width(const std::shared_ptr req) { boost::system::error_code ec; const std::array cmd{TMSG_SET_WIDTH, TMSG_DELIM}; @@ -146,6 +163,7 @@ private: msg.crc = crcFooter(msg); asio::write(this->serial_, asio::buffer(cmd), ec); asio::write(this->serial_, asio::buffer(&msg, sizeof(msg)), ec); + serial_ec_recovery(ec); } void set_ratio(const std::shared_ptr req) { @@ -155,6 +173,7 @@ private: msg.crc = crcFooter(msg); asio::write(this->serial_, asio::buffer(cmd), ec); asio::write(this->serial_, asio::buffer(&msg, sizeof(msg)), ec); + serial_ec_recovery(ec); } void end_calib(const std::shared_ptr) { @@ -166,12 +185,15 @@ private: if(!ec.failed()) { RCLCPP_INFO(this->get_logger(), "Calculated ratio: %lf, %lf", resp.left_gain, resp.right_gain); } + serial_ec_recovery(ec); + } void zero(const std::shared_ptr) { boost::system::error_code ec; const std::array cmd{TMSG_ZERO, TMSG_DELIM}; asio::write(this->serial_, asio::buffer(cmd), ec); + serial_ec_recovery(ec); } void publish_robot_state() { @@ -245,6 +267,17 @@ private: ec.what().c_str() ); } + + if(!ec.failed()) { + try { + for(int i = 0; i < 64; i++) { + asio::write(serial_, asio::buffer("\x00")); + } + } catch(const std::runtime_error& err) { + + } + } + return false; } return state.crc == crcFooter(state); -- 2.49.1 From d09294c1a50c1550211ee51cc9aacc791de9a32b Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Fri, 17 Apr 2026 08:04:03 +0200 Subject: [PATCH 79/86] New pico --- toid_control/launch/toid.launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/toid_control/launch/toid.launch.py b/toid_control/launch/toid.launch.py index 792383d..567408f 100644 --- a/toid_control/launch/toid.launch.py +++ b/toid_control/launch/toid.launch.py @@ -38,7 +38,7 @@ def generate_launch_description(): parameters=[ { "mock_odom": use_mock, - "serial_port": "/dev/serial/by-id/usb-Mg_Robotics_Magrob_Odometry_MCU_504430456075AF1C-if02", + "serial_port": "/dev/serial/by-id/usb-Mg_Robotics_Magrob_Odometry_MCU_E66368254F4F4036-if02", } ], ) @@ -57,7 +57,7 @@ def generate_launch_description(): default_model_path, " use_mock:=", use_mock, - " serial_port:=/dev/serial/by-id/usb-Mg_Robotics_Magrob_Odometry_MCU_504430456075AF1C-if00", + " serial_port:=/dev/serial/by-id/usb-Mg_Robotics_Magrob_Odometry_MCU_E66368254F4F4036-if00", ] ) } -- 2.49.1 From d26adb576a36ad752fed62a6b52ab920958a693d Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Fri, 17 Apr 2026 11:22:07 +0200 Subject: [PATCH 80/86] Final changes for homologation --- .../src/toid_bot_description.urdf | 2 +- toid_bt/behavior_trees/behavior1.xml | 60 +++++++++---------- .../behavior_trees/calibration_routines.xml | 23 +++---- toid_bt/behavior_trees/toid_behaviors.btproj | 10 ++-- .../params/toid_general_params.yaml | 3 +- 5 files changed, 49 insertions(+), 49 deletions(-) diff --git a/toid_bot_description/src/toid_bot_description.urdf b/toid_bot_description/src/toid_bot_description.urdf index 520af05..e47500b 100644 --- a/toid_bot_description/src/toid_bot_description.urdf +++ b/toid_bot_description/src/toid_bot_description.urdf @@ -73,7 +73,7 @@ - + diff --git a/toid_bt/behavior_trees/behavior1.xml b/toid_bt/behavior_trees/behavior1.xml index 990968a..868ff06 100644 --- a/toid_bt/behavior_trees/behavior1.xml +++ b/toid_bt/behavior_trees/behavior1.xml @@ -15,39 +15,35 @@ - + + + + + + + - - - - - + @@ -126,4 +122,4 @@ - + \ No newline at end of file diff --git a/toid_bt/behavior_trees/calibration_routines.xml b/toid_bt/behavior_trees/calibration_routines.xml index b2085e1..81becab 100644 --- a/toid_bt/behavior_trees/calibration_routines.xml +++ b/toid_bt/behavior_trees/calibration_routines.xml @@ -4,6 +4,7 @@ + + - + @@ -113,6 +115,7 @@ + @@ -195,4 +198,4 @@ - + \ No newline at end of file diff --git a/toid_bt/behavior_trees/toid_behaviors.btproj b/toid_bt/behavior_trees/toid_behaviors.btproj index cd4d140..78b09b6 100644 --- a/toid_bt/behavior_trees/toid_behaviors.btproj +++ b/toid_bt/behavior_trees/toid_behaviors.btproj @@ -59,11 +59,11 @@ Service name - Y position in meters - Heading in degrees - Reference frame - X position in meters - Topic name + Y position in meters + Heading in degrees + Reference frame + X position in meters + Topic name diff --git a/toid_navigation/params/toid_general_params.yaml b/toid_navigation/params/toid_general_params.yaml index 48da697..8c32a56 100644 --- a/toid_navigation/params/toid_general_params.yaml +++ b/toid_navigation/params/toid_general_params.yaml @@ -29,12 +29,13 @@ behavior_server: plugin: "nav2_behaviors::BackUp" rotate: plugin: "toid::SimpleRotateBehavior" - max_angular_accel: 4.0 + max_angular_accel: 3.0 max_angular_decel: 1.0 translateX: plugin: "toid::SimpleTranslateXBehavior" moveCoords: plugin: "toid::MoveCoords" + max_vel_accel: 1.0 approachAcorns: plugin: "toid::ApproachAcorns" kphi: 1.2 -- 2.49.1 From 6ff7af65eae8c51002ad800c31048a6e725798b1 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Fri, 17 Apr 2026 11:39:29 +0200 Subject: [PATCH 81/86] Reverted mechanism code to ol'reliable --- .../toid_interaction/interaction_node.py | 33 ++++--------------- .../toid_interaction/mechanism/zidovi.py | 29 ++++++++-------- 2 files changed, 19 insertions(+), 43 deletions(-) diff --git a/toid_interaction/toid_interaction/interaction_node.py b/toid_interaction/toid_interaction/interaction_node.py index 036f28d..dc69da4 100644 --- a/toid_interaction/toid_interaction/interaction_node.py +++ b/toid_interaction/toid_interaction/interaction_node.py @@ -25,11 +25,6 @@ class InteracitionNode(Node): output_pin_: OutputDevice start_pin_action_: ActionServer - servo_2_pos: float = 0 - servo_3_pos: float = 0 - servo_4_pos: float = 0 - servo_5_pos: float = 0 - def __init__(self): super().__init__("ToidInteractionNode") @@ -76,11 +71,13 @@ class InteracitionNode(Node): if self.step != 1: return SendString.Response() - self.move_wall(600,210) + zidovi = Zidovi(self.st_motor_device_name) + + zidovi.zidovi(1, 1500, 600, 210, 120, 120) okreni_niz(request.text) - self.move_wall(180,160) + zidovi.zidovi(0, 1500, 420, 50, 120, 120) self.step = 2 return response @@ -89,32 +86,14 @@ class InteracitionNode(Node): return Empty.Response() zupcanik = ZupcanikAction(self.st_motor_device_name) + zidovi = Zidovi(self.st_motor_device_name) zupcanik.zupcanik(1, 1015, 25) - - self.move_wall(3,3) - + zidovi.zidovi(0, 1500, 180, 160, 120, 120) okreni(5) self.step = 0 return response - def move_wall(self, white : float, blue : float, max_current:float = 110): - zidovi = Zidovi(self.st_motor_device_name) - sign = -1 if self.servo_3_pos > white else 1 - targets = ( - abs(self.servo_2_pos - blue), - abs(self.servo_3_pos - white), - abs(self.servo_4_pos - blue), - abs(self.servo_5_pos - white), - ) - - _, dist1, dist2, dist3, dist4 = zidovi.zidovi(max(sign, 0), TargetPos=targets) - - self.servo_2_pos += dist1 * sign - self.servo_3_pos += dist2 * sign - self.servo_4_pos += dist3 * sign - self.servo_5_pos += dist4 * sign - async def start_plug_action_cb(self, goal_handle: ServerGoalHandle): while not self.btn_.is_active: pass diff --git a/toid_interaction/toid_interaction/mechanism/zidovi.py b/toid_interaction/toid_interaction/mechanism/zidovi.py index cab7695..4ee2140 100644 --- a/toid_interaction/toid_interaction/mechanism/zidovi.py +++ b/toid_interaction/toid_interaction/mechanism/zidovi.py @@ -54,13 +54,10 @@ class Zidovi: time.sleep(0.01) def zidovi(self, smer, brzina=1500, - TargetPos : tuple[float,float,float,float] = (0,0,0,0), + TargetPos_beli=600, TargetPos_plavi=300, opterecenje_threshold_beli=120, opterecenje_threshold_plavi=150): - - TargetPos_2 = TargetPos[0] * 4096 / 360 - TargetPos_3 = TargetPos[1] * 4096 / 360 - TargetPos_4 = TargetPos[2] * 4096 / 360 - TargetPos_5 = TargetPos[3] * 4096 / 360 + TargetPos_beli = TargetPos_beli * 4096 / 360 + TargetPos_plavi = TargetPos_plavi * 4096 / 360 if smer == 1: Speed_ID2 = brzina @@ -90,19 +87,19 @@ class Zidovi: curr_time = time.time() if smer == 1: - motor3.doCycle(TargetPos_3, Speed_ID3) - motor5.doCycle(TargetPos_5, Speed_ID5) + motor3.doCycle(TargetPos_beli, Speed_ID3) + motor5.doCycle(TargetPos_beli, Speed_ID5) if brojac >= 50: - motor2.doCycle(TargetPos_2, Speed_ID2) - motor4.doCycle(TargetPos_4, Speed_ID4) + motor2.doCycle(TargetPos_plavi, Speed_ID2) + motor4.doCycle(TargetPos_plavi, Speed_ID4) else: brojac += 1 else: - motor2.doCycle(TargetPos_2, Speed_ID2) - motor4.doCycle(TargetPos_4, Speed_ID4) + motor2.doCycle(TargetPos_plavi, Speed_ID2) + motor4.doCycle(TargetPos_plavi, Speed_ID4) if brojac >= 50: - motor3.doCycle(TargetPos_3, Speed_ID3) - motor5.doCycle(TargetPos_5, Speed_ID5) + motor3.doCycle(TargetPos_beli, Speed_ID3) + motor5.doCycle(TargetPos_beli, Speed_ID5) else: brojac += 1 @@ -127,7 +124,7 @@ class Zidovi: if napon2 > opterecenje_threshold_plavi or napon4 > opterecenje_threshold_plavi: load_blue_count+=1 - if load_blue_count > 10: + if load_blue_count > 2: print("High load detected: [ID:002] Load:%.1f [ID:004] Load:%.1f" % (opterecenje2, opterecenje4)) stop_motor(self.packetHandler, 2) stop_motor(self.packetHandler, 4) @@ -138,7 +135,7 @@ class Zidovi: if napon3 > opterecenje_threshold_beli or napon5 > opterecenje_threshold_beli: load_white_count+=1 - if load_white_count > 10: + if load_white_count > 2: print("High load detected: [ID:003] Load:%.1f [ID:005] Load:%.1f" % (opterecenje3, opterecenje5)) stop_motor(self.packetHandler, 3) stop_motor(self.packetHandler, 5) -- 2.49.1 From b3ea552cfb50f62f1e45f27b4fc30612d9dd4eaf Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Fri, 17 Apr 2026 12:42:39 +0200 Subject: [PATCH 82/86] Strategy blue part 1 --- toid_bt/behavior_trees/behavior1.xml | 113 +++++++++++++++++- .../toid_bt/plugins/translate_x_action.hpp | 17 ++- toid_bt/src/bt_executor.cpp | 2 +- 3 files changed, 126 insertions(+), 6 deletions(-) diff --git a/toid_bt/behavior_trees/behavior1.xml b/toid_bt/behavior_trees/behavior1.xml index 868ff06..d9b071a 100644 --- a/toid_bt/behavior_trees/behavior1.xml +++ b/toid_bt/behavior_trees/behavior1.xml @@ -1,5 +1,9 @@ + + + + @@ -48,6 +52,83 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -116,10 +197,40 @@ Service name + + Y position in meters + Heading in degrees + Reference frame + X position in meters + Topic name + + + + + Action server name + + + Action server name + Service name - \ No newline at end of file + diff --git a/toid_bt/include/toid_bt/plugins/translate_x_action.hpp b/toid_bt/include/toid_bt/plugins/translate_x_action.hpp index 8ec9f6c..fe41c10 100644 --- a/toid_bt/include/toid_bt/plugins/translate_x_action.hpp +++ b/toid_bt/include/toid_bt/plugins/translate_x_action.hpp @@ -5,18 +5,18 @@ #include "tf2/utils.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "toid_bt/plugin.hpp" -#include "toid_msgs/action/simple_translate_x.hpp" +#include "toid_msgs/action/simple_move_coords.hpp" namespace toid { -class TranslateXNode : public BT::RosActionNode +class TranslateXNode : public BT::RosActionNode { public: TranslateXNode( const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params, PoseFunc pose) - : BT::RosActionNode(name, conf, params), get_pose(pose) + : BT::RosActionNode(name, conf, params), get_pose(pose) { } @@ -34,7 +34,16 @@ public: auto x_goal = getInput("x").value(); auto max_speed = getInput("max_speed").value(); - goal.distance = x_goal; + geometry_msgs::msg::PoseStamped pose; + get_pose(pose); + + double yaw = tf2::getYaw(pose.pose.orientation); + + goal.x = pose.pose.position.x + cos(yaw) * x_goal; + goal.y = pose.pose.position.y + sin(yaw) * x_goal; + goal.theta = yaw; + goal.backwards = x_goal < 0; + goal.mode = 1; goal.max_speed = max_speed; return true; diff --git a/toid_bt/src/bt_executor.cpp b/toid_bt/src/bt_executor.cpp index ff50348..a52ee36 100644 --- a/toid_bt/src/bt_executor.cpp +++ b/toid_bt/src/bt_executor.cpp @@ -75,7 +75,7 @@ void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory) "RotateTowards", BT::RosNodeParams(nh, "/rotate"), get_pose); factory.registerNodeType( - "TranslateX", BT::RosNodeParams(nh, "/translateX"), get_pose); + "TranslateX", BT::RosNodeParams(nh, "/moveCoords"), get_pose); factory.registerNodeType( "SetWidth", BT::RosNodeParams(nh, "/set_width"), get_pose); -- 2.49.1 From 49bbd58e49d35a48d846fb2332a084408f0db6cd Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Fri, 17 Apr 2026 14:03:32 +0200 Subject: [PATCH 83/86] Rotate towards backwards --- toid_bt/behavior_trees/behavior1.xml | 175 ++++++++++-------- toid_bt/behavior_trees/toid_behaviors.btproj | 1 + .../toid_bt/plugins/rotate_towards_action.hpp | 9 +- 3 files changed, 103 insertions(+), 82 deletions(-) diff --git a/toid_bt/behavior_trees/behavior1.xml b/toid_bt/behavior_trees/behavior1.xml index d9b071a..0c0d1b5 100644 --- a/toid_bt/behavior_trees/behavior1.xml +++ b/toid_bt/behavior_trees/behavior1.xml @@ -25,30 +25,34 @@ x="1.325" topic_name="__default__placeholder__"/> - - - - - - - - + + + + + + + + + + + + @@ -61,71 +65,76 @@ x="1.328" topic_name="__default__placeholder__"/> - - - - - - - - - - + - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + @@ -134,6 +143,7 @@ @@ -174,6 +184,9 @@ type="double"/> + diff --git a/toid_bt/behavior_trees/toid_behaviors.btproj b/toid_bt/behavior_trees/toid_behaviors.btproj index 78b09b6..655d646 100644 --- a/toid_bt/behavior_trees/toid_behaviors.btproj +++ b/toid_bt/behavior_trees/toid_behaviors.btproj @@ -44,6 +44,7 @@ + Action server name diff --git a/toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp b/toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp index 9649a8f..4cf1382 100644 --- a/toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp +++ b/toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp @@ -25,6 +25,7 @@ public: return providedBasicPorts({ BT::InputPort("x", {}), BT::InputPort("y", {}), + BT::InputPort("backwards", false, {}), BT::InputPort("min_angle", 0, {}), BT::InputPort("max_speed", 0, {}), //BT::InputPort("options"), @@ -37,11 +38,17 @@ public: auto y = getInput("y").value(); auto min_angle = getInput("min_angle").value(); auto max_speed = getInput("max_speed").value(); + auto backwards = getInput("backwards").value(); geometry_msgs::msg::PoseStamped pose; get_pose(pose); - goal.angle = std::atan2(y - pose.pose.position.y, x - pose.pose.position.x); + if(!backwards) { + goal.angle = std::atan2(y - pose.pose.position.y, x - pose.pose.position.x); + } + if(backwards) { + goal.angle = std::atan2(pose.pose.position.y - y, pose.pose.position.x - x); + } goal.min_angle = angles::from_degrees(min_angle); goal.max_speed = max_speed; goal.mode = 1; -- 2.49.1 From 542cc2023997cc124a5be2fc3fdf29f979bb7442 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Fri, 17 Apr 2026 15:59:03 +0200 Subject: [PATCH 84/86] New blue strat --- toid_bt/behavior_trees/behavior1.xml | 108 +++++++++++++++++++++++++-- 1 file changed, 101 insertions(+), 7 deletions(-) diff --git a/toid_bt/behavior_trees/behavior1.xml b/toid_bt/behavior_trees/behavior1.xml index 0c0d1b5..66f3171 100644 --- a/toid_bt/behavior_trees/behavior1.xml +++ b/toid_bt/behavior_trees/behavior1.xml @@ -59,16 +59,22 @@ - + - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -- 2.49.1 From a1c39cb943a69f2dec8148ffd42516edea331778 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Fri, 17 Apr 2026 16:13:36 +0200 Subject: [PATCH 85/86] New blue strat1 --- toid_bt/behavior_trees/behavior1.xml | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/toid_bt/behavior_trees/behavior1.xml b/toid_bt/behavior_trees/behavior1.xml index 66f3171..0291580 100644 --- a/toid_bt/behavior_trees/behavior1.xml +++ b/toid_bt/behavior_trees/behavior1.xml @@ -115,6 +115,12 @@ max_speed="0.400000" backwards="true" action_name=""/> + + + + -- 2.49.1 From be9744ac478cd69d33271ba055a0a5b9c69d091b Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Sat, 18 Apr 2026 04:32:02 +0200 Subject: [PATCH 86/86] Fixed camera based approach --- .../include/toid_behaviors/rolling_average.hpp | 10 +++++----- toid_behaviors/src/approach_acorns.cpp | 14 ++++++-------- 2 files changed, 11 insertions(+), 13 deletions(-) diff --git a/toid_behaviors/include/toid_behaviors/rolling_average.hpp b/toid_behaviors/include/toid_behaviors/rolling_average.hpp index 1a9050f..0305bdf 100644 --- a/toid_behaviors/include/toid_behaviors/rolling_average.hpp +++ b/toid_behaviors/include/toid_behaviors/rolling_average.hpp @@ -21,7 +21,7 @@ public: if(size_ == 0) { return {}; } - if (size_-1 == data_count_) { + if (size_ == data_count_) { accum_x_ -= poses_[front_idx_].position.x; accum_y_ -= poses_[front_idx_].position.y; accum_theta_ -= tf2::getYaw(poses_[front_idx_].orientation); @@ -30,16 +30,16 @@ public: data_count_--; } - back_idx_ += 1; - back_idx_ %= size_; - data_count_++; - poses_[back_idx_] = pose; accum_x_ += poses_[back_idx_].position.x; accum_y_ += poses_[back_idx_].position.y; accum_theta_ += tf2::getYaw(poses_[back_idx_].orientation); + back_idx_ += 1; + back_idx_ %= size_; + data_count_++; + return {accum_x_ / data_count_, accum_y_ / data_count_, accum_theta_ / data_count_}; } diff --git a/toid_behaviors/src/approach_acorns.cpp b/toid_behaviors/src/approach_acorns.cpp index 3c7cd8b..95c7eda 100644 --- a/toid_behaviors/src/approach_acorns.cpp +++ b/toid_behaviors/src/approach_acorns.cpp @@ -93,7 +93,7 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg) double x = pose_local.pose.position.x; double y = pose_local.pose.position.y; - if (x * x + y * y > distance_ + 0.005) { + if (std::sqrt(x * x + y * y) > distance_ + 0.005) { return; } @@ -105,15 +105,13 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg) double yaw = angles::normalize_angle(tf2::getYaw(pose_global.pose.orientation)); if (tf2::getYaw(pose_local.pose.orientation) > 0) { - yaw += M_PI; + pose_global.pose.position.x += std::cos(yaw) * 0.01 - std::sin(yaw) * 0.35; + pose_global.pose.position.y += std::sin(yaw) * 0.01 + std::cos(yaw) * 0.35; + } else { + pose_global.pose.position.x += std::cos(yaw) * -0.01 - std::sin(yaw) * -0.35; + pose_global.pose.position.y += std::sin(yaw) * -0.01 + std::cos(yaw) * -0.35; } - yaw += M_PI / 2; - - pose_global.pose.position.x += std::cos(yaw) * -0.35 - std::sin(yaw) * 0.010; - pose_global.pose.position.y += std::sin(yaw) * -0.35 + std::cos(yaw) * 0.010; - - std::lock_guard _lock(mutex_); const double dx = pose_global.pose.position.x - initial_pose_.position.x; -- 2.49.1