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/.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/Dockerfile b/Dockerfile index 111d950..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 ]] +if [[ -f ./install/setup.bash ]]; then source ./install/setup.bash -if +fi EOF 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/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 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/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; diff --git a/scripts/idi.sh b/scripts/idi.sh new file mode 100755 index 0000000..33274bb --- /dev/null +++ b/scripts/idi.sh @@ -0,0 +1,54 @@ +#!/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 + +if [[ $# -ge 3 ]]; then + MIN_ROT="$(echo "$3*a(1)/45" | bc -l)" +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}" + ;; + "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 + ;; + *) + echo "Target not defined" + exit 1 + ;; +esac +exit 0 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 diff --git a/toid_behaviors/CMakeLists.txt b/toid_behaviors/CMakeLists.txt new file mode 100644 index 0000000..b1652f5 --- /dev/null +++ b/toid_behaviors/CMakeLists.txt @@ -0,0 +1,85 @@ +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/scl.cpp + src/move_coords.cpp + src/simple_move.cpp + src/simple_rotate.cpp + src/simple_translate_x.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/move_coords.hpp b/toid_behaviors/include/toid_behaviors/move_coords.hpp new file mode 100644 index 0000000..9b84d4b --- /dev/null +++ b/toid_behaviors/include/toid_behaviors/move_coords.hpp @@ -0,0 +1,72 @@ +#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; + + 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::LOCAL; + } + +protected: + + SmoothControlLaw scl_; + + //Goal + geometry_msgs::msg::Pose target_pose_; + double target_angle_; + double target_sign_; + bool backwards_; + unsigned char mode_; + + //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..d80528f --- /dev/null +++ b/toid_behaviors/include/toid_behaviors/scl.hpp @@ -0,0 +1,35 @@ +#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); + + 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 new file mode 100644 index 0000000..fefd9f5 --- /dev/null +++ b/toid_behaviors/include/toid_behaviors/simple_move.hpp @@ -0,0 +1,175 @@ +#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" +#include "toid_msgs/msg/rival.hpp" + +namespace toid +{ + +template +class SimpleMove : public nav2_behaviors::TimedBehavior +{ +public: + using Rival = toid_msgs::msg::Rival; + + 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(); + + 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_); + current_vel_ = msg.twist.twist; + }); + control_duration_ = 1.0 / this->cycle_frequency_; + configureCB(); + } + + void onCleanup() override + { + odom_sub_.reset(); + cleanupCB(); + } + + 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(); + } + + nav2_behaviors::ResultStatus onRun( + const std::shared_ptr command) override + { + geometry_msgs::msg::PoseStamped pose; + geometry_msgs::msg::Twist vel; + { + std::lock_guard lock(mut_); + vel = current_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::PoseStamped pose; + geometry_msgs::msg::Twist vel; + geometry_msgs::msg::Twist out_vel; + auto vel_p = std::make_unique(); + { + std::lock_guard lock(mut_); + vel = current_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_; + this->vel_pub_->publish(std::move(vel_p)); + 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 new file mode 100644 index 0000000..758eb68 --- /dev/null +++ b/toid_behaviors/include/toid_behaviors/simple_rotate.hpp @@ -0,0 +1,60 @@ +#pragma once + +#include "toid_behaviors/simple_move.hpp" +#include "toid_msgs/action/simple_rotate.h" +#include "toid_msgs/msg/rival.hpp" + +namespace toid +{ +using RotateAction = toid_msgs::action::SimpleRotate; +using namespace nav2_behaviors; + +class SimpleRotateBehavior : public SimpleMove +{ +public: + SimpleRotateBehavior(); + ~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; + + virtual nav2_core::CostmapInfoType getResourceInfo() override + { + return nav2_core::CostmapInfoType::LOCAL; + } + + void calc_err_and_sign( + double last_angle, double current_yaw, double & min_turn_angle, double & err, double & sign); + + double check_space(const geometry_msgs::msg::Pose pose, const double e, const double sign); + + double calc_speed(const double err, const double sign, const double angular_speed); + +protected: + //Goal + double target_angle_; + double min_turn_angle_; + double initial_direction_; + unsigned char mode_; + + //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_; + double lookahead_; +}; + +} // namespace toid 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/package.xml b/toid_behaviors/package.xml new file mode 100644 index 0000000..4fafaa1 --- /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_behaviors + pluginlib + tf2 + tf2_geometry_msgs + tf2_ros + toid_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/toid_behaviors/src/move_coords.cpp b/toid_behaviors/src/move_coords.cpp new file mode 100644 index 0000000..c71336e --- /dev/null +++ b/toid_behaviors/src/move_coords.cpp @@ -0,0 +1,204 @@ +#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; + max_vel_speed_ = command->max_speed; + + if (command->max_speed == 0) { + auto node = node_.lock(); + node->get_parameter(behavior_name_ + ".max_vel_speed", max_vel_speed_); + } + + mode_ = command->mode; + + scl_.k_phi = k_phi_; + scl_.k_delta = k_delta_; + scl_.bbeta = beta_; + scl_.lam = lambda_; + scl_.slowdown_radius = slowdown_radius_; + scl_.v_angular_max = max_angular_speed_; + scl_.v_linear_min = min_vel_speed_; + scl_.v_linear_max = max_vel_speed_; + + last_speed_ = vel.angular.x; + + return ResultStatus{Status::SUCCEEDED}; +} + +double 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; + const bool check_map = !(mode_ & MoveAction::Goal::IGNORE_OBSTACLES); + for (int i = 0; i < samples; i++) { + scl_.step(target_pose_, current_pose, control_duration_, backwards_); + + geometry_msgs::msg::Pose2D p; + p.x = current_pose.position.x; + p.y = current_pose.position.y; + p.theta = tf2::getYaw(current_pose.orientation); + + if (check_map && !local_collision_checker_->isCollisionFree(p, i == 0)) { + return true; + } + + if (check_rival_collision(p)) { + return true; + } + + last_ok_pose = current_pose; + const double dist_left = + distanceToTarget(current_pose, target_pose_.position, target_angle_, backwards_); + if (dist_left < 0.005) { + return false; + } + } + return false; +} + +ResultStatus 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) { + out_vel.linear.x = 0; + out_vel.angular.z = 0; + return ResultStatus{Status::SUCCEEDED}; + } + + const double current_yaw = tf2::getYaw(pose.orientation); + double angle_dist = angles::shortest_angular_distance(current_yaw, target_angle_); + + geometry_msgs::msg::Pose last_ok_pose; + if (collisionDetection(pose, last_ok_pose)) { + dist_left = distanceToTarget( + pose, last_ok_pose.position, tf2::getYaw(last_ok_pose.orientation), backwards_); + if (dist_left <= 0.02) { + out_vel.linear.x = 0; + out_vel.angular.z = 0; + } else { + scl_.v_linear_max = target_sign_ * velocityTarget(dist_left); + scl_.calculate_vel(last_ok_pose, pose, out_vel, backwards_); + } + + last_speed_ = out_vel.linear.x; + + RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Distance Left: %lf", dist_left); + RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Max Speed: %lf", scl_.v_linear_max); + + return ResultStatus{Status::RUNNING}; + } + + if (dist_left <= 0.02) { + out_vel.linear.x = velocityTarget(dist_left); + out_vel.angular.z = std::clamp(kp_ * angle_dist, -max_angular_speed_, max_angular_speed_); + last_speed_ = out_vel.linear.x; + return ResultStatus{Status::RUNNING}; + } + + scl_.v_linear_max = target_sign_ * velocityTarget(dist_left); + scl_.calculate_vel(target_pose_, pose, out_vel, backwards_); + + last_speed_ = out_vel.linear.x; + + RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Distance Left: %lf", dist_left); + RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Max Speed: %lf", scl_.v_linear_max); + + return ResultStatus{Status::RUNNING}; +} + +} // namespace toid + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(toid::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..fe22bdc --- /dev/null +++ b/toid_behaviors/src/scl.cpp @@ -0,0 +1,79 @@ +#include "toid_behaviors/scl.hpp" + +#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 +{ +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); +} + +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_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..9fdde0f --- /dev/null +++ b/toid_behaviors/src/simple_rotate.cpp @@ -0,0 +1,173 @@ +#include "toid_behaviors/simple_rotate.hpp" + +#include + +#include "angles/angles.h" +#include "tf2/convert.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace toid +{ + +SimpleRotateBehavior::SimpleRotateBehavior() : SimpleMove() {} +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_); + + 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( + const std::shared_ptr command, const geometry_msgs::msg::Pose & pose, + const geometry_msgs::msg::Twist & vel) +{ + 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; + mode_ = command->mode; + + if (command->max_speed == 0) { + auto node = node_.lock(); + node->get_parameter(behavior_name_ + ".max_angular_vel", max_angular_speed_); + } + + geometry_msgs::msg::Pose2D pose2d; + pose2d.x = pose.position.x; + pose2d.y = pose.position.y; + pose2d.theta = initial_direction_; + local_collision_checker_->isCollisionFree(pose2d, true); + + last_angle_ = tf2::getYaw(pose.orientation); + + angular_speed_ = vel.angular.z; + last_time_ = clock_->now().seconds(); + return ResultStatus{Status::SUCCEEDED}; +} + +void 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); + double min_turn_angle = min_turn_angle_; + double angular_speed = angular_speed_; + double err, sign; + + calc_err_and_sign(last_angle_, current_yaw, min_turn_angle, err, sign); + + if (!(mode_ & RotateAction::Goal::IGNORE_OBSTACLES)) { + err = check_space(pose, err, sign); + } + + double speed = 0.0; + + if (err != 0.0) { + speed = calc_speed(err, sign, angular_speed); + } + + if (min_turn_angle_ == 0 && std::fabs(current_yaw - target_angle_) < 0.01) { + return ResultStatus{Status::SUCCEEDED}; + } + + min_turn_angle_ = min_turn_angle; + last_angle_ = current_yaw; + angular_speed_ = speed; + out_vel.angular.z = speed; + return ResultStatus{Status::RUNNING}; +} + +double 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" +PLUGINLIB_EXPORT_CLASS(toid::SimpleRotateBehavior, nav2_core::Behavior); \ No newline at end of file diff --git a/toid_behaviors/src/simple_translate_x.cpp b/toid_behaviors/src/simple_translate_x.cpp new file mode 100644 index 0000000..e38a431 --- /dev/null +++ b/toid_behaviors/src/simple_translate_x.cpp @@ -0,0 +1,102 @@ +#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; + 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_; + + 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 new file mode 100644 index 0000000..10cc958 --- /dev/null +++ b/toid_behaviors/toid_behaviors.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + \ No newline at end of file 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..e2a1bdb --- /dev/null +++ b/toid_bt/behavior_trees/behavior1.xml @@ -0,0 +1,111 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Action server name + + + + + + + Action server name + + + Service name + + + + Service name + + + Service name + + + + + Action server name + + + Service name + + + + diff --git a/toid_bt/behavior_trees/calibration_routines.xml b/toid_bt/behavior_trees/calibration_routines.xml new file mode 100644 index 0000000..19b8c42 --- /dev/null +++ b/toid_bt/behavior_trees/calibration_routines.xml @@ -0,0 +1,173 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Service name + + + + + + + + Action server name + + + + + + 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 new file mode 100644 index 0000000..054a172 --- /dev/null +++ b/toid_bt/behavior_trees/toid_behaviors.btproj @@ -0,0 +1,59 @@ + + + + + + + + + + + Service name + + + + + + + + Action server name + + + + + + Action server name + + + + + + + Action server name + + + Service name + + + + Service name + + + Service name + + + + + + Service name + + + + + Action server 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..2512afc --- /dev/null +++ b/toid_bt/include/toid_bt/plugins/end_calib_action.hpp @@ -0,0 +1,59 @@ +#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(); + double 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; + + RCLCPP_INFO(logger(), "width is %lf", 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..2b759c0 --- /dev/null +++ b/toid_bt/include/toid_bt/plugins/move_coords_action.hpp @@ -0,0 +1,77 @@ +#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("max_speed", 0, {}), + BT::InputPort("backwards", false, {}), + //BT::InputPort("options"), + }); + } + + bool setGoal(Goal & goal) override + { + auto x_goal = getInput("x"); + 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(); + 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)); + } + + goal.max_speed = max_speed; + goal.backwards = backwards; + goal.mode = 1; + + 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..7597dc9 --- /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("max_speed", 0, {}), + //BT::InputPort("options"), + }); + } + + bool setGoal(Goal & goal) override + { + auto angle = getInput("angle"); + auto min_angle = getInput("min_angle"); + auto max_speed = getInput("max_speed"); + goal.angle = angle.value(); + goal.min_angle = min_angle.value(); + 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; + } + + 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..9649a8f --- /dev/null +++ b/toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp @@ -0,0 +1,68 @@ +#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("max_speed", 0, {}), + //BT::InputPort("options"), + }); + } + + bool setGoal(Goal & goal) override + { + auto x = getInput("x").value(); + auto y = getInput("y").value(); + 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 = angles::from_degrees(min_angle); + goal.max_speed = max_speed; + goal.mode = 1; + + 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/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/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/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/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..889aede --- /dev/null +++ b/toid_bt/src/bt_executor.cpp @@ -0,0 +1,116 @@ +#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" +#include "toid_bt/plugins/send_text_action.hpp" +#include "toid_bt/plugins/stuck_detector_decorator.hpp" +#include "toid_bt/plugins/translate_x_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( + "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("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; +} + +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 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')) ) diff --git a/toid_costmaps/CMakeLists.txt b/toid_costmaps/CMakeLists.txt new file mode 100644 index 0000000..2aae4e9 --- /dev/null +++ b/toid_costmaps/CMakeLists.txt @@ -0,0 +1,83 @@ +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 + ament_index_cpp + Boost + 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 + src/rival_layer.cpp +) + +find_package(ament_cmake REQUIRED) +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 + + 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/ +) + +install( + DIRECTORY elements + DESTINATION share/${PROJECT_NAME}/ +) + +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/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 new file mode 100644 index 0000000..f589d50 --- /dev/null +++ b/toid_costmaps/include/toid_costmaps/game_elements_layer.hpp @@ -0,0 +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() {} + + 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 placeElement(nav2_costmap_2d::Costmap2D & grid, const GameElement & element); + + bool isClearable() override { return true; } + + void initGameElements(); + + void active_elements_cb(ActiveElements::SharedPtr 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::SharedPtr 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..87c6914 --- /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::SharedPtr msg); + +private: + double last_min_x_, last_min_y_, last_max_x_, last_max_y_; + + bool need_recalculation_; + + uint debounce_ = 0; + + Rivals::SharedPtr 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 new file mode 100644 index 0000000..80080d8 --- /dev/null +++ b/toid_costmaps/package.xml @@ -0,0 +1,32 @@ + + + + toid_costmaps + 0.0.0 + TODO: Package description + Pimpest + Apache-2.0 + + ament_cmake + + angles + ament_index_cpp + boost + 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..fb11c92 --- /dev/null +++ b/toid_costmaps/src/game_elements_layer.cpp @@ -0,0 +1,138 @@ +#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() +{ + 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::SharedPtr msg) +{ + active_elements_ = msg; +} + +void GameElementLayer::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 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(ex + 0.001, ey + 0.001, mx, my); + element.end(ex, ey); + uint mmx, mmy; + in_bounds &= worldToMap(ex + 0.001, ey + 0.001, 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); + } + } + } +} + +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" +PLUGINLIB_EXPORT_CLASS(toid::GameElementLayer, nav2_costmap_2d::Layer); \ No newline at end of file diff --git a/toid_costmaps/src/rival_layer.cpp b/toid_costmaps/src/rival_layer.cpp new file mode 100644 index 0000000..7ae1fbf --- /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::SharedPtr msg) +{ + if (msg->point.size() != 0 || debounce_++ > 10) { + rivals_ = 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 new file mode 100644 index 0000000..3211914 --- /dev/null +++ b/toid_costmaps/toid_costmaps.xml @@ -0,0 +1,10 @@ + + + + + + + + + + \ No newline at end of file diff --git a/toid_interaction/package.xml b/toid_interaction/package.xml new file mode 100644 index 0000000..2959e79 --- /dev/null +++ b/toid_interaction/package.xml @@ -0,0 +1,20 @@ + + + + toid_interaction + 0.0.0 + TODO: Package description + pimpest + MIT + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + python3-serial + + + ament_python + + diff --git a/toid_interaction/resource/toid_interaction b/toid_interaction/resource/toid_interaction new file mode 100644 index 0000000..e69de29 diff --git a/toid_interaction/setup.cfg b/toid_interaction/setup.cfg new file mode 100644 index 0000000..eb87452 --- /dev/null +++ b/toid_interaction/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/toid_interaction +[install] +install_scripts=$base/lib/toid_interaction diff --git a/toid_interaction/setup.py b/toid_interaction/setup.py new file mode 100644 index 0000000..6473e97 --- /dev/null +++ b/toid_interaction/setup.py @@ -0,0 +1,31 @@ +from setuptools import find_packages, setup + +package_name = 'toid_interaction' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='pimpest', + maintainer_email='82343504+pimpest@users.noreply.github.com', + description='TODO: Package description', + license='MIT', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + 'sequence = toid_interaction.mechanism.sekvenca_2026:main', + 'node = toid_interaction.interaction_node:main' + ], + }, +) diff --git a/toid_interaction/test/test_copyright.py b/toid_interaction/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/toid_interaction/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/toid_interaction/test/test_flake8.py b/toid_interaction/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/toid_interaction/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/toid_interaction/test/test_pep257.py b/toid_interaction/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/toid_interaction/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/toid_interaction/toid_interaction/__init__.py b/toid_interaction/toid_interaction/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/toid_interaction/toid_interaction/__pycache__/__init__.cpython-312.pyc b/toid_interaction/toid_interaction/__pycache__/__init__.cpython-312.pyc new file mode 100644 index 0000000..91a3dda Binary files /dev/null and b/toid_interaction/toid_interaction/__pycache__/__init__.cpython-312.pyc differ 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 0000000..f4f6b27 Binary files /dev/null and b/toid_interaction/toid_interaction/__pycache__/interaction_node.cpython-312.pyc differ diff --git a/toid_interaction/toid_interaction/interaction_node.py b/toid_interaction/toid_interaction/interaction_node.py new file mode 100644 index 0000000..1a2bdcc --- /dev/null +++ b/toid_interaction/toid_interaction/interaction_node.py @@ -0,0 +1,97 @@ +import rclpy +from rclpy.node import Node +from std_srvs.srv import Empty + +from serial.tools import list_ports + +from toid_interaction.mechanism.sekvenca_2026 import okreni, okreni_niz +from toid_interaction.mechanism.zidovi_load import ZidoviAction +from toid_interaction.mechanism.zupcanik import ZupcanikAction +from toid_msgs.srv import SendString + +class InteracitionNode(Node): + step: int = 0 + def __init__(self): + super().__init__('ToidInteractionNode') + + self.find_sigma() + + self.srv = self.create_service( + Empty, + '/sequence1', + self.sequence1_cb + ) + + self.srv = self.create_service( + SendString, + '/sequence2', + self.sequence2_cb + ) + + self.srv = self.create_service( + Empty, + '/sequence3', + self.sequence3_cb + ) + + + + self.get_logger().info("Service 'sequence1' ready.") + + def find_sigma(self): + for port_info in list_ports.comports(): + if port_info.vid == 0x18a6 and port_info.pid == 0x55d3: + break + + print(port_info.device) + self.st_motor_device_name = port_info.device + + def sequence1_cb(self, request, response): + if(self.step != 0): + return Empty.Response() + okreni(5) + zupcanik = ZupcanikAction(self.st_motor_device_name) + zupcanik.zupcanik(1, -1010, 25) + self.step = 1 + return response + + def sequence2_cb(self, request : SendString.Request, response : SendString.Response): + if(self.step != 1): + return Empty.Response() + zidovi = ZidoviAction(self.st_motor_device_name) + zidovi.beli_zid(1) + zidovi.plavi_zid(1) + + okreni_niz(request.text) + + zidovi.plavi_zid(0, TargetPos=150) + zidovi.beli_zid(0, TargetPos=450) + self.step = 2 + return response + + def sequence3_cb(self, request, response): + if(self.step != 2): + return Empty.Response() + + zupcanik = ZupcanikAction(self.st_motor_device_name) + zidovi = ZidoviAction(self.st_motor_device_name) + + zupcanik.zupcanik(1, 1010, 25) + zidovi.plavi_zid(0, TargetPos=150) + zidovi.beli_zid(0, TargetPos=150) + okreni(5) + self.step = 0 + return response + + +def main(args=None): + rclpy.init(args=args) + + node = InteracitionNode() + rclpy.spin(node) + + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/__init__.py b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__init__.py new file mode 100644 index 0000000..58551ab --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__init__.py @@ -0,0 +1,8 @@ +#!/usr/bin/env python + +from .port_handler import * +from .protocol_packet_handler import * +from .group_sync_write import * +from .group_sync_read import * +from .sts import * +from .scscl import * diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/__init__.cpython-312.pyc b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/__init__.cpython-312.pyc new file mode 100644 index 0000000..99ba63e Binary files /dev/null and b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/__init__.cpython-312.pyc differ diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/group_sync_read.cpython-312.pyc b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/group_sync_read.cpython-312.pyc new file mode 100644 index 0000000..d14c148 Binary files /dev/null and b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/group_sync_read.cpython-312.pyc differ 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 0000000..3ad1f28 Binary files /dev/null and b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/group_sync_write.cpython-312.pyc differ 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 0000000..512aaa2 Binary files /dev/null and b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/port_handler.cpython-312.pyc differ diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/protocol_packet_handler.cpython-312.pyc b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/protocol_packet_handler.cpython-312.pyc new file mode 100644 index 0000000..22f2fa5 Binary files /dev/null and b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/protocol_packet_handler.cpython-312.pyc differ 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 0000000..01ac8dd Binary files /dev/null and b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/scscl.cpython-312.pyc differ 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 0000000..963885b Binary files /dev/null and b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/sts.cpython-312.pyc differ 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 0000000..31b89e3 Binary files /dev/null and b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/stservo_def.cpython-312.pyc differ 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/__pycache__/__init__.cpython-312.pyc b/toid_interaction/toid_interaction/mechanism/__pycache__/__init__.cpython-312.pyc new file mode 100644 index 0000000..f2cf6a5 Binary files /dev/null and b/toid_interaction/toid_interaction/mechanism/__pycache__/__init__.cpython-312.pyc differ 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 0000000..e9d566b Binary files /dev/null and b/toid_interaction/toid_interaction/mechanism/__pycache__/sekvenca_2026.cpython-312.pyc differ 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 0000000..aa5f819 Binary files /dev/null and b/toid_interaction/toid_interaction/mechanism/__pycache__/zidovi_load.cpython-312.pyc differ 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 0000000..1589cda Binary files /dev/null and b/toid_interaction/toid_interaction/mechanism/__pycache__/zupcanik.cpython-312.pyc differ diff --git a/toid_interaction/toid_interaction/mechanism/sekvenca_2026.py b/toid_interaction/toid_interaction/mechanism/sekvenca_2026.py new file mode 100644 index 0000000..442f370 --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/sekvenca_2026.py @@ -0,0 +1,67 @@ +# u ovom fajlu sam napisala celu sekvencu koju obuhvata hvatanje zirova podizanje, okretanje, spustanje i odlaganje +# dole cu napisaci koje funkicje su koja sekvenca ono sto smo pricali , nisam htela da pravim jos jedan fajl sa sekvencama da +# ne bude da ima opet previse fajlova +# sekvecncu 4 nije jos zavrsila poslacu ti to naknadno +from .zidovi_load import ZidoviAction +from .zupcanik import ZupcanikAction +import time +import serial +import serial.tools.list_ports as list_ports + +SERIAL_ID = "50443405C8C3B21C" + + +def okreni(i): + for port_info in list_ports.comports(): + if port_info.serial_number == SERIAL_ID: + break + + ser = serial.Serial(port_info.device, 115200, timeout=1) + ser.write(str(i).encode()) + + +def okreni_niz(broj): + """ + Funkcija koja prima string od 4 karaktera (0 ili 1) + """ + if len(broj) != 4: + raise ValueError("Binarni niz mora imati tačno 4 karaktera") + + okreni(6) + for i, char in enumerate(broj): + if char == "1": + okreni(i + 1) + + +def main(): + + zidovi = ZidoviAction('/dev/ttyACM1') + zupcanik = ZupcanikAction('/dev/ttyACM1') + + + # sekvenca 1 + okreni(5) + zupcanik.zupcanik(1, -1010, 25) + + # sekvenca 2 + zidovi.beli_zid(1) + zidovi.plavi_zid(1) + + okreni_niz("1010") + + zidovi.plavi_zid(0, TargetPos=150) + zidovi.beli_zid(0, TargetPos=450) + + + # sekvenca 3 + + zupcanik.zupcanik(1, 1010, 25) + + zidovi.plavi_zid(0, TargetPos=150) + zidovi.beli_zid(0, TargetPos=150) + + time.sleep(1) + + +if __name__ == "__main__": + main() diff --git a/toid_interaction/toid_interaction/mechanism/zidovi_load.py b/toid_interaction/toid_interaction/mechanism/zidovi_load.py new file mode 100644 index 0000000..b623e2b --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/zidovi_load.py @@ -0,0 +1,366 @@ +from .STservo_sdk import PortHandler, sts, COMM_SUCCESS, STS_ID +# da li ovo dole treba da importujem ako je nalazi u STservo_sdk? +# from stservo_def import COMM_SUCCESS + +# Default settings + + +class ZidoviAction: + BAUDRATE : int + DEVICENAME : str + STS_MOVING_ACC: int + def __init__( + self, + devicename, # Check which port is being used on your controller + baudrate=1000000, # STServo default baudrate + sts_moving_acc=0, # STServo moving acceleration + ): + self.BAUDRATE = baudrate + self.DEVICENAME = devicename + self.STS_MOVING_ACC = sts_moving_acc + + + def plavi_zid(self, smer, brzina=1500, opterecenje_threshold=75, TargetPos=300): + # TargetPos = 300 # Target position in degrees + TargetPos = TargetPos * 4096 / 360 # Convert to motor ticks + if smer == 1: + Speed_ID2 = brzina # Speed for motor ID 2 + Speed_ID4 = -brzina # Speed for motor ID 4 (opposite sign) + else: + Speed_ID2 = -brzina # Speed for motor ID 2 + Speed_ID4 = brzina # Speed for motor ID 4 (opposite sign) + + # Initialize PortHandler instance + portHandler = PortHandler(self.DEVICENAME) + + # Initialize PacketHandler instance + packetHandler = sts(portHandler) + + # Open port + if portHandler.openPort(): + print("Succeeded to open the port") + else: + print("Failed to open the port") + print("Press any key to terminate...") + + quit() + + # Set port baudrate + if portHandler.setBaudRate(self.BAUDRATE): + print("Succeeded to change the baudrate") + else: + print("Failed to change the baudrate") + print("Press any key to terminate...") + + quit() + + # Set motors to Wheel Mode + for motor_id in [2, 4]: + sts_comm_result, sts_error = packetHandler.WheelMode(motor_id) + if sts_comm_result != COMM_SUCCESS: + print( + "[ID:%01d] %s" + % (motor_id, packetHandler.getTxRxResult(sts_comm_result)) + ) + elif sts_error != 0: + print( + "[ID:%01d] %s" + % (motor_id, packetHandler.getRxPacketError(sts_error)) + ) + + # Read initial positions for both motors + sts_present_position_ID2, sts_present_speed_ID2, sts_comm_result, sts_error = ( + packetHandler.ReadPosSpeed(2) + ) + sts_present_position_ID4, sts_present_speed_ID4, sts_comm_result, sts_error = ( + packetHandler.ReadPosSpeed(4) + ) + + print("Initial positions:") + print( + "[ID:002] PresPos:%d PresSpd:%d" + % (sts_present_position_ID2, sts_present_speed_ID2) + ) + print( + "[ID:004] PresPos:%d PresSpd:%d" + % (sts_present_position_ID4, sts_present_speed_ID4) + ) + + # Initialize variables for tracking movement + TrenutnaPos_ID2 = sts_present_position_ID2 + ProslaPos_ID2 = sts_present_position_ID2 + PredjeniPut_ID2 = 0 + + TrenutnaPos_ID4 = sts_present_position_ID4 + ProslaPos_ID4 = sts_present_position_ID4 + PredjeniPut_ID4 = 0 + packetHandler.SetMaxLoad(STS_ID, 80) + + while True: + # Write goal position, speed, and acceleration for both motors + sts_comm_result, sts_error = packetHandler.WriteSpec( + 2, Speed_ID2, self.STS_MOVING_ACC + ) + if sts_comm_result != COMM_SUCCESS: + print("[ID:002] %s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("[ID:002] %s" % packetHandler.getRxPacketError(sts_error)) + + sts_comm_result, sts_error = packetHandler.WriteSpec( + 4, Speed_ID4, self.STS_MOVING_ACC + ) + if sts_comm_result != COMM_SUCCESS: + print("[ID:004] %s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("[ID:004] %s" % packetHandler.getRxPacketError(sts_error)) + + # Update positions for both motors + ProslaPos_ID2 = TrenutnaPos_ID2 + ProslaPos_ID4 = TrenutnaPos_ID4 + + ( + sts_present_position_ID2, + sts_present_speed_ID2, + sts_comm_result, + sts_error, + ) = packetHandler.ReadPosSpeed(2) + if sts_comm_result != COMM_SUCCESS: + print("[ID:002] %s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("[ID:002] %s" % packetHandler.getRxPacketError(sts_error)) + + ( + sts_present_position_ID4, + sts_present_speed_ID4, + sts_comm_result, + sts_error, + ) = packetHandler.ReadPosSpeed(4) + if sts_comm_result != COMM_SUCCESS: + print("[ID:004] %s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("[ID:004] %s" % packetHandler.getRxPacketError(sts_error)) + sts_present_load2, sts_comm_result, sts_error = packetHandler.ReadLoad(2) + sts_present_load4, sts_comm_result, sts_error = packetHandler.ReadLoad(4) + opterecenje2 = (sts_present_load2 & ((1 << 10) - 1)) * 0.1 + opterecenje4 = (sts_present_load4 & ((1 << 10) - 1)) * 0.1 + print("Current positions:") + print( + "[ID:002] PresPos:%d PresSpd:%d Load:%d" + % (sts_present_position_ID2, sts_present_speed_ID2, opterecenje2) + ) + print( + "[ID:004] PresPos:%d PresSpd:%d Load:%d" + % (sts_present_position_ID4, sts_present_speed_ID4, opterecenje4) + ) + + TrenutnaPos_ID2 = sts_present_position_ID2 + TrenutnaPos_ID4 = sts_present_position_ID4 + + # Check if the positions have changed + if ( + opterecenje2 > opterecenje_threshold + or opterecenje4 > opterecenje_threshold + ): + print( + "High load detected: [ID:002] PresPos:%d PresSpd:%d Load:%d" + % (sts_present_position_ID2, sts_present_speed_ID2, opterecenje2) + ) + print( + "High load detected: [ID:004] PresPos:%d PresSpd:%d Load:%d" + % (sts_present_position_ID4, sts_present_speed_ID4, opterecenje4) + ) + packetHandler.WriteSpec(2, 0, 0) # Stop motor ID 2 + packetHandler.WriteSpec(4, 0, 0) # Stop motor ID 4 + break + if ( + abs(TrenutnaPos_ID2 - ProslaPos_ID2) > 3000 + or abs(TrenutnaPos_ID4 - ProslaPos_ID4) > 3000 + ): + PredjeniPut_ID2 = PredjeniPut_ID2 + PredjeniPut_ID4 = PredjeniPut_ID4 + else: + PredjeniPut_ID2 += abs(TrenutnaPos_ID2 - ProslaPos_ID2) + PredjeniPut_ID4 += abs(TrenutnaPos_ID4 - ProslaPos_ID4) + + print("PredjeniPut ID2:%d deg " % (PredjeniPut_ID2 * 360 / 4096)) + print("PredjeniPut ID4:%d deg " % (PredjeniPut_ID4 * 360 / 4096)) + + # Stop if both motors reach their target positions + if PredjeniPut_ID2 >= TargetPos and PredjeniPut_ID4 >= TargetPos + 10: + print("Target positions reached.") + packetHandler.WriteSpec(2, 0, 0) # Stop motor ID 2 + packetHandler.WriteSpec(4, 0, 0) # Stop motor ID 4 + break + + # Close port + portHandler.closePort() + + def beli_zid(self, smer, brzina=1500, opterecenje_threshold=75, TargetPos=600): + # TargetPos = 600 # Target position in degrees + TargetPos = TargetPos * 4096 / 360 # Convert to motor ticks + if smer == 1: + Speed_ID3 = -brzina # Speed for motor ID 3 + Speed_ID5 = brzina # Speed for motor ID 5 (opposite sign) + + else: + Speed_ID3 = brzina # Speed for motor ID 3 + Speed_ID5 = -brzina # Speed for motor ID 5 (opposite sign) + + # Initialize PortHandler instance + portHandler = PortHandler(self.DEVICENAME) + + # Initialize PacketHandler instance + packetHandler = sts(portHandler) + + # Open port + if portHandler.openPort(): + print("Succeeded to open the port") + else: + print("Failed to open the port") + print("Press any key to terminate...") + + quit() + + # Set port baudrate + if portHandler.setBaudRate(self.BAUDRATE): + print("Succeeded to change the baudrate") + else: + print("Failed to change the baudrate") + print("Press any key to terminate...") + + quit() + + # Set motors to Wheel Mode + for motor_id in [3, 5]: + sts_comm_result, sts_error = packetHandler.WheelMode(motor_id) + if sts_comm_result != COMM_SUCCESS: + print( + "[ID:%01d] %s" + % (motor_id, packetHandler.getTxRxResult(sts_comm_result)) + ) + elif sts_error != 0: + print( + "[ID:%01d] %s" + % (motor_id, packetHandler.getRxPacketError(sts_error)) + ) + + # Read initial positions for both motors + sts_present_position_ID3, sts_present_speed_ID3, sts_comm_result, sts_error = ( + packetHandler.ReadPosSpeed(3) + ) + sts_present_position_ID5, sts_present_speed_ID5, sts_comm_result, sts_error = ( + packetHandler.ReadPosSpeed(5) + ) + + print("Initial positions:") + # print("[ID:003] PresPos:%d PresSpd:%d" % (sts_present_position_ID3, sts_present_speed_ID3)) + # print("[ID:005] PresPos:%d PresSpd:%d" % (sts_present_position_ID5, sts_present_speed_ID5)) + + # Initialize variables for tracking movement + TrenutnaPos_ID3 = sts_present_position_ID3 + ProslaPos_ID3 = sts_present_position_ID3 + PredjeniPut_ID3 = 0 + + TrenutnaPos_ID5 = sts_present_position_ID5 + ProslaPos_ID5 = sts_present_position_ID5 + PredjeniPut_ID5 = 0 + packetHandler.SetMaxLoad(STS_ID, 80) + + while True: + # Write goal position, speed, and acceleration for both motors + sts_comm_result, sts_error = packetHandler.WriteSpec( + 3, Speed_ID3, self.STS_MOVING_ACC + ) + if sts_comm_result != COMM_SUCCESS: + print("[ID:003] %s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("[ID:003] %s" % packetHandler.getRxPacketError(sts_error)) + + sts_comm_result, sts_error = packetHandler.WriteSpec( + 5, Speed_ID5, self.STS_MOVING_ACC + ) + if sts_comm_result != COMM_SUCCESS: + print("[ID:005] %s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("[ID:005] %s" % packetHandler.getRxPacketError(sts_error)) + + # Update positions for both motors + ProslaPos_ID3 = TrenutnaPos_ID3 + ProslaPos_ID5 = TrenutnaPos_ID5 + + ( + sts_present_position_ID3, + sts_present_speed_ID3, + sts_comm_result, + sts_error, + ) = packetHandler.ReadPosSpeed(3) + if sts_comm_result != COMM_SUCCESS: + print("[ID:003] %s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("[ID:003] %s" % packetHandler.getRxPacketError(sts_error)) + + ( + sts_present_position_ID5, + sts_present_speed_ID5, + sts_comm_result, + sts_error, + ) = packetHandler.ReadPosSpeed(5) + if sts_comm_result != COMM_SUCCESS: + print("[ID:005] %s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("[ID:005] %s" % packetHandler.getRxPacketError(sts_error)) + sts_present_load3, sts_comm_result, sts_error = packetHandler.ReadLoad(3) + sts_present_load5, sts_comm_result, sts_error = packetHandler.ReadLoad(5) + opterecenje3 = (sts_present_load3 & ((1 << 10) - 1)) * 0.1 + opterecenje5 = (sts_present_load5 & ((1 << 10) - 1)) * 0.1 + print("Current positions:") + print( + "[ID:003] PresPos:%d PresSpd:%d Load:%d" + % (sts_present_position_ID3, sts_present_speed_ID3, opterecenje3) + ) + print( + "[ID:005] PresPos:%d PresSpd:%d Load:%d" + % (sts_present_position_ID5, sts_present_speed_ID5, opterecenje5) + ) + + TrenutnaPos_ID3 = sts_present_position_ID3 + TrenutnaPos_ID5 = sts_present_position_ID5 + + # Check if the positions have changed + if ( + opterecenje3 > opterecenje_threshold + or opterecenje5 > opterecenje_threshold + ): + print( + "High load detected: [ID:003] PresPos:%d PresSpd:%d Load:%d" + % (sts_present_position_ID3, sts_present_speed_ID3, opterecenje3) + ) + print( + "High load detected: [ID:005] PresPos:%d PresSpd:%d Load:%d" + % (sts_present_position_ID5, sts_present_speed_ID5, opterecenje5) + ) + packetHandler.WriteSpec(3, 0, 0) # Stop motor ID 3 + packetHandler.WriteSpec(5, 0, 0) # Stop motor ID 5 + break + if ( + abs(TrenutnaPos_ID3 - ProslaPos_ID3) > 3000 + or abs(TrenutnaPos_ID5 - ProslaPos_ID5) > 3000 + ): + PredjeniPut_ID3 = PredjeniPut_ID3 + PredjeniPut_ID5 = PredjeniPut_ID5 + else: + PredjeniPut_ID3 += abs(TrenutnaPos_ID3 - ProslaPos_ID3) + PredjeniPut_ID5 += abs(TrenutnaPos_ID5 - ProslaPos_ID5) + + print("PredjeniPut ID3:%d deg " % (PredjeniPut_ID3 * 360 / 4096)) + print("PredjeniPut ID5:%d deg " % (PredjeniPut_ID5 * 360 / 4096)) + + # Stop if both motors reach their target positions + if PredjeniPut_ID3 >= TargetPos and PredjeniPut_ID5 >= TargetPos + 10: + print("Target positions reached.") + packetHandler.WriteSpec(3, 0, 0) # Stop motor ID 3 + packetHandler.WriteSpec(5, 0, 0) # Stop motor ID 5 + break + + # Close port + portHandler.closePort() diff --git a/toid_interaction/toid_interaction/mechanism/zupcanik.py b/toid_interaction/toid_interaction/mechanism/zupcanik.py new file mode 100644 index 0000000..4eec341 --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/zupcanik.py @@ -0,0 +1,130 @@ +from .STservo_sdk import PortHandler, sts, COMM_SUCCESS + +class ZupcanikAction: + BAUDRATE: int + DEVICENAME: str + STS_MOVING_ACC: int + + def __init__(self, devicename, baudrate=1000000, sts_moving_acc=50): + self.BAUDRATE = baudrate + self.DEVICENAME = devicename + self.STS_MOVING_ACC = sts_moving_acc + + def zupcanik(self, STS_ID, STS_MOVING_SPEED, TargetPos): + + # Initialize PortHandler instance + portHandler = PortHandler(self.DEVICENAME) + + # Initialize PacketHandler instance + packetHandler = sts(portHandler) + + # Open port + if not portHandler.openPort(): + print("Failed to open the port") + return + + # Set port baudrate + if not portHandler.setBaudRate(self.BAUDRATE): + print("Failed to change the baudrate") + return + + sts_comm_result, sts_error = packetHandler.WheelMode(STS_ID) + if sts_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(sts_comm_result)) + return + elif sts_error != 0: + print("%s" % packetHandler.getRxPacketError(sts_error)) + return + # Read STServo present position + sts_present_position, sts_present_speed, sts_comm_result, sts_error = ( + packetHandler.ReadPosSpeed(STS_ID) + ) + if sts_comm_result != COMM_SUCCESS: + print(packetHandler.getTxRxResult(sts_comm_result)) + else: + print( + "[ID:%03d] PresPos:%d PresSpd:%d" + % (STS_ID, sts_present_position, sts_present_speed) + ) + if sts_error != 0: + print(packetHandler.getRxPacketError(sts_error)) + + TargetPos = TargetPos * 4096 / 360 # Convert to motor ticks + sts_present_position, sts_present_speed, sts_comm_result, sts_error = ( + packetHandler.ReadPosSpeed(STS_ID) + ) + + print( + "Initial position: [ID:%03d] PresPos:%d PresSpd:%d" + % (STS_ID, sts_present_position, sts_present_speed) + ) + + TrenutnaPos = sts_present_position + ProslaPos = sts_present_position + PredjeniPut = TrenutnaPos - ProslaPos + opterecenje = 0 + packetHandler.SetMaxLoad(STS_ID, 80) + + while True: + # Write STServo goal position/moving speed/moving acc + sts_comm_result, sts_error = packetHandler.WriteSpec( + STS_ID, STS_MOVING_SPEED, self.STS_MOVING_ACC + ) + if sts_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("%s" % packetHandler.getRxPacketError(sts_error)) + + ProslaPos = sts_present_position + sts_present_position, sts_present_speed, sts_comm_result, sts_error = ( + packetHandler.ReadPosSpeed(STS_ID) + ) + sts_present_load, sts_comm_result, sts_error = packetHandler.ReadLoad( + STS_ID + ) + sts_max_load, sts_comm_result, sts_error = packetHandler.MaxLoad(STS_ID) + opterecenje = (sts_present_load & ((1 << 10) - 1)) * 0.1 + max_opterecenje = sts_max_load & 255 + print( + "Current position: [ID:%03d] PresPos:%d PresSpd:%d Load:%d Max:%d" + % ( + STS_ID, + sts_present_position, + sts_present_speed, + opterecenje, + max_opterecenje, + ) + ) + + if opterecenje > 75: + print( + "High load detected: [ID:%03d] PresPos:%d PresSpd:%d Load:%d" + % (STS_ID, sts_present_position, sts_present_speed, opterecenje) + ) + sts_comm_result, sts_error = packetHandler.WriteSpec(STS_ID, 0, 0) + if sts_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("%s" % packetHandler.getRxPacketError(sts_error)) + break + TrenutnaPos = sts_present_position + if abs(TrenutnaPos - ProslaPos) > 3000: + PredjeniPut = PredjeniPut + else: + PredjeniPut = PredjeniPut + abs(TrenutnaPos - ProslaPos) + + print("Distance traveled: %d deg " % (PredjeniPut * 360 / 4096)) + if PredjeniPut >= TargetPos: + print( + "Target position reached: [ID:%03d] PresPos:%d PresSpd:%d" + % (STS_ID, sts_present_position, sts_present_speed) + ) + sts_comm_result, sts_error = packetHandler.WriteSpec(STS_ID, 0, 0) + if sts_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("%s" % packetHandler.getRxPacketError(sts_error)) + break + + # Close port + portHandler.closePort() diff --git a/toid_lidar/CMakeLists.txt b/toid_lidar/CMakeLists.txt new file mode 100644 index 0000000..1e83fe0 --- /dev/null +++ b/toid_lidar/CMakeLists.txt @@ -0,0 +1,60 @@ +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 + visualization_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 +) + +install( + DIRECTORY + launch + params + rviz + DESTINATION share/${PROJECT_NAME}/ +) + +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..7a22732 --- /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 "visualization_msgs/msg/marker.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; + using Marker = visualization_msgs::msg::Marker; + +public: + ToidRivalDetect(); +private: + + void process_scan(LaserScan::ConstSharedPtr msg); + + rclcpp::Subscription::SharedPtr scan_sub_; + rclcpp::Publisher::SharedPtr rival_pub_; + rclcpp::Publisher::SharedPtr marker_pub_; + + std::shared_ptr tf_buf_; + std::shared_ptr tf_listener_; + + 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_; + +}; + +} // namespace toid \ No newline at end of file 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/package.xml b/toid_lidar/package.xml new file mode 100644 index 0000000..da1b7c3 --- /dev/null +++ b/toid_lidar/package.xml @@ -0,0 +1,32 @@ + + + + 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 + visualization_msgs + rplidar_ros + + + + ament_lint_auto + ament_lint_common + + + ament_cmake + + 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 diff --git a/toid_lidar/src/toid_lidar.cpp b/toid_lidar/src/toid_lidar.cpp new file mode 100644 index 0000000..5cf2fe2 --- /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::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 new file mode 100644 index 0000000..52062cf --- /dev/null +++ b/toid_lidar/src/toid_lidar_node.cpp @@ -0,0 +1,149 @@ +#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" +#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)); + marker_pub_ = create_publisher("/rivalMaker", 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, "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) +{ + Eigen::Isometry3d pose; + + 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 f35ea38..6c0b3fd 100644 --- a/toid_msgs/CMakeLists.txt +++ b/toid_msgs/CMakeLists.txt @@ -9,8 +9,17 @@ endif() find_package(ament_cmake REQUIRED) 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" + "srv/SendString.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/action/SimpleMoveCoords.action b/toid_msgs/action/SimpleMoveCoords.action new file mode 100644 index 0000000..f178d8a --- /dev/null +++ b/toid_msgs/action/SimpleMoveCoords.action @@ -0,0 +1,17 @@ +uint8 IGNORE_OBSTACLES=1 + +float64 x +float64 y +float64 theta +uint8 backwards 0 +uint8 mode 0 + +float64 max_speed 0 +--- +uint16 NONE=0 +uint16 TF_ERROR=1 + +builtin_interfaces/Duration total_elapsed_time +uint16 error_code +string error_msg +--- diff --git a/toid_msgs/action/SimpleRotate.action b/toid_msgs/action/SimpleRotate.action new file mode 100644 index 0000000..e403b9c --- /dev/null +++ b/toid_msgs/action/SimpleRotate.action @@ -0,0 +1,15 @@ +uint8 IGNORE_OBSTACLES=1 + +float64 angle +float64 min_angle 0 +uint8 mode 0 + +float64 max_speed 0 +--- +uint16 NONE=0 +uint16 TF_ERROR=1 + +builtin_interfaces/Duration total_elapsed_time +uint16 error_code +string error_msg +--- diff --git a/toid_msgs/action/SimpleTranslateX.action b/toid_msgs/action/SimpleTranslateX.action new file mode 100644 index 0000000..5469ae8 --- /dev/null +++ b/toid_msgs/action/SimpleTranslateX.action @@ -0,0 +1,14 @@ +uint8 IGNORE_OBSTACLES=1 + +float64 distance +uint8 mode 0 + +float64 max_speed 0 +--- +uint16 NONE=0 +uint16 TF_ERROR=1 + +builtin_interfaces/Duration total_elapsed_time +uint16 error_code +string error_msg +--- 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_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 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 diff --git a/toid_navigation/launch/navigation_launch.py b/toid_navigation/launch/main.py similarity index 60% rename from toid_navigation/launch/navigation_launch.py rename to toid_navigation/launch/main.py index 8193379..9c02541 100644 --- a/toid_navigation/launch/navigation_launch.py +++ b/toid_navigation/launch/main.py @@ -8,39 +8,45 @@ 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") + 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") - 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='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(launch_dir, 'mock_control_launch.py') + os.path.join(ctrl_launch_dir, 'toid.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) + 'visualize': 'False', + 'use_mock': use_mock + }.items(), + condition=IfCondition(run_nodes), ) map_server = Node( @@ -48,7 +54,8 @@ def generate_launch_description(): executable='map_server', name='map_server', output='screen', - parameters=[{'yaml_filename': map}] + parameters=[{'yaml_filename': map}], + condition=IfCondition(run_nodes), ) planner_server = Node( @@ -56,7 +63,8 @@ def generate_launch_description(): executable='planner_server', name='planner_server', output='screen', - parameters=[{'GridBased':{'lattice_filepath': lattice_path}}, params] + parameters=[{'GridBased':{'lattice_filepath': lattice_path}}, params], + condition=IfCondition(run_nodes), ) controller_server = Node( @@ -64,7 +72,8 @@ def generate_launch_description(): executable='controller_server', name='controller_server', output='screen', - parameters=[params] + parameters=[params], + condition=IfCondition(run_nodes), ) bt_navigator = Node( @@ -72,7 +81,8 @@ def generate_launch_description(): executable='bt_navigator', name='bt_navigator', output='screen', - parameters=[{'default_nav_to_pose_bt_xml': bt_path}, params] + 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" @@ -82,7 +92,8 @@ def generate_launch_description(): executable='behavior_server', name='behavior_server', output='screen', - parameters=[params] + parameters=[params], + condition=IfCondition(run_nodes), ) lifecycle_manager_node = Node( @@ -90,7 +101,8 @@ def generate_launch_description(): executable='lifecycle_manager', name='lifecycle_manager', output='screen', - parameters=[params] + parameters=[params], + condition=IfCondition(run_nodes), ) rviz_node = Node( @@ -104,7 +116,8 @@ def generate_launch_description(): return LaunchDescription([ visualize_arg, - set_position, + use_mock_arg, + run_nodes_arg, rviz_node, map_server, bt_navigator, 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/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 diff --git a/toid_navigation/package.xml b/toid_navigation/package.xml index 23e46aa..4b1a3c1 100644 --- a/toid_navigation/package.xml +++ b/toid_navigation/package.xml @@ -10,11 +10,13 @@ ament_cmake robot_state_publisher nav2_behaviors + nav2_map_server nav2_planner + nav2_regulated_pure_pursuit_controller + nav2_smac_planner nav2_controller nav2_bt_navigator nav2_lifecycle_manager - nav2_bringup ros2_control rsl 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: diff --git a/toid_navigation/params/toid_general_params.yaml b/toid_navigation/params/toid_general_params.yaml index 7e91c2c..fc0776b 100644 --- a/toid_navigation/params/toid_general_params.yaml +++ b/toid_navigation/params/toid_general_params.yaml @@ -21,20 +21,20 @@ 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", "rotate", "translateX", "moveCoords"] 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 + translateX: + plugin: "toid::SimpleTranslateXBehavior" + moveCoords: + plugin: "toid::MoveCoords" local_frame: map global_frame: map robot_base_frame: base_footprint @@ -188,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