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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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 26ed379300a3ef11c1d0aa181fcd1c950e4c2444 Mon Sep 17 00:00:00 2001 From: pimpest <82343504+pimpest@users.noreply.github.com> Date: Fri, 3 Apr 2026 14:14:17 +0200 Subject: [PATCH 37/38] created toid_interaction package --- toid_interaction/package.xml | 20 + toid_interaction/resource/toid_interaction | 0 toid_interaction/setup.cfg | 4 + toid_interaction/setup.py | 30 + 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 .../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 | 64 +++ .../toid_interaction/mechanism/zidovi_load.py | 284 ++++++++++ .../toid_interaction/mechanism/zuocanik.py | 105 ++++ 21 files changed, 1733 insertions(+) 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/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/zuocanik.py 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..7bfc388 --- /dev/null +++ b/toid_interaction/setup.py @@ -0,0 +1,30 @@ +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' + ], + }, +) 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/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..30677b9 --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/sekvenca_2026.py @@ -0,0 +1,64 @@ +#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 * +from .zuocanik import * +import time +import serial +import serial.tools.list_ports as list_ports +import platform + +SERIAL_ID = '503359270A70619F' + + +def okreni(i): + # Funkci + for port_info in list_ports.comports(): + print(port_info.name) + 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") + + for i, char in enumerate(broj): + if char == '1': + okreni(i + 1) + + +def main(): + + #sekvenca 1 + zupcanik(1, -1000, 25) + + #sekvenca 2 + beli_zid(1) + plavi_zid(1) + + okreni_niz("1010") + + plavi_zid(0) + beli_zid(0) + + #sekvenca 3 + zupcanik(1, 1000, 25) + + 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..62d1c9d --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/zidovi_load.py @@ -0,0 +1,284 @@ +import sys +import os + +from .STservo_sdk import * +# da li ovo dole treba da importujem ako je nalazi u STservo_sdk? +#from stservo_def import COMM_SUCCESS + +# Default settings +BAUDRATE = 1000000 # STServo default baudrate +DEVICENAME = 'COM4' # Check which port is being used on your controller +STS_MOVING_ACC = 0 # STServo moving acceleration + + +def plavi_zid(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(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(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, 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, 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(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(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(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, 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, 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/zuocanik.py b/toid_interaction/toid_interaction/mechanism/zuocanik.py new file mode 100644 index 0000000..e358549 --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/zuocanik.py @@ -0,0 +1,105 @@ + +import sys +import os + + +from .STservo_sdk import * # Uses STServo SDK library + +# Default setting + +BAUDRATE = 1000000 # STServo default baudrate : 1000000 +DEVICENAME = 'COM4' # Check which port is being used on your controller +STS_MOVING_ACC = 50 # STServo moving acc + + + + +# Function to move the STServo to the target position +def zupcanik(STS_ID ,STS_MOVING_SPEED , TargetPos): + + # Initialize PortHandler instance + portHandler = PortHandler(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(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, 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() -- 2.49.1 From 038568c7e3b861656c24e02c43234dac57e109d8 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Fri, 3 Apr 2026 21:00:57 +0200 Subject: [PATCH 38/38] finished toid_interaction node --- 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/setup.py | 3 +- .../__pycache__/__init__.cpython-312.pyc | Bin 0 -> 205 bytes .../interaction_node.cpython-312.pyc | Bin 0 -> 4581 bytes .../toid_interaction/interaction_node.py | 97 +++ .../__pycache__/__init__.cpython-312.pyc | Bin 0 -> 400 bytes .../group_sync_read.cpython-312.pyc | Bin 0 -> 6784 bytes .../group_sync_write.cpython-312.pyc | Bin 0 -> 3179 bytes .../__pycache__/port_handler.cpython-312.pyc | Bin 0 -> 5587 bytes .../protocol_packet_handler.cpython-312.pyc | Bin 0 -> 20947 bytes .../__pycache__/scscl.cpython-312.pyc | Bin 0 -> 6216 bytes .../__pycache__/sts.cpython-312.pyc | Bin 0 -> 7957 bytes .../__pycache__/stservo_def.cpython-312.pyc | Bin 0 -> 766 bytes .../__pycache__/__init__.cpython-312.pyc | Bin 0 -> 215 bytes .../__pycache__/sekvenca_2026.cpython-312.pyc | Bin 0 -> 2419 bytes .../__pycache__/zidovi_load.cpython-312.pyc | Bin 0 -> 10590 bytes .../__pycache__/zupcanik.cpython-312.pyc | Bin 0 -> 4610 bytes .../mechanism/sekvenca_2026.py | 63 +- .../toid_interaction/mechanism/zidovi_load.py | 582 ++++++++++-------- .../toid_interaction/mechanism/zuocanik.py | 105 ---- .../toid_interaction/mechanism/zupcanik.py | 130 ++++ toid_lidar/package.xml | 2 +- toid_msgs/CMakeLists.txt | 1 + toid_msgs/srv/SendString.srv | 2 + 28 files changed, 708 insertions(+), 407 deletions(-) create mode 100644 toid_bt/include/toid_bt/plugins/send_text_action.hpp create mode 100644 toid_interaction/toid_interaction/__pycache__/__init__.cpython-312.pyc create mode 100644 toid_interaction/toid_interaction/__pycache__/interaction_node.cpython-312.pyc create mode 100644 toid_interaction/toid_interaction/interaction_node.py create mode 100644 toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/__init__.cpython-312.pyc create mode 100644 toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/group_sync_read.cpython-312.pyc create mode 100644 toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/group_sync_write.cpython-312.pyc create mode 100644 toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/port_handler.cpython-312.pyc create mode 100644 toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/protocol_packet_handler.cpython-312.pyc create mode 100644 toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/scscl.cpython-312.pyc create mode 100644 toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/sts.cpython-312.pyc create mode 100644 toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/stservo_def.cpython-312.pyc create mode 100644 toid_interaction/toid_interaction/mechanism/__pycache__/__init__.cpython-312.pyc create mode 100644 toid_interaction/toid_interaction/mechanism/__pycache__/sekvenca_2026.cpython-312.pyc create mode 100644 toid_interaction/toid_interaction/mechanism/__pycache__/zidovi_load.cpython-312.pyc create mode 100644 toid_interaction/toid_interaction/mechanism/__pycache__/zupcanik.cpython-312.pyc delete mode 100644 toid_interaction/toid_interaction/mechanism/zuocanik.py create mode 100644 toid_interaction/toid_interaction/mechanism/zupcanik.py create mode 100644 toid_msgs/srv/SendString.srv 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/setup.py b/toid_interaction/setup.py index 7bfc388..6473e97 100644 --- a/toid_interaction/setup.py +++ b/toid_interaction/setup.py @@ -24,7 +24,8 @@ setup( }, entry_points={ 'console_scripts': [ - 'sequence = toid_interaction.mechanism.sekvenca_2026:main' + 'sequence = toid_interaction.mechanism.sekvenca_2026:main', + 'node = toid_interaction.interaction_node:main' ], }, ) diff --git a/toid_interaction/toid_interaction/__pycache__/__init__.cpython-312.pyc b/toid_interaction/toid_interaction/__pycache__/__init__.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..91a3ddac982c278f3b019b1c43654c16efe646ad GIT binary patch literal 205 zcmX@j%ge<81ZD@$XM*U*AOanHW&w&!XQ*V*Wb|9fP{ah}eFmxdm7`x&nx~(UUt9tt za#QsSQcDtx^j-3kOLJ56N{aOZit@8klYtD^(xUvN{1PJrBQyPy{LB>nq|(fs6fiSB zGp{7IC^5MtGd~YgARZ{4SrQ+wS5Wzj!zMRBr8Fniu80+AIU^7kgBTx~85tRin1L(+ DTX8vS literal 0 HcmV?d00001 diff --git a/toid_interaction/toid_interaction/__pycache__/interaction_node.cpython-312.pyc b/toid_interaction/toid_interaction/__pycache__/interaction_node.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..f4f6b278faf9f5e7fdc03eed1fdfe57039f384df GIT binary patch literal 4581 zcmc&%U2GKB6~41OyL)%NKQ?P%7h=c64rCKzCx+rsjPoNy5~R8aLy<~d5>y|a+}oV$T2NhPDv?8rdqn3kum}a zkq-%rUnMM|gzkjhv!!;HVr^FisYBVuk{4bhgwqgFI_`L%CXiWSRw$$&}%-m>)}d~&Gr|PcEp^nDBPy9tm>*I%XVTy)tPLnljWkFE`gq<$fmmkN37b(#x`>4 zoE+V?5|fH0YsTax93`sygaLshnu<1I$7Q*s6-`r?O~hNzmG7(p zKCKU&H%tqfvNBLntRf#6HA>TEMYqg>liZk6N+2AY=Egb08hmT;(0~QT88|nsYRnPg z&p2R4Lv0=iOv*YSy}vTcJK*rS{KgpQ_sxP3GJgxrw^zt2O&P>aic#uB>YqV?IBi7n6qM@^eTazIJ&rx|G@b zyTiXfdi&@fk1xIP!8h&O*CL?sl#+P5s@BupwRHC~eR-wt&3fO#THnE{c%Sa!U9fEl zUFSay()}7nZXv1qGl3qQGX!7WX*E{1a-r|&abh)ymxY>i({ve}(u#OJmt79eho@43 z<2{junF#ddeFG$DAYHkkHXnj)6CHDs%VYdkk8wLS{qOI;f1C#iyo!6eR6|cGX2sBf zeh_LLvBdkJvFV(bUJIh@#27~&G%cHQf5{u5Xd;Ar;BkG5d2+66lTU+$_BO8iF*H}m zN?Yc};cJH%gkO%VW^=bjz8G1`_TEeOuF`b%;`R7ycF$s8{gorNSB@-YhpTjjw%2LC zM)M1M7S1ewuuS)@bZ&>RkFR8QRx^%E`F_}#zX{DV&c%;JbFGhnE&lh;1vfUb7M$We zzQY|iw)vyY{+S%!+R=e5Z)Nm!N70Pqq4{5Q_x0SacJ# zVq+W#in#dt4M?{IJAS&zApxBTd(uu=;uq+AvN25LC=hpm`43I=U8Ey?dzEJ+##EPBpqh z(@6dc!ZO_ja-HTN6)kiu)7_8gwwpt@hQAnIracdvGmE8rG`CD&Z!BCKuR^*iY(nQV zm~0Tvq5lget%mnsfL?J65&}Fiu}MPn;#32nU~cbW5f;503!qM%!a88Xvr)^+`U;Ht zQ3Ny>*@VUT7#{>x2axI$h;WWM!+|CrKs(wSnel_z9KZ(Yun=*m zuL(o$C>??+6TOGD=kXpEn9o5XmS`X0vY$hbhYSvF3wv(y8d2EY6Yk z-5ND^%1sk)#AASRw2htes?d_6X|h}p_-+_Ak+a@&Ge;6tvB97DE;QeMMpoK8u8dZD zfAOQIp?u`%lS9$S51zEgBDu9~$w+Qds=qo^dv$1yz{}Ss>Z50Cqi5IgQZPE*dG2`qWg$@VwJ6r+gR4D)Casf&w3>pfU)i&$v~D=b?q7ge1fhsm?>GxtLSrIZ-| z{KTgxZjRm;^N-qgFST`D8TH1h*0R|724$=C1_5xhC{mzC0axWt>~ep9(H1J|sg zjJFN98gUZB-sh}wlLB0Oip&XOyZ3p@UP0p&Id;m?ESr<=7h@IX7}v2De+OP~6Bj{P ztA0m^-S8OUC*c)6X&!@SEi4Ga-$?QS+5LcY|AX}YlYIDqy!(KRJRp5f#g7Cbchh=G huwVa4C@O4U+eXA>b?&d>{5K-~?D$%kh`XF){sU!|syhGx literal 0 HcmV?d00001 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/__pycache__/__init__.cpython-312.pyc b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/__init__.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..99ba63eb5984c5fa2a2dbf4b62b194f43a75b0d4 GIT binary patch literal 400 zcmZY4y-EZz5CGt0*E>XA6cI!a?S!>3a>Z357TSuSeb`Ha7jHKcCRycLU&3dw@EvT$ z!hTsRD?8z=t}^S-DmcaD%M24Hk4Z8|LiF#S^g}pbn#)f_-RWQtvZdyQ5wI}@Jvc8x_z8>s|5?_`7kpwS6HoD1P=moZZk1bzm27Hgwk?1HiBQF z_NrR^Q$dx86Dw{Dma6uEQ9@pG{^40<6hq zrKmF7NCCAHj#Q~+UkZv=rTfzC%PuRew5j%?z9x~aQ64O9wNl<#veHU@**)iu$AEFF zs#~r0V$R&ZIrpA>&iT%{^H+<-$Utda|7Q5_4u<&~Hq@d^R$f#>Wr^V#o*iR;t6>=p zbC%&XcNkv#P@~Mb!p`Vi`sXaPT&&F24J2Qs5A4PAq65eh<72L{Jaa|Evv-&)TI6#@ z$7_M=c^yy#uLqjN8-Q|r7SL>-18U^6ftq+DP&01=YMC*+EVAXS5SX0kn(+^G`n>#r zx@lYxH>ZXF7asyyVuEat@jy)}K@GIjtua9@w35OwASkr(O};3JYJp`e4;3uc(3I3jQRXS(skK1<`pFm z^iPgSvTa!OOn3$FxM$$H*FWUrWj$@m+4L2DctDbk17kj~Z~2o>cKK~o@^ zmv>Lh$eafl4ND%+Y{4!yy48L>TCo|(Z6==Q2$v^3O)&yH z!{X^>8V#~1fDOOP4QDhiZAV-8szx~l6HI`5QhI5R^c*;kB7B8g0wbB^nPNfOm&$rl z0hN>$Q7GsZih-q20%Vs~O@ePcFqPD6@ov5D7^=ees)11H!j8|%?w1kM0aPz&vSYp@ z+!!%eQrVtzrHWvw4Zv$D!O%Kh<^UrodRp0uXaa-{fHkTxSx;eTm8B{ZaV^D8g&e5P z7VVZ~8?p|;sEAeuf`G$P3mNGXYaxa+C=R~?XNM{7Y3 z1}TN)U8qRJ&a}d7CK=HTW5%=rXo@!INAFE5>S`?V#DhpHBe`iE(1e+I9%%)7@wC$l z^k&%2PjM~}b;?p*47lZUtarOivKE4rqHf)&Z$^~!&YW*=_jH`^_B8i4|NPzNx87|P z&~qt>358h0P$U}>`Y}j%Pj81Mp#p|>Sc=R^(-*u0qdqA;B&hxM_`2HPE~Q8*2%Tf4R6qdjrL0b!$uJ{mxEaylHzAlJhris7Cza}H`T96L8tco;Gj~p$fW`@P^PW)F9dIADz=QW0MV-CUs6H z-|y-)cNhsc&;j zLxyi(>RHbe&vNentm%GJ%z2nN53les_bK8&^=IvR%U?SF)DdgBL|QIwv|QdeePzRW zm2gEN!vcpar$0Iw?v9lnCZ&g0yri@~YB~}xDf`TQ-@V0XEr%Bj@rL7HIlpv1>RX?B zEXB@y$a&93!w1CXUeGP&Earr#h^=PD5Vse9+P`6U#Sa`>XnE?W+RTENH*JiqAY|Fp zGmr_kQ2)*kS@!y9D(dxt*fi>;A_#OOi|T9C9#Vdb_8@3b(_lj>4_4n{81=0#Mv01A z0m2F+xREpfNoH~ag)ro*>d7cgcdD97i;~+Tm~4q?E}*O2{H4;x(vSCt43O7+(jPTd zz?p>F5z^-7=RW#nM2(zi4{AZ9r0Db=MW-QVPPZdUq|(tjJd zxI4LBSLe=gJNf`qlsD;W9MZY$AUDL$>4LgU*%5MqKYDH@jEZ#YiKYoJw+|q;G34?HDiyGG=*2k|$dM|x_GSYu7`p&h4qj=NG*q!0a8+kRGc}#9`ICs$)egmP*6}7$^ zx8-lvGB(F2o`|(7!c}b^V1PY`Ny+G1#e?yv^;le8FJUW+n;jrQd+6=BV_&$BIBG(z z+g+L#iTg8;LGRA>U^y&F(16Gs)KCb6J8I^YgA~Z%3jix&fTrd9W~#0ZfI*>C<<4VL zqB7X=RQN0KiH-{CgGQSdUtCqv0P10KR`Mg#?hidm3Sb%* zo2R_PW8Q0HzS*?9*8>X5S(^~VYC2DF^<|>!+ z^Y0PB;$aIhvaQNr@7JA=GYrJG~Z|@v|R(&AS;8c=&3ZZ1LC>&cJ)A zFT0AvXASwJ76`Vp`Mcv|vil=^V~_^OxTzpq5j8mhu$N9Po?5OUcGt>*wW`?B76QMz zmi3FIu8r8wM$Bj97KnIJODWE6T5O8h4-or-72Ov(t2t{^r1sRKo9j~S>}3MKH!nx7 zTqSQ_CAIzW@~Y2s9^|Y{k@Cj1n~$W}n->ZEnl45OOLx_?H&TZR!`&uFKGp!x&|xO@ob`^`t!mEg|Uj`q~iD*|CRqs|9UTJ zeE0FCNYCY1&k%uM`%vWiFliqqjUzvZZ6(jX&}$t&5JzhPYGfSw6v#SUIrypD?;ZDf zJhIW_84vK2V^}wPJU1r2W6FqtAA*z*Ci}2fh6FztDc1d;`cGW=mt#muwYf>1(#~vhkGE(^w7>Z z03)5ONnih>#H?2~Pq{&0l1-wde9Y&4gS1n?8_D+C-*G1txlRceU;s>v5etB9YFU>3 Tr-5Oe|I&48*!(R9DShuhPtIP^ literal 0 HcmV?d00001 diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/group_sync_write.cpython-312.pyc b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/group_sync_write.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..3ad1f28670d2ba484d71c0581378014395bd5ed9 GIT binary patch literal 3179 zcmcImO>7fa5Pr{pvAre^Nt*wJ5J-$eg%P4rBdFW70mPr7ibab>SsCw=IPp*Vb{*o# zjW}>fXr+KuDR6=#R#dg>r9D*~sd|7VM0QskEVUPIG^tvNQ)k}Vo5TcEp>|}?oA)f7EUwD2iglZ5r4j$WyR1UYX00@og1(y`Zh(4GF6O95|b507o?~f z6%%r5R2|dn;)*yKWznP<8;hn!Wl6X5uI}Qiq`I za{*J(%x<6;*8?n*3XG){6j;U zEjii>iWSI#ES_WHX#KQt6EW*!Il5FnmA+~hMz@MyXTiF@uRgVbEqNO9+n5J5FaenE zh)zz*DM@$8(<-!i3#;YBimHflNkO~2yO|fqT*>H!%%L{ZQWZ3FW;-lYE&+q6c^ejH zug?}e^(7-frKYyU)Xmf?vAYK5juso57lSv0cl#bU_N`M&vCNv+qHSoyO z!@qIG+4<%m!4Lf3LBiV57wyP8@w+y`KrqU_frM(0Y?wQMJ54aGd*sc)Ujn^K>@EnXT64jjh_l3Cf0hP)i(TP0}B!`OE5~mSFKi;xcBv~>< z@xUCyyo!`M7=P{K0Oqm&R?#??x+;Q|=#MIb(Z;GF=)&k0*8C@afk)C-C(gjBGjPEz z*$it&h~E}^hH{u8s{8`$fQAM_Zzo%FW zuEx7h_u@-}zwE-(XHtpm`csGX!TbhL=}vX}Y&13@t1rDWDicGv<7;47$y2PH3$ydH z1#ge$?OFEjUKV=qZGSBEKW*rJr--Bk;|LCrV#n_megt{(Jj_i=%jjyFtG%1Us?iR&lTFN9)_K4!=nP|e8 zVRi5khL?7rv=iAbWQaHRHn2S53i%zw@bSSJ3JN;z8d}{(}f_-4M&24X7ZE@Ip zSL?V$?K{>SPJ7uVzA@yt=mIZNqh5q7Kjww=uwibHVU@x}R4|gyt(4L~ T9fY?3Z5yWa0$nA%;XD5U>@#dJ literal 0 HcmV?d00001 diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/port_handler.cpython-312.pyc b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/port_handler.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..512aaa271fec20ea0288309131a45783348bd4f7 GIT binary patch literal 5587 zcmbtYTTC0-8J-!>m;u`uFeVVL2260Yfi|LPE+h*HkkD>u8d#~JN~Xp$zz}1TGh-4g zSmhzAV3mlVZcEfB@|2CT(&n*`+dfrorP_h5t;s%6wNjP02-1h{OZ)$4JcEr%s9DO>* z9Wpo0%Z_n@V=gi>e~*y`#re!(o-tmsz=oW!a4_^%n01`4MG74YnFrz`+1k)1$AxddpbECMZ)OM$xNGN5kR1=J(EftJf2pkBEg zXoc(rS}9imt&%H&`s6C0)eF8*wc)xFC;FvuOpYof!sT&@eH3SRkuoT=i;QG1>-J8V zwfiy$qnrvDhK_ja6*{t14622lPHSxuM*qXj197#aG9H4K#+C8{RH=aiUH zF)%pP|M5p-()s?62d?&y4I1J^I3bg;t{9bb;mA!zmoz<0bV*ld6{9>ig@brPH!Acy zbU~U^h%~VPFNtt+LQ`W?M(G@AMT&*tHHW5Q&e4&lGS;(?6PJ7=8Se0 z$XzCTs9~AE@BXH?6CR#ypnaL&5}UG30eseHTiWp1xLw;wm+M;LQJ(c3TIL^!yKn## zu|SIhv@^xWN$9MD&1UjfaR)S*wb5bDAbpnGIm$d`M?#$8(iHtXSneuVj<{gMQ57xr1bhS-sZHod4qrAdF=hMLnd2VOz$LhFeoyD<*oSkQ}rIn?W*zgX1x?um?@N-ZY%wfuMxH2^+-43fdT4a+D@(u=>OxKd|Ke}Cc;s{Q)Pog<|3MQe6eSnv8jEV_DprRXtx{+W2 zC*?p4&qq*kj$F7VjSXKOyh;F>m^!KEJh@8KL{NbOD=<0H(iD$_GBnwc@5lRB`{7ip zcnbtGfu3}rXESiZtlOH~GR-~d=AP$E>E?moS7(NAq=#>$!g6|8PB$rA(}{FbVwwM1 ztS?r(oq2oV58g}dn4|3+Zj}rUf_68RldlMVR#PGc^4IK_@Uj|>s#>!3U0N{r45BXT zfdI6MbkPLK-Imk_32e3{9f84rm-L{BN8_DHuPpY-3*3Su#GBc3YzS&tjL>M(Q8gA( zM)B4`AyE{`y{cAd|N4B%_bm>i$e2DsWS2MN3Zz|uO;_87oas1~?l|?rbt>E1wmh)q zZie8p?X50OPKaz8BrqbaGrBE+jiF$V^F?7uS4j-+G!wQcYj^R-EOxD;Aw$bNpCDs-Dyq)Dg00F78;1BjUQUMCEp|Q7WDS%~kyQZA6 z!|2{Vc-y|Z7tGS{E_*ZB(-EAs&0tvF*JSn!?pbaJsNJqBWCx7wz3_h+jrX+~yn^j( zdlJYzymbIS2yR(06NqHj7~tnl)6YY^!Oub7H>+R~KN)ChQZY)<&=AVU%*4^R+Glo{ z_P$V=c>}~tk>P~P^e3^4o4rb%J2~MlLd(xjxj6vaahuB0imH}wfEEBth-`$WMyQ_-SBr` z9abgTO8$9G{lRRt|Ba`lx(vPth_9=dTKW}X+q~Rm#J059 zwj~C)14lk@|MJ+=W9h*6f76q0`Oy5B;Pn+F(RN$B*#Q#4`R1WQTLgz%;NVUYa;aZI z?W*1l0_lVDMrD48Wb}9985?9hy$NrnA2Kp<=?J6x$yyHmpo-8)WBXxtH5p>D^?3T;T5cATX`C$2W;=6K=kj++%a zaPvI7BXY1&6a@cHIV+smaSFoWopPU0y?c}wYRrGeKMjfG2l(GVMuPVwL!66-b*QFi z33?X!5a*on3H$lUymRCRY=wx#XsDCd4whwioeb-K$#`BeWiJ`;w(oF? YsmNAT-SxcTr&+eUcxpHKd|HR*n@3s@B?hXo*D1#YGhhA*nm7O;lYfU z@up@s@|Kgqn@KEZx3ai)DvPs|EL%INc=D(?sjbADq!KldLQ%O=RAqj|e^L%J`LmT& z@}1kQ?v{{%*+(j`uHxwSz31M(k8{3z>9yRepXRhpYYZ-wt0t!EygcH_6MQBtCr& ziF=&D35F5wcZ>#3FkUp^&nTEKngs5mSukI;2$qXh!Fn+Vf4q=$F&FuH$hYC%F7Oxg zh1`n;M(zYBP5Bb`m`9Sdz_y`|2Xu|@GgfQ8bQs13DhE(L9K!X6g>q+UqN#P9yCwL1+`HR z(PtZIzF-F}5b{ACLIG%@-~cTW3PCpsMW9Y$6KJvE1T7JYK}&@a&@!PEv|K0y-7J)Y zZV@(vRtQ@_D}@TstwJSem9Q1GTBrhb3Dux(!3A0)xIwoGHK4V^Hqbhu7PMZd18orM zK^uhzP>;|E+9Y^Dw+l_6JB00^JB1yf%{O;?niFN$#Xu-97#Q(h_YGe2hrC0+F=50n z4k`=5CXl9_?mu;b+~dNAFy}=|zQRV_C8wIZ$vrgmc#H|#^?(@alG{q;1Vcfue@yV0 z66T&{XNmL zyI+_-6tlaRoyC!^HAX_imPc|8T@-V}1{+XR8C4FajKE{+5eenkFhm!n+#snV$fYF? z1%iXYN%vY(z*x3Z^(qk7|A_BvN747`BT;ws-2K|Sr($;3dOd#ym9vSgV7S(ZJ{Dj_ zjI=gkqx>tF*lNk+Qu2~K`m%>+dA!g50}l~tBVv-4VbFFO^r-Ke|IL6XOxCV7?j%Tb_a|pB zNmZ?qqcz62uA%6J30lwlq1Zj*j${{AEgxB}<~cweOkz*rbo=ee7{7Uq zE+w%?FiZj)WFbPwpbk5!W2<{Nw7Pdg!|V4>=6R8NCqp+)U*FxFXuITa$9VS|U8Etc zqX(9xj1wL~&UHf(p(nC%KTN{to2!Q~61a3TmGO0013Y$mp(r%E8=F{>i5=7*jLXg_R$5u7=$@7*_2He zSM#2x<_Nj59F9$E%=aVkknV->D_;yj4L1$QqZwnDl4nF74MCr0LY_4(&x|~>i`k{r zg)D}cWlxz(o)vkdlECHQX)58^3JDRsKX1_Po9V=`m= zo)q4mz?&|x`iE}}k6lT9gnP0fg>_)aFZyY0V}W$Wx)et3`6*p?Pwq`&OG=7W{ zjBxxqnb)q+5T0^*e2h(uJHdH!6ZWIMXU=&0&mTS7+25ZiV0UMG`v$y6&iB8nln%V& zJ>JoMTDj{}?(!Kg$~yb{dixR%c8kK7JGuwDdroK>1Knpjd(RJO8Ap5j`p%ynNNi$N zdU^-E9T(7k$C1;W$&JG*GT}&3|9mW#CO4B=H@OtHUWi|C7=$3M=>6B`ZSJ^pe>~s$ z{@e5Ub#Z4KCByUi+v3hnEqQ{HQ}g+?apz%5zB8X+A9o(ok_WZq0VEg8JoEXUxbx() zvt$ju9YQ}+^_H+uOw(H>FGX*aycE4v@>29x$+O(FdUARuYrfD={z^X+@GrW$$F7fu zTsHzEA>S3h%P)!n(LGs}%F{7+WyJ3a`p1&xTT{z>Z}`O_zb}+5u3TN*i3XI$GgKZ4 z_yna`JcRl1SP})0pd;M_-V42_2Rcr4ChW?6N6(4Voe76>*L$I}udB0TKxL35YI zL$*34dTu!#HhfpH1*R-vQ`mA1#`1<{g*;<+^>ZQeca^kl z{6yZ_Qv=@9ojoT8y5wjFKOFRq2eEmGFM%ylIPl8Zj-#hK2fSxGULi*DC}rEI8nohA zXJ4!SrY&K?UUN<`$XD36S;4S!y!K<#SVJ%VGxg1_Hp^pguG8C_`xCn;I-iR zXu=r^gnVk8XT(2tB{Y;U4GR;A5^4z%gk%gzE(_9l5R$Os2W5} zkYE~4Fdwg~|6o$8+BJ7hs%l5XVcF(bvQs#Q{-S-IlP(V|#o z%bZ)P+`Zu3v%(p2>mx_w_Tnf%$ns zpExSi;EE!Bm==zsd1V;U1bUCV!cFNAVhZueDHHz8f^o`{3oW@yylEG-B?C@eg@>7b zg)Lz-(@muZNj)%dAz1J7rPB^`*bMD>jpL>bqA6rcav;`%SZSJ$Lm#GKJ%+6e)y!lQ z1`T)YlmlK=P024eaAO8D_l{MG5Bx}vF^to44OkVdM81kNl&`ZosT}Qx$TMUYt7oNp z6g?3PFLJD(WrRL=zJ;-;x23jB{gZpiu%LJ&oKu!NKVtI(oD-X~tU(6!Ng|f$MDOJ2 zNKQyamMPa~r2|+HmK(z-MmXtF9ErVZ3aF0)aF_vfVfQVFZ+~Z(yn6UaMC@89+ujuy)3^h#vH<7|u2`gc3_c5`bZVLO- z@Hml6#fmLF9F&1?V0hFY7!Qe^^t9td2z12(5D%{)uc#oe*hNLqBrcIFPIwi(wY}*> zBw4-@zeI(G;UH}y3}-c{71xE@B*%SJ4*rt@wIvb$lcKE=s|>ouC6U~CX<5X!T(AiM z_+Ibr-srKV${kYWj-|@IQsv&cYmd97_A?8WJ&X3w47xxb5SYr9GWbiWl~r`TO#%&n@73 z&U3|OQTNyPgZYC}sl0J^&wP2y+$(={RodIVc>1;Zy|2Y9Yd?5Z0!-J=SGEI*x7N*^ zi=K*?yJybbFNo%%A~MUj%}jihCpEl)iVa;)+N6fl^JQn^_01nnO7#c+=%Q4A;)z?T zKNYosf4R&x^TK>t^KwP?QpI+uV*66X3sS`kixn@%s~cxKe&g+@p8XH3pLq7qSmTxM z<;q&@6;<12+GeiJbvy_xY;DI*QPLVU&;Yw;&pkLLmABmIu~U?^F1s3|H)r=q)h*Lq z)GgPHZPxd|A(gexbeOd<eB+aHlP$F1AdOHavARXHcD~ZZ29>w+!L}z^iNJ zvB=PPTz*Gz3B?|}_&R=MfEq$7VL#FbZ|Z1AKSMnhekR~bI2aE0^bho%KRVFe+mp!c z#*McRR%oIS?`Kk=FHu0;XFK{j&U6mINENTsL#~2&EDQj}FHpt^k+Vd)Kv)or0i5!> zC4_L1VoGL>8*4nv)I_d=aAhzi3yUHa=;5Z)dt2{rjZQ2SH3IR}Xh22u!p!-Yvngh8 z0#km;`xjt0eqf)s??^S$;?7h~d-40dlCAc`T(UUrg|fZ5WZx;-cg_xd^!5{jw69xg zKKbOF)ZFuT_TJ?*7T==170+`xaYxBFE9V_`v*jPvOP*hN z^vVw}OD~?8_ne6rmORfPWzXa6n^-7gX6VBkQhocQ#visz2Tsn{pNtn*e)CnSxFOQH zOaL%b5_8nY`1&;KqVoxIxCYbJXlOdO^lOD5Ajrm@gOE1-lkbo zeag{%OO(J8kuNHN^1#nazrw15y#`Bm$}(kz=vp-!kEzCf)Xk{w0_@J@sQ^|#Cu99` z)X(56hI0f1Q>h%mLcd856<)b948N6t&SO=4IZ_*DLx^NY%br}GC{p?a5z->rT97s5 zEW%=9kTS{LVVaiU$6q)}j{_c_Bt!af=h{3G76@zF~NW-$HG)P*!9kt_H@ zJ})(r!74CchyOTPnJvUYrFgT%1?NEEc;QW|o#_e!|I@n>`rjAfmsK@^Z?$_4dU7CU zKd@Zkh67a!$17g#LTZb|S3a%XiBz@3yO(R~zti%dWv+3d<^_qbj_}j<(K@xEhV9>( zdN4J2;nBIrhJ}VBlD#(4J$)lOo)mki!%}stRNXqa`BB^VD<9o_QuJ|nq5AZ+ec2%^ zpra|qH?0ClKBu`V{#v0 zVrH5$Lrl#OkL2qNRXl)nnkd*9SavYv)Px}khHs&LhKLD+HzO2UiiRoNC*E-!&r z2almKEURuYGcH?8a@oo%?!9yOotcS+vew)7h#}HG-L~u~iR8;utxgYGr8JxB0tG$P zsa}m5Rt+=ANcgGn0vnhXXTobfbb~DrVUB7rz@JIxb64pwMB1g{3t~GKowwMCxvv5) zsNSMhG1DfGXp|+@j0jtF=s-v(@P;}CwsQy!DUCds$w#vsF znjuA1Gj6G9n}p~`17U%ByIfSF+j+6+Dce{2u6cRxjfYnk?Z2RtI+C*FMM}w}?lUCr zahgIa@ek}3h>cj8QKqk_)K7ZV%qX)!Sxwr`MLsXzVTHxGLkF-fLJt{)lxXk0hTQ~Vc*joGvL{E5;9W;rGuW6_ zCHg6kj=`;rTCJvvBhMjZylNGo7hV-^EFhy#XV&n zBpF%DPBOO~MG-T3kZE=eRyD=0xiIfJN+wM%nKbsKOe9U3`uX~obk>Z$@V!&FPeu6; zsy=FdTq^B45i9Rnw0AE%oYNPg=VOkV7_Ypc*5i~aF$Y%R6eNYBJs5y|DHe`JLo2}` zyEXrv5Q}X!)~TE+KAe+=T#c}iRODiE$A>K$am%Vno0bZ*2waNXS;9GCp2?l!F34}_ zNw{TWXG3tN+B%widUAE5`7Slb7~f15?dBYNwSBPlSWlH38`Y; zXuu{5sA|}hEpFaEdoJc|j@c0mSj(_k2jw!EJ*u0b8Ri=oWj%2>VYsD%~{(KoE*V^u`Qc*p=0_Ip8qNNzA!Ox zGVrb2Z$;Z@_RV%IIGSU8^J?>q^pXvLf@#?YfbP?NiHpg)aSDbNuvf`bP9Y%dAQ(x; z$Kb8dE}lflhdAuRwp*Gmya>FFABK<*{rTvd@R6+T zM^l)Os+wbtk_l%x+nD(@1l837U`5cS^BMmL`BRr zW2<(Ke4TV)(z3pwPhq=E({=#Qkf&|*{^;!SIZVz%)#0DP#C;xRp_8AqVTfc2iN6ez zK|^+JL_-v&YZ#jN9yLaRn3VIfsy4rklI$zHA!Q&}mfB0Zk}85<6RmnutyZZF{1rBA zYD30#lcqP!Efv3xqRbk?hPs}HC`@Ct)F;tYb`3drE#dR~nY}9apO933lGXZvm4ZzY;&t<5?p}QbF0^^Z=e$%lcrhnbp9H0R~ueT zeH77IPkmH2I(`;BjrP`3pP%8ZeqM#*(X}-dYR^U#N?}5fJDWn$bJ*_kmq=t^_6;f2 zp)3l;Wqe<%>_q8mhBgQw(=Kqa^9}e-i7#bUPQ;qn6spRp1)Mx)otie|LF?%%o5l5X zRbhIIfGXa-~1`xa}!sxd8xQwDz2XiEj8?y8umXbe&qYO>~ZbmH~zF? zq2Xk_rhchrw^Xxx?%1QE@1J-y@YwzF%L_G~pW$W62}68n7)|7ET3&lIR9E<~(W~q$ zyg|kOd~2LV-LtH*Nm*mW2lT+3vc`Il_%;TF#Zx zZnXauYpUO>D9yC?6Sk%lvIC?t517TE;i17xMwxucu^#y~4)Rr-FfDwi^Ws?zquwKD zIe4ocHflxcU8~&nQ<5 z8y`pDcZhP>yX8CX0l!t*!io*eJ}pD8?grydSB zjxIb*I@}LwJ(;YKupyqYhnRwfqXbfzRwC zv3Nu>3e~@ff-?;bo=bU_q-3i zj|LWY9*LD4{YjSpzr%#BBSxz0vW^(3eBYzV+P#s<+K6oNXo#5{^=LM$Y^@_vI?2=T zNNAd__2f}k8+}N>P+K(_v@%Vab+t1;dqcIan3+ywdat=!CuYzOyrE<|(T23F7<(v1 zAi0KiDAwl&v_r@F16pv})O6bMoxp>@oUl;;!b0gmfTOH3KUwDU98>9`NZxavD=gj^ zy8ge+U@!1820#Q-@t;6>Z&+f;uXPk** zM`!lJYfbS_GD5^ZVZ@5-itjgQA>_D+7V}7tbFl9fz=wP@e%Qt0ALR3}OCQx>(GH5C zPWHymo{RN+V<%r%^OHf79a7!#7=L2bnO5zzmfj&! zz2VoGC-ODJInb&h-T*!Lvt5SM&3=Q5RDA;OM$U87Vdk>f-tw5Q0n_!pG%BO>J0 z$!8))DMc{3decWzl!!gFKq4x3bLT`PYs*PYaVp%@wCu?@EY@t`n}#n zp4W>r*yAIVwtKyAjQd9960wA4XA6<7L~4oD6KNvSOoaK$dnl!alW0d4yNR45@;Z_0 zL}*DwGFHT2B=S`vw~5dmEB*=*nl|y*iQFghO(MTZj!lEdqjSp z$j3zfkjNhsc}(Qbi2Nmyzb5jxL|jDZ&;na*kv4Zm=yg``6Oebg6{E>gyOL+HRIXSJ zmdz_xv!!Iku8>_Tc7vsj3bqibq0((sT1f@Y6>G7jbY(}OWz$Nn3wg&3od%0*rN_9{ z(y(%<*s^_PM=lsjoght{P_xkq(qyo-tXNH!#+5uqGToX{ipPNDqi&VWvUO#P(ejdE zg*RFb5mA#CN)}j+B#udgH1*|#>Z`tAn4v^fuTh@4f!L7XbCg&{&{@>WG+<4KZ lTLok+IByli!D*L^C^x>T_?F`{>&+ZPY4q~Ha`el3@t^) literal 0 HcmV?d00001 diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/scscl.cpython-312.pyc b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/scscl.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..01ac8ddb9d519972190feba709f04df0eda70da7 GIT binary patch literal 6216 zcmeHLOKcm*8Qxt!ypl+*UbbXOw&Z8bI1VKxQWQTV(-JMp=0g@K+u0`B&>UK#O>&i4 zDzOr&;vt9R)D$QnrzjA&hgO1H)W;MFPy;Pc6%CLjs6xcTx1Mx!?F0pKDEj}qTt4(j ziZ(@i=m7jU^UrJN|G)YFnfa5`S<65gc=$!)5A_W57c7{`R?Iy895TxcVF;UKe#Nmy zjxFUlJ;#X_!iZL45pBdO+KElvL+m0?_J}ov7i&RQ2Qml9oFJ3pV&!S#RH^GJV^G7U8G&?CLQ7- z(kUJ$2gD=fpm>yYiN{E{c$^#(pW&D>h8%v6AxGlueT%Ug@x=U5*Rf3&N-kDopWQ_9 zCWnA6HmwM_@=ZI!9)ubM)YE1if&-x)p#$Lr!m|h`5uQUhh44Ir8=(hb5aDHn(+Fn} z&LW&c7(y6ExPahAxCjt~Un${NV|pqAEHiONWI^W~Vc%mw`y3$X9}p}62sQu&D*%EW z0KpP~wWJ2Hj?@A=NFAV)H~{O36Hu71cL|zJi7ByU4E16rR0^u->FWT?OvaQmw~DGQ z2_Un;EU=`cbe74OWP+LuYF1B#GtIKVNT_)|%`+}bkmA%7HSe-%t+Q0l$T2x7%|>Il z;~8ltnkLCO)p$utq!SrQ((1>kJU1JjPsgs&L?*874UdFJ{L)y+E>5#5@#IadZdRrlpE0U}Q+IcNz2v5IY4?n*WB^ie_iQ{9rS4HVHkXR0Gm3kH z%D3V%$av?dd_&Ik_4Ez6Gjf8sZ_FiTms){-ye9`=SlLv2umx8rU+?GDsvnE!avU+wK=B7iL!3AkeH9R4pat8}q;)JwKDs z^EGcHr6PAG?@-+5nk5-eui7c@bj^-^5=7%?WhH@D)~uOCDh?ZHHXa9Y=8i#G1xeqn zCyI_i<8_`+$ckxUvy47LxeJhG3N3BRp&y3s`SLBNRx-a{T3!0&PTuWXJNkwDk8V|b zJ%343qw$UCY%V&hzB!kVzNOw-_#4B_vp(*x#@}Ox8Rvo=@L-+|aZtL#O>xlV3ik#! z!9Ng2xtrMJ3O9|VmKpASz92OJ;QJqZ|L)E8<7Yk#KWzK-wKZQ}m{R#EiaxE%HJ~1K zPMwF$uAE{sC3kYan0pm^Epm&NMe72$U|F!g)gc6!DmB%($^|zCn8#(I7+Xw-XzDLK;q{J!L)3Ymk>zQ2Q-E^18|jAHWv0MOgu*^R+Vxxq_o1MA_L{9q!-_pAK= zyTdEv$hD;pmej7E2Z!@Q|AVEq8dbQW@>jO&5Y44i?dOpBZgprn2Hl}x;NOzUPI1w` zfKI36RLTt(>{~2h1wwnF9^8vg=od{wQw;W|%XwR-#B@2Br#|emr-In+mG&@+Ep0Ij zJrCXJIDmrRuhY7_+weDzB9jd}HI?lIWY=XlrGf_wFqOx<;R)wFVKpMIznaN==5qXy z${)TLdhkl2>&VATA1$fw@qE`M;C-R9`{U3@A@#Y7`OY!0C+>mBFQlJJYX8MGE`RLu znxwuytqRjBKfT?YR+#TU<2iawMQaYja|A+H$Ae>4B)6yJw-V{;-I-mBN(<Z~suZ2in;C;nE7QxmV?TjpbK{s!}li zI}Eccn=6AYDr-wB^KzG9UKwn6>R(y^T?At^>XvkCr~Sl2FfIlH)9qVnAM~UzAmDLY zneZU;7R4~#GS0WPCSS1<~u5mT*bcTPUAGYaV^mt z7bZiV(GgELB8`u_SUu<{*t!@wAiPg|^cze$ycY!+E|f+9*j*6A-~Hg-pPbDLCsh8# zc2|!cK>uDpWR{s}cux2kyU6Y0vnmdY6?ez7C*;c9aB)GGm7>$x0&w(s5u{pcF#wDQ zB~Nh7@0I-Hf$@ms*9ilSP=WgKk>aIX!H;Qs8GNz_nV4CpLvZA!@)U79WbuT zqSfoymhuU@vEoV`84rLOjB{{Ch7D1r27INcRzvZ&axGAOTe%j9V$-7lIs*(JcW+?* zZgp^?k0 zT-m_ItG!{`8owO7?S-d>l*#LofWW9vTLG$8^~=)_-pmUxtNhE`+5K-X#0s;~1VR#^ z>O<@1(xopmy;p^38at&CkZM6!13rtSqp3JNi_}U|N+xqj%sV9M&ADjOsL>x6DBd;c z2*M=ENLicfiJh}u*Rz0j)^_?Sg1#($^q|Y7)hQX{sf5ID(q~{M ztwDd&kra6Qk#x#yykY&`fL7!|kJMkv_x1N&Hs!s2gO|S_Ciy0 zRw%T0s7!02y>lxzVC%-_Y(2DY-LEpug|>E;*;~YxeJayfFmP|H$}|-WbPCy8=y0G* sW!efI2M{Z};Xo|~0}XlSL6w0!Oe1`7#W#Fu?PX7^jw4?&@UN@oANFj4VE_OC literal 0 HcmV?d00001 diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/sts.cpython-312.pyc b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/sts.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..963885b590293e08f71272805dd6c4dc5cd2c594 GIT binary patch literal 7957 zcmeHMYiv{39lw6&+HriHHvvk*vzV9j0Ak9kCNXg!FJcGEE$yxGF(DAg;asOB;55-z zZLo(*wGX7yre@Nl1<^^FHf^9yD<7r_Y1MWuGsU{%+dlDSLePEbr~Uuu`d&MUL$|h@ zrfCP|$2tG=xaa(z=luQ0iV7zOW&eXekNl{Ha07FIKhR>FxkViE1cDwYwO$P>HhAZ4PH@M1a8TtKS;nj2^Waflw`6e~%&SVdf7 zHK`D5h+C{Bf>=j9Vm+x8*N`f)fmDlYNsYLU)Qam#o%j-|7dMbKVk2n~o5)&mBUvYI zBJ0J?nd{=n_V+mA zjhpUTwDXF-$sOLEPfXDAnpD${CrEx`M!*qItO&@PCw7D~1P20g@<};@3t=t7I)p}q zCWMU$I}mmv>_XU$umxc|f)`;Af)Ak?p&h}GZ~);Igo6l&5Dp_8K{$$V3?Ybc93TdN zM!_>-dHycIZ7$AMHgq9lZ@GuoY&e%QGm1o0VU?Os`cYEnM%fz zqtbXZb~&DsE=I@5Xq>9NB#n%Xq$EjochTg;czALwc7~3m;;MUiN!x2yt#W+yf?7VF zq^XcLUBtj zx9<0)k|V@-eqv;luqx~$6{pcyY9u+fl8}hUV9O(N!WWLnar$;rlF4Nsuy@b+Bt>1i z=E8=48dMW`E5Ho*m~ULq3H6K}MUAU6naNvgZFZ{aaFp!HG z+=T{eJCoC&pmLi_ThrVnUA2Ism$dnAo(lbj4D?=*pwUl2lB=v?`GPf!{^7DI8>~^h z&-|y-rW9H#D=FMND`}gtvh6Oa1e7p$$E@jx*QPpwWzuN!{3M8*)h~{)y5k+xfXLOs zt7=&|5|#oT9jawCK6b}WTVM^4?@|!y$`P_ zgJEUp?BWo~3=t)MDLZsoNnBY>q%(>1Q;vJr)L{luj+r{m(5eZVZ}2&x`u%rrzB_y2 zC&M4h3;w%P_bz6IE`{%+d*NG4OcNQzME)97K;)$bjs}@G=yNc7$~?IE~9-O_ITM z0jaTMA|XM~#Ar%&AR$g^lFFzgMVv1igzo_3uazU)^NlVTBkuxRGf<;$nVao7`I(wF| zvXp*<1J}nItOPv{!gstDicvtVLQNeQ42FY!5viv?&?)s$JmC^XdWso2nv4?K4MXS& zfaNUXkxM-=MDr|q>m&}%Gp+sLVtaR{z5C&|M}s8W9?v-TDvpNPqw@!Ho|@aj2ZGYn zys#3!7zu@R^efgu8_J<|(h4#WnbY-b7R+jKUFj!JsDvPy+dAQZp#euvEqZhnZp>v8g z!|+*MVf=>{{Ug-3ko9y0eWZz^&%i zH+|IqVZXBLcy>JmqXt9xc^A9o6W@KGa^iL64JrG|Tgv$>ikw!2w8E!X+K{0Qzk_Y8 z?mh}UU*c;^hOH%9P|_P4BTx%U>4~i<^pzfAFB+k1K_?vHj+MR+|Is%P-b6qrP9=bq z;$aR7#Iw{vR zLRsOk!XJKttp4|2z?#%-1*xen8XsnFQGA6_Og>ReK9v?<#bAE0COy0)!lgi)&qxy} zJT$(4gYcR%LepH`$MwJ5ym0XsyC1B{3UHw~p^2|JL4jt*ObruotNGF*Pnw?#DxLS6 zb}bX5x{eLPTdE@vj!4~|UK6`q$v8THLxOwnaDXLk^m}i?F!$n9U{L87{+lt{4ti}ee$0< zL{UBu-YeID3HrUNa5yi#axvHHwd4<1NEW!*;R3I*N1ked^clQvFh8T^46LINfMwhe z+-URpmvB6#yLOg;DYU0yoZJYo^tgw9TYWF`VAK6GzgeFZjw}3e?bucrs29Vj{|YFp z9=qs4HU%*n(wFStYMB$A5zt>8K=R)#JVPKQhC!s%71WeMfv}Rolr<}9tFf}RkP^eY zO%VE7-qdT?@+hgK9Fq$^f?{5};2MfUDm{u_uOb8hmM0P%%(K9IA(mG$Os-+`8#wqB z0Pr001?~^rvlDaYvmWma8|J_&X9KfW<~Ghnek%P~S{TZ1J$NtjiF99jIFvmcR;nZa zQg-VrSs1+zn*&fWI`NbILPn-60mBuO_`p#iqULq#m>D5M-WcJFq6&u%x<#z z5xPvz>=sk>@ zrXdjmL!MrWN@dYMlU0WwC{JE zkvfBcPL+A$4!y+=(tjM1Z`zr#8lqFkICn2BRFueIX{fILKu>`fQU%;LBSyM=VcW|I zA$F%%D{)w)fAG{$Pzv@1j`akYcTzpZ^n6}x1tmnk37etcLpX=qDxGHuQA}^BB~oP} z+J`+WGp?bfqfhtuL;_twwH~*mr)DF;-hm*;8TmqWctj`nbtk#wms%HJmwmo+Bs+Wj6GNGn&FF} z!ad{6xhiHH1$fGHf@h{Y=N4wl^;w?EnTp(+28FB5ty#Mq-8fpPoN?t`Zd?eVykdrj z$yGHo?p)m(g{#g%RnVZa8i+M@3RjtPx@O99)wK%e$)ivW%j74oU9WI;xrTL!+8P?x k)+=09PD71?BkOY&0-U92d!Jc*O(wtM-1v76D~xUb08`EJ3;+NC literal 0 HcmV?d00001 diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/stservo_def.cpython-312.pyc b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/stservo_def.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..31b89e349eb72c3769eb799fbde138a02cd2d77b GIT binary patch literal 766 zcmZXRKX21O7{=}Vj}zOmlZsD(P#26kbV5juV=7CH9oZM!-eRRWqnb8xWjjdSED)c8 z&%n1}XFzP02Nou8l{z3oDZFD!Y$IqZO8>8;122FF6m;Ch*%;LmWhme zq=yxvVD(Df?U#4Zw!EdFgGxHCggbC$m7+#br+7>8hT=D@@{Zyc#Vy58K)9y(LGc}6 z@AixC&~@~Yp?k=;Mnz++AJcEsL!NKiqoQrukW4K5U@Kiy9~A)fpq z*4(cS zh-dROjF$fw7Qy^%whWVn=Ak5rUq^mI&b1`nXcD}9e0jBQJzYd(brC#^AHl8Yk5A44 z*Hxb9uNz!%kahF6m^E^tlQp3rN?AMaiU3eTuBe%mOLEr9)tyYvl|d%aQwp3?`Vh%f XG4#|-%+&#Gy2s9gFPs`bgn54eJwp8$ literal 0 HcmV?d00001 diff --git a/toid_interaction/toid_interaction/mechanism/__pycache__/__init__.cpython-312.pyc b/toid_interaction/toid_interaction/mechanism/__pycache__/__init__.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..f2cf6a54fb013678eda7999e80acffcea8e50ed4 GIT binary patch literal 215 zcmX@j%ge<81ZD@$XM*U*AOanHW&w&!XQ*V*Wb|9fP{ah}eFmxdRinq|(fs6fiSB zGp{7IC^5MtGd~YgAU8ERBQYPO4oIE6^E?KwJ!B Nd}L;1WGrF^vH<#uJy`$% literal 0 HcmV?d00001 diff --git a/toid_interaction/toid_interaction/mechanism/__pycache__/sekvenca_2026.cpython-312.pyc b/toid_interaction/toid_interaction/mechanism/__pycache__/sekvenca_2026.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..e9d566be9fe78b1a97331a4e0675dd312877a306 GIT binary patch literal 2419 zcma)7U2GIp6ux)&?(FPe3#Fw$C=3Nj*Fd)m1x-XvX-h0hK_NV>n$2`)fV(?C<<4wP zYpV-FSOSDKCerxCgAbY#pN)^kH(y#?(=yaZVqf}J3%(E&Ja=|ycdHP&$((!U+;h*p z=bZ1H`*SQ7MldFYS$al5=nwwm4PS+MXaI8?DM-N@(s2r7&igcCvLGyCq%Yf8=Ig1w+54MBGZhcATRd59_U`ZLKtNHP3{&(Y zMvVIlK@8edSIxW~4?AIvT6TKWWVYpq8B;HFF-v7s*3w2^A5obTIyLy-P~SW0p#euI zuQm7NWK2bk`yEW35Vx47##s{fJ9 zV&lI8UCo>aq*!{wQgdT)CppcHbdQcZf|+BgK^bHiHNa=R52S#WTXrnAbj`JN&9}U~ zj(mZpVrVJSycp@6i*$ZX=Oc-G(amcCkUtVpWb>_k*Y-_^7Tdb!+Pc2k^Zm|+@Zm!L z5(!=%x;S(rQ`&j`>@EG8e%<_)Y<~#m(bF;c6|B@_vX)&1=U##Gh}acm08)HjRxe?j zquL#c-;-iw2R&)c4j{Q5p${PUQ<$SmAr;IM3sjRRUi704&`hP3KJ)yb65gQoEMFr3^5tnaeclr{+{>;u)$AtEyV_juJv7Yd z@I*8Ltu0~O<9+?dldC+LE+A>c-%p;bLO}u2l0C^D##dIq1OkmUbXsQTRQsf9QE0b_ zu0!d@LyQ+N{=6G>FSA#CrW(_Wa|!3z7mpP$TsiL6kRztk+^KXr-rz*aey254 zR?1=I5h%xUFXuQ=o!0WB)NPxlW_8!SXaQHkcs(%w^1AVN1MG&3A7#k8nofU<+_T-7 zJ{Aw_rjpmxH`pQA!{1t~56GGyWBgYb;XS{h-S<)CK8pR0_RgZc52D)(p>;vPJJy@R fxMRIz8*W-Vj7YRN@r%Fx9ucn`UGpPSZV~?gLjefo literal 0 HcmV?d00001 diff --git a/toid_interaction/toid_interaction/mechanism/__pycache__/zidovi_load.cpython-312.pyc b/toid_interaction/toid_interaction/mechanism/__pycache__/zidovi_load.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..aa5f819e26045cb6a6be7691b4d2775dd07f1fde GIT binary patch literal 10590 zcmeHNTWk~A86MvfJ09EPYi>Ap0)#b}I9FKMEFlmsS;%FRu)$l~&rUctL%lZ|Os;mHJQ{2W2!uYNR4nc?%0wrM$HNKk>wu*a=YB zRV60MIsf_p|NQ4a|GD^_Z|1KiQw;^r@jw49Joyeq{R0ciqsVl{Ww8#N|%H?%(8stQUztz;aH6aHK<#zr_IX%c)w zqEC$s4F$$8o;r1Ud|av;pBN7e4uo>8$(bfsczO_&)al{=q0^EX{TUkhU~u?sp#RjVkP-%T2|I|sJ#B;JOG;1+Q~;=rM^Hft zJ1swg21>brP}B{|ryBNYhyi2-Lr#p4Kyo{;7C^ha}4RXE;7c`~&gOY?O=f3I8aGPjew4PS29~ zr8wW&-g(&1$HT1u(rh@w%48rM<2e$PBe9hcb3>Ok%?x$Sh*h!Ibu)zVRtYsgMrS+@ptF4(l%< zJ`h$I;yIAB>N$u~QY)xf&DFOcW>tbZsT0&jDp?iV$RSNu@%f-r)0-hzj8oLzoIF|0 zZ?-B_Z*uiQ^>lS!TcPge2txBXYVyNsm0Bt)<}Kjs^85mBRRI^rTYeO~Dhk>)DcpQ6 zP}AmoNud-=Mc!=op~M@#qOGEy-y7C2Kz+ zzj_45r!XQl0@`sZ@_a7TD0VKZ=c@9fqoQt|5R9x5EhSX9&J=jQVXHQ8L!ks?MO&A( z6trKjXy$X4R_c5N3o1z`O>$q#>zaym%}ERD7I0NM$?Xv}-BQ2Z{1Vy())tAgzhv^|O>pR=@}gL&o6a}?@sZmX?0 zrtSj2Z99!<@pvkuNlI;OJCY?f%J;`4RK0@!*k3_^(8i1Ohe=umd(r{5o_}tZuCvm3 zZoA4#;+b1v|KpgH^S$g^*|J9z`u=0YUq8KDnfv*?U{8SckaXpCgmRksUZQ5Ltm$GM zbMPC(I`LO4StryMmFo*ukgM!|QH%S9PTt&p>Pt1`Jp=cOIze-F0QOCt zP`^Qu!GhZLJKArkV88t8qgj|m%FY45W979>#agjH@Ozbr3yff$gJX~q4YpDC-ddnx z7aUtPh)%G}LUV%o5Jz2w95Tjj!4bB^Q>QLdSqvA-o1!rYF z!6s<0egwaS#{?&wVw&cl!*aQ^pe2ve>xz+kx%jGYYi{A0VxJP_e4amZz(LeIyhYcHNz~Tt@6uucB|O@Ec97(?1&=IJURFgZ`_^rzW(k3VccBBVn{~@N;JpCnX>=r~SILo0 zy<_LXSEjsp3iq-c4~Kh>J?4GRHnyp|Q@gA8aV^wBAJ}Ow79O4Dy@Lat7)+MC;w=?) zD}q(L;;h$C1j!YyAS_C)Ask^zkFySMijr=Iz|}<3eli>8C1Zl) zPr`j@3}%&ld6Eo2nc}#}P@Lrm9_A$zv@mgf?D`m&n2qof1I4lHqruQsjz3LEoJjN- z7i3`ujL*Ofmukj2ekgby!%yJwNr62Q<~b0CGr)(L7f`0Y+>EnOlE7}u>`E27 zrLq;eCCdcN>RCP(gl@W+K6B;Q-uZ=UhdX)=fSh;1Xe&PoGjN4)?q^sb|5+cLlB8~<1StgAC+gmV3^JLj`?Z>3CY zRTN#jZ1iM|o+V@RBifp_wLPT$kC8aAL?4tn9ZPiQW16{jCa2w!F}BQifAj8F?=BfT zWR32JbdOvIRkzfs_k2$o4Yt%d(RX;+cOv6Eu~?V&y_ca|QzLgi6fI3(AADeGd&D?J zM~hhNnRhQY_hy=V?+3EYLuuFW6OGC_qWYIcW3{d6DO=;6;BT9Xl>0KxeT#-{^LuI6 zc~riTQ?^2*YfY4?VVQ1)#^yiC(!M7e$}piy>%HlgBlkVu`M~0~@@IO=U{0G`mh`(H z=}jViP_*nz`;Mp0ebAEWgzA|_W2ljpD#!p~XzWT^#JctrfDDZ(%SuCMVNujOm-XI^ z-uty{!M3D7xMH@gC}9#A>+Pd=*yW~FVmgZZC~siT<#sq^p2&+C$ha4GxX6k z?Y?vKo}sc}*P(mv%&xx0u1w>Z)CIA)C@+!LC3+taBJ;LrKaf6ja`8gi_CCxi>ln@| zlNn}}$yQOiRzrD?q(;Q1!>JLN#8!9O)BS+y7VRFG*$?bT9^-Nv6>I(Jjx*oUY1c(u z86Ti!s|_q$T@|Hh4F+pphS|5ETIg5^E-~#Qk^a1D5bwtslUD=c{TT6oJs_S<2J!5h zAYNg)5X5uj0h|I@$GIcO=Bfa)A@IL5i1+d!Vfoefav;-oAYT2BAfBx88bQ2<=LGQ@ zUjW40RTji^=jQH~Ovc+9qn1 z(*qmZ>urNj?byaPb$4o)?8VNMpj%xEh*c`+Q3RwF1A#Crfw$6B4j3Ybk#Pi*w=p>i zNgna(%9A|=zf~abVsadlK1@zPB2|KDy2?W|T@@?zl&{cJis^I}Lo{8b5DhSjL7<-E z3Sh7p)9Kj?;B;+e^%S8vT}7py=fZUKm$=9Vq*MGIg2Mg_gfr2z9ip>|;(R^`*PMD! z4&Ql$>1>PHya1*Hx9B(M%*G5{W~Q{F)LiwqFg4R!3Ir z5FNEyZ7u9gwtBI)?x{hqJM`2QRqMRZey*E>Y{L@P9rjg8#_@>+N3O*K3HIv7dnkMb zCBIIR-wAEB7C|1ORI&%_`reHW9Vap1K>(-(p19UDDwXQ*R9}|r`?vYBPUZcc!c?yK EUxp`c(*OVf literal 0 HcmV?d00001 diff --git a/toid_interaction/toid_interaction/mechanism/__pycache__/zupcanik.cpython-312.pyc b/toid_interaction/toid_interaction/mechanism/__pycache__/zupcanik.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..1589cdac5e53e5442004c44d9edc275dc52dbc9a GIT binary patch literal 4610 zcmdT|T}&I<6`t|fFt*1YV;hJe29rP<^JhYSNCOE0>?DMr5Fn{acWXJG2^jphGh>=q z<94OZN>;QFv@db1KCseCOO-yLKG8Q`Dn;r;J=l>)qew++Uw9)+q*nW~J$GQRLpDGA zQnmNt-gECc_nvdUd+xnw=C2lu34;>*%iki)P7M18>S#rOKI(?8UE4LdmWyo6$&N^ruBP@IXfoI>yduTUemCMJS2 zcSc46Gc(}>4QfLiboMR*@dRVBIh?`fbayb8VD$_>0Hdq{D4j~7F#=IcHKTuo&s8yo zN7!8T!zwSSSm#p7a3~&G8VU=ML|h?A&)QMOSsQn`1&_fQYMI@ki;5>#s#Ju>Vm*7a?aYj@j ze7$-_&$H3{iiu&DBVjfkim?iLFO*`qkiaTd045kqEJxz=!B9BNBTGC`zFqclskm<; z!3$8uSYMJALY&W^2&ZCfT;P3ETw;+80})7ZiF*m5x2N}lPe?=<-@Q~M%BW;85*Ju5 zq$2U=K#UD908)I+mp;I(JNZx{gD@*11cT}N*P-mz7|1{V7*wBP@-a_V|Aj?vXoZHB zn>w@l=Vap^@UYh~MN?Ev|6PSJ70L)=i--83S^MSb0Y9S`4BD8eKO8lP`o(H(yxcxL zAbCna=&OanwOKgq56m-boN_B-i&VL%QQExMR?}&t>S~5XT9Yof;-SWmz_UzMrdq5L zY#M*555l6U!ahPYEIKr9xfQE4O=&<(qgAaC^WVElZF#mviPhT7H&!t$)@xGbw!(IZ z!wi`*ey{zFW&7V}y`~?>;x*-2i~-IODH`t+uiGVRzBQC(LBl&eD{Bx5rYa5b!c?bW zmoo_25B9OFk0IY;8<{bQWX3F-L~|Mrc7|f~84KuemGRRk5oe6rrHErj45S)0UbzK5 zaJMWTFOO){(}Vl)tr6s|u=fW>&>_Z0*LWw&NA?Nf@B`2V*=EhRa$6B?;BUsN#+qTV zr7WRQM-NBv1f{7`Kbhdsf|&| znIr>b1cSFGvH%AmeP$Hi@kl5N3V0wA@k^ffOx~FS6_M}YG|vC-x2XJiaOXRu_3B89 z;~?$$@TENCi4d~KGZA_K&CVEFrvFA{e!&w3j)!3d$X;0Hy*6nJMK(}?HTWaE5Q>KZ z9xk-Z0+g3LpoC%PJ?V?HA#R>6+vwrgPAFa<*y6+0+D2G5%zMWup@bhUFmsF~tI^XCF*Im}dD@R8VZtoqjMC3NNujfa4OJ zLQS(F2KH+v$+C=MnqdWi2B8E?aS?%q@vxdPDkPFrIn|m75xU1K#&9&jv&bxO4Tn+| zr3&S-qkhF!kxDN*CQ_Lv9v_#yJR87*lG)h0iiC8QM!G!EKq5)Pl*onv_ zjKjtUwHQ)rDo1(M7xOH{MoNf>kTxSErhHT-%@Co?A{&oPr2xT15};!t0bXPkTje4s z9ux|V#6k~(YL7-##RR?vyhUm8YlTB;l|pLS>l+EH#;|}GZ-9;c6nj;L)t^{x{o41X zuTa;UHACNZJU3Q2b~t-@@LCHka9pDfaCFQ}Tly`xBV%9QE|uD%Up!8I(|hl|w6zGh29i8>`wr&gB>RQF2)GmhhV@_4?< zx8A(z1xl6Wo$=1GhpHnE%o09OF;bqsK1z}j^`3<4}p6zfS3BlrM~fJcME;9 z5_K*+p7X8!SZ?fC%ServHyu)gKRYHjwXe-cO+DFL0J3bG-gY(s4zjaNb~ek-le^|B zhy5i{U1NVm7-2{1ApUaGTXZv6E<10__3nK0+5Guy&&>I{S%9?m4uZ7C4qMmVU+VHp zUH;9x&u-_ZZWp?y!Ic$y)g?Pld==et^uBPqa;>ZGQp^Q!P z|K`dedr)c_+%y(iZb}U|i_S3_G&gi-$6nC(mHt(i!LJsaMk#31}KPQ_xJ6s2oUR~tsFpcf>LI-{uz*jH&Fb@V5QxHDK znN$?fR518SDil575a=PGgTYWdo)ALlLFGXP{%mj@x}Lc6Q2hZ@msf8l4t=Al6?$EG zH&mZvvbE;33EAfO?3V1TFX-#{^n{^n&y>Oqp8c6<(xlg*m}h2rmRn8)d1h(M6-FRHR{$$@>duW*Af2Jb({qpzP`0>|;cV|^0V_b>ggaNP3^hH7= 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 + 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)) + 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 - # 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() - # 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) - - -def beli_zid(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 PortHandler instance - portHandler = PortHandler(DEVICENAME) + # Initialize PacketHandler instance + packetHandler = sts(portHandler) - # 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...") - # 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() + quit() - # Set port baudrate - if portHandler.setBaudRate(BAUDRATE): - print("Succeeded to change the baudrate") - else: - print("Failed to change the baudrate") - 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...") - # 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))) + quit() - # 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) + # 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)) + ) - 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)) + # 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) + ) - # Initialize variables for tracking movement - TrenutnaPos_ID3 = sts_present_position_ID3 - ProslaPos_ID3 = sts_present_position_ID3 - PredjeniPut_ID3 = 0 + 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)) - 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, 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, 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)) - + # Initialize variables for tracking movement TrenutnaPos_ID3 = sts_present_position_ID3 - TrenutnaPos_ID5 = sts_present_position_ID5 - + ProslaPos_ID3 = sts_present_position_ID3 + PredjeniPut_ID3 = 0 - # 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: + 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: + 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)) - 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() - + # 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/zuocanik.py b/toid_interaction/toid_interaction/mechanism/zuocanik.py deleted file mode 100644 index e358549..0000000 --- a/toid_interaction/toid_interaction/mechanism/zuocanik.py +++ /dev/null @@ -1,105 +0,0 @@ - -import sys -import os - - -from .STservo_sdk import * # Uses STServo SDK library - -# Default setting - -BAUDRATE = 1000000 # STServo default baudrate : 1000000 -DEVICENAME = 'COM4' # Check which port is being used on your controller -STS_MOVING_ACC = 50 # STServo moving acc - - - - -# Function to move the STServo to the target position -def zupcanik(STS_ID ,STS_MOVING_SPEED , TargetPos): - - # Initialize PortHandler instance - portHandler = PortHandler(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(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, 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_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