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/.gitignore b/.gitignore index fe1e202..a8ed90f 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ +__pycache__ .cache build install diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..9963ab5 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,6 @@ +[submodule "ext/BehaviorTree.ROS2"] + path = ext/BehaviorTree.ROS2 + url = https://github.com/BehaviorTree/BehaviorTree.ROS2.git +[submodule "ext/camera_ros"] + path = ext/camera_ros + url = https://github.com/christianrauch/camera_ros.git diff --git a/Dockerfile b/Dockerfile index 111d950..9df8837 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,15 +1,33 @@ -FROM ros:jazzy-ros-base +FROM ros:jazzy-perception ENV DEBIAN_FRONTEND=noninteractive # ---------- System dependencies ---------- -RUN apt-get update && apt-get install -y \ +RUN apt-get update && apt-get upgrade && apt-get install -y \ python3-colcon-common-extensions \ python3-rosdep \ + meson cmake \ build-essential \ udev \ git +# ---------- Libcamera ---------- + +WORKDIR /extras + +RUN apt-get install -y libboost-dev \ + libgnutls28-dev openssl libtiff5-dev \ + pybind11-dev \ + qtbase5-dev libqt5core5a libqt5gui5 libqt5widgets5 \ + python3-yaml python3-ply python3-jinja2 python3-pip\ + libglib2.0-dev libgstreamer-plugins-base1.0-dev \ + build-essential + +RUN git clone https://github.com/raspberrypi/libcamera.git \ + && cd libcamera \ + && meson setup build --buildtype=release -Dpipelines=rpi/vc4,rpi/pisp -Dipas=rpi/vc4,rpi/pisp -Dv4l2=true -Dgstreamer=enabled -Dtest=false -Dlc-compliance=disabled -Dcam=disabled -Dqcam=disabled -Ddocumentation=disabled -Dpycamera=enabled \ + && ninja -C build install + # ---------- Initialize rosdep ---------- RUN rosdep init || true RUN rosdep update @@ -18,25 +36,33 @@ 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_lidar/package.xml toid_lidar/package.xml +COPY toid_interaction/package.xml toid_interaction/package.xml +COPY toid_costmaps/package.xml toid_costmaps/package.xml +COPY toid_bt/package.xml toid_bt/package.xml COPY toid_spinner_controller/package.xml toid_spinner_controller/package.xml -#COPY toid_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 +COPY toid_vision/package.xml toid_vision/package.xml +COPY ext/camera_ros/package.xml ext/camera_ros/package.xml +COPY ext/BehaviorTree.ROS2/ ext/BehaviorTree.ROS2/ # ---------- Install dependencies ---------- RUN . /opt/ros/jazzy/setup.sh && \ - rosdep install --from-paths ./ --ignore-src -r -y \ + rosdep install --from-paths ./ --ignore-src -r -y --skip-keys=libcamera \ && rm -rf /var/lib/apt/lists/* RUN rm -rf ./* 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..b2fe803 100644 --- a/docker-compose.yaml +++ b/docker-compose.yaml @@ -1,17 +1,37 @@ services: toid: image: localhost:5000/toid + build: . container_name: toid privileged: true network_mode: host volumes: - - ./:/ros_ws/src + - ./:/ros_ws - /dev/:/dev/ - /sys/:/sys/ + - /run/udev/:/run/udev/ entrypoint: ["sleep","infinity"] profiles: - - base \ No newline at end of file + - base + + toid_forever: + image: localhost:5000/toid + build: . + container_name: toid + + restart: unless-stopped + + privileged: true + network_mode: host + + volumes: + - ./:/ros_ws + - /dev/:/dev/ + - /sys/:/sys/ + - /run/udev/:/run/udev/ + + entrypoint: ["bash", "-c", "source install/setup.bash && ros2 launch toid_navigation main.py use_lidar:=False use_mock:=False"] \ No newline at end of file diff --git a/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/ext/camera_ros b/ext/camera_ros new file mode 160000 index 0000000..121c98a --- /dev/null +++ b/ext/camera_ros @@ -0,0 +1 @@ +Subproject commit 121c98a7fef9ea745ad000ff28bf48da6dee994a diff --git a/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..0c1190f --- /dev/null +++ b/scripts/toid_base_build.sh @@ -0,0 +1,6 @@ +#!/bin/env bash + +source /opt/ros/jazzy/setup.bash + +colcon build --packages-up-to-regex "toid_(?!bt)" +colcon build --packages-up-to-regex "camera_ros" \ No newline at end of file diff --git a/toid_behaviors/CMakeLists.txt b/toid_behaviors/CMakeLists.txt new file mode 100644 index 0000000..9426654 --- /dev/null +++ b/toid_behaviors/CMakeLists.txt @@ -0,0 +1,87 @@ +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 + src/approach_acorns.cpp + src/rotate_acorns.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/approach_acorns.hpp b/toid_behaviors/include/toid_behaviors/approach_acorns.hpp new file mode 100644 index 0000000..6cc6397 --- /dev/null +++ b/toid_behaviors/include/toid_behaviors/approach_acorns.hpp @@ -0,0 +1,91 @@ +#pragma once + +#include "geometry_msgs/msg/pose.hpp" +#include "toid_behaviors/scl.hpp" +#include "toid_behaviors/simple_move.hpp" +#include "toid_behaviors/rolling_average.hpp" +#include "toid_msgs/action/simple_move_coords.hpp" +#include "visualization_msgs/msg/marker.hpp" + +namespace toid +{ +using MoveAction = toid_msgs::action::SimpleMoveCoords; +using PoseStamped = geometry_msgs::msg::PoseStamped; +using namespace nav2_behaviors; + +class ApproachAcorns : public SimpleMove +{ +public: + ApproachAcorns(); + ~ApproachAcorns(); + + void configureCB() override; + + void activateCB() override; + void deactivateCB() override; + + ResultStatus onStart( + const std::shared_ptr command, const geometry_msgs::msg::Pose & pose, + const geometry_msgs::msg::Twist & vel) override; + + double distanceToTarget( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & target_point, + const double target_theta, bool backwards); + + double velocityTarget(const double dist_left); + + bool collisionDetection( + const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Pose & last_ok_pose); + + void acorn_position_cb(PoseStamped::ConstSharedPtr msg); + + ResultStatus updateVel( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & vel, + geometry_msgs::msg::Twist & out_vel) override; + + virtual nav2_core::CostmapInfoType getResourceInfo() override + { + return nav2_core::CostmapInfoType::LOCAL; + } + +protected: + SmoothControlLaw scl_; + rclcpp::Subscription::SharedPtr acorn_pose_sub_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr target_pose_pub_; + std::shared_mutex mutex_; + RollingAverage avg_ = RollingAverage(10); + + //Goal + geometry_msgs::msg::Pose new_target_pose_; + double new_target_angle_; + + geometry_msgs::msg::Pose target_pose_; + geometry_msgs::msg::Pose initial_pose_; + double target_angle_; + double target_sign_; + bool backwards_; + unsigned char mode_; + + //State + double last_speed_; + rclcpp::Time last_seen_; + double distance_; + + //Config + double max_vel_accel_; + double max_vel_decel_; + double max_vel_speed_; + double min_vel_speed_; + + double max_angular_speed_; + + double kp_; + double k_phi_; + double k_delta_; + double beta_; + double lambda_; + double slowdown_radius_; + bool debug_marker_; +}; + +} // namespace toid diff --git a/toid_behaviors/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/rolling_average.hpp b/toid_behaviors/include/toid_behaviors/rolling_average.hpp new file mode 100644 index 0000000..0305bdf --- /dev/null +++ b/toid_behaviors/include/toid_behaviors/rolling_average.hpp @@ -0,0 +1,67 @@ +#pragma once + +#include +#include + +#include "geometry_msgs/msg/pose.hpp" +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace toid +{ + +class RollingAverage +{ +public: + using Pose2D = std::tuple; + RollingAverage(size_t size = 0) : poses_(size), size_(size) {} + + Pose2D push(geometry_msgs::msg::Pose & pose) + { + if(size_ == 0) { + return {}; + } + if (size_ == data_count_) { + accum_x_ -= poses_[front_idx_].position.x; + accum_y_ -= poses_[front_idx_].position.y; + accum_theta_ -= tf2::getYaw(poses_[front_idx_].orientation); + front_idx_ += 1; + front_idx_ %= size_; + data_count_--; + } + + poses_[back_idx_] = pose; + + accum_x_ += poses_[back_idx_].position.x; + accum_y_ += poses_[back_idx_].position.y; + accum_theta_ += tf2::getYaw(poses_[back_idx_].orientation); + + back_idx_ += 1; + back_idx_ %= size_; + data_count_++; + + return {accum_x_ / data_count_, accum_y_ / data_count_, accum_theta_ / data_count_}; + } + + void reset() { + data_count_ = 0; + front_idx_ = 0; + back_idx_ = 0; + accum_x_ = 0; + accum_y_ = 0; + accum_theta_ = 0; + } + +private: + std::vector poses_; + const size_t size_; + size_t front_idx_ = 0; + size_t back_idx_ = 0; + size_t data_count_ = 0; + + double accum_x_ = 0; + double accum_y_ = 0; + double accum_theta_ = 0; +}; + +} // namespace toid \ No newline at end of file diff --git a/toid_behaviors/include/toid_behaviors/rotate_acorns.hpp b/toid_behaviors/include/toid_behaviors/rotate_acorns.hpp new file mode 100644 index 0000000..411fc06 --- /dev/null +++ b/toid_behaviors/include/toid_behaviors/rotate_acorns.hpp @@ -0,0 +1,80 @@ +#pragma once + +#include "geometry_msgs/msg/pose.hpp" +#include "toid_behaviors/scl.hpp" +#include "toid_behaviors/simple_move.hpp" +#include "toid_behaviors/rolling_average.hpp" +#include "toid_msgs/action/simple_move_coords.hpp" +#include "visualization_msgs/msg/marker.hpp" + +namespace toid +{ +using RotateAction = toid_msgs::action::SimpleRotate; +using PoseStamped = geometry_msgs::msg::PoseStamped; +using namespace nav2_behaviors; + +class RotateAcorns : public SimpleMove +{ +public: + RotateAcorns(); + ~RotateAcorns(); + + void configureCB() override; + + void activateCB() override; + void deactivateCB() override; + + ResultStatus onStart( + const std::shared_ptr command, const geometry_msgs::msg::Pose & pose, + const geometry_msgs::msg::Twist & vel) override; + + bool collisionDetection( + const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Pose & last_ok_pose); + + void acorn_position_cb(PoseStamped::ConstSharedPtr msg); + + ResultStatus updateVel( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & vel, + geometry_msgs::msg::Twist & out_vel) override; + + virtual nav2_core::CostmapInfoType getResourceInfo() override + { + return nav2_core::CostmapInfoType::LOCAL; + } + + void calc_err_and_sign( + double last_angle, double current_yaw, double & min_turn_angle, double & err, double & sign); + + double check_space(const geometry_msgs::msg::Pose pose, const double e, const double sign); + + double calc_speed(const double err, const double sign, const double angular_speed); + +protected: + SmoothControlLaw scl_; + rclcpp::Subscription::SharedPtr acorn_pose_sub_; + double new_target_angle_; + std::shared_mutex mutex_; + RollingAverage avg_ = RollingAverage(10); + + + //Goal + double target_angle_; + double min_turn_angle_; + double initial_direction_; + unsigned char mode_; + + //State + double angular_speed_; + double last_angle_; + double last_time_; + double distance_ = 1.0; + + //Config + double max_angular_speed_; + double min_angular_speed_; + double max_angular_accel_; + double max_angular_decel_; + double lookahead_; +}; + +} // namespace toid diff --git a/toid_behaviors/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..673dd39 --- /dev/null +++ b/toid_behaviors/include/toid_behaviors/simple_move.hpp @@ -0,0 +1,179 @@ +#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_ || (this->clock_->now() - rivals_->header.stamp ) > rclcpp::Duration::from_seconds(1.0)) { + return false; + } + const double cosp = std::cos(pose.theta); + const double sinp = std::sin(pose.theta); + int i = 0; + for (const auto & rival : rivals_->point) { + geometry_msgs::msg::Point local_rival; + const double dx = rival.x - pose.x; + const double dy = rival.y - pose.y; + local_rival.x = dx * cosp - dy * sinp; + local_rival.y = dx * sinp + dy * cosp; + + local_rival.x -= 0.105; + + const double qx = std::abs(local_rival.x) - robot_length_ / 2.0; + const double qy = std::abs(local_rival.y) - robot_width_ / 2.0; + + 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); + RCLCPP_DEBUG(this->logger_, "Distance to rival %d: %lf", i++, sdf-rival_radius_); + 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.22; + + 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/approach_acorns.cpp b/toid_behaviors/src/approach_acorns.cpp new file mode 100644 index 0000000..95c7eda --- /dev/null +++ b/toid_behaviors/src/approach_acorns.cpp @@ -0,0 +1,311 @@ +#include "toid_behaviors/approach_acorns.hpp" + +#include "angles/angles.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace toid +{ + +ApproachAcorns::ApproachAcorns() : SimpleMove() {} +ApproachAcorns::~ApproachAcorns() {} + +void ApproachAcorns::configureCB() +{ + auto node = node_.lock(); + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".max_vel_accel", rclcpp::ParameterValue(2.0)); + node->get_parameter(behavior_name_ + ".max_vel_accel", max_vel_accel_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".max_vel_decel", rclcpp::ParameterValue(1.0)); + node->get_parameter(behavior_name_ + ".max_vel_decel", max_vel_decel_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".max_vel_speed", rclcpp::ParameterValue(0.4)); + node->get_parameter(behavior_name_ + ".max_vel_speed", max_vel_speed_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".min_vel_speed", rclcpp::ParameterValue(0.10)); + node->get_parameter(behavior_name_ + ".min_vel_speed", min_vel_speed_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".max_angular_speed", rclcpp::ParameterValue(1.0)); + node->get_parameter(behavior_name_ + ".max_angular_speed", max_angular_speed_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".kp", rclcpp::ParameterValue(5.0)); + node->get_parameter(behavior_name_ + ".kp", kp_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".kphi", rclcpp::ParameterValue(5.0)); + node->get_parameter(behavior_name_ + ".kphi", k_phi_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".kdelta", rclcpp::ParameterValue(2.0)); + node->get_parameter(behavior_name_ + ".kdelta", k_delta_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".beta", rclcpp::ParameterValue(0.4)); + node->get_parameter(behavior_name_ + ".beta", beta_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".lambda", rclcpp::ParameterValue(2.0)); + node->get_parameter(behavior_name_ + ".lambda", lambda_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".slow_down_radius", rclcpp::ParameterValue(0.20)); + node->get_parameter(behavior_name_ + ".slow_down_radius", slowdown_radius_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".debug_marker", rclcpp::ParameterValue(false)); + node->get_parameter(behavior_name_ + ".debug_marker", debug_marker_); + + target_pose_pub_ = node->create_publisher("marker", 1); +} + +void ApproachAcorns::activateCB() +{ + auto node = node_.lock(); + using namespace std::placeholders; + acorn_pose_sub_ = node->create_subscription( + "closest_acorn", rclcpp::QoS(2).keep_last(2), std::bind(&ApproachAcorns::acorn_position_cb, this, _1)); + target_pose_pub_->on_activate(); + distance_ = 1.0; +} + +void ApproachAcorns::deactivateCB() +{ + acorn_pose_sub_.reset(); + target_pose_pub_->on_deactivate(); +} + +void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg) +{ + geometry_msgs::msg::PoseStamped pose_local; + geometry_msgs::msg::PoseStamped pose_global; + try { + pose_local = tf_->transform(*msg, robot_base_frame_); + pose_global = tf_->transform(*msg, global_frame_); + } catch (const tf2::TransformException & e) { + RCLCPP_ERROR_ONCE(logger_, "Transform timeout [showing once]..."); + return; + } + + double x = pose_local.pose.position.x; + double y = pose_local.pose.position.y; + if (std::sqrt(x * x + y * y) > distance_ + 0.005) { + return; + } + + // Smooth out approach + if (x * x + y * y < 0.42 * 0.42) { + return; + } + + double yaw = angles::normalize_angle(tf2::getYaw(pose_global.pose.orientation)); + + if (tf2::getYaw(pose_local.pose.orientation) > 0) { + pose_global.pose.position.x += std::cos(yaw) * 0.01 - std::sin(yaw) * 0.35; + pose_global.pose.position.y += std::sin(yaw) * 0.01 + std::cos(yaw) * 0.35; + } else { + pose_global.pose.position.x += std::cos(yaw) * -0.01 - std::sin(yaw) * -0.35; + pose_global.pose.position.y += std::sin(yaw) * -0.01 + std::cos(yaw) * -0.35; + } + + std::lock_guard _lock(mutex_); + + const double dx = pose_global.pose.position.x - initial_pose_.position.x; + const double dy = pose_global.pose.position.y - initial_pose_.position.y; + double yaw_to_goal = std::atan2(dy,dx); + + tf2::convert(tf2::Quaternion::createFromRPY(0, 0, yaw_to_goal), pose_global.pose.orientation); + yaw = yaw_to_goal; + auto [a, b, c] = avg_.push(pose_global.pose); + distance_ = x * x + y * y; + new_target_pose_.position.x = a; + new_target_pose_.position.y = b; + tf2::convert(tf2::Quaternion::createFromRPY(0, 0, c), new_target_pose_.orientation); + new_target_angle_ = c; + + if (debug_marker_) { + visualization_msgs::msg::Marker marker; + marker.lifetime.sec = 1.0; + marker.header = pose_global.header; + marker.pose = new_target_pose_; + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.scale.x = 0.02; + marker.scale.y = 0.02; + marker.scale.z = 0.02; + marker.color.a = 1.0; + marker.color.r = 1.0; + marker.color.g = 0; + marker.color.b = 0; + target_pose_pub_->publish(marker); + } +} + +ResultStatus ApproachAcorns::onStart( + const std::shared_ptr command, const geometry_msgs::msg::Pose & pose, + const geometry_msgs::msg::Twist & vel) +{ + std::lock_guard _lock(mutex_); + distance_ = 1.0; + new_target_pose_.position.x = command->x; + new_target_pose_.position.y = command->y; + + backwards_ = command->backwards; + initial_pose_ = pose; + + new_target_pose_.orientation = tf2::toMsg(tf2::Quaternion::createFromRPY(0, 0, command->theta)); + new_target_angle_ = command->theta; + target_sign_ = backwards_ ? -1.0 : 1.0; + max_vel_speed_ = command->max_speed; + + avg_.reset(); + + if (command->max_speed == 0) { + auto node = node_.lock(); + node->get_parameter(behavior_name_ + ".max_vel_speed", max_vel_speed_); + } + + mode_ = command->mode; + + scl_.k_phi = k_phi_; + scl_.k_delta = k_delta_; + scl_.bbeta = beta_; + scl_.lam = lambda_; + scl_.slowdown_radius = slowdown_radius_; + scl_.v_angular_max = max_angular_speed_; + scl_.v_linear_min = min_vel_speed_; + scl_.v_linear_max = max_vel_speed_; + + last_speed_ = vel.angular.x; + + return ResultStatus{Status::SUCCEEDED}; +} + +double ApproachAcorns::distanceToTarget( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & target_point, + const double target_theta, bool backwards) +{ + const double dx = target_point.x - pose.position.x; + const double dy = target_point.y - pose.position.y; + const double target_sign = backwards ? -1.0 : 1.0; + + return target_sign * (dx * cos(target_theta) + dy * sin(target_theta)); +} + +double ApproachAcorns::velocityTarget(const double dist_left) +{ + const double lower_bound = last_speed_ - control_duration_ * max_vel_accel_; + const double upper_bound = last_speed_ + control_duration_ * max_vel_accel_; + + double vel = max_vel_speed_; + double max_vel_to_stop = 0.8 * std::sqrt(2.0 * max_vel_decel_ * dist_left); + vel = std::min(vel, max_vel_to_stop); + return std::clamp(target_sign_ * vel, lower_bound, upper_bound); +} + +bool ApproachAcorns::collisionDetection( + const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Pose & last_ok_pose) +{ + const int samples = static_cast(0.5 / control_duration_); + geometry_msgs::msg::Pose current_pose = pose; + last_ok_pose = pose; + const bool check_map = !(mode_ & MoveAction::Goal::IGNORE_OBSTACLES); + for (int i = 0; i < samples; i++) { + scl_.step(target_pose_, current_pose, control_duration_, backwards_); + + geometry_msgs::msg::Pose2D p; + p.x = current_pose.position.x; + p.y = current_pose.position.y; + p.theta = tf2::getYaw(current_pose.orientation); + + if (check_map && !local_collision_checker_->isCollisionFree(p, i == 0)) { + return true; + } + + if (check_rival_collision(p)) { + return true; + } + + last_ok_pose = current_pose; + const double dist_left = + distanceToTarget(current_pose, target_pose_.position, target_angle_, backwards_); + if (dist_left < 0.005) { + return false; + } + } + return false; +} + +ResultStatus ApproachAcorns::updateVel( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist &, + geometry_msgs::msg::Twist & out_vel) +{ + { + std::lock_guard _lock(mutex_); + target_pose_ = new_target_pose_; + target_angle_ = new_target_angle_; + } + + double dist_left = distanceToTarget(pose, target_pose_.position, target_angle_, backwards_); + + const double current_yaw = tf2::getYaw(pose.orientation); + double angle_dist = angles::shortest_angular_distance(current_yaw, target_angle_); + + if (dist_left <= 0.001 && std::fabs(angle_dist) <= 0.030) { + out_vel.linear.x = 0; + out_vel.angular.z = 0; + return ResultStatus{Status::SUCCEEDED}; + } + + 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.001 && dist_left <= 0.02) { + out_vel.linear.x = velocityTarget(dist_left); + out_vel.angular.z = std::clamp(kp_ * angle_dist, -max_angular_speed_, max_angular_speed_); + last_speed_ = out_vel.linear.x; + return ResultStatus{Status::RUNNING}; + } + + if (dist_left <= 0.001) { + out_vel.linear.x = 0; + out_vel.angular.z = std::clamp(kp_ * angle_dist, -max_angular_speed_, max_angular_speed_); + last_speed_ = out_vel.linear.x; + return ResultStatus{Status::RUNNING}; + } + + scl_.v_linear_max = target_sign_ * velocityTarget(dist_left); + scl_.calculate_vel(target_pose_, pose, out_vel, backwards_); + + last_speed_ = out_vel.linear.x; + + RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Distance Left: %lf", dist_left); + RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Max Speed: %lf", scl_.v_linear_max); + + return ResultStatus{Status::RUNNING}; +} + +} // namespace toid + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(toid::ApproachAcorns, nav2_core::Behavior); \ No newline at end of file diff --git a/toid_behaviors/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/rotate_acorns.cpp b/toid_behaviors/src/rotate_acorns.cpp new file mode 100644 index 0000000..98c3a15 --- /dev/null +++ b/toid_behaviors/src/rotate_acorns.cpp @@ -0,0 +1,234 @@ +#include "toid_behaviors/rotate_acorns.hpp" + +#include + +#include "angles/angles.h" +#include "tf2/convert.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace toid +{ + +RotateAcorns::RotateAcorns() : SimpleMove() {} +RotateAcorns::~RotateAcorns() {} + +void RotateAcorns::configureCB() +{ + auto node = node_.lock(); + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".max_angular_vel", rclcpp::ParameterValue(2.0)); + + node->get_parameter(behavior_name_ + ".max_angular_vel", max_angular_speed_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".min_angular_vel", rclcpp::ParameterValue(0.1)); + 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(0.5)); + node->get_parameter(behavior_name_ + ".max_angular_decel", max_angular_decel_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".lookahead", rclcpp::ParameterValue(0.5)); + node->get_parameter(behavior_name_ + ".lookahead", lookahead_); +} + + +void RotateAcorns::activateCB() +{ + auto node = node_.lock(); + using namespace std::placeholders; + acorn_pose_sub_ = node->create_subscription( + "closest_acorn", rclcpp::QoS(2).keep_last(2), std::bind(&RotateAcorns::acorn_position_cb, this, _1)); + distance_ = 1.0; +} + +void RotateAcorns::deactivateCB() { + acorn_pose_sub_.reset(); +} + +void RotateAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg) +{ + geometry_msgs::msg::PoseStamped pose_local; + geometry_msgs::msg::PoseStamped pose_global; + try { + pose_local = tf_->transform(*msg, robot_base_frame_); + pose_global = tf_->transform(*msg, global_frame_); + } catch (const tf2::TransformException & e) { + RCLCPP_ERROR_ONCE(logger_, "Transform timeout [showing once]..."); + return; + } + + double x = pose_local.pose.position.x; + double y = pose_local.pose.position.y; + if (x * x + y * y > distance_ + 0.005) { + return; + } + + double yaw = angles::normalize_angle(tf2::getYaw(pose_global.pose.orientation)); + + if(tf2::getYaw(pose_local.pose.orientation) > 0) { + yaw += M_PI; + } + + yaw += M_PI/2; + + + pose_global.pose.position.x += std::cos(yaw) * 0.0 - std::sin(yaw) * - 0.005; + pose_global.pose.position.y += std::sin(yaw) * 0.0 + std::cos(yaw) * - 0.005; + tf2::convert(tf2::Quaternion::createFromRPY(0,0,yaw ), pose_global.pose.orientation); + + yaw = angles::normalize_angle(yaw); + + std::lock_guard _lock(mutex_); + auto[a,b,c] = avg_.push(pose_global.pose); + distance_ = x * x + y * y; + new_target_angle_ = c; + +} + +ResultStatus RotateAcorns::onStart( + const std::shared_ptr command, const geometry_msgs::msg::Pose & pose, + const geometry_msgs::msg::Twist & vel) +{ + std::lock_guard _lock(mutex_); + target_angle_ = new_target_angle_; + min_turn_angle_ = abs(command->min_angle); + initial_direction_ = (command->min_angle >= 0.0) ? 1.0 : -1.0; + max_angular_speed_ = command->max_speed; + mode_ = command->mode; + distance_ = 1.0; + avg_.reset(); + + if (command->max_speed == 0) { + auto node = node_.lock(); + node->get_parameter(behavior_name_ + ".max_angular_vel", max_angular_speed_); + } + + geometry_msgs::msg::Pose2D pose2d; + pose2d.x = pose.position.x; + pose2d.y = pose.position.y; + pose2d.theta = initial_direction_; + local_collision_checker_->isCollisionFree(pose2d, true); + + last_angle_ = tf2::getYaw(pose.orientation); + + angular_speed_ = vel.angular.z; + last_time_ = clock_->now().seconds(); + return ResultStatus{Status::SUCCEEDED}; +} + +void RotateAcorns::calc_err_and_sign( + double last_angle, double current_yaw, double & min_turn_angle, double & err, double & sign) +{ + err = angles::shortest_angular_distance(current_yaw, target_angle_); + sign = (err < 0) ? -1.0 : 1.0; + err = std::abs(err); + + if (min_turn_angle > 0.0) { + const double angle_change = angles::shortest_angular_distance(last_angle, current_yaw); + min_turn_angle = std::max(0.0, min_turn_angle - initial_direction_ * angle_change); + err = std::max(initial_direction_ * sign * err, 0.0); + err = std::max(min_turn_angle, err); + sign = initial_direction_; + } +} + +double RotateAcorns::calc_speed( + const double err, const double sign, const double angular_speed) +{ + const double upper_vel_ = angular_speed + max_angular_accel_ * control_duration_; + const double lower_vel_ = angular_speed - max_angular_accel_ * control_duration_; + + const double speed_until_overshoot = std::sqrt(2.0 * max_angular_decel_ * std::fabs(err)); + + const double requested_speed = sign * std::min(speed_until_overshoot, max_angular_speed_); + const double speed = std::clamp(requested_speed, lower_vel_, upper_vel_); + return speed; +} + +ResultStatus RotateAcorns::updateVel( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist &, + geometry_msgs::msg::Twist & out_vel) +{ + { + std::lock_guard _lock(mutex_); + target_angle_ = new_target_angle_; + } + const double current_yaw = tf2::getYaw(pose.orientation); + double min_turn_angle = min_turn_angle_; + double angular_speed = angular_speed_; + 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.004) { + return ResultStatus{Status::SUCCEEDED}; + } + + min_turn_angle_ = min_turn_angle; + last_angle_ = current_yaw; + angular_speed_ = speed; + out_vel.angular.z = speed; + return ResultStatus{Status::RUNNING}; +} + +double RotateAcorns::check_space( + const geometry_msgs::msg::Pose pose, const double e, const double sign) +{ + geometry_msgs::msg::Pose2D pose2d; + pose2d.x = pose.position.x; + pose2d.y = pose.position.y; + double initial_theta = tf2::getYaw(pose.orientation); + pose2d.theta = initial_theta; + const double step_size = 0.1; + const double err = std::min(e, 1.0); + const bool check_map = !(mode_ & RotateAction::Goal::IGNORE_OBSTACLES); + + for (int i = 1; i < err / step_size; i++) { + pose2d.theta += sign * step_size; + + if (check_map && !local_collision_checker_->isCollisionFree(pose2d, false)) { + RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked"); + return step_size * (i - 1); + } + + if (check_rival_collision(pose2d)) { + RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked"); + return step_size * (i - 1); + } + } + + pose2d.theta = initial_theta + sign * err; + + if (check_map && !local_collision_checker_->isCollisionFree(pose2d, false)) { + RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked"); + return step_size * ((int)(err / step_size)); + } + + if (check_rival_collision(pose2d)) { + RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked"); + return step_size * ((int)(err / step_size)); + } + + return e; +} + +} // namespace toid + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(toid::RotateAcorns, nav2_core::Behavior); \ No newline at end of file diff --git a/toid_behaviors/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..d64afa7 --- /dev/null +++ b/toid_behaviors/toid_behaviors.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/toid_bot_description/rviz/default.rviz b/toid_bot_description/rviz/default.rviz index 5c953cf..34836b4 100644 --- a/toid_bot_description/rviz/default.rviz +++ b/toid_bot_description/rviz/default.rviz @@ -1,13 +1,14 @@ Panels: - Class: rviz_common/Displays - Help Height: 78 + Help Height: 138 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 + - /Pose1 Splitter Ratio: 0.5 - Tree Height: 542 + Tree Height: 732 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -74,6 +75,14 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + camera: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_base: + Alpha: 1 + Show Axes: false + Show Trail: false drivewhl_l_link: Alpha: 1 Show Axes: false @@ -118,6 +127,10 @@ Visualization Manager: Value: true base_link: Value: true + camera: + Value: true + camera_base: + Value: true drivewhl_l_link: Value: true drivewhl_r_link: @@ -134,21 +147,43 @@ Visualization Manager: Show Axes: true Show Names: true Tree: - base_link: - base_footprint: - {} - drivewhl_l_link: - {} - drivewhl_r_link: - {} - left_caster: - {} - lidar: - {} - right_caster: - {} + base_footprint: + base_link: + drivewhl_l_link: + {} + drivewhl_r_link: + {} + left_caster: + {} + lidar: + {} + right_caster: + {} + camera_base: + camera: + {} Update Interval: 0 Value: true + - Alpha: 1 + Axes Length: 0.10000000149011612 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.10000000149011612 + Head Radius: 0.029999999329447746 + Name: Pose + Shaft Length: 0.10000000149011612 + Shaft Radius: 0.009999999776482582 + Shape: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /closest_acorn + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -195,7 +230,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/XYOrbit - Distance: 1.59040105342865 + Distance: 1.2598285675048828 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -210,18 +245,18 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.6053992509841919 + Pitch: 0.015398791059851646 Target Frame: Value: XYOrbit (rviz_default_plugins) - Yaw: 1.088563323020935 + Yaw: 0.09356015175580978 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 846 + Height: 1250 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016c000002acfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000002ac000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002acfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f000002ac000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002de00fffffffb0000000800540069006d0065010000000000000450000000000000000000000229000002ac00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000027a000003d8fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000070000003d80000018600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015b000003d8fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000070000003d80000013800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007620000005efc0100000002fb0000000800540069006d00650100000000000007620000048700fffffffb0000000800540069006d0065010000000000000450000000000000000000000375000003d800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -230,6 +265,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1200 + Width: 1890 X: 60 - Y: 60 + Y: 64 diff --git a/toid_bot_description/src/toid_bot_control.xacro b/toid_bot_description/src/toid_bot_control.xacro index 4117934..9262876 100644 --- a/toid_bot_description/src/toid_bot_control.xacro +++ b/toid_bot_description/src/toid_bot_control.xacro @@ -1,12 +1,13 @@ - + toid_control/StepperInterface + ${serial_port} diff --git a/toid_bot_description/src/toid_bot_description.urdf b/toid_bot_description/src/toid_bot_description.urdf index 83de634..e47500b 100644 --- a/toid_bot_description/src/toid_bot_description.urdf +++ b/toid_bot_description/src/toid_bot_description.urdf @@ -2,6 +2,7 @@ + @@ -11,7 +12,7 @@ - + @@ -22,7 +23,11 @@ - + + + + + @@ -39,6 +44,9 @@ + + + @@ -56,10 +64,16 @@ + + + + + + - + @@ -112,6 +126,6 @@ - + \ 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..0291580 --- /dev/null +++ b/toid_bt/behavior_trees/behavior1.xml @@ -0,0 +1,367 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Action server name + + + + + + + + Action server name + + + Service name + + + + Service name + + + Service name + + + Y position in meters + Heading in degrees + Reference frame + X position in meters + Topic name + + + + + Action server name + + + Action server name + + + Service name + + + + diff --git a/toid_bt/behavior_trees/calibration_routines.xml b/toid_bt/behavior_trees/calibration_routines.xml new file mode 100644 index 0000000..81becab --- /dev/null +++ b/toid_bt/behavior_trees/calibration_routines.xml @@ -0,0 +1,201 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Service name + + + + + + + + Action server name + + + + + + Action server name + + + + + + + Action server name + + + + + + Service name + + + Service name + + + + \ No newline at end of file diff --git a/toid_bt/behavior_trees/toid_behaviors.btproj b/toid_bt/behavior_trees/toid_behaviors.btproj new file mode 100644 index 0000000..655d646 --- /dev/null +++ b/toid_bt/behavior_trees/toid_behaviors.btproj @@ -0,0 +1,92 @@ + + + + + + + + + + + + + Action server name + + + + + + Service name + + + + + + + + + + + Action server name + + + + + + Action server name + + + + + + Action server name + + + + + + + + Action server name + + + Service name + + + + Service name + + + Service name + + + Y position in meters + Heading in degrees + Reference frame + X position in meters + Topic name + + + + + + + + + + + Service name + + + + + Action server 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..f93fd43 --- /dev/null +++ b/toid_bt/include/toid_bt/bt_executor.hpp @@ -0,0 +1,47 @@ +#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" +#include "std_msgs/msg/string.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); + + void acorn_position(geometry_msgs::msg::PoseStamped &msg); + + void acorn_pose_cb(geometry_msgs::msg::PoseStamped::ConstSharedPtr msg); + + void to_flip_cb(std_msgs::msg::String::ConstSharedPtr msg); + + std::string describeCustomNodes(); + + private: + std::shared_ptr logger_cout_; + tf2_ros::Buffer::SharedPtr tf_buffer_; + std::shared_ptr tf_listener_; + + rclcpp::Subscription::SharedPtr acorn_pose_sub_; + rclcpp::Subscription::SharedPtr on_rotate_sub_; + + geometry_msgs::msg::PoseStamped acorn_pose_; + std::string to_flip_ = "0000"; + + std::string base_frame_; + std::string global_frame_; + }; +} \ 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..303551d --- /dev/null +++ b/toid_bt/include/toid_bt/plugin.hpp @@ -0,0 +1,9 @@ +#pragma once +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" + +namespace toid { + using PoseFunc = std::function; + using FlipFunc = std::function; +} \ No newline at end of file diff --git a/toid_bt/include/toid_bt/plugins/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/in_position_decorator.hpp b/toid_bt/include/toid_bt/plugins/in_position_decorator.hpp new file mode 100644 index 0000000..dc5e81f --- /dev/null +++ b/toid_bt/include/toid_bt/plugins/in_position_decorator.hpp @@ -0,0 +1,74 @@ +#pragma once + +#include "angles/angles.h" +#include "behaviortree_ros2/bt_service_node.hpp" +#include "std_srvs/srv/empty.hpp" +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "toid_bt/plugin.hpp" + +namespace toid +{ + +class InPositionNode : public BT::DecoratorNode +{ +public: + InPositionNode( + const std::string & name, const BT::NodeConfig & conf, PoseFunc get_pose) + : BT::DecoratorNode(name, conf), get_pose(get_pose) + { + } + + static BT::PortsList providedPorts() { + return { + BT::InputPort("timeout", 1, {}) + }; + } + + private: + bool checkIfPosesAreClose(geometry_msgs::msg::PoseStamped &poseA, geometry_msgs::msg::PoseStamped &poseB) { + const double dx = abs(poseA.pose.position.x - poseB.pose.position.x); + return dx < 0.005; + } + + bool checkIfInPose(geometry_msgs::msg::PoseStamped &pose) { + const double dx = abs(0.2575 - pose.pose.position.x); + return dx < 0.005; + } + + BT::NodeStatus tick() override + { + if(status() == BT::NodeStatus::IDLE) { + setStatus(BT::NodeStatus::RUNNING); + last_pos_change = clock.now(); + } + + double timeout = getInput("timeout").value_or(1.0); + + + geometry_msgs::msg::PoseStamped pose; + get_pose(pose); + + if(checkIfInPose(pose)) { + if(checkIfPosesAreClose(last_pos, pose)) { + if(rclcpp::Time(pose.header.stamp) - last_pos_change > rclcpp::Duration::from_seconds(timeout)) { + haltChild(); + return BT::NodeStatus::SUCCESS; + } + } else { + last_pos = pose; + last_pos_change = pose.header.stamp; + } + } + + 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/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/pull_pin_action.hpp b/toid_bt/include/toid_bt/plugins/pull_pin_action.hpp new file mode 100644 index 0000000..71561a2 --- /dev/null +++ b/toid_bt/include/toid_bt/plugins/pull_pin_action.hpp @@ -0,0 +1,48 @@ +#pragma once + +#include "angles/angles.h" +#include "behaviortree_ros2/bt_action_node.hpp" +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "toid_bt/plugin.hpp" +#include "toid_msgs/action/empty_action.hpp" + +namespace toid +{ + +class PullPinNode : public BT::RosActionNode +{ +public: + PullPinNode( + const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params) + : BT::RosActionNode(name, conf, params) + { + } + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ + //BT::InputPort("options"), + }); + } + + bool setGoal(Goal &) override + { + return true; + } + + void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); } + + BT::NodeStatus onResultReceived(const WrappedResult &) override + { + return BT::NodeStatus::SUCCESS; + } + + BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override + { + RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error)); + return BT::NodeStatus::FAILURE; + } +}; + +} // namespace toid \ No newline at end of file diff --git a/toid_bt/include/toid_bt/plugins/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..4cf1382 --- /dev/null +++ b/toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp @@ -0,0 +1,75 @@ +#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("backwards", false, {}), + 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(); + auto backwards = getInput("backwards").value(); + + geometry_msgs::msg::PoseStamped pose; + get_pose(pose); + + if(!backwards) { + goal.angle = std::atan2(y - pose.pose.position.y, x - pose.pose.position.x); + } + if(backwards) { + goal.angle = std::atan2(pose.pose.position.y - y, pose.pose.position.x - x); + } + goal.min_angle = angles::from_degrees(min_angle); + goal.max_speed = max_speed; + goal.mode = 1; + + 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..27a75d1 --- /dev/null +++ b/toid_bt/include/toid_bt/plugins/send_text_action.hpp @@ -0,0 +1,48 @@ +#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, FlipFunc get_text) + : BT::RosServiceNode(name, conf, params), get_text(get_text) + { + } + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ + BT::InputPort("text"), + }); + } + + bool setRequest(typename Request::SharedPtr &req) override { + std::string text = getInput("text").value_or(""); + if(text=="") { + get_text(text); + } + req->text = text; + + return true; + } + + BT::NodeStatus onResponseReceived(const typename Response::SharedPtr &) override + { + return BT::NodeStatus::SUCCESS; + } +protected: + FlipFunc get_text; +}; + +} // namespace toid \ No newline at end of file diff --git a/toid_bt/include/toid_bt/plugins/set_initial_pose_action.hpp b/toid_bt/include/toid_bt/plugins/set_initial_pose_action.hpp new file mode 100644 index 0000000..24618c3 --- /dev/null +++ b/toid_bt/include/toid_bt/plugins/set_initial_pose_action.hpp @@ -0,0 +1,60 @@ +#pragma once + +#include "angles/angles.h" +#include "behaviortree_ros2/bt_topic_pub_node.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "tf2/LinearMath/Quaternion.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace toid +{ + +class SetInitialPoseNode +: public BT::RosTopicPubNode +{ +public: + SetInitialPoseNode( + const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params) + : BT::RosTopicPubNode(name, conf, params) + { + } + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ + BT::InputPort("x", 0.0, "X position in meters"), + BT::InputPort("y", 0.0, "Y position in meters"), + BT::InputPort("theta", 0.0, "Heading in degrees"), + BT::InputPort("frame_id", "map", "Reference frame"), + }); + } + + bool setMessage(geometry_msgs::msg::PoseWithCovarianceStamped & msg) override + { + double x = getInput("x").value_or(0.0); + double y = getInput("y").value_or(0.0); + double theta_deg = getInput("theta").value_or(0.0); + std::string frame_id = getInput("frame_id").value_or("map"); + + msg.header.stamp = node_->get_clock()->now(); + msg.header.frame_id = frame_id; + + msg.pose.pose.position.x = x; + msg.pose.pose.position.y = y; + msg.pose.pose.position.z = 0.0; + + tf2::Quaternion q; + q.setRPY(0.0, 0.0, angles::from_degrees(theta_deg)); + msg.pose.pose.orientation = tf2::toMsg(q); + + // Default covariance: small diagonal for x, y, yaw + msg.pose.covariance = {}; + msg.pose.covariance[0] = 0.25; // x + msg.pose.covariance[7] = 0.25; // y + msg.pose.covariance[35] = 0.068; // yaw + + return true; + } +}; + +} // namespace toid diff --git a/toid_bt/include/toid_bt/plugins/set_parameter_action.hpp b/toid_bt/include/toid_bt/plugins/set_parameter_action.hpp new file mode 100644 index 0000000..9d973ba --- /dev/null +++ b/toid_bt/include/toid_bt/plugins/set_parameter_action.hpp @@ -0,0 +1,50 @@ +#pragma once + +#include "angles/angles.h" +#include "behaviortree_ros2/bt_service_node.hpp" +#include "std_srvs/srv/empty.hpp" +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "toid_bt/plugin.hpp" +#include "rcl_interfaces/srv/set_parameters.hpp" + +namespace toid +{ + +class SetParameterNode : public BT::RosServiceNode +{ +public: + SetParameterNode( + const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params) + : BT::RosServiceNode(name, conf, params) + { + } + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ + BT::InputPort("node"), + BT::InputPort("parameter"), + BT::InputPort("value") + }); + } + + bool setRequest(typename Request::SharedPtr & request) override { + std::string parameter = getInput("parameter").value(); + std::string value = getInput("value").value(); + std::string node = getInput("node").value(); + + setServiceName("/" + node + "/set_parameters"); + request->parameters.emplace_back(); + request->parameters.front().name = parameter; + request->parameters.front().value.string_value = value; + return true; +} + + BT::NodeStatus onResponseReceived(const typename Response::SharedPtr &) override + { + return BT::NodeStatus::SUCCESS; + } +}; + +} // namespace \ No newline at end of file diff --git a/toid_bt/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..fe41c10 --- /dev/null +++ b/toid_bt/include/toid_bt/plugins/translate_x_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_move_coords.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(); + + geometry_msgs::msg::PoseStamped pose; + get_pose(pose); + + double yaw = tf2::getYaw(pose.pose.orientation); + + goal.x = pose.pose.position.x + cos(yaw) * x_goal; + goal.y = pose.pose.position.y + sin(yaw) * x_goal; + goal.theta = yaw; + goal.backwards = x_goal < 0; + goal.mode = 1; + goal.max_speed = max_speed; + + return true; + } + + 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..a52ee36 --- /dev/null +++ b/toid_bt/src/bt_executor.cpp @@ -0,0 +1,169 @@ +#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/in_position_decorator.hpp" +#include "toid_bt/plugins/move_coords_action.hpp" +#include "toid_bt/plugins/pull_pin_action.hpp" +#include "toid_bt/plugins/rotate_action.hpp" +#include "toid_bt/plugins/rotate_towards_action.hpp" +#include "toid_bt/plugins/send_text_action.hpp" +#include "toid_bt/plugins/set_parameter_action.hpp" +#include "toid_bt/plugins/stuck_detector_decorator.hpp" +#include "toid_bt/plugins/set_initial_pose_action.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_); + using namespace std::placeholders; + + acorn_pose_sub_ = node()->create_subscription( + "/closest_acorn", rclcpp::QoS(1), std::bind(&TreeExecutor::acorn_pose_cb, this, _1)); + + on_rotate_sub_ = node()->create_subscription( + "/to_flip", rclcpp::QoS(1), std::bind(&TreeExecutor::to_flip_cb, this, _1)); +} + +void TreeExecutor::onTreeCreated(BT::Tree & tree) +{ + logger_cout_ = std::make_shared(tree); +} + +void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory) +{ + FlipFunc get_to_flip = [this](std::string & pose) { pose = this->to_flip_; }; + PoseFunc get_pose = [this](geometry_msgs::msg::PoseStamped & pose) { this->position(pose); }; + PoseFunc get_acorn_pose = [this](geometry_msgs::msg::PoseStamped & pose) { + this->acorn_position(pose); + }; + + rclcpp::Node::SharedPtr nh = node(); + factory.registerNodeType( + "MovePointSimple", BT::RosNodeParams(nh, "/moveCoords"), get_pose); + + factory.registerNodeType( + "ApproachAcorns", BT::RosNodeParams(nh, "/approachAcorns"), get_pose); + + factory.registerNodeType( + "RotateSimple", BT::RosNodeParams(nh, "/rotate"), get_pose); + + factory.registerNodeType( + "RotateAcorns", BT::RosNodeParams(nh, "/rotateAcorns"), get_pose); + + factory.registerNodeType( + "RotateTowards", BT::RosNodeParams(nh, "/rotate"), get_pose); + + factory.registerNodeType( + "TranslateX", BT::RosNodeParams(nh, "/moveCoords"), get_pose); + + factory.registerNodeType( + "SetWidth", BT::RosNodeParams(nh, "/set_width"), get_pose); + + factory.registerNodeType("WaitPullPin", BT::RosNodeParams(nh, "/start_plug")); + + factory.registerNodeType( + "SetParameter", BT::RosNodeParams(nh, "/compressed_image_subscriber/set_parameters")); + + factory.registerNodeType( + "SetInitialPose", BT::RosNodeParams(nh, "/initialpose")); + + factory.registerNodeType("ZeroOdom", BT::RosNodeParams(nh, "/zero")); + + factory.registerNodeType("EndCalib", BT::RosNodeParams(nh, "/end_calib")); + + BT::RosNodeParams service_params1(nh, "/sequence1"); + service_params1.server_timeout = std::chrono::seconds(15); + factory.registerNodeType("Seq1", service_params1); + + BT::RosNodeParams service_params2(nh, "/sequence2"); + service_params2.server_timeout = std::chrono::seconds(20); + factory.registerNodeType("Seq2", service_params2, get_to_flip); + + BT::RosNodeParams service_params3(nh, "/sequence3"); + service_params3.server_timeout = std::chrono::seconds(15); + factory.registerNodeType("Seq3", service_params3); + + factory.registerNodeType("DetectStuck", get_pose); + + factory.registerNodeType("InPose", get_acorn_pose); + + std::cout << describeCustomNodes() << std::endl; +} + +void TreeExecutor::position(geometry_msgs::msg::PoseStamped & pose) +{ + nav2_util::getCurrentPose(pose, *tf_buffer_, global_frame_, base_frame_); +} + +void TreeExecutor::acorn_position(geometry_msgs::msg::PoseStamped & pose) { pose = acorn_pose_; } + +void TreeExecutor::acorn_pose_cb(geometry_msgs::msg::PoseStamped::ConstSharedPtr msg) +{ + auto node = this->node(); + try { + acorn_pose_ = tf_buffer_->transform(*msg, "base_footprint"); + } catch (const tf2::TransformException & e) { + RCLCPP_ERROR_THROTTLE(node->get_logger(), *node->get_clock(), 5000, "Transform timeout"); + } +} + +void TreeExecutor::to_flip_cb(std_msgs::msg::String::ConstSharedPtr msg) +{ + this->to_flip_ = msg->data; +} + +std::optional TreeExecutor::onTreeExecutionCompleted( + BT::NodeStatus status, bool was_cancelled) +{ + (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..567408f 100644 --- a/toid_control/launch/toid.launch.py +++ b/toid_control/launch/toid.launch.py @@ -1,98 +1,106 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument -from launch.conditions import IfCondition, UnlessCondition +from launch.conditions import IfCondition, UnlessCondition from launch.substitutions import Command, LaunchConfiguration, IfElseSubstitution from launch_ros.actions import Node, LifecycleNode from launch_ros.substitutions import FindPackageShare import os + def generate_launch_description(): - pkg_share = FindPackageShare("").find('toid_control') - params = os.path.join(pkg_share, 'params', 'toid_general_params.yaml') - default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'rviz.rviz') + pkg_share = FindPackageShare("").find("toid_control") + params = os.path.join(pkg_share, "params", "toid_general_params.yaml") + default_rviz_config_path = os.path.join(pkg_share, "rviz", "rviz.rviz") - description_pkg_share = FindPackageShare("").find('toid_bot_description') + description_pkg_share = FindPackageShare("").find("toid_bot_description") default_model_path = os.path.join( - description_pkg_share, - 'src', - 'toid_bot_description.urdf' + description_pkg_share, "src", "toid_bot_description.urdf" ) visualize = LaunchConfiguration("visualize") visualize_arg = DeclareLaunchArgument( - 'visualize', - default_value='False', - description="Whether to launch rviz2" + "visualize", default_value="False", description="Whether to launch rviz2" ) use_mock = LaunchConfiguration("use_mock") use_mock_arg = DeclareLaunchArgument( - 'use_mock', - default_value='True', - description="Whether to use mock controller" + "use_mock", default_value="True", description="Whether to use mock controller" ) odom_broadcast = Node( - package='toid_odometry', - executable='toid_odometry', - name='map_to_odom_broadcaster', + package="toid_odometry", + executable="toid_odometry", + name="map_to_odom_broadcaster", emulate_tty=True, - parameters=[{'mock_odom': use_mock}], - condition=IfCondition(LaunchConfiguration('visualize')) + parameters=[ + { + "mock_odom": use_mock, + "serial_port": "/dev/serial/by-id/usb-Mg_Robotics_Magrob_Odometry_MCU_E66368254F4F4036-if02", + } + ], ) - robot_state_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - name='robot_state_publisher', - output='screen', + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", emulate_tty=True, - parameters=[{'robot_description': Command(['xacro ', default_model_path, ' use_mock:=', use_mock])}] + parameters=[ + { + "robot_description": Command( + [ + "xacro ", + default_model_path, + " use_mock:=", + use_mock, + " serial_port:=/dev/serial/by-id/usb-Mg_Robotics_Magrob_Odometry_MCU_E66368254F4F4036-if00", + ] + ) + } + ], ) controller_manager = Node( - package='controller_manager', - executable='ros2_control_node', - output='screen', + package="controller_manager", + executable="ros2_control_node", + output="screen", emulate_tty=True, parameters=[params], - arguments=['--ros-args', '--log-level', 'warn'] + arguments=["--ros-args", "--log-level", "warn"], ) joint_state_broadcaster = Node( - package='controller_manager', - executable='spawner', - output='screen', + package="controller_manager", + executable="spawner", + output="screen", emulate_tty=True, - arguments=[ - "joint_state_broadcaster", - '--ros-args', '--log-level', 'warn' - ] + arguments=["joint_state_broadcaster", "--ros-args", "--log-level", "warn"], ) velocity_controller = Node( - package='controller_manager', - executable='spawner', - output='screen', + package="controller_manager", + executable="spawner", + output="screen", emulate_tty=True, arguments=[ "velocity_controller", "--inactive", "-p", params, - '--ros-args', '--log-level', 'warn' - ], + "--ros-args", + "--log-level", + "warn", + ], ) - diffbot_base_controller = Node( - package='controller_manager', - executable='spawner', - output='both', + package="controller_manager", + executable="spawner", + output="both", emulate_tty=True, arguments=[ "diffdrive_controller", @@ -103,39 +111,43 @@ def generate_launch_description(): "--controller-ros-args", "-r diffdrive_controller/odom:=/odom", "--controller-ros-args", - IfElseSubstitution(use_mock, - "--param odom_frame_id:=odom", - "--param odom_frame_id:=odom_expected" + IfElseSubstitution( + use_mock, + "--param odom_frame_id:=odom", + "--param odom_frame_id:=odom_expected", ), "--controller-ros-args", - IfElseSubstitution(use_mock, - "--param enable_odom_tf:=true", - "--param enable_odom_tf:=false" + IfElseSubstitution( + use_mock, + "--param enable_odom_tf:=true", + "--param enable_odom_tf:=false", ), - '--ros-args', '--log-level', 'warn' - ] + "--ros-args", + "--log-level", + "warn", + ], ) rviz_node = Node( - package='rviz2', - executable='rviz2', - name='rviz2', - output='screen', + package="rviz2", + executable="rviz2", + name="rviz2", + output="screen", emulate_tty=True, - arguments=['-d', default_rviz_config_path, - '--ros-args', '--log-level', 'warn' - ], - condition=IfCondition(visualize) + arguments=["-d", default_rviz_config_path, "--ros-args", "--log-level", "warn"], + condition=IfCondition(visualize), ) - return LaunchDescription([ - visualize_arg, - use_mock_arg, - odom_broadcast, - robot_state_publisher, - controller_manager, - joint_state_broadcaster, - diffbot_base_controller, - velocity_controller, - rviz_node - ]) + return LaunchDescription( + [ + visualize_arg, + use_mock_arg, + odom_broadcast, + robot_state_publisher, + controller_manager, + joint_state_broadcaster, + diffbot_base_controller, + velocity_controller, + rviz_node, + ] + ) diff --git a/toid_control/src/toid_control.cpp b/toid_control/src/toid_control.cpp index af6f261..5534873 100644 --- a/toid_control/src/toid_control.cpp +++ b/toid_control/src/toid_control.cpp @@ -82,6 +82,15 @@ hardware_interface::return_type toid::StepperInterface::write(const rclcpp::Time boost::system::error_code ec; ec = serial_port_.close(ec); ec = serial_port_.open(serial_port_name_, ec); + if(!ec.failed()) { + try { + for(int i = 0; i < 64; i++) { + asio::write(serial_port_, asio::buffer("\x00")); + } + } catch(const std::runtime_error& err) { + + } + } } return hardware_interface::return_type::OK; } diff --git a/toid_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..d48ec2b --- /dev/null +++ b/toid_interaction/package.xml @@ -0,0 +1,22 @@ + + + + toid_interaction + 0.0.0 + TODO: Package description + pimpest + MIT + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + python3-serial + python3-gpiozero + toid_msgs + + + 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..aadf802 --- /dev/null +++ b/toid_interaction/setup.py @@ -0,0 +1,33 @@ +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', + 'cam_calib = toid_interaction.camera:main', + 'cam_calib1 = toid_interaction.camera1:main' + ], + }, +) diff --git a/toid_interaction/test/test_copyright.py b/toid_interaction/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/toid_interaction/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/toid_interaction/test/test_flake8.py b/toid_interaction/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/toid_interaction/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/toid_interaction/test/test_pep257.py b/toid_interaction/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/toid_interaction/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/toid_interaction/toid_interaction/__init__.py b/toid_interaction/toid_interaction/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/toid_interaction/toid_interaction/interaction_node.py b/toid_interaction/toid_interaction/interaction_node.py new file mode 100644 index 0000000..dc69da4 --- /dev/null +++ b/toid_interaction/toid_interaction/interaction_node.py @@ -0,0 +1,117 @@ +import rclpy +import rclpy.callback_groups +import rclpy.executors +from rclpy.node import Node +from std_srvs.srv import Empty + +from gpiozero import Button, OutputDevice + +from serial.tools import list_ports + +from toid_interaction.mechanism.sekvenca_2026 import okreni, okreni_niz +from toid_interaction.mechanism.zidovi_load import ZidoviAction +from toid_interaction.mechanism.zidovi import Zidovi +from toid_interaction.mechanism.zupcanik import ZupcanikAction +from toid_msgs.action import EmptyAction +from toid_msgs.srv import SendString + +from rclpy.action.server import ServerGoalHandle +from rclpy.action.server import ActionServer + + +class InteracitionNode(Node): + step: int = 0 + btn_: Button + output_pin_: OutputDevice + start_pin_action_: ActionServer + + def __init__(self): + super().__init__("ToidInteractionNode") + + self.find_sigma() + + self.srv = self.create_service(Empty, "/sequence1", self.sequence1_cb) + self.get_logger().info("Service 'sequence1' ready.") + + self.srv = self.create_service(SendString, "/sequence2", self.sequence2_cb) + self.get_logger().info("Service 'sequence2' ready.") + + self.srv = self.create_service(Empty, "/sequence3", self.sequence3_cb) + self.get_logger().info("Service 'sequence3' ready.") + + self.btn_ = Button(17) + self.output_pin_ = OutputDevice(27) + + self.start_pin_action_ = ActionServer( + self, EmptyAction, "/start_plug", execute_callback=self.start_plug_action_cb, + callback_group=rclpy.callback_groups.ReentrantCallbackGroup() + ) + + self.get_logger().info("Action 'start_plug' ready.") + + + def find_sigma(self): + for port_info in list_ports.comports(): + if port_info.vid == 0x1A86 and port_info.pid == 0x55D3: + 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, -1015, 25) + self.step = 1 + return response + + def sequence2_cb(self, request: SendString.Request, response: SendString.Response): + if self.step != 1: + return SendString.Response() + + zidovi = Zidovi(self.st_motor_device_name) + + zidovi.zidovi(1, 1500, 600, 210, 120, 120) + + okreni_niz(request.text) + + zidovi.zidovi(0, 1500, 420, 50, 120, 120) + 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 = Zidovi(self.st_motor_device_name) + + zupcanik.zupcanik(1, 1015, 25) + zidovi.zidovi(0, 1500, 180, 160, 120, 120) + okreni(5) + self.step = 0 + return response + + async def start_plug_action_cb(self, goal_handle: ServerGoalHandle): + while not self.btn_.is_active: + pass + while self.btn_.is_active: + pass + goal_handle.succeed() + return EmptyAction.Result() + + +def main(args=None): + rclpy.init(args=args) + + node = InteracitionNode() + executor = rclpy.executors.MultiThreadedExecutor() + executor.add_node(node) + executor.spin() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/__init__.py b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__init__.py new file mode 100644 index 0000000..58551ab --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__init__.py @@ -0,0 +1,8 @@ +#!/usr/bin/env python + +from .port_handler import * +from .protocol_packet_handler import * +from .group_sync_write import * +from .group_sync_read import * +from .sts import * +from .scscl import * diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/group_sync_read.py b/toid_interaction/toid_interaction/mechanism/STservo_sdk/group_sync_read.py new file mode 100644 index 0000000..dad8d33 --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/group_sync_read.py @@ -0,0 +1,151 @@ +#!/usr/bin/env python + +from .stservo_def import * + +class GroupSyncRead: + def __init__(self, ph, start_address, data_length): + self.ph = ph + self.start_address = start_address + self.data_length = data_length + + self.last_result = False + self.is_param_changed = False + self.param = [] + self.data_dict = {} + + self.clearParam() + + def makeParam(self): + if not self.data_dict: # len(self.data_dict.keys()) == 0: + return + + self.param = [] + + for scs_id in self.data_dict: + self.param.append(scs_id) + + def addParam(self, sts_id): + if sts_id in self.data_dict: # sts_id already exist + return False + + self.data_dict[sts_id] = [] # [0] * self.data_length + + self.is_param_changed = True + return True + + def removeParam(self, sts_id): + if sts_id not in self.data_dict: # NOT exist + return + + del self.data_dict[sts_id] + + self.is_param_changed = True + + def clearParam(self): + self.data_dict.clear() + + def txPacket(self): + if len(self.data_dict.keys()) == 0: + + return COMM_NOT_AVAILABLE + + if self.is_param_changed is True or not self.param: + self.makeParam() + + return self.ph.syncReadTx(self.start_address, self.data_length, self.param, len(self.data_dict.keys())) + + def rxPacket(self): + self.last_result = True + + result = COMM_RX_FAIL + + if len(self.data_dict.keys()) == 0: + return COMM_NOT_AVAILABLE + + result, rxpacket = self.ph.syncReadRx(self.data_length, len(self.data_dict.keys())) + # print(rxpacket) + if len(rxpacket) >= (self.data_length+6): + for sts_id in self.data_dict: + self.data_dict[sts_id], result = self.readRx(rxpacket, sts_id, self.data_length) + if result != COMM_SUCCESS: + self.last_result = False + # print(sts_id) + else: + self.last_result = False + # print(self.last_result) + return result + + def txRxPacket(self): + result = self.txPacket() + if result != COMM_SUCCESS: + return result + + return self.rxPacket() + + def readRx(self, rxpacket, sts_id, data_length): + # print(sts_id) + # print(rxpacket) + data = [] + rx_length = len(rxpacket) + # print(rx_length) + rx_index = 0; + while (rx_index+6+data_length) <= rx_length: + headpacket = [0x00, 0x00, 0x00] + while rx_index < rx_length: + headpacket[2] = headpacket[1]; + headpacket[1] = headpacket[0]; + headpacket[0] = rxpacket[rx_index]; + rx_index += 1 + if (headpacket[2] == 0xFF) and (headpacket[1] == 0xFF) and headpacket[0] == sts_id: + # print(rx_index) + break + # print(rx_index+3+data_length) + if (rx_index+3+data_length) > rx_length: + break; + if rxpacket[rx_index] != (data_length+2): + rx_index += 1 + # print(rx_index) + continue + rx_index += 1 + Error = rxpacket[rx_index] + rx_index += 1 + calSum = sts_id + (data_length+2) + Error + data = [Error] + data.extend(rxpacket[rx_index : rx_index+data_length]) + for i in range(0, data_length): + calSum += rxpacket[rx_index] + rx_index += 1 + calSum = ~calSum & 0xFF + # print(calSum) + if calSum != rxpacket[rx_index]: + return None, COMM_RX_CORRUPT + return data, COMM_SUCCESS + # print(rx_index) + return None, COMM_RX_CORRUPT + + def isAvailable(self, sts_id, address, data_length): + #if self.last_result is False or sts_id not in self.data_dict: + if sts_id not in self.data_dict: + return False, 0 + + if (address < self.start_address) or (self.start_address + self.data_length - data_length < address): + return False, 0 + if not self.data_dict[sts_id]: + return False, 0 + if len(self.data_dict[sts_id])<(data_length+1): + return False, 0 + return True, self.data_dict[sts_id][0] + + def getData(self, sts_id, address, data_length): + if data_length == 1: + return self.data_dict[sts_id][address-self.start_address+1] + elif data_length == 2: + return self.ph.scs_makeword(self.data_dict[sts_id][address-self.start_address+1], + self.data_dict[sts_id][address-self.start_address+2]) + elif data_length == 4: + return self.ph.scs_makedword(self.ph.scs_makeword(self.data_dict[sts_id][address-self.start_address+1], + self.data_dict[sts_id][address-self.start_address+2]), + self.ph.scs_makeword(self.data_dict[sts_id][address-self.start_address+3], + self.data_dict[sts_id][address-self.start_address+4])) + else: + return 0 diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/group_sync_write.py b/toid_interaction/toid_interaction/mechanism/STservo_sdk/group_sync_write.py new file mode 100644 index 0000000..b2ce36d --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/group_sync_write.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python + +from .stservo_def import * + +class GroupSyncWrite: + def __init__(self, ph, start_address, data_length): + self.ph = ph + self.start_address = start_address + self.data_length = data_length + + self.is_param_changed = False + self.param = [] + self.data_dict = {} + + self.clearParam() + + def makeParam(self): + if not self.data_dict: + return + + self.param = [] + + for sts_id in self.data_dict: + if not self.data_dict[sts_id]: + return + + self.param.append(sts_id) + self.param.extend(self.data_dict[sts_id]) + + def addParam(self, sts_id, data): + if sts_id in self.data_dict: # sts_id already exist + return False + + if len(data) > self.data_length: # input data is longer than set + return False + + self.data_dict[sts_id] = data + + self.is_param_changed = True + return True + + def removeParam(self, sts_id): + if sts_id not in self.data_dict: # NOT exist + return + + del self.data_dict[sts_id] + + self.is_param_changed = True + + def changeParam(self, sts_id, data): + if sts_id not in self.data_dict: # NOT exist + return False + + if len(data) > self.data_length: # input data is longer than set + return False + + self.data_dict[sts_id] = data + + self.is_param_changed = True + return True + + def clearParam(self): + self.data_dict.clear() + + def txPacket(self): + if len(self.data_dict.keys()) == 0: + return COMM_NOT_AVAILABLE + + if self.is_param_changed is True or not self.param: + self.makeParam() + + return self.ph.syncWriteTxOnly(self.start_address, self.data_length, self.param, + len(self.data_dict.keys()) * (1 + self.data_length)) diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/port_handler.py b/toid_interaction/toid_interaction/mechanism/STservo_sdk/port_handler.py new file mode 100644 index 0000000..63f0943 --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/port_handler.py @@ -0,0 +1,115 @@ +#!/usr/bin/env python + +import time +import serial +import sys +import platform + +DEFAULT_BAUDRATE = 1000000 +LATENCY_TIMER = 50 + +class PortHandler(object): + def __init__(self, port_name): + self.is_open = False + self.baudrate = DEFAULT_BAUDRATE + self.packet_start_time = 0.0 + self.packet_timeout = 0.0 + self.tx_time_per_byte = 0.0 + + self.is_using = False + self.port_name = port_name + self.ser = None + + def openPort(self): + return self.setBaudRate(self.baudrate) + + def closePort(self): + self.ser.close() + self.is_open = False + + def clearPort(self): + self.ser.flush() + + def setPortName(self, port_name): + self.port_name = port_name + + def getPortName(self): + return self.port_name + + def setBaudRate(self, baudrate): + baud = self.getCFlagBaud(baudrate) + + if baud <= 0: + # self.setupPort(38400) + # self.baudrate = baudrate + return False # TODO: setCustomBaudrate(baudrate) + else: + self.baudrate = baudrate + return self.setupPort(baud) + + def getBaudRate(self): + return self.baudrate + + def getBytesAvailable(self): + return self.ser.in_waiting + + def readPort(self, length): + if (sys.version_info > (3, 0)): + return self.ser.read(length) + else: + return [ord(ch) for ch in self.ser.read(length)] + + def writePort(self, packet): + return self.ser.write(packet) + + def setPacketTimeout(self, packet_length): + self.packet_start_time = self.getCurrentTime() + self.packet_timeout = (self.tx_time_per_byte * packet_length) + (self.tx_time_per_byte * 3.0) + LATENCY_TIMER + + def setPacketTimeoutMillis(self, msec): + self.packet_start_time = self.getCurrentTime() + self.packet_timeout = msec + + def isPacketTimeout(self): + if self.getTimeSinceStart() > self.packet_timeout: + self.packet_timeout = 0 + return True + + return False + + def getCurrentTime(self): + return round(time.time() * 1000000000) / 1000000.0 + + def getTimeSinceStart(self): + time_since = self.getCurrentTime() - self.packet_start_time + if time_since < 0.0: + self.packet_start_time = self.getCurrentTime() + + return time_since + + def setupPort(self, cflag_baud): + if self.is_open: + self.closePort() + + self.ser = serial.Serial( + port=self.port_name, + baudrate=self.baudrate, + # parity = serial.PARITY_ODD, + # stopbits = serial.STOPBITS_TWO, + bytesize=serial.EIGHTBITS, + timeout=0 + ) + + self.is_open = True + + self.ser.reset_input_buffer() + + self.tx_time_per_byte = (1000.0 / self.baudrate) * 10.0 + + return True + + def getCFlagBaud(self, baudrate): + if baudrate in [4800, 9600, 14400, 19200, 38400, 57600, 115200, 128000, 250000, 500000, 1000000]: + return baudrate + else: + return -1 \ No newline at end of file diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/protocol_packet_handler.py b/toid_interaction/toid_interaction/mechanism/STservo_sdk/protocol_packet_handler.py new file mode 100644 index 0000000..3e8412c --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/protocol_packet_handler.py @@ -0,0 +1,530 @@ +#!/usr/bin/env python + +from .stservo_def import * + +TXPACKET_MAX_LEN = 250 +RXPACKET_MAX_LEN = 250 + +# for Protocol Packet +PKT_HEADER0 = 0 +PKT_HEADER1 = 1 +PKT_ID = 2 +PKT_LENGTH = 3 +PKT_INSTRUCTION = 4 +PKT_ERROR = 4 +PKT_PARAMETER0 = 5 + +# Protocol Error bit +ERRBIT_VOLTAGE = 1 +ERRBIT_ANGLE = 2 +ERRBIT_OVERHEAT = 4 +ERRBIT_OVERELE = 8 +ERRBIT_OVERLOAD = 32 + + +class protocol_packet_handler(object): + def __init__(self, portHandler, protocol_end): + #self.sts_setend(protocol_end)# STServo bit end(STS/SMS=0, SCS=1) + self.portHandler = portHandler + self.sts_end = protocol_end + + def sts_getend(self): + return self.sts_end + + def sts_setend(self, e): + self.sts_end = e + + def sts_tohost(self, a, b): + if (a & (1<> 16) & 0xFFFF + + def sts_lobyte(self, w): + if self.sts_end==0: + return w & 0xFF + else: + return (w >> 8) & 0xFF + + def sts_hibyte(self, w): + if self.sts_end==0: + return (w >> 8) & 0xFF + else: + return w & 0xFF + + def getProtocolVersion(self): + return 1.0 + + def getTxRxResult(self, result): + if result == COMM_SUCCESS: + return "[TxRxResult] Communication success!" + elif result == COMM_PORT_BUSY: + return "[TxRxResult] Port is in use!" + elif result == COMM_TX_FAIL: + return "[TxRxResult] Failed transmit instruction packet!" + elif result == COMM_RX_FAIL: + return "[TxRxResult] Failed get status packet from device!" + elif result == COMM_TX_ERROR: + return "[TxRxResult] Incorrect instruction packet!" + elif result == COMM_RX_WAITING: + return "[TxRxResult] Now receiving status packet!" + elif result == COMM_RX_TIMEOUT: + return "[TxRxResult] There is no status packet!" + elif result == COMM_RX_CORRUPT: + return "[TxRxResult] Incorrect status packet!" + elif result == COMM_NOT_AVAILABLE: + return "[TxRxResult] Protocol does not support this function!" + else: + return "" + + def getRxPacketError(self, error): + if error & ERRBIT_VOLTAGE: + return "[ServoStatus] Input voltage error!" + + if error & ERRBIT_ANGLE: + return "[ServoStatus] Angle sen error!" + + if error & ERRBIT_OVERHEAT: + return "[ServoStatus] Overheat error!" + + if error & ERRBIT_OVERELE: + return "[ServoStatus] OverEle error!" + + if error & ERRBIT_OVERLOAD: + return "[ServoStatus] Overload error!" + + return "" + + def txPacket(self, txpacket): + checksum = 0 + total_packet_length = txpacket[PKT_LENGTH] + 4 # 4: HEADER0 HEADER1 ID LENGTH + + if self.portHandler.is_using: + return COMM_PORT_BUSY + self.portHandler.is_using = True + + # check max packet length + if total_packet_length > TXPACKET_MAX_LEN: + self.portHandler.is_using = False + return COMM_TX_ERROR + + # make packet header + txpacket[PKT_HEADER0] = 0xFF + txpacket[PKT_HEADER1] = 0xFF + + # add a checksum to the packet + for idx in range(2, total_packet_length - 1): # except header, checksum + checksum += txpacket[idx] + + txpacket[total_packet_length - 1] = ~checksum & 0xFF + + #print "[TxPacket] %r" % txpacket + + # tx packet + self.portHandler.clearPort() + written_packet_length = self.portHandler.writePort(txpacket) + if total_packet_length != written_packet_length: + self.portHandler.is_using = False + return COMM_TX_FAIL + + return COMM_SUCCESS + + def rxPacket(self): + rxpacket = [] + + result = COMM_TX_FAIL + checksum = 0 + rx_length = 0 + wait_length = 6 # minimum length (HEADER0 HEADER1 ID LENGTH ERROR CHKSUM) + + while True: + rxpacket.extend(self.portHandler.readPort(wait_length - rx_length)) + rx_length = len(rxpacket) + if rx_length >= wait_length: + # find packet header + for idx in range(0, (rx_length - 1)): + if (rxpacket[idx] == 0xFF) and (rxpacket[idx + 1] == 0xFF): + break + + if idx == 0: # found at the beginning of the packet + if (rxpacket[PKT_ID] > 0xFD) or (rxpacket[PKT_LENGTH] > RXPACKET_MAX_LEN) or ( + rxpacket[PKT_ERROR] > 0x7F): + # unavailable ID or unavailable Length or unavailable Error + # remove the first byte in the packet + del rxpacket[0] + rx_length -= 1 + continue + + # re-calculate the exact length of the rx packet + if wait_length != (rxpacket[PKT_LENGTH] + PKT_LENGTH + 1): + wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1 + continue + + if rx_length < wait_length: + # check timeout + if self.portHandler.isPacketTimeout(): + if rx_length == 0: + result = COMM_RX_TIMEOUT + else: + result = COMM_RX_CORRUPT + break + else: + continue + + # calculate checksum + for i in range(2, wait_length - 1): # except header, checksum + checksum += rxpacket[i] + checksum = ~checksum & 0xFF + + # verify checksum + if rxpacket[wait_length - 1] == checksum: + result = COMM_SUCCESS + else: + result = COMM_RX_CORRUPT + break + + else: + # remove unnecessary packets + del rxpacket[0: idx] + rx_length -= idx + + else: + # check timeout + if self.portHandler.isPacketTimeout(): + if rx_length == 0: + result = COMM_RX_TIMEOUT + else: + result = COMM_RX_CORRUPT + break + + self.portHandler.is_using = False + return rxpacket, result + + def txRxPacket(self, txpacket): + rxpacket = None + error = 0 + + # tx packet + result = self.txPacket(txpacket) + if result != COMM_SUCCESS: + return rxpacket, result, error + + # (ID == Broadcast ID) == no need to wait for status packet or not available + if (txpacket[PKT_ID] == BROADCAST_ID): + self.portHandler.is_using = False + return rxpacket, result, error + + # set packet timeout + if txpacket[PKT_INSTRUCTION] == INST_READ: + self.portHandler.setPacketTimeout(txpacket[PKT_PARAMETER0 + 1] + 6) + else: + self.portHandler.setPacketTimeout(6) # HEADER0 HEADER1 ID LENGTH ERROR CHECKSUM + + # rx packet + while True: + rxpacket, result = self.rxPacket() + if result != COMM_SUCCESS or txpacket[PKT_ID] == rxpacket[PKT_ID]: + break + + if result == COMM_SUCCESS and txpacket[PKT_ID] == rxpacket[PKT_ID]: + error = rxpacket[PKT_ERROR] + + return rxpacket, result, error + + def ping(self, sts_id): + model_number = 0 + error = 0 + + txpacket = [0] * 6 + + if sts_id >= BROADCAST_ID: + return model_number, COMM_NOT_AVAILABLE, error + + txpacket[PKT_ID] = sts_id + txpacket[PKT_LENGTH] = 2 + txpacket[PKT_INSTRUCTION] = INST_PING + + rxpacket, result, error = self.txRxPacket(txpacket) + + if result == COMM_SUCCESS: + data_read, result, error = self.readTxRx(sts_id, 3, 2) # Address 3 : Model Number + if result == COMM_SUCCESS: + model_number = self.sts_makeword(data_read[0], data_read[1]) + + return model_number, result, error + + def action(self, sts_id): + txpacket = [0] * 6 + + txpacket[PKT_ID] = sts_id + txpacket[PKT_LENGTH] = 2 + txpacket[PKT_INSTRUCTION] = INST_ACTION + + _, result, _ = self.txRxPacket(txpacket) + + return result + + def readTx(self, sts_id, address, length): + + txpacket = [0] * 8 + + if sts_id >= BROADCAST_ID: + return COMM_NOT_AVAILABLE + + txpacket[PKT_ID] = sts_id + txpacket[PKT_LENGTH] = 4 + txpacket[PKT_INSTRUCTION] = INST_READ + txpacket[PKT_PARAMETER0 + 0] = address + txpacket[PKT_PARAMETER0 + 1] = length + + result = self.txPacket(txpacket) + + # set packet timeout + if result == COMM_SUCCESS: + self.portHandler.setPacketTimeout(length + 6) + + return result + + def readRx(self, sts_id, length): + result = COMM_TX_FAIL + error = 0 + + rxpacket = None + data = [] + + while True: + rxpacket, result = self.rxPacket() + + if result != COMM_SUCCESS or rxpacket[PKT_ID] == sts_id: + break + + if result == COMM_SUCCESS and rxpacket[PKT_ID] == sts_id: + error = rxpacket[PKT_ERROR] + + data.extend(rxpacket[PKT_PARAMETER0 : PKT_PARAMETER0+length]) + + return data, result, error + + def readTxRx(self, sts_id, address, length): + txpacket = [0] * 8 + data = [] + + if sts_id >= BROADCAST_ID: + return data, COMM_NOT_AVAILABLE, 0 + + txpacket[PKT_ID] = sts_id + txpacket[PKT_LENGTH] = 4 + txpacket[PKT_INSTRUCTION] = INST_READ + txpacket[PKT_PARAMETER0 + 0] = address + txpacket[PKT_PARAMETER0 + 1] = length + + rxpacket, result, error = self.txRxPacket(txpacket) + if result == COMM_SUCCESS: + error = rxpacket[PKT_ERROR] + + data.extend(rxpacket[PKT_PARAMETER0 : PKT_PARAMETER0+length]) + + return data, result, error + + def read1ByteTx(self, sts_id, address): + return self.readTx(sts_id, address, 1) + + def read1ByteRx(self, sts_id): + data, result, error = self.readRx(sts_id, 1) + data_read = data[0] if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read1ByteTxRx(self, sts_id, address): + data, result, error = self.readTxRx(sts_id, address, 1) + data_read = data[0] if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read2ByteTx(self, sts_id, address): + return self.readTx(sts_id, address, 2) + + def read2ByteRx(self, sts_id): + data, result, error = self.readRx(sts_id, 2) + data_read = self.sts_makeword(data[0], data[1]) if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read2ByteTxRx(self, sts_id, address): + data, result, error = self.readTxRx(sts_id, address, 2) + data_read = self.sts_makeword(data[0], data[1]) if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read4ByteTx(self, sts_id, address): + return self.readTx(sts_id, address, 4) + + def read4ByteRx(self, sts_id): + data, result, error = self.readRx(sts_id, 4) + data_read = self.sts_makedword(self.sts_makeword(data[0], data[1]), + self.sts_makeword(data[2], data[3])) if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read4ByteTxRx(self, sts_id, address): + data, result, error = self.readTxRx(sts_id, address, 4) + data_read = self.sts_makedword(self.sts_makeword(data[0], data[1]), + self.sts_makeword(data[2], data[3])) if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def writeTxOnly(self, sts_id, address, length, data): + txpacket = [0] * (length + 7) + + txpacket[PKT_ID] = sts_id + txpacket[PKT_LENGTH] = length + 3 + txpacket[PKT_INSTRUCTION] = INST_WRITE + txpacket[PKT_PARAMETER0] = address + + txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length] + + result = self.txPacket(txpacket) + self.portHandler.is_using = False + + return result + + def writeTxRx(self, sts_id, address, length, data): + txpacket = [0] * (length + 7) + + txpacket[PKT_ID] = sts_id + txpacket[PKT_LENGTH] = length + 3 + txpacket[PKT_INSTRUCTION] = INST_WRITE + txpacket[PKT_PARAMETER0] = address + + txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length] + rxpacket, result, error = self.txRxPacket(txpacket) + + return result, error + + def write1ByteTxOnly(self, sts_id, address, data): + data_write = [data] + return self.writeTxOnly(sts_id, address, 1, data_write) + + def write1ByteTxRx(self, sts_id, address, data): + data_write = [data] + return self.writeTxRx(sts_id, address, 1, data_write) + + def write2ByteTxOnly(self, sts_id, address, data): + data_write = [self.sts_lobyte(data), self.sts_hibyte(data)] + return self.writeTxOnly(sts_id, address, 2, data_write) + + def write2ByteTxRx(self, sts_id, address, data): + data_write = [self.sts_lobyte(data), self.sts_hibyte(data)] + return self.writeTxRx(sts_id, address, 2, data_write) + + def write4ByteTxOnly(self, sts_id, address, data): + data_write = [self.sts_lobyte(self.sts_loword(data)), + self.sts_hibyte(self.sts_loword(data)), + self.sts_lobyte(self.sts_hiword(data)), + self.sts_hibyte(self.sts_hiword(data))] + return self.writeTxOnly(sts_id, address, 4, data_write) + + def write4ByteTxRx(self, sts_id, address, data): + data_write = [self.sts_lobyte(self.sts_loword(data)), + self.sts_hibyte(self.sts_loword(data)), + self.sts_lobyte(self.sts_hiword(data)), + self.sts_hibyte(self.sts_hiword(data))] + return self.writeTxRx(sts_id, address, 4, data_write) + + def regWriteTxOnly(self, sts_id, address, length, data): + txpacket = [0] * (length + 7) + + txpacket[PKT_ID] = sts_id + txpacket[PKT_LENGTH] = length + 3 + txpacket[PKT_INSTRUCTION] = INST_REG_WRITE + txpacket[PKT_PARAMETER0] = address + + txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length] + + result = self.txPacket(txpacket) + self.portHandler.is_using = False + + return result + + def regWriteTxRx(self, sts_id, address, length, data): + txpacket = [0] * (length + 7) + + txpacket[PKT_ID] = sts_id + txpacket[PKT_LENGTH] = length + 3 + txpacket[PKT_INSTRUCTION] = INST_REG_WRITE + txpacket[PKT_PARAMETER0] = address + + txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length] + + _, result, error = self.txRxPacket(txpacket) + + return result, error + + def syncReadTx(self, start_address, data_length, param, param_length): + txpacket = [0] * (param_length + 8) + # 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN CHKSUM + + txpacket[PKT_ID] = BROADCAST_ID + txpacket[PKT_LENGTH] = param_length + 4 # 7: INST START_ADDR DATA_LEN CHKSUM + txpacket[PKT_INSTRUCTION] = INST_SYNC_READ + txpacket[PKT_PARAMETER0 + 0] = start_address + txpacket[PKT_PARAMETER0 + 1] = data_length + + txpacket[PKT_PARAMETER0 + 2: PKT_PARAMETER0 + 2 + param_length] = param[0: param_length] + + # print(txpacket) + result = self.txPacket(txpacket) + return result + + def syncReadRx(self, data_length, param_length): + wait_length = (6 + data_length) * param_length + self.portHandler.setPacketTimeout(wait_length) + rxpacket = [] + rx_length = 0 + while True: + rxpacket.extend(self.portHandler.readPort(wait_length - rx_length)) + rx_length = len(rxpacket) + if rx_length >= wait_length: + result = COMM_SUCCESS + break + else: + # check timeout + if self.portHandler.isPacketTimeout(): + if rx_length == 0: + result = COMM_RX_TIMEOUT + else: + result = COMM_RX_CORRUPT + break + self.portHandler.is_using = False + return result, rxpacket + + def syncWriteTxOnly(self, start_address, data_length, param, param_length): + txpacket = [0] * (param_length + 8) + # 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM + + txpacket[PKT_ID] = BROADCAST_ID + txpacket[PKT_LENGTH] = param_length + 4 # 4: INST START_ADDR DATA_LEN ... CHKSUM + txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE + txpacket[PKT_PARAMETER0 + 0] = start_address + txpacket[PKT_PARAMETER0 + 1] = data_length + + txpacket[PKT_PARAMETER0 + 2: PKT_PARAMETER0 + 2 + param_length] = param[0: param_length] + + _, result, _ = self.txRxPacket(txpacket) + + return result diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/scscl.py b/toid_interaction/toid_interaction/mechanism/STservo_sdk/scscl.py new file mode 100644 index 0000000..0b64349 --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/scscl.py @@ -0,0 +1,104 @@ +#!/usr/bin/env python + +from .stservo_def import * +from .protocol_packet_handler import * +from .group_sync_write import * + +#波特率定义 +SCSCL_1M = 0 +SCSCL_0_5M = 1 +SCSCL_250K = 2 +SCSCL_128K = 3 +SCSCL_115200 = 4 +SCSCL_76800 = 5 +SCSCL_57600 = 6 +SCSCL_38400 = 7 + +#内存表定义 +#-------EPROM(只读)-------- +SCSCL_MODEL_L = 3 +SCSCL_MODEL_H = 4 + +#-------EPROM(读写)-------- +scs_id = 5 +SCSCL_BAUD_RATE = 6 +SCSCL_MIN_ANGLE_LIMIT_L = 9 +SCSCL_MIN_ANGLE_LIMIT_H = 10 +SCSCL_MAX_ANGLE_LIMIT_L = 11 +SCSCL_MAX_ANGLE_LIMIT_H = 12 +SCSCL_CW_DEAD = 26 +SCSCL_CCW_DEAD = 27 + +#-------SRAM(读写)-------- +SCSCL_TORQUE_ENABLE = 40 +SCSCL_GOAL_POSITION_L = 42 +SCSCL_GOAL_POSITION_H = 43 +SCSCL_GOAL_TIME_L = 44 +SCSCL_GOAL_TIME_H = 45 +SCSCL_GOAL_SPEED_L = 46 +SCSCL_GOAL_SPEED_H = 47 +SCSCL_LOCK = 48 + +#-------SRAM(只读)-------- +SCSCL_PRESENT_POSITION_L = 56 +SCSCL_PRESENT_POSITION_H = 57 +SCSCL_PRESENT_SPEED_L = 58 +SCSCL_PRESENT_SPEED_H = 59 +SCSCL_PRESENT_LOAD_L = 60 +SCSCL_PRESENT_LOAD_H = 61 +SCSCL_PRESENT_VOLTAGE = 62 +SCSCL_PRESENT_TEMPERATURE = 63 +SCSCL_MOVING = 66 +SCSCL_PRESENT_CURRENT_L = 69 +SCSCL_PRESENT_CURRENT_H = 70 + +class scscl(protocol_packet_handler): + def __init__(self, portHandler): + protocol_packet_handler.__init__(self, portHandler, 1) + self.groupSyncWrite = GroupSyncWrite(self, SCSCL_GOAL_POSITION_L, 6) + + def WritePos(self, scs_id, position, time, speed): + txpacket = [self.scs_lobyte(position), self.scs_hibyte(position), self.scs_lobyte(time), self.scs_hibyte(time), self.scs_lobyte(speed), self.scs_hibyte(speed)] + return self.writeTxRx(scs_id, SCSCL_GOAL_POSITION_L, len(txpacket), txpacket) + + def ReadPos(self, scs_id): + scs_present_position, scs_comm_result, scs_error = self.read2ByteTxRx(scs_id, SCSCL_PRESENT_POSITION_L) + return scs_present_position, scs_comm_result, scs_error + + def ReadSpeed(self, scs_id): + scs_present_speed, scs_comm_result, scs_error = self.read2ByteTxRx(scs_id, SCSCL_PRESENT_SPEED_L) + return self.scs_tohost(scs_present_speed, 15), scs_comm_result, scs_error + + def ReadPosSpeed(self, scs_id): + scs_present_position_speed, scs_comm_result, scs_error = self.read4ByteTxRx(scs_id, SCSCL_PRESENT_POSITION_L) + scs_present_position = self.scs_loword(scs_present_position_speed) + scs_present_speed = self.scs_hiword(scs_present_position_speed) + return scs_present_position, self.scs_tohost(scs_present_speed, 15), scs_comm_result, scs_error + + def ReadMoving(self, scs_id): + moving, scs_comm_result, scs_error = self.read1ByteTxRx(scs_id, SCSCL_MOVING) + return moving, scs_comm_result, scs_error + + def SyncWritePos(self, scs_id, position, time, speed): + txpacket = [self.scs_lobyte(position), self.scs_hibyte(position), self.scs_lobyte(time), self.scs_hibyte(time), self.scs_lobyte(speed), self.scs_hibyte(speed)] + return self.groupSyncWrite.addParam(scs_id, txpacket) + + def RegWritePos(self, scs_id, position, time, speed): + txpacket = [self.scs_lobyte(position), self.scs_hibyte(position), self.scs_lobyte(time), self.scs_hibyte(time), self.scs_lobyte(speed), self.scs_hibyte(speed)] + return self.regWriteTxRx(scs_id, SCSCL_GOAL_POSITION_L, len(txpacket), txpacket) + + def RegAction(self): + return self.action(BROADCAST_ID) + + def PWMMode(self, scs_id): + txpacket = [0, 0, 0, 0] + return self.writeTxRx(scs_id, SCSCL_MIN_ANGLE_LIMIT_L, len(txpacket), txpacket) + + def WritePWM(self, scs_id, time): + return self.write2ByteTxRx(scs_id, SCSCL_GOAL_TIME_L, self.scs_toscs(time, 10)) + + def LockEprom(self, scs_id): + return self.write1ByteTxRx(scs_id, SCSCL_LOCK, 1) + + def unLockEprom(self, scs_id): + return self.write1ByteTxRx(scs_id, SCSCL_LOCK, 0) diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/setup.py b/toid_interaction/toid_interaction/mechanism/STservo_sdk/setup.py new file mode 100644 index 0000000..f9bc198 --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/setup.py @@ -0,0 +1,7 @@ +from setuptools import setup, find_packages + +setup( + name='STservo_sdk', + version='0.1', + packages=find_packages(), +) \ No newline at end of file diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/sts.py b/toid_interaction/toid_interaction/mechanism/STservo_sdk/sts.py new file mode 100644 index 0000000..4941c79 --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/sts.py @@ -0,0 +1,144 @@ +#!/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 ReadCurrent(self, sts_id): + sts_present_current, sts_comm_result,sts_error = self.read2ByteTxRx(sts_id, STS_PRESENT_CURRENT_L) # 69 + return self.sts_tohost(sts_present_current, 15), sts_comm_result, sts_error + + def ReadMaxLoad(self, sts_id): + sts_max_load, sts_comm_result,sts_error = self.read2ByteTxRx(sts_id, STS_LOAD_LIMIT) # 0x36 + 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/motori.py b/toid_interaction/toid_interaction/mechanism/motori.py new file mode 100644 index 0000000..2e3f05c --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/motori.py @@ -0,0 +1,83 @@ +from .STservo_sdk.stservo_def import COMM_SUCCESS +from .STservo_sdk import sts + +STS_MOVING_ACC = 0 + +def get_load(packetHandler : sts, id_motora: int) -> float: + sts_present_load, _, _ = packetHandler.ReadLoad(id_motora) + return (sts_present_load & ((1 << 10) - 1)) * 0.1 + +def get_current(packetHandler : sts, id_motora: int) -> float: + sts_present_current, _, _ = packetHandler.ReadCurrent(id_motora) + return (sts_present_current * 6.5) + +def stop_motor(packetHandler : sts, id_motora: int): + packetHandler.WriteSpec(id_motora, 0, 0) + +def print_motor_error(packetHandler : sts, sts_com_result, sts_error, id_motora): + if sts_com_result != COMM_SUCCESS: + print("[ID:%d] %s" % (id_motora, packetHandler.getTxRxResult(sts_com_result))) + if sts_error != 0: + print("[ID:%d] %s" % (id_motora, packetHandler.getRxPacketError(sts_error))) + +def get_motor_pos_speed(packetHandler : sts, id_motora: int) -> tuple[float, float]: + sts_present_position, sts_present_speed, sts_com_result, sts_error = packetHandler.ReadPosSpeed(id_motora) + print_motor_error(packetHandler, sts_com_result, sts_error, id_motora) + + return sts_present_position, sts_present_speed + +def set_motor_speed(packetHandler : sts, id_motora: int, speed: float) -> tuple[float, float]: + sts_com_result, sts_error = packetHandler.WriteSpec(id_motora, speed, STS_MOVING_ACC) + print_motor_error(packetHandler, sts_com_result, sts_error, id_motora) + +def normalize_dpos(curr: float, prev: float) -> float: + if abs(curr - prev) > 3000: + return 4096 - abs(curr - prev) + else: + return abs(curr - prev) + +class CrniMotor: + def __init__(self, packetHandler : sts, id_motora: int): + self.packetHandler = packetHandler + self.id_motora = id_motora + self.targret_reached = False + self.brzina = 0 + + + sts_comm_result, sts_error = packetHandler.WheelMode(id_motora) + if sts_comm_result != COMM_SUCCESS: + print("[ID:%01d] %s" % (id_motora, packetHandler.getTxRxResult(sts_comm_result))) + elif sts_error != 0: + print("[ID:%01d] %s" % (id_motora, packetHandler.getRxPacketError(sts_error))) + + sts_present_position, sts_present_speed = get_motor_pos_speed(packetHandler, id_motora) + self.trenutna_pos = sts_present_position + self.prosla_pos = sts_present_position + self.predjeni_put = 0 + + packetHandler.SetMaxLoad(id_motora, 80) + + + def doCycle(self, target_pos: float, brzina: float) -> bool | None: + if not self.targret_reached: + sts_present_position, sts_present_speed = get_motor_pos_speed(self.packetHandler, self.id_motora) + self.trenutna_pos = sts_present_position + self.predjeni_put += normalize_dpos(self.trenutna_pos, self.prosla_pos) + self.prosla_pos = self.trenutna_pos + + if self.predjeni_put >= target_pos: + self.stop() + self.targret_reached = True + else: + set_motor_speed(self.packetHandler, self.id_motora, brzina) + self.brzina = brzina + + # TODO: if overload stop motor and return false: + + return True + return True + + def stop(self): + stop_motor(self.packetHandler, self.id_motora) + self.brzina = 0 + \ No newline at end of file diff --git a/toid_interaction/toid_interaction/mechanism/sekvenca_2026.py b/toid_interaction/toid_interaction/mechanism/sekvenca_2026.py new file mode 100644 index 0000000..e813eff --- /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 = "259B221729115453" + + +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.py b/toid_interaction/toid_interaction/mechanism/zidovi.py new file mode 100644 index 0000000..4ee2140 --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/zidovi.py @@ -0,0 +1,167 @@ +from .STservo_sdk import PortHandler, sts +from .motori import get_load, stop_motor, get_current, CrniMotor +import time + + +BAUDRATE = 1000000 +STS_MOVING_ACC = 0 + + +class Zidovi: + def __init__(self, devicename: str = '/dev/ttyUSB0'): + self.devicename = devicename + self.portHandler = None + self.packetHandler = None + + def _open_port(self): + self.portHandler = PortHandler(self.devicename) + self.packetHandler = sts(self.portHandler) + if self.portHandler.openPort(): + print("Succeeded to open the port") + else: + print("Failed to open the port") + raise RuntimeError(f"Failed to open port: {self.devicename}") + + def _close_port(self): + if self.portHandler is not None: + for motor_id in (3, 5, 2, 4): + stopped = False + while not stopped: + try: + stop_motor(self.packetHandler, motor_id) + stopped = True + except Exception: + self.portHandler.ser.reset_input_buffer() + self.portHandler.ser.reset_output_buffer() + time.sleep(0.01) + self.portHandler.closePort() + + def _stop_all(self): + # Reset serial buffers — a KeyboardInterrupt can fire mid-packet, + # leaving garbage in the buffers that would corrupt subsequent writes. + if self.portHandler is not None and self.portHandler.ser is not None: + self.portHandler.ser.reset_input_buffer() + self.portHandler.ser.reset_output_buffer() + for motor_id in (2, 3, 4, 5): + stopped = False + while not stopped: + try: + stop_motor(self.packetHandler, motor_id) + stopped = True + except Exception: + self.portHandler.ser.reset_input_buffer() + self.portHandler.ser.reset_output_buffer() + time.sleep(0.01) + + def zidovi(self, smer, brzina=1500, + TargetPos_beli=600, TargetPos_plavi=300, + opterecenje_threshold_beli=120, opterecenje_threshold_plavi=150): + TargetPos_beli = TargetPos_beli * 4096 / 360 + TargetPos_plavi = TargetPos_plavi * 4096 / 360 + + if smer == 1: + Speed_ID2 = brzina + Speed_ID4 = -brzina + Speed_ID3 = -brzina + Speed_ID5 = brzina + else: + Speed_ID2 = -brzina + Speed_ID4 = brzina + Speed_ID3 = brzina + Speed_ID5 = -brzina + + self._open_port() + try: + motor2 = CrniMotor(self.packetHandler, 2) + motor3 = CrniMotor(self.packetHandler, 3) + motor4 = CrniMotor(self.packetHandler, 4) + motor5 = CrniMotor(self.packetHandler, 5) + + brojac = 0 + + + load_blue_count = 0 + load_white_count = 0 + + while True: + curr_time = time.time() + + if smer == 1: + motor3.doCycle(TargetPos_beli, Speed_ID3) + motor5.doCycle(TargetPos_beli, Speed_ID5) + if brojac >= 50: + motor2.doCycle(TargetPos_plavi, Speed_ID2) + motor4.doCycle(TargetPos_plavi, Speed_ID4) + else: + brojac += 1 + else: + motor2.doCycle(TargetPos_plavi, Speed_ID2) + motor4.doCycle(TargetPos_plavi, Speed_ID4) + if brojac >= 50: + motor3.doCycle(TargetPos_beli, Speed_ID3) + motor5.doCycle(TargetPos_beli, Speed_ID5) + else: + brojac += 1 + + print("Time elapsed: %.4f seconds" % (time.time() - curr_time)) + + opterecenje2 = get_load(self.packetHandler, 2) + opterecenje4 = get_load(self.packetHandler, 4) + opterecenje3 = get_load(self.packetHandler, 3) + opterecenje5 = get_load(self.packetHandler, 5) + + napon2 = get_current(self.packetHandler, 2) + napon4 = get_current(self.packetHandler, 4) + napon3 = get_current(self.packetHandler, 3) + napon5 = get_current(self.packetHandler, 5) + + print("Current positions:") + print("[ID:002] PresPos:%d Load:%.1f Current:%.1f" % (motor2.prosla_pos, opterecenje2, napon2)) + print("[ID:004] PresPos:%d Load:%.1f Current:%.1f" % (motor4.prosla_pos, opterecenje4, napon4)) + print("[ID:003] PresPos:%d Load:%.1f Current:%.1f" % (motor3.prosla_pos, opterecenje3, napon3)) + print("[ID:005] PresPos:%d Load:%.1f Current:%.1f" % (motor5.prosla_pos, opterecenje5, napon5)) + + if napon2 > opterecenje_threshold_plavi or napon4 > opterecenje_threshold_plavi: + + load_blue_count+=1 + if load_blue_count > 2: + print("High load detected: [ID:002] Load:%.1f [ID:004] Load:%.1f" % (opterecenje2, opterecenje4)) + stop_motor(self.packetHandler, 2) + stop_motor(self.packetHandler, 4) + motor2.targret_reached = True + motor4.targret_reached = True + else: + load_blue_count = max(0, load_white_count - 1) + + if napon3 > opterecenje_threshold_beli or napon5 > opterecenje_threshold_beli: + load_white_count+=1 + if load_white_count > 2: + print("High load detected: [ID:003] Load:%.1f [ID:005] Load:%.1f" % (opterecenje3, opterecenje5)) + stop_motor(self.packetHandler, 3) + stop_motor(self.packetHandler, 5) + motor3.targret_reached = True + motor5.targret_reached = True + else: + load_white_count = max(0, load_white_count - 1) + + print("PredjeniPut ID2: %d deg" % (motor2.predjeni_put * 360 / 4096)) + print("PredjeniPut ID4: %d deg" % (motor4.predjeni_put * 360 / 4096)) + print("PredjeniPut ID3: %d deg" % (motor3.predjeni_put * 360 / 4096)) + print("PredjeniPut ID5: %d deg" % (motor5.predjeni_put * 360 / 4096)) + + targets_reached = ( + motor2.targret_reached + and motor3.targret_reached + and motor4.targret_reached + and motor5.targret_reached + ) + if targets_reached: + print("Svi motori su stigli na cilj!") + break + + return smer, motor2.predjeni_put, motor3.predjeni_put, motor4.predjeni_put, motor5.predjeni_put + except KeyboardInterrupt: + print("\nCtrl+C detected, stopping all motors.") + self._stop_all() + finally: + self._close_port() 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..6f03521 --- /dev/null +++ b/toid_lidar/launch/launch.py @@ -0,0 +1,67 @@ +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..47532d1 --- /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, tf2::TimePointZero); + } 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..2d33e00 100644 --- a/toid_msgs/CMakeLists.txt +++ b/toid_msgs/CMakeLists.txt @@ -9,8 +9,18 @@ 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" + "action/EmptyAction.action" + DEPENDENCIES geometry_msgs ) ament_package() \ No newline at end of file diff --git a/toid_msgs/action/EmptyAction.action b/toid_msgs/action/EmptyAction.action new file mode 100644 index 0000000..a49ba48 --- /dev/null +++ b/toid_msgs/action/EmptyAction.action @@ -0,0 +1,2 @@ +--- +--- \ No newline at end of file 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/main.py b/toid_navigation/launch/main.py new file mode 100644 index 0000000..e009910 --- /dev/null +++ b/toid_navigation/launch/main.py @@ -0,0 +1,179 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration, AllSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import os + +def generate_launch_description(): + nav_pkg_share = FindPackageShare("").find('toid_navigation') + control_pkg_share = FindPackageShare("").find('toid_control') + lidar_pkg_share = FindPackageShare("").find('toid_lidar') + vision_pkg_share = FindPackageShare("").find('toid_vision') + params = os.path.join(nav_pkg_share, 'params', 'toid_general_params.yaml') + map = os.path.join(nav_pkg_share, 'maps', 'mapb2_5cm.yaml') + ctrl_launch_dir = os.path.join(control_pkg_share, 'launch') + default_rviz_config_path = os.path.join(nav_pkg_share, 'rviz', 'nav2.rviz') + bt_path = os.path.join(nav_pkg_share, "behaviors", "navigate_to_pose_w_backtracking.xml") + lattice_path = os.path.join(nav_pkg_share, "params", "output.json") + + visualize = LaunchConfiguration("visualize") + visualize_arg = DeclareLaunchArgument( + 'visualize', + default_value='False', + description="Whether to launch rviz2" + ) + + use_mock = LaunchConfiguration("use_mock") + use_mock_arg = DeclareLaunchArgument( + 'use_mock', + default_value='True', + description="Whether to launch rviz2" + ) + + run_nodes = LaunchConfiguration("run_nodes") + run_nodes_arg = DeclareLaunchArgument( + 'run_nodes', + default_value='True', + description="Whether to launch rviz2" + ) + + use_lidar = LaunchConfiguration("use_lidar") + use_lidar_arg = DeclareLaunchArgument( + 'use_lidar', + default_value='True', + description="Whether to launch rviz2" + ) + + is_blue = LaunchConfiguration("is_blue") + is_blue_arg = DeclareLaunchArgument( + 'is_blue', + default_value='True', + description="Whether to launch rviz2" + ) + + toid_control = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ctrl_launch_dir, 'toid.launch.py') + ), + launch_arguments={ + 'visualize': 'False', + 'use_mock': use_mock + }.items(), + condition=IfCondition(run_nodes), + ) + + toid_lidar = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(lidar_pkg_share, 'launch' , 'launch.py') + ), + launch_arguments={ + 'visualize': 'False', + 'lidar_frame': 'lidar', + }.items(), + condition=IfCondition(AllSubstitution(run_nodes, use_lidar)), + ) + + toid_vision = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(vision_pkg_share, 'launch' , 'launch.py') + ), + launch_arguments={ + 'is_blue': is_blue, + }.items(), + condition=IfCondition(AllSubstitution(run_nodes, use_lidar)), + ) + + toid_interaction = Node( + package='toid_interaction', + executable='node', + name='toid_interaction', + output='screen', + condition=IfCondition(run_nodes), + ) + + map_server = Node( + package='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', + parameters=[{'yaml_filename': map}], + condition=IfCondition(run_nodes), + ) + + planner_server = Node( + package='nav2_planner', + executable='planner_server', + name='planner_server', + output='screen', + parameters=[{'GridBased':{'lattice_filepath': lattice_path}}, params], + condition=IfCondition(run_nodes), + ) + + controller_server = Node( + package='nav2_controller', + executable='controller_server', + name='controller_server', + output='screen', + parameters=[params], + condition=IfCondition(run_nodes), + ) + + bt_navigator = Node( + package='nav2_bt_navigator', + executable='bt_navigator', + name='bt_navigator', + output='screen', + parameters=[{'default_nav_to_pose_bt_xml': bt_path}, params], + condition=IfCondition(run_nodes), + ) + +# default_nav_to_pose_bt_xml: "$(find-pkg-share toid_navigation)/behaviors/navigate_to_pose_w_backtracking.xml" + + behavior_server = Node( + package='nav2_behaviors', + executable='behavior_server', + name='behavior_server', + output='screen', + parameters=[params], + condition=IfCondition(run_nodes), + ) + + lifecycle_manager_node = Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager', + output='screen', + parameters=[params], + condition=IfCondition(run_nodes), + ) + + rviz_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', default_rviz_config_path], + condition=IfCondition(visualize) + ) + + return LaunchDescription([ + visualize_arg, + use_mock_arg, + run_nodes_arg, + use_lidar_arg, + is_blue_arg, + toid_lidar, + toid_vision, + toid_interaction, + rviz_node, + map_server, + bt_navigator, + behavior_server, + planner_server, + controller_server, + lifecycle_manager_node, + toid_control + ]) \ No newline at end of file diff --git a/toid_navigation/launch/mock_control_launch.py b/toid_navigation/launch/mock_control_launch.py deleted file mode 100644 index 5d4ccfd..0000000 --- a/toid_navigation/launch/mock_control_launch.py +++ /dev/null @@ -1,93 +0,0 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.conditions import IfCondition, UnlessCondition -from launch.substitutions import Command, LaunchConfiguration -from launch_ros.actions import Node, LifecycleNode -from launch_ros.substitutions import FindPackageShare -import os - -def generate_launch_description(): - pkg_share = FindPackageShare("").find('toid_navigation') - params = os.path.join(pkg_share, 'params', 'toid_general_params.yaml') - map = os.path.join(pkg_share, 'maps', 'mapb2_5cm.yaml') - default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'ros_control.rviz') - - - description_pkg_share = FindPackageShare("").find('toid_bot_description') - default_model_path = os.path.join( - description_pkg_share, - 'src', - 'toid_bot_description.urdf' - ) - - visualize = LaunchConfiguration("visualize") - - visualize_arg = DeclareLaunchArgument( - 'visualize', - default_value='True', - description="Whether to launch rviz2" - ) - - odom_broadcast = Node( - package='tf2_ros', - executable='static_transform_publisher', - name='map_to_odom_broadcaster', - arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'], - condition=IfCondition(LaunchConfiguration('visualize')) - ) - - robot_state_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - name='robot_state_publisher', - output='screen', - parameters=[{'robot_description': Command(['xacro ', default_model_path])}] - ) - - controller_manager = Node( - package='controller_manager', - executable='ros2_control_node', - output='screen', - parameters=[params] - ) - - joint_state_broadcaster = Node( - package='controller_manager', - executable='spawner', - output='screen', - arguments=["joint_state_broadcaster"] - ) - - diffbot_base_controller = Node( - package='controller_manager', - executable='spawner', - output='both', - arguments=[ - "diffbot_base_controller", - "-p", - params, - "--controller-ros-args", - "-r diffbot_base_controller/cmd_vel:=/cmd_vel", - "--controller-ros-args", - "-r diffbot_base_controller/odom:=/odom" - ] - ) - - rviz_node = Node( - package='rviz2', - executable='rviz2', - name='rviz2', - output='screen', - arguments=['-d', default_rviz_config_path], - condition=IfCondition(visualize) - ) - - return LaunchDescription([ - visualize_arg, - odom_broadcast, - robot_state_publisher, - controller_manager, - joint_state_broadcaster, - diffbot_base_controller, - rviz_node - ]) \ No newline at end of file diff --git a/toid_navigation/launch/navigation_launch.py b/toid_navigation/launch/navigation_launch.py deleted file mode 100644 index 8193379..0000000 --- a/toid_navigation/launch/navigation_launch.py +++ /dev/null @@ -1,116 +0,0 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.conditions import IfCondition, UnlessCondition -from launch.substitutions import LaunchConfiguration -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare -import os - -def generate_launch_description(): - pkg_share = FindPackageShare("").find('toid_navigation') - params = os.path.join(pkg_share, 'params', 'toid_general_params.yaml') - map = os.path.join(pkg_share, 'maps', 'mapb2_5cm.yaml') - launch_dir = os.path.join(pkg_share, 'launch') - default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'nav2.rviz') - bt_path = os.path.join(pkg_share, "behaviors", "navigate_to_pose_w_backtracking.xml") - lattice_path = os.path.join(pkg_share, "params", "output.json") - - position = "0.756 0.225 0 0 0 0".split(' ') - #position = "0 0 0 0 0 0".split(' ') - - visualize = LaunchConfiguration("visualize") - visualize_arg = DeclareLaunchArgument( - 'visualize', - default_value='True', - description="Whether to launch rviz2" - ) - - toid_control = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_dir, 'mock_control_launch.py') - ), - launch_arguments={ - 'visualize': 'False' - }.items() - ) - - set_position = Node( - package='tf2_ros', - executable='static_transform_publisher', - name='map_to_odom_broadcaster', - arguments=position + ['map', 'odom'], - condition=IfCondition(visualize) - ) - - map_server = Node( - package='nav2_map_server', - executable='map_server', - name='map_server', - output='screen', - parameters=[{'yaml_filename': map}] - ) - - planner_server = Node( - package='nav2_planner', - executable='planner_server', - name='planner_server', - output='screen', - parameters=[{'GridBased':{'lattice_filepath': lattice_path}}, params] - ) - - controller_server = Node( - package='nav2_controller', - executable='controller_server', - name='controller_server', - output='screen', - parameters=[params] - ) - - bt_navigator = Node( - package='nav2_bt_navigator', - executable='bt_navigator', - name='bt_navigator', - output='screen', - parameters=[{'default_nav_to_pose_bt_xml': bt_path}, params] - ) - -# default_nav_to_pose_bt_xml: "$(find-pkg-share toid_navigation)/behaviors/navigate_to_pose_w_backtracking.xml" - - behavior_server = Node( - package='nav2_behaviors', - executable='behavior_server', - name='behavior_server', - output='screen', - parameters=[params] - ) - - lifecycle_manager_node = Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager', - output='screen', - parameters=[params] - ) - - rviz_node = Node( - package='rviz2', - executable='rviz2', - name='rviz2', - output='screen', - arguments=['-d', default_rviz_config_path], - condition=IfCondition(visualize) - ) - - return LaunchDescription([ - visualize_arg, - set_position, - rviz_node, - map_server, - bt_navigator, - behavior_server, - planner_server, - controller_server, - lifecycle_manager_node, - toid_control - ]) \ No newline at end of file diff --git a/toid_navigation/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..8c32a56 100644 --- a/toid_navigation/params/toid_general_params.yaml +++ b/toid_navigation/params/toid_general_params.yaml @@ -21,20 +21,29 @@ 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", "approachAcorns", "rotateAcorns"] 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: 3.0 + max_angular_decel: 1.0 + translateX: + plugin: "toid::SimpleTranslateXBehavior" + moveCoords: + plugin: "toid::MoveCoords" + max_vel_accel: 1.0 + approachAcorns: + plugin: "toid::ApproachAcorns" + kphi: 1.2 + kdelta: 2.0 + lambda: 3.0 + debug_marker: true + rotateAcorns: + plugin: "toid::RotateAcorns" local_frame: map global_frame: map robot_base_frame: base_footprint @@ -52,7 +61,7 @@ global_costmap: publish_frequency: 1.0 global_frame: map robot_base_frame: base_footprint - footprint: "[[-0.08, 0.15], [0.22, 0.15], [0.22, -0.15], [-0.08, -0.15]]" + footprint: "[[-0.045, 0.15], [0.255, 0.15], [0.255, -0.15], [-0.045, -0.15]]" footprint_padding: 0.02 track_unknown_space: false rolling_window: false @@ -73,7 +82,7 @@ local_costmap: ros__parameters: update_frequency: 5.0 publish_frequency: 2.0 - footprint: "[[-0.08, 0.15], [0.22, 0.15], [0.22, -0.15], [-0.08, -0.15]]" + footprint: "[[-0.045, 0.15], [0.255, 0.15], [0.255, -0.15], [-0.045, -0.15]]" footprint_padding: 0.01 #robot_radius: 0.18 global_frame: map @@ -83,11 +92,14 @@ local_costmap: height: 1 resolution: 0.01 introspection_mode: "disabled" - plugins: ["static_layer", "inflation_layer"] + plugins: ["static_layer", "rival_layer", "inflation_layer"] static_layer: plugin: "nav2_costmap_2d::StaticLayer" footprint_clearing_enabled: true map_subscribe_transient_local: True + rival_layer: + plugin: "toid::RivalLayer" + rival_size: 0.15 inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 @@ -188,38 +200,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 diff --git a/toid_navigation/rviz/nav2.rviz b/toid_navigation/rviz/nav2.rviz index 5eaf224..981ff64 100644 --- a/toid_navigation/rviz/nav2.rviz +++ b/toid_navigation/rviz/nav2.rviz @@ -1,13 +1,14 @@ Panels: - Class: rviz_common/Displays - Help Height: 78 + Help Height: 138 Name: Displays Property Tree Widget: Expanded: - /RobotModel1 - /GlobalCostMap1/Topic1 + - /Pose1 Splitter Ratio: 0.5 - Tree Height: 592 + Tree Height: 668 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -74,6 +75,10 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + camera: + Alpha: 1 + Show Axes: false + Show Trail: false drivewhl_l_link: Alpha: 1 Show Axes: false @@ -247,6 +252,39 @@ Visualization Manager: Reliability Policy: Reliable Value: /start_point Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Marker + Namespaces: + "": true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /marker + Value: true + - Alpha: 1 + Axes Length: 0.10000000149011612 + Axes Radius: 0.019999999552965164 + Class: rviz_default_plugins/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /closest_acorn + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -293,33 +331,33 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/XYOrbit - Distance: 3.863811731338501 + Distance: 2.2019217014312744 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.1778966784477234 - Y: -1.1747734546661377 - Z: 1.7285346984863281e-06 + X: 0.3222081661224365 + Y: -0.08033189922571182 + Z: 0 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.3697962760925293 + Pitch: 1.064796805381775 Target Frame: Value: XYOrbit (rviz_default_plugins) - Yaw: 4.628584861755371 + Yaw: 4.925206184387207 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 896 + Height: 1186 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000254000002defc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000002de000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002defc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f000002de000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005d30000003efc0100000002fb0000000800540069006d00650100000000000005d30000027b00fffffffb0000000800540069006d0065010000000000000450000000000000000000000264000002de00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000025400000398fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000070000003980000018600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015b00000398fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000070000003980000013800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000070c0000005efc0100000002fb0000000800540069006d006501000000000000070c0000048700fffffffb0000000800540069006d00650100000000000004500000000000000000000003450000039800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -328,6 +366,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1491 - X: 429 - Y: 75 + Width: 1804 + X: 428 + Y: 74 diff --git a/toid_odometry/src/odometry.cpp b/toid_odometry/src/odometry.cpp index b99d7d8..891109e 100644 --- a/toid_odometry/src/odometry.cpp +++ b/toid_odometry/src/odometry.cpp @@ -139,6 +139,23 @@ private: tf_static_broadcaster_->sendTransform(t); } + void serial_ec_recovery(boost::system::error_code ec) { + if(ec.failed()) { + boost::system::error_code ec; + ec = this->serial_.close(ec); + ec = this->serial_.open(serial_port_path_, ec); + if(!ec.failed()) { + try { + for(int i = 0; i < 64; i++) { + asio::write(this->serial_, asio::buffer("\x00")); + } + } catch(const std::runtime_error& err) { + + } + } + } + } + void set_width(const std::shared_ptr req) { boost::system::error_code ec; const std::array cmd{TMSG_SET_WIDTH, TMSG_DELIM}; @@ -146,6 +163,7 @@ private: msg.crc = crcFooter(msg); asio::write(this->serial_, asio::buffer(cmd), ec); asio::write(this->serial_, asio::buffer(&msg, sizeof(msg)), ec); + serial_ec_recovery(ec); } void set_ratio(const std::shared_ptr req) { @@ -155,6 +173,7 @@ private: msg.crc = crcFooter(msg); asio::write(this->serial_, asio::buffer(cmd), ec); asio::write(this->serial_, asio::buffer(&msg, sizeof(msg)), ec); + serial_ec_recovery(ec); } void end_calib(const std::shared_ptr) { @@ -166,12 +185,15 @@ private: if(!ec.failed()) { RCLCPP_INFO(this->get_logger(), "Calculated ratio: %lf, %lf", resp.left_gain, resp.right_gain); } + serial_ec_recovery(ec); + } void zero(const std::shared_ptr) { boost::system::error_code ec; const std::array cmd{TMSG_ZERO, TMSG_DELIM}; asio::write(this->serial_, asio::buffer(cmd), ec); + serial_ec_recovery(ec); } void publish_robot_state() { @@ -203,6 +225,7 @@ private: o.child_frame_id = t.child_frame_id; t.transform = make_transform(state.x, state.y, state.theta); + RCLCPP_DEBUG_THROTTLE(this->get_logger(), *this->get_clock(), 500, "Robot position: %lf %lf %lf", state.x, state.y, state.theta); const auto& [x,y,z] = t.transform.translation; o.pose.pose.position.x = x; @@ -244,6 +267,17 @@ private: ec.what().c_str() ); } + + if(!ec.failed()) { + try { + for(int i = 0; i < 64; i++) { + asio::write(serial_, asio::buffer("\x00")); + } + } catch(const std::runtime_error& err) { + + } + } + return false; } return state.crc == crcFooter(state); diff --git a/toid_vision/CMakeLists.txt b/toid_vision/CMakeLists.txt new file mode 100644 index 0000000..d09cf28 --- /dev/null +++ b/toid_vision/CMakeLists.txt @@ -0,0 +1,97 @@ + +cmake_minimum_required(VERSION 3.8) +project(toid_vision) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) + +set(PACKAGE_DEPS + rclcpp + sensor_msgs + message_filters + cv_bridge + OpenCV + rclcpp_components + tf2 + tf2_ros + tf2_geometry_msgs +) + +foreach(PACKAGE ${PACKAGE_DEPS}) + find_package(${PACKAGE} REQUIRED) +endforeach() + +find_package(OpenCV 4 REQUIRED + COMPONENTS + opencv_core + opencv_aruco + opencv_imgproc + opencv_imgcodecs +) + + +set(SOURCES + src/toid_vision.cpp + src/vision.cpp +) + +set(SOURCES_COMPOSABLE + src/toid_vision.cpp +) + +add_executable(toid_vision ${SOURCES}) + +target_include_directories( + toid_vision + PRIVATE + include +) + +ament_target_dependencies(toid_vision ${PACKAGE_DEPS}) + +install(TARGETS toid_vision DESTINATION lib/${PROJECT_NAME}) + +# TOID_VISION COMPOSABLE NODE + +add_library(toid_vision_component SHARED ${SOURCES_COMPOSABLE}) + +rclcpp_components_register_node( + toid_vision_component + PLUGIN "toid::NutDetector" + EXECUTABLE nut_detector +) + +target_include_directories( + toid_vision_component + PRIVATE + include +) + +ament_target_dependencies(toid_vision_component ${PACKAGE_DEPS}) + +ament_export_targets(export_toid_vision_component) +install(TARGETS toid_vision_component + EXPORT export_toid_vision_component + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME}/ +) + +target_compile_features( + toid_vision PUBLIC + c_std_99 + cxx_std_17 +) + + +ament_package() diff --git a/toid_vision/LICENSE b/toid_vision/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/toid_vision/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/toid_vision/config/camera_info.yaml b/toid_vision/config/camera_info.yaml new file mode 100644 index 0000000..bc031db --- /dev/null +++ b/toid_vision/config/camera_info.yaml @@ -0,0 +1,27 @@ +image_width: 1640 +image_height: 1232 +camera_name: imx219__base_axi_pcie_1000120000_rp1_i2c_88000_imx219_10_1640x1232 +camera_matrix: + rows: 3 + cols: 3 + data: [1281.9678810697528, 0.0, 792.9357401565763, + 0.0, 1283.1760537785433, 615.1915708814056, + 0.0, 0.0, 1.0] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [0.19247275241900028, -0.5098314224324741, 6.710711247494001e-05, + -0.00018449989636263572, 0.40356075592228546] +rectification_matrix: + rows: 3 + cols: 3 + data: [1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0] +projection_matrix: + rows: 3 + cols: 4 + data: [1281.9678810697528, 0.0, 792.9357401565763, 0.0, + 0.0, 1283.1760537785433, 615.1915708814056, 0.0, + 0.0, 0.0, 1.0, 0.0] \ No newline at end of file diff --git a/toid_vision/include/toid_vision/toid_vision.hpp b/toid_vision/include/toid_vision/toid_vision.hpp new file mode 100644 index 0000000..e1232a9 --- /dev/null +++ b/toid_vision/include/toid_vision/toid_vision.hpp @@ -0,0 +1,71 @@ +#pragma once + +#include +#include + +#include "message_filters/subscriber.hpp" +#include "message_filters/sync_policies/approximate_time.hpp" +#include "message_filters/time_synchronizer.hpp" +#include "opencv2/aruco.hpp" +#include "opencv2/objdetect.hpp" +#include "opencv2/opencv.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/camera_info.hpp" +#include "sensor_msgs/msg/compressed_image.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "std_msgs/msg/string.hpp" +#include "tf2/LinearMath/Quaternion.hpp" +#include "tf2/LinearMath/Transform.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/buffer.hpp" + +namespace toid +{ + +class NutDetector : public rclcpp::Node +{ + using SyncPolicy = message_filters::sync_policies::ApproximateTime< + sensor_msgs::msg::CompressedImage, sensor_msgs::msg::CameraInfo>; + +public: + NutDetector(const rclcpp::NodeOptions & options); + +public: + void compressed_topic_callback( + const sensor_msgs::msg::CompressedImage::ConstSharedPtr & msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & cam_info); + + void process_image( + cv::Mat & decodedImage, const std_msgs::msg::Header & header, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & cam_info); + + void extract_markers(cv::Mat & image); + + void get_pose(cv::Vec3d & tvec, cv::Vec3d & rvec, geometry_msgs::msg::Pose & out_pose); + + message_filters::Subscriber compressed_img_sub_; + message_filters::Subscriber cam_info_sub_; + std::shared_ptr> sync_; + + rclcpp::Publisher::SharedPtr pose_pub_; + rclcpp::Publisher::SharedPtr flip_pub_; + + std::vector> markerCorners_; + std::vector markerIds_; + std::vector, int>> markers_; + + std::string camera_frame_id_ = "camera"; + bool is_blue_; + + cv::Ptr detectorParams_; + cv::Ptr dictionary_; + + message_filters::Subscriber img_sub_; + + tf2_ros::Buffer::SharedPtr tf_; + std::shared_ptr tf_listener_; +}; + +} // namespace toid \ No newline at end of file diff --git a/toid_vision/launch/launch.py b/toid_vision/launch/launch.py new file mode 100644 index 0000000..ee848a1 --- /dev/null +++ b/toid_vision/launch/launch.py @@ -0,0 +1,55 @@ +import launch +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare +import os + +def generate_launch_description(): + """Generate launch description with multiple components.""" + + vision_share = FindPackageShare("").find("toid_vision") + camera_info = os.path.join(vision_share, 'config/camera_info.yaml') + + is_blue_arg = DeclareLaunchArgument( + name= "is_blue", + default_value = "True" + ) + + is_blue = LaunchConfiguration("is_blue") + + container = ComposableNodeContainer( + name='vision_container', + namespace='', + package='rclcpp_components', + executable='component_container_mt', + composable_node_descriptions=[ + ComposableNode( + package='toid_vision', + plugin='toid::NutDetector', + name='nut_detector', + parameters= [{ + 'is_blue': is_blue, + }]), + ComposableNode( + package='camera_ros', + plugin='camera::CameraNode', + name='camera', + parameters= [{ + 'width': 1640, + 'height': 1242, + 'orientation': 180, + 'camera': 0, + 'frame_id': 'camera', + 'camera_info_url': "file://" + camera_info + }], + ) + ], + output='screen', + ) + + return launch.LaunchDescription([ + is_blue_arg, + container + ]) \ No newline at end of file diff --git a/toid_vision/package.xml b/toid_vision/package.xml new file mode 100644 index 0000000..4250143 --- /dev/null +++ b/toid_vision/package.xml @@ -0,0 +1,26 @@ + + + + toid_vision + 0.0.0 + TODO: Package description + pimpest + Apache-2.0 + + ament_cmake + + rclcpp + sensor_msgs + message_filters + cv_bridge + libopencv-dev + rclcpp_components + + tf2 + tf2_ros + tf2_geometry_msgs + + + ament_cmake + + diff --git a/toid_vision/src/toid_vision.cpp b/toid_vision/src/toid_vision.cpp new file mode 100644 index 0000000..369df71 --- /dev/null +++ b/toid_vision/src/toid_vision.cpp @@ -0,0 +1,203 @@ +#include "toid_vision/toid_vision.hpp" + +#include +#include +#include "tf2/utils.hpp" + +namespace toid +{ + +NutDetector::NutDetector(const rclcpp::NodeOptions & options) +: Node("compressed_image_subscriber", options) +{ + detectorParams_ = cv::aruco::DetectorParameters::create(); + dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_100); + + tf_ = std::make_shared(this->get_clock()); + tf_listener_ = std::make_shared(*tf_); + + bool use_compressed = true; + this->declare_parameter("use_compressed", rclcpp::ParameterValue(true)); + this->get_parameter("use_compressed", use_compressed); + + this->declare_parameter("camera_frame_id", rclcpp::ParameterValue("camera")); + this->get_parameter("camera_frame_id", camera_frame_id_); + + this->declare_parameter("is_blue", rclcpp::ParameterValue(true)); + this->get_parameter("is_blue", is_blue_); + + compressed_img_sub_.subscribe(this, "/camera/image_raw/compressed"); + cam_info_sub_.subscribe(this, "/camera/camera_info"); + + sync_ = std::make_shared>( + SyncPolicy(2), compressed_img_sub_, cam_info_sub_); + + using namespace std::placeholders; + sync_->registerCallback(std::bind(&NutDetector::compressed_topic_callback, this, _1, _2)); + RCLCPP_INFO(this->get_logger(), "NutDetector activated... scanning for DEEZ NUTZ"); + + pose_pub_ = + this->create_publisher("/closest_acorn", rclcpp::QoS(1).keep_last(1)); + flip_pub_ = this->create_publisher("/to_flip", rclcpp::QoS(1)); +} + +void NutDetector::compressed_topic_callback( + const sensor_msgs::msg::CompressedImage::ConstSharedPtr & msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & cam_info) +{ + try { + cv::Mat rawData(1, msg->data.size(), CV_8UC1, (void *)msg->data.data()); + cv::Mat decodedImage = cv::imdecode(rawData, cv::IMREAD_COLOR); + + if (decodedImage.empty()) { + RCLCPP_ERROR(this->get_logger(), "Could not decode image!"); + return; + } + + process_image(decodedImage, msg->header, cam_info); + + } catch (const std::exception & e) { + RCLCPP_ERROR(this->get_logger(), "Error processing image: %s", e.what()); + } +} + +void NutDetector::process_image( + cv::Mat & decodedImage, const std_msgs::msg::Header & header, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & cam_info) +{ + markers_.clear(); + + extract_markers(decodedImage); + + if (markers_.empty()) { + return; + } + + std::vector rvecs, tvecs; + + cv::Mat k = cv::Mat(3, 3, CV_64F, (void *)cam_info->k.data()); + cv::aruco::estimatePoseSingleMarkers(markerCorners_, 0.03, k, cam_info->d, rvecs, tvecs); + + size_t prev = 0; + std::string colors = ""; + size_t first_ok = -1; + + for (size_t i = 0; i < markerIds_.size(); i++) { + const int idx = markers_[i].second; + if (markerIds_[idx] != 47 && markerIds_[idx] != 36) { + continue; + } + if (first_ok == (size_t)-1) { + first_ok = idx; + } + + RCLCPP_DEBUG( + this->get_logger(), "Marker %ld at %lf %lf", i, markerCorners_[idx][0].x, + markerCorners_[idx][0].y); + + RCLCPP_DEBUG( + this->get_logger(), "World coords: %lf %lf %lf", tvecs[idx][0], tvecs[idx][1], tvecs[idx][2]); + + float dx = tvecs[idx][0] - tvecs[prev][0]; + float dy = tvecs[idx][1] - tvecs[prev][1]; + float dz = tvecs[idx][2] - tvecs[prev][2]; + + float dist = std::sqrt(dx * dx + dy * dy + dz * dz); + + RCLCPP_DEBUG(this->get_logger(), "Dist from prev: %lf", dist); + + if (dist > 0.07) { + continue; + } + + prev = idx; + + colors += (markerIds_[idx] == 36) ? "b" : "y"; + + if (colors.size() > 3) { + break; + } + } + + if (colors.size() == 4) { + RCLCPP_DEBUG(this->get_logger(), "Last seen orientation: %s", colors.c_str()); + for (int i = 0; i < 4; i++) { + if (colors[i] == 'b') { + colors[i] = (is_blue_) ? '0' : '1'; + } else { + colors[i] = (is_blue_) ? '1' : '0'; + } + } + + RCLCPP_DEBUG(this->get_logger(), "Last seen orientation: %s", colors.c_str()); + std_msgs::msg::String m; + m.data = colors; + flip_pub_->publish(m); + } + + if (first_ok == (size_t)-1) { + return; + } + + geometry_msgs::msg::PoseStamped pose; + get_pose(tvecs[markers_[0].second], rvecs[markers_[0].second], pose.pose); + pose.header.frame_id = camera_frame_id_; + pose.header.stamp = header.stamp; + pose_pub_->publish(pose); + + try { + geometry_msgs::msg::PoseStamped out_pose; + out_pose = tf_->transform(pose, "base_footprint"); + RCLCPP_DEBUG_THROTTLE( + this->get_logger(), *this->get_clock(), 2000, "Closest guy is at: %lf %lf %lf", + out_pose.pose.position.x, out_pose.pose.position.y, tf2::getYaw(out_pose.pose.orientation) + ); + } catch (const tf2::TransformException & e) { + RCLCPP_ERROR_THROTTLE( + this->get_logger(), *this->get_clock(), 2000, "Failed to transfrom point"); + } +} + +void NutDetector::extract_markers(cv::Mat & image) +{ + cv::aruco::detectMarkers(image, dictionary_, markerCorners_, markerIds_, detectorParams_); + + // cv::aruco::drawDetectedMarkers(image, markerCorners_, markerIds_); + + markers_.reserve(markerIds_.size()); + markers_.resize(markerIds_.size()); + + for (size_t i = 0; i < markerIds_.size(); i++) { + markers_[i].first = markerCorners_[i]; + markers_[i].second = i; + } + + std::sort( + markers_.begin(), markers_.end(), + [](std::pair, int> & a, std::pair, int> & b) { + return a.first[0].y > b.first[0].y; + }); +} + +void NutDetector::get_pose(cv::Vec3d & tvec, cv::Vec3d & rvec, geometry_msgs::msg::Pose & out_pose) +{ + cv::Mat R; + cv::Rodrigues(rvec, R); + tf2::Matrix3x3 tf2_rot{R.at(0, 0), R.at(0, 1), R.at(0, 2), + R.at(1, 0), R.at(1, 1), R.at(1, 2), + R.at(2, 0), R.at(2, 1), R.at(2, 2)}; + + tf2::Quaternion q; + tf2_rot.getRotation(q); + + tf2::convert(q, out_pose.orientation); + + out_pose.position.x = tvec[0]; + out_pose.position.y = tvec[1]; + out_pose.position.z = tvec[2]; +} + +} // namespace toid + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(toid::NutDetector) \ No newline at end of file diff --git a/toid_vision/src/vision.cpp b/toid_vision/src/vision.cpp new file mode 100644 index 0000000..fe41f4f --- /dev/null +++ b/toid_vision/src/vision.cpp @@ -0,0 +1,10 @@ +#include "toid_vision/toid_vision.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + rclcpp::spin(std::make_shared(options)); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file