Pimpest/toid
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

wip-behaviors #3

Merged
Pimpest merged 86 commits from wip-behaviors into main 2026-05-28 06:21:26 +00:00
Conversation 0 Commits 86 Files Changed 129 +9175 -396
129 changed files with 9175 additions and 396 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
.clang-format Normal file
View File

@@ -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

1
.gitignore vendored
View File

@@ -1,3 +1,4 @@
__pycache__
.cache
build
install

6
.gitmodules vendored Normal file
View File

@@ -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

38
Dockerfile
View File

@@ -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
@@ -22,21 +40,29 @@ 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 <<EOF >> /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

22
docker-compose.yaml
View File

@@ -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
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"]

1
ext/BehaviorTree.ROS2 Submodule

Submodule ext/BehaviorTree.ROS2 added at 6c6aa078ee

1
ext/camera_ros Submodule

Submodule ext/camera_ros added at 121c98a7fe

5
firmware/base/src/config.h
View File

@@ -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
//======================================================

4
firmware/base/src/main.c
View File

@@ -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() {

16
firmware/base/src/msgs.h
View File

@@ -1,5 +1,6 @@
#pragma once
#include <pico/stdio.h>
#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);
}

2
firmware/base/tusb/tusb_stdio_driver.c
View File

@@ -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;

54
scripts/idi.sh Executable file
View File

@@ -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

6
scripts/toid_base_build.sh Executable file
View File

@@ -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"

87
toid_behaviors/CMakeLists.txt Normal file
View File

@@ -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()

202
toid_behaviors/LICENSE Normal file
View File

@@ -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.

91
toid_behaviors/include/toid_behaviors/approach_acorns.hpp Normal file
View File

@@ -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<MoveAction>
{
public:
ApproachAcorns();
~ApproachAcorns();
void configureCB() override;
void activateCB() override;
void deactivateCB() override;
ResultStatus onStart(
const std::shared_ptr<const MoveAction::Goal> 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<PoseStamped>::SharedPtr acorn_pose_sub_;
rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::Marker>::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

72
toid_behaviors/include/toid_behaviors/move_coords.hpp Normal file
View File

@@ -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<MoveAction>
{
public:
MoveCoords();
~MoveCoords();
void configureCB() override;
ResultStatus onStart(
const std::shared_ptr<const MoveAction::Goal> 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

67
toid_behaviors/include/toid_behaviors/rolling_average.hpp Normal file
View File

@@ -0,0 +1,67 @@
#pragma once
#include <algorithm>
#include <vector>
#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<double, double, double>;
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<geometry_msgs::msg::Pose> 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

80
toid_behaviors/include/toid_behaviors/rotate_acorns.hpp Normal file
View File

@@ -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<RotateAction>
{
public:
RotateAcorns();
~RotateAcorns();
void configureCB() override;
void activateCB() override;
void deactivateCB() override;
ResultStatus onStart(
const std::shared_ptr<const RotateAction::Goal> 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<PoseStamped>::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

35
toid_behaviors/include/toid_behaviors/scl.hpp Normal file
View File

@@ -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

179
toid_behaviors/include/toid_behaviors/simple_move.hpp Normal file
View File

@@ -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 <typename ActionT>
class SimpleMove : public nav2_behaviors::TimedBehavior<ActionT>
{
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<const typename ActionT::Goal> 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<nav_msgs::msg::Odometry>(
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<ActionT>::activate();
rclcpp_lifecycle::LifecycleNode::SharedPtr node = this->node_.lock();
using namespace std::placeholders;
rivals_sub_ = node->create_subscription<Rival>(
"/dynamicObstacle", 1, std::bind(&SimpleMove<ActionT>::rival_cb, this, _1));
activateCB();
}
void deactivate() override
{
nav2_behaviors::TimedBehavior<ActionT>::deactivate();
rivals_sub_.reset();
deactivateCB();
}
nav2_behaviors::ResultStatus onRun(
const std::shared_ptr<const typename ActionT::Goal> 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<geometry_msgs::msg::TwistStamped>();
{
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<nav_msgs::msg::Odometry>::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<Rival>::SharedPtr rivals_sub_;
};
} // namespace toid

60
toid_behaviors/include/toid_behaviors/simple_rotate.hpp Normal file
View File

@@ -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<RotateAction>
{
public:
SimpleRotateBehavior();
~SimpleRotateBehavior();
void configureCB() override;
ResultStatus onStart(
const std::shared_ptr<const RotateAction::Goal> 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

53
toid_behaviors/include/toid_behaviors/simple_translate_x.hpp Normal file
View File

@@ -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<TranslateAction>
{
public:
SimpleTranslateXBehavior();
~SimpleTranslateXBehavior();
void configureCB() override;
ResultStatus onStart(
const std::shared_ptr<const TranslateAction::Goal> 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

31
toid_behaviors/package.xml Normal file
View File

@@ -0,0 +1,31 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>toid_behaviors</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="82343504+pimpest@users.noreply.github.com">Pimpest</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>angles</depend>
<depend>geometry_msgs</depend>
<depend>nav_msgs</depend>
<depend>nav2_costmap_2d</depend>
<depend>nav2_util</depend>
<depend>nav2_core</depend>
<depend>nav2_behaviors</depend>
<depend>pluginlib</depend>
<depend>tf2</depend>
<depend>tf2_geometry_msgs</depend>
<depend>tf2_ros</depend>
<depend>toid_msgs</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

311
toid_behaviors/src/approach_acorns.cpp Normal file
View File

@@ -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<MoveAction>() {}
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<visualization_msgs::msg::Marker>("marker", 1);
}
void ApproachAcorns::activateCB()
{
auto node = node_.lock();
using namespace std::placeholders;
acorn_pose_sub_ = node->create_subscription<PoseStamped>(
"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<const MoveAction::Goal> 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<int>(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);

204
toid_behaviors/src/move_coords.cpp Normal file
View File

@@ -0,0 +1,204 @@
#include "toid_behaviors/move_coords.hpp"
#include "angles/angles.h"
namespace toid
{
MoveCoords::MoveCoords() : SimpleMove<MoveAction>() {}
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<const MoveAction::Goal> 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<int>(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);

234
toid_behaviors/src/rotate_acorns.cpp Normal file
View File

@@ -0,0 +1,234 @@
#include "toid_behaviors/rotate_acorns.hpp"
#include <cmath>
#include "angles/angles.h"
#include "tf2/convert.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
namespace toid
{
RotateAcorns::RotateAcorns() : SimpleMove<RotateAction>() {}
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<PoseStamped>(
"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<const RotateAction::Goal> 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);

79
toid_behaviors/src/scl.cpp Normal file
View File

@@ -0,0 +1,79 @@
#include "toid_behaviors/scl.hpp"
#include <cmath>
#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

1
toid_behaviors/src/simple_move.cpp Normal file
View File

@@ -0,0 +1 @@
#include "toid_behaviors/simple_move.hpp"

173
toid_behaviors/src/simple_rotate.cpp Normal file
View File

@@ -0,0 +1,173 @@
#include "toid_behaviors/simple_rotate.hpp"
#include <cmath>
#include "angles/angles.h"
#include "tf2/convert.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
namespace toid
{
SimpleRotateBehavior::SimpleRotateBehavior() : SimpleMove<RotateAction>() {}
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<const RotateAction::Goal> 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);

102
toid_behaviors/src/simple_translate_x.cpp Normal file
View File

@@ -0,0 +1,102 @@
#include "toid_behaviors/simple_translate_x.hpp"
#include <cmath>
#include "angles/angles.h"
#include "tf2/convert.hpp"
namespace toid
{
SimpleTranslateXBehavior::SimpleTranslateXBehavior() : SimpleMove<TranslateAction>() {}
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<const TranslateAction::Goal> 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);

19
toid_behaviors/toid_behaviors.xml Normal file
View File

@@ -0,0 +1,19 @@
<class_libraries>
<library path="toid_behaviors">
<class type="toid::SimpleRotateBehavior" base_class_type="nav2_core::Behavior">
<description></description>
</class>
<class type="toid::SimpleTranslateXBehavior" base_class_type="nav2_core::Behavior">
<description></description>
</class>
<class type="toid::MoveCoords" base_class_type="nav2_core::Behavior">
<description></description>
</class>
<class type="toid::ApproachAcorns" base_class_type="nav2_core::Behavior">
<description></description>
</class>
<class type="toid::RotateAcorns" base_class_type="nav2_core::Behavior">
<description></description>
</class>
</library>
</class_libraries>

57
toid_bot_description/rviz/default.rviz
View File

@@ -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,9 +147,8 @@ Visualization Manager:
Show Axes: true
Show Names: true
Tree:
base_link:
base_footprint:
{}
base_link:
drivewhl_l_link:
{}
drivewhl_r_link:
@@ -147,8 +159,31 @@ Visualization Manager:
{}
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: <Fixed 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

3
toid_bot_description/src/toid_bot_control.xacro
View File

@@ -1,12 +1,13 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="toid_control" params="name:=toid_control use_mock_hardware:=^|true">
<xacro:macro name="toid_control" params="name:=toid_control use_mock_hardware:=^|true serial_port:=/dev/ttyACM0">
<ros2_control name="${name}" type="system">
<xacro:unless value="${use_mock_hardware}">
<hardware>
<plugin>toid_control/StepperInterface</plugin>
<param name="device_path">${serial_port}</param>
</hardware>
</xacro:unless>
<xacro:if value="${use_mock_hardware}">

22
toid_bot_description/src/toid_bot_description.urdf
View File

@@ -2,6 +2,7 @@
<robot name="toid_bot" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:arg name="use_mock" default="true" />
<xacro:arg name="serial_port" default="/dev/ttyACM0" />
<xacro:property name="base_width" value="0.30"/>
<xacro:property name="base_length" value="0.30"/>
@@ -11,7 +12,7 @@
<xacro:property name="wheel_width" value="0.03"/>
<xacro:property name="wheel_zoff" value="0.03"/>
<xacro:property name="wheel_xoff" value="-0.07"/>
<xacro:property name="wheel_xoff" value="-0.105"/>
<xacro:property name="wheel_inset" value="0.01"/>
<xacro:property name="caster_radius" value="0.02"/>
@@ -22,7 +23,11 @@
<xacro:property name="lidar_radius" value="0.03"/>
<xacro:property name="lidar_height" value="0.02"/>
<xacro:property name="lidar_xoff" value="0.09"/>
<xacro:property name="lidar_xoff" value="0.00"/>
<xacro:property name="camera_xoff" value="0.07369"/>
<xacro:property name="camera_zoff" value="0.16664"/>
<xacro:property name="camera_pitch" value="${-105 * pi/180}"/>
<xacro:property name="base_zoff" value="${-wheel_zoff + wheel_radius + base_height/2}"/>
@@ -39,6 +44,9 @@
<link name="base_footprint"/>
<link name="camera" />
<link name="lidar">
<visual>
<geometry>
@@ -56,10 +64,16 @@
<origin xyz="${-wheel_xoff} 0.0 ${base_zoff}" rpy="0 0 0"/>
</joint>
<joint name="camera_base_joint" type="fixed">
<parent link="base_footprint"/>
<child link="camera"/>
<origin xyz="${camera_xoff} 0 ${camera_zoff}" rpy="${camera_pitch} 0 ${-pi/2}"/>
</joint>
<joint name="lidar_joint" type="fixed">
<parent link="base_link"/>
<child link="lidar"/>
<origin xyz="${lidar_xoff} 0 ${(base_height + lidar_height)/2}" rpy="0 0 0"/>
<origin xyz="${lidar_xoff} 0 ${(base_height + lidar_height)/2}" rpy="0 0 ${pi}"/>
</joint>
@@ -112,6 +126,6 @@
<xacro:cstr prefix="right" y_reflect="-1" />
<xacro:include filename="$(find toid_bot_description)/src/toid_bot_control.xacro"/>
<xacro:toid_control name="toid_bot_control" use_mock_hardware="$(arg use_mock)"/>
<xacro:toid_control name="toid_bot_control" use_mock_hardware="$(arg use_mock)" serial_port="$(arg serial_port)"/>
</robot>

62
toid_bt/CMakeLists.txt Normal file
View File

@@ -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()

202
toid_bt/LICENSE Normal file
View File

@@ -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.

367
toid_bt/behavior_trees/behavior1.xml Normal file
View File

@@ -0,0 +1,367 @@
<?xml version="1.0" encoding="UTF-8"?>
<root BTCPP_format="4">
<BehaviorTree ID="pickup_crates">
<Seq1 service_name=""/>
</BehaviorTree>
<BehaviorTree ID="seq">
<Sequence>
<InPose timeout="1.000000">
<Sleep msec="999999"/>
</InPose>
<Seq1 service_name=""/>
<Seq2 text="1010"
service_name=""/>
<Seq3 service_name=""/>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="seq1">
<Sequence>
<ZeroOdom service_name=""/>
<SetInitialPose y="0.805"
theta="-90"
frame_id="map"
x="1.325"
topic_name="__default__placeholder__"/>
<WaitPullPin action_name=""/>
<Timeout msec="100000">
<Sequence>
<InPose timeout="1.000000">
<MovePointSimple x="1.325"
y="0.200"
theta="-90"
max_speed="0.200000"
backwards="false"
action_name=""/>
</InPose>
<Seq1 service_name=""/>
<Seq2 text=""
service_name=""/>
<MovePointSimple x="1.325"
y="-0.05"
theta="-90"
max_speed="0.200000"
backwards="false"
action_name=""/>
<Seq3 service_name=""/>
<MovePointSimple x="1.325"
y="0.805"
theta="-90"
max_speed="0.200000"
backwards="true"
action_name=""/>
</Sequence>
</Timeout>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="seq2">
<Sequence>
<ZeroOdom service_name=""/>
<SetInitialPose y="0.955"
theta="-90"
frame_id="map"
x="1.330"
topic_name="__default__placeholder__"/>
<MovePointSimple x="1.330"
y="0.805"
theta="-90"
max_speed="0.300000"
backwards="false"
action_name=""/>
<WaitPullPin action_name=""/>
<Timeout msec="100000">
<Sequence>
<InPose timeout="1.000000">
<MovePointSimple x="1.33"
y="0.354"
theta="-90"
max_speed="0.300000"
backwards="false"
action_name=""/>
</InPose>
<Seq1 service_name=""/>
<Parallel failure_count="1"
success_count="-1">
<Seq2 text=""
service_name=""/>
<MovePointSimple x="0.795"
y="-0.2"
theta="180"
max_speed="0.500000"
backwards="false"
action_name=""/>
</Parallel>
<Parallel failure_count="1"
success_count="-1">
<Seq3 service_name=""/>
<Sequence>
<Sleep msec="500"/>
<TranslateX x="-0.3"
max_speed="0.000000"
action_name=""/>
<RotateTowards x="1.33"
y="0.354"
backwards="true"
max_speed="2.000000"
min_angle="0.000000"
action_name=""/>
</Sequence>
</Parallel>
<MovePointSimple x="1.330"
y="0.354"
theta="-90"
max_speed="0.400000"
backwards="true"
action_name=""/>
<MovePointSimple x="1.330"
y="-0.52"
theta="-90"
max_speed="0.000000"
backwards="false"
action_name=""/>
<Seq1 service_name=""/>
<ParallelAll max_failures="1">
<Seq2 text=""
service_name=""/>
<MovePointSimple x="1.33"
y="-0.2"
theta="-90"
max_speed="0.200000"
backwards="false"
action_name=""/>
</ParallelAll>
<Parallel failure_count="1"
success_count="-1">
<Seq3 service_name=""/>
<Sequence>
<Sleep msec="500"/>
<MovePointSimple x="1.33"
y="0.354"
theta="-90"
max_speed="0.400000"
backwards="true"
action_name=""/>
<MovePointSimple x="1.37"
y="0.48"
theta="-70"
max_speed="0.400000"
backwards="true"
action_name=""/>
</Sequence>
</Parallel>
<RotateTowards x="1.003"
y="-0.029"
backwards="false"
max_speed="1.300000"
min_angle="0.000000"
action_name=""/>
<MovePointSimple x="1.003"
y="-0.029"
theta="-90"
max_speed="0.400000"
backwards="false"
action_name=""/>
</Sequence>
</Timeout>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="seq2_yellow">
<Sequence>
<ZeroOdom service_name=""/>
<SetInitialPose y="0.806"
theta="-90"
frame_id="map"
x="-1.328"
topic_name="__default__placeholder__"/>
<WaitPullPin action_name=""/>
<Timeout msec="100000">
<Sequence>
<InPose timeout="1.000000">
<MovePointSimple x="-1.328"
y="0.354"
theta="-90"
max_speed="0.300000"
backwards="false"
action_name=""/>
</InPose>
<Seq1 service_name=""/>
<Parallel failure_count="1"
success_count="-1">
<Seq2 text=""
service_name=""/>
<MovePointSimple x="-0.795"
y="-0.2"
theta="0"
max_speed="0.500000"
backwards="false"
action_name=""/>
</Parallel>
<Parallel failure_count="1"
success_count="-1">
<Seq3 service_name=""/>
<Sequence>
<Sleep msec="500"/>
<TranslateX x="-0.3"
max_speed="0.000000"
action_name=""/>
<RotateTowards x="-1.328"
y="0.354"
backwards="true"
max_speed="2.000000"
min_angle="0.000000"
action_name=""/>
</Sequence>
</Parallel>
<MovePointSimple x="-1.328"
y="0.354"
theta="-90"
max_speed="0.400000"
backwards="true"
action_name=""/>
<Seq1 service_name=""/>
<ParallelAll max_failures="1">
<Seq2 text=""
service_name=""/>
<MovePointSimple x="-1.328"
y="-0.2"
theta="-90"
max_speed="0.200000"
backwards="false"
action_name=""/>
</ParallelAll>
<Parallel failure_count="1"
success_count="-1">
<Seq3 service_name=""/>
<Sequence>
<Sleep msec="500"/>
<MovePointSimple x="-1.328"
y="0.354"
theta="-90"
max_speed="0.400000"
backwards="true"
action_name=""/>
</Sequence>
</Parallel>
<MovePointSimple x="-1.328"
y="0.806"
theta="-90"
max_speed="0.400000"
backwards="true"
action_name=""/>
</Sequence>
</Timeout>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="test_1">
<Sequence>
<DetectStuck timeout="1.000000">
<RotateTowards x="0.4"
y="0.0"
backwards="false"
max_speed="0.000000"
min_angle="0.000000"
action_name=""/>
</DetectStuck>
</Sequence>
</BehaviorTree>
<!-- Description of Node Models (used by Groot) -->
<TreeNodesModel>
<Decorator ID="DetectStuck">
<input_port name="timeout"
default="1.000000"
type="double"/>
</Decorator>
<Decorator ID="InPose">
<input_port name="timeout"
default="1.000000"
type="double"/>
</Decorator>
<Action ID="MovePointSimple">
<input_port name="x"
type="double"/>
<input_port name="y"
type="double"/>
<input_port name="theta"
type="double"/>
<input_port name="max_speed"
default="0.000000"
type="double"/>
<input_port name="backwards"
default="false"
type="bool"/>
<input_port name="action_name"
type="std::string">Action server name</input_port>
</Action>
<Action ID="RotateTowards">
<input_port name="x"
type="double"/>
<input_port name="y"
type="double"/>
<input_port name="backwards"
default="false"
type="bool"/>
<input_port name="max_speed"
default="0.000000"
type="double"/>
<input_port name="min_angle"
default="0.000000"
type="double"/>
<input_port name="action_name"
type="std::string">Action server name</input_port>
</Action>
<Action ID="Seq1">
<input_port name="service_name"
type="std::string">Service name</input_port>
</Action>
<Action ID="Seq2">
<input_port name="text"
type="std::string"/>
<input_port name="service_name"
type="std::string">Service name</input_port>
</Action>
<Action ID="Seq3">
<input_port name="service_name"
type="std::string">Service name</input_port>
</Action>
<Condition ID="SetInitialPose">
<input_port name="y"
default="0.000000"
type="double">Y position in meters</input_port>
<input_port name="theta"
default="0.000000"
type="double">Heading in degrees</input_port>
<input_port name="frame_id"
default="map"
type="std::string">Reference frame</input_port>
<input_port name="x"
default="0.000000"
type="double">X position in meters</input_port>
<input_port name="topic_name"
default="__default__placeholder__"
type="std::string">Topic name</input_port>
</Condition>
<Action ID="TranslateX">
<input_port name="x"
type="double"/>
<input_port name="max_speed"
default="0.000000"
type="double"/>
<input_port name="action_name"
type="std::string">Action server name</input_port>
</Action>
<Action ID="WaitPullPin">
<input_port name="action_name"
type="std::string">Action server name</input_port>
</Action>
<Action ID="ZeroOdom">
<input_port name="service_name"
type="std::string">Service name</input_port>
</Action>
</TreeNodesModel>
</root>

201
toid_bt/behavior_trees/calibration_routines.xml Normal file
View File

@@ -0,0 +1,201 @@
<?xml version="1.0" encoding="UTF-8"?>
<root BTCPP_format="4">
<BehaviorTree ID="track_calib">
<Sequence>
<SetBlackboard value="0.265"
output_key="width"/>
<WaitPullPin />
<Sleep msec="1000"/>
<ZeroOdom service_name=""/>
<RotateTowards x="0.4"
y="0.0"
max_speed="0.000000"
min_angle="0.000000"
action_name=""/>
<Sequence>
<Sleep msec="1000"/>
<MovePointSimple x="0.4"
y="0"
theta="0"
max_speed="0.10000"
backwards="false"
action_name=""/>
<Sleep msec="500"/>
<RotateSimple angle="0"
max_speed="0.500000"
min_angle="700"
action_name=""/>
</Sequence>
<ForceSuccess>
<DetectStuck timeout="1.000000">
<MovePointSimple x="-1.0"
y="0"
theta="0"
max_speed="0.070000"
backwards="true"
action_name=""/>
</DetectStuck>
</ForceSuccess>
<SetWidth width="{width}"
count="2"
new_width="{width}"
service_name=""/>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="wheel_ratio_calibration">
<Sequence>
<Sleep msec="5000"/>
<ZeroOdom service_name=""/>
<WaitPullPin />
<Sleep msec="1000"/>
<Repeat num_cycles="5">
<Sequence>
<MovePointSimple x="1.1"
y="0"
theta="0"
max_speed="0.30000"
backwards="false"
action_name=""/>
<Sleep msec="1000"/>
<RotateSimple angle="180"
max_speed="1.300000"
min_angle="10"
action_name=""/>
<Sleep msec="500"/>
<MovePointSimple x="0.35"
y="0"
theta="180"
max_speed="0.300000"
backwards="false"
action_name=""/>
<RotateSimple angle="0"
max_speed="1.300000"
min_angle="-10"
action_name=""/>
<Sleep msec="500"/>
<MovePointSimple x="1.1"
y="0"
theta="0"
max_speed="0.30000"
backwards="false"
action_name=""/>
<Sleep msec="500"/>
<RotateSimple angle="180"
max_speed="1.300000"
min_angle="-10"
action_name=""/>
<Sleep msec="500"/>
<MovePointSimple x="0.35"
y="0"
theta="180"
max_speed="0.300000"
backwards="false"
action_name=""/>
<RotateSimple angle="0"
max_speed="1.300000"
min_angle="10"
action_name=""/>
</Sequence>
</Repeat>
<ForceSuccess>
<DetectStuck timeout="1.000000">
<MovePointSimple x="-0.2"
y="0"
theta="0"
max_speed="0.05"
backwards="true"
action_name=""/>
</DetectStuck>
</ForceSuccess>
<Sleep msec="1000"/>
<EndCalib service_name=""/>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="wheel_size">
<Sequence>
<WaitPullPin />
<Sleep msec="1000"/>
<ZeroOdom service_name=""/>
<Sleep msec="1000"/>
<MovePointSimple x="1.200"
y="0"
theta="0"
max_speed="0.250000"
backwards="false"
action_name=""/>
</Sequence>
</BehaviorTree>
<!-- Description of Node Models (used by Groot) -->
<TreeNodesModel>
<Decorator ID="DetectStuck">
<input_port name="timeout"
default="1.000000"
type="double"/>
</Decorator>
<Action ID="EndCalib">
<input_port name="service_name"
type="std::string">Service name</input_port>
</Action>
<Action ID="MovePointSimple">
<input_port name="x"
type="double"/>
<input_port name="y"
type="double"/>
<input_port name="theta"
type="double"/>
<input_port name="max_speed"
default="0.000000"
type="double"/>
<input_port name="backwards"
default="false"
type="bool"/>
<input_port name="action_name"
type="std::string">Action server name</input_port>
</Action>
<Action ID="RotateSimple">
<input_port name="angle"
type="double"/>
<input_port name="max_speed"
default="0.000000"
type="double"/>
<input_port name="min_angle"
default="0.000000"
type="double"/>
<input_port name="action_name"
type="std::string">Action server name</input_port>
</Action>
<Action ID="RotateTowards">
<input_port name="x"
type="double"/>
<input_port name="y"
type="double"/>
<input_port name="max_speed"
default="0.000000"
type="double"/>
<input_port name="min_angle"
default="0.000000"
type="double"/>
<input_port name="action_name"
type="std::string">Action server name</input_port>
</Action>
<Action ID="SetWidth">
<input_port name="width"
type="double"/>
<input_port name="count"
default="1"
type="int"/>
<output_port name="new_width"
type="double"/>
<input_port name="service_name"
type="std::string">Service name</input_port>
</Action>
<Action ID="ZeroOdom">
<input_port name="service_name"
type="std::string">Service name</input_port>
</Action>
</TreeNodesModel>
</root>

92
toid_bt/behavior_trees/toid_behaviors.btproj Normal file
View File

@@ -0,0 +1,92 @@
<?xml version="1.0" encoding="UTF-8"?>
<root BTCPP_format="4" project_name="Project">
<include path="behavior1.xml"/>
<include path="calibration_routines.xml"/>
<!-- Description of Node Models (used by Groot) -->
<TreeNodesModel>
<Action ID="ApproachAcorns">
<input_port name="x" type="double"/>
<input_port name="y" type="double"/>
<input_port name="theta" type="double"/>
<input_port name="max_speed" default="0.000000" type="double"/>
<input_port name="backwards" default="false" type="bool"/>
<input_port name="action_name" type="std::string">Action server name</input_port>
</Action>
<Decorator ID="DetectStuck">
<input_port name="timeout" default="1.000000" type="double"/>
</Decorator>
<Action ID="EndCalib">
<input_port name="service_name" type="std::string">Service name</input_port>
</Action>
<Decorator ID="InPose">
<input_port name="timeout" default="1.000000" type="double"/>
</Decorator>
<Action ID="MovePointSimple">
<input_port name="x" type="double"/>
<input_port name="y" type="double"/>
<input_port name="theta" type="double"/>
<input_port name="max_speed" default="0.000000" type="double"/>
<input_port name="backwards" default="false" type="bool"/>
<input_port name="action_name" type="std::string">Action server name</input_port>
</Action>
<Action ID="RotateAcorns">
<input_port name="angle" type="double"/>
<input_port name="max_speed" default="0.000000" type="double"/>
<input_port name="min_angle" default="0.000000" type="double"/>
<input_port name="action_name" type="std::string">Action server name</input_port>
</Action>
<Action ID="RotateSimple">
<input_port name="angle" type="double"/>
<input_port name="max_speed" default="0.000000" type="double"/>
<input_port name="min_angle" default="0.000000" type="double"/>
<input_port name="action_name" type="std::string">Action server name</input_port>
</Action>
<Action ID="RotateTowards">
<input_port name="x" type="double"/>
<input_port name="y" type="double"/>
<input_port name="backwards" default="false" type="bool"/>
<input_port name="max_speed" default="0.000000" type="double"/>
<input_port name="min_angle" default="0.000000" type="double"/>
<input_port name="action_name" type="std::string">Action server name</input_port>
</Action>
<Action ID="Seq1">
<input_port name="service_name" type="std::string">Service name</input_port>
</Action>
<Action ID="Seq2">
<input_port name="text" type="std::string"/>
<input_port name="service_name" type="std::string">Service name</input_port>
</Action>
<Action ID="Seq3">
<input_port name="service_name" type="std::string">Service name</input_port>
</Action>
<Condition ID="SetInitialPose">
<input_port name="y" default="0.000000" type="double">Y position in meters</input_port>
<input_port name="theta" default="0.000000" type="double">Heading in degrees</input_port>
<input_port name="frame_id" default="map" type="std::string">Reference frame</input_port>
<input_port name="x" default="0.000000" type="double">X position in meters</input_port>
<input_port name="topic_name" default="__default__placeholder__" type="std::string">Topic name</input_port>
</Condition>
<Action ID="SetParameter">
<input_port name="parameter" type="std::string"/>
<input_port name="value" type="std::string"/>
<input_port name="node" type="std::string"/>
</Action>
<Action ID="SetWidth">
<input_port name="width" type="double"/>
<input_port name="count" default="1" type="int"/>
<output_port name="new_width" type="double"/>
<input_port name="service_name" type="std::string">Service name</input_port>
</Action>
<Action ID="TranslateX">
<input_port name="x" type="double"/>
<input_port name="max_speed" default="0.000000" type="double"/>
<input_port name="action_name" type="std::string">Action server name</input_port>
</Action>
<Action ID="WaitPullPin">
<input_port name="action_name" type="std::string">Action server name</input_port>
</Action>
<Action ID="ZeroOdom">
<input_port name="service_name" type="std::string">Service name</input_port>
</Action>
</TreeNodesModel>
</root>

47
toid_bt/include/toid_bt/bt_executor.hpp Normal file
View File

@@ -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<std::string> 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<BT::StdCoutLogger> logger_cout_;
tf2_ros::Buffer::SharedPtr tf_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr acorn_pose_sub_;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr on_rotate_sub_;
geometry_msgs::msg::PoseStamped acorn_pose_;
std::string to_flip_ = "0000";
std::string base_frame_;
std::string global_frame_;
};
}

9
toid_bt/include/toid_bt/plugin.hpp Normal file
View File

@@ -0,0 +1,9 @@
#pragma once
#include <functional>
#include "geometry_msgs/msg/pose_stamped.hpp"
namespace toid {
using PoseFunc = std::function<void(geometry_msgs::msg::PoseStamped &)>;
using FlipFunc = std::function<void(std::string &)>;
}

30
toid_bt/include/toid_bt/plugins/empty_srv_action.hpp Normal file
View File

@@ -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<std_srvs::srv::Empty>
{
public:
EmptySrvNode(
const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
: BT::RosServiceNode<std_srvs::srv::Empty>(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

59
toid_bt/include/toid_bt/plugins/end_calib_action.hpp Normal file
View File

@@ -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<toid_msgs::srv::SendDouble>
{
public:
EndCalibNode(
const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params,
PoseFunc pose)
: BT::RosServiceNode<toid_msgs::srv::SendDouble>(name, conf, params), get_pose(pose)
{
}
static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::InputPort<double>("width"),
BT::InputPort<int>("count", 1, {}),
BT::OutputPort<double>("new_width"),
//BT::InputPort<double>("options"),
});
}
bool setRequest(typename Request::SharedPtr & request) override
{
auto width = getInput<double>("width").value();
double count = getInput<int>("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

74
toid_bt/include/toid_bt/plugins/in_position_decorator.hpp Normal file
View File

@@ -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<double>("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<double>("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

77
toid_bt/include/toid_bt/plugins/move_coords_action.hpp Normal file
View File

@@ -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<toid_msgs::action::SimpleMoveCoords>
{
public:
MovePointNode(
const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params,
PoseFunc pose)
: BT::RosActionNode<toid_msgs::action::SimpleMoveCoords>(name, conf, params), get_pose(pose)
{
std::cout << "wtf? " << params.default_port_value << std::endl;
}
static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::InputPort<double>("x"),
BT::InputPort<double>("y"),
BT::InputPort<double>("theta"),
BT::InputPort<double>("max_speed", 0, {}),
BT::InputPort<bool>("backwards", false, {}),
//BT::InputPort<double>("options"),
});
}
bool setGoal(Goal & goal) override
{
auto x_goal = getInput<double>("x");
auto y_goal = getInput<double>("y");
auto theta = getInput<double>("theta");
auto max_speed = getInput<double>("max_speed").value();
auto backwards = getInput<bool>("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

48
toid_bt/include/toid_bt/plugins/pull_pin_action.hpp Normal file
View File

@@ -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<toid_msgs::action::EmptyAction>
{
public:
PullPinNode(
const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
: BT::RosActionNode<toid_msgs::action::EmptyAction>(name, conf, params)
{
}
static BT::PortsList providedPorts()
{
return providedBasicPorts({
//BT::InputPort<double>("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

63
toid_bt/include/toid_bt/plugins/rotate_action.hpp Normal file
View File

@@ -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<toid_msgs::action::SimpleRotate>
{
public:
RotateNode(
const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params,
PoseFunc pose)
: BT::RosActionNode<toid_msgs::action::SimpleRotate>(name, conf, params), get_pose(pose)
{
}
static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::InputPort<double>("angle", {}),
BT::InputPort<double>("min_angle", 0, {}),
BT::InputPort<double>("max_speed", 0, {}),
//BT::InputPort<double>("options"),
});
}
bool setGoal(Goal & goal) override
{
auto angle = getInput<double>("angle");
auto min_angle = getInput<double>("min_angle");
auto max_speed = getInput<double>("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

75
toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp Normal file
View File

@@ -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<toid_msgs::action::SimpleRotate>
{
public:
RotateTowardsNode(
const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params,
PoseFunc pose)
: BT::RosActionNode<toid_msgs::action::SimpleRotate>(name, conf, params), get_pose(pose)
{
}
static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::InputPort<double>("x", {}),
BT::InputPort<double>("y", {}),
BT::InputPort<bool>("backwards", false, {}),
BT::InputPort<double>("min_angle", 0, {}),
BT::InputPort<double>("max_speed", 0, {}),
//BT::InputPort<double>("options"),
});
}
bool setGoal(Goal & goal) override
{
auto x = getInput<double>("x").value();
auto y = getInput<double>("y").value();
auto min_angle = getInput<double>("min_angle").value();
auto max_speed = getInput<double>("max_speed").value();
auto backwards = getInput<bool>("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

48
toid_bt/include/toid_bt/plugins/send_text_action.hpp Normal file
View File

@@ -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<toid_msgs::srv::SendString>
{
public:
SendTextNode(
const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params, FlipFunc get_text)
: BT::RosServiceNode<toid_msgs::srv::SendString>(name, conf, params), get_text(get_text)
{
}
static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::InputPort<std::string>("text"),
});
}
bool setRequest(typename Request::SharedPtr &req) override {
std::string text = getInput<std::string>("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

60
toid_bt/include/toid_bt/plugins/set_initial_pose_action.hpp Normal file
View File

@@ -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<geometry_msgs::msg::PoseWithCovarianceStamped>
{
public:
SetInitialPoseNode(
const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
: BT::RosTopicPubNode<geometry_msgs::msg::PoseWithCovarianceStamped>(name, conf, params)
{
}
static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::InputPort<double>("x", 0.0, "X position in meters"),
BT::InputPort<double>("y", 0.0, "Y position in meters"),
BT::InputPort<double>("theta", 0.0, "Heading in degrees"),
BT::InputPort<std::string>("frame_id", "map", "Reference frame"),
});
}
bool setMessage(geometry_msgs::msg::PoseWithCovarianceStamped & msg) override
{
double x = getInput<double>("x").value_or(0.0);
double y = getInput<double>("y").value_or(0.0);
double theta_deg = getInput<double>("theta").value_or(0.0);
std::string frame_id = getInput<std::string>("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

50
toid_bt/include/toid_bt/plugins/set_parameter_action.hpp Normal file
View File

@@ -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<rcl_interfaces::srv::SetParameters>
{
public:
SetParameterNode(
const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
: BT::RosServiceNode<rcl_interfaces::srv::SetParameters>(name, conf, params)
{
}
static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::InputPort<std::string>("node"),
BT::InputPort<std::string>("parameter"),
BT::InputPort<std::string>("value")
});
}
bool setRequest(typename Request::SharedPtr & request) override {
std::string parameter = getInput<std::string>("parameter").value();
std::string value = getInput<std::string>("value").value();
std::string node = getInput<std::string>("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

69
toid_bt/include/toid_bt/plugins/stuck_detector_decorator.hpp Normal file
View File

@@ -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<double>("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<double>("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

68
toid_bt/include/toid_bt/plugins/translate_x_action.hpp Normal file
View File

@@ -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<toid_msgs::action::SimpleMoveCoords>
{
public:
TranslateXNode(
const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params,
PoseFunc pose)
: BT::RosActionNode<toid_msgs::action::SimpleMoveCoords>(name, conf, params), get_pose(pose)
{
}
static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::InputPort<double>("x"),
BT::InputPort<double>("max_speed", 0, {}),
//BT::InputPort<double>("options"),
});
}
bool setGoal(Goal & goal) override
{
auto x_goal = getInput<double>("x").value();
auto max_speed = getInput<double>("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

41
toid_bt/launch/bt.launch.py Normal file
View File

@@ -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,
])

31
toid_bt/package.xml Normal file
View File

@@ -0,0 +1,31 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>toid_bt</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="82343504+pimpest@users.noreply.github.com">pimpest</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>angles</depend>
<depend>behaviortree_ros2</depend>
<depend>btcpp_ros2_interfaces</depend>
<depend>nav2_util</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tf2_geometry_msgs</depend>
<depend>geometry_msgs</depend>
<depend>toid_msgs</depend>
<depend>std_msgs</depend>
<depend>std_srvs</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

9
toid_bt/params/bt_params.yaml Normal file
View File

@@ -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

169
toid_bt/src/bt_executor.cpp Normal file
View File

@@ -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<rclcpp::Node>("toid_bt", opts))
{
/*
executeRegistration();
std::cout << BT::writeTreeNodesModelXML(factory()) << std::endl;
*/
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node()->get_clock());
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*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<geometry_msgs::msg::PoseStamped>(
"/closest_acorn", rclcpp::QoS(1), std::bind(&TreeExecutor::acorn_pose_cb, this, _1));
on_rotate_sub_ = node()->create_subscription<std_msgs::msg::String>(
"/to_flip", rclcpp::QoS(1), std::bind(&TreeExecutor::to_flip_cb, this, _1));
}
void TreeExecutor::onTreeCreated(BT::Tree & tree)
{
logger_cout_ = std::make_shared<BT::StdCoutLogger>(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<toid::MovePointNode>(
"MovePointSimple", BT::RosNodeParams(nh, "/moveCoords"), get_pose);
factory.registerNodeType<toid::MovePointNode>(
"ApproachAcorns", BT::RosNodeParams(nh, "/approachAcorns"), get_pose);
factory.registerNodeType<toid::RotateNode>(
"RotateSimple", BT::RosNodeParams(nh, "/rotate"), get_pose);
factory.registerNodeType<toid::RotateNode>(
"RotateAcorns", BT::RosNodeParams(nh, "/rotateAcorns"), get_pose);
factory.registerNodeType<toid::RotateTowardsNode>(
"RotateTowards", BT::RosNodeParams(nh, "/rotate"), get_pose);
factory.registerNodeType<toid::TranslateXNode>(
"TranslateX", BT::RosNodeParams(nh, "/moveCoords"), get_pose);
factory.registerNodeType<toid::EndCalibNode>(
"SetWidth", BT::RosNodeParams(nh, "/set_width"), get_pose);
factory.registerNodeType<toid::PullPinNode>("WaitPullPin", BT::RosNodeParams(nh, "/start_plug"));
factory.registerNodeType<toid::SetParameterNode>(
"SetParameter", BT::RosNodeParams(nh, "/compressed_image_subscriber/set_parameters"));
factory.registerNodeType<toid::SetInitialPoseNode>(
"SetInitialPose", BT::RosNodeParams(nh, "/initialpose"));
factory.registerNodeType<toid::EmptySrvNode>("ZeroOdom", BT::RosNodeParams(nh, "/zero"));
factory.registerNodeType<toid::EmptySrvNode>("EndCalib", BT::RosNodeParams(nh, "/end_calib"));
BT::RosNodeParams service_params1(nh, "/sequence1");
service_params1.server_timeout = std::chrono::seconds(15);
factory.registerNodeType<toid::EmptySrvNode>("Seq1", service_params1);
BT::RosNodeParams service_params2(nh, "/sequence2");
service_params2.server_timeout = std::chrono::seconds(20);
factory.registerNodeType<toid::SendTextNode>("Seq2", service_params2, get_to_flip);
BT::RosNodeParams service_params3(nh, "/sequence3");
service_params3.server_timeout = std::chrono::seconds(15);
factory.registerNodeType<toid::EmptySrvNode>("Seq3", service_params3);
factory.registerNodeType<toid::StuckDetectorNode>("DetectStuck", get_pose);
factory.registerNodeType<toid::InPositionNode>("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<std::string> 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<toid::TreeExecutor>(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;
}

132
toid_control/launch/toid.launch.py
View File

@@ -6,93 +6,101 @@ 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,32 +111,35 @@ def generate_launch_description():
"--controller-ros-args",
"-r diffdrive_controller/odom:=/odom",
"--controller-ros-args",
IfElseSubstitution(use_mock,
IfElseSubstitution(
use_mock,
"--param odom_frame_id:=odom",
"--param odom_frame_id:=odom_expected"
"--param odom_frame_id:=odom_expected",
),
"--controller-ros-args",
IfElseSubstitution(use_mock,
IfElseSubstitution(
use_mock,
"--param enable_odom_tf:=true",
"--param enable_odom_tf:=false"
"--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([
return LaunchDescription(
[
visualize_arg,
use_mock_arg,
odom_broadcast,
@@ -137,5 +148,6 @@ def generate_launch_description():
joint_state_broadcaster,
diffbot_base_controller,
velocity_controller,
rviz_node
])
rviz_node,
]
)

9
toid_control/src/toid_control.cpp
View File

@@ -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;
}

83
toid_costmaps/CMakeLists.txt Normal file
View File

@@ -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()

202
toid_costmaps/LICENSE Normal file
View File

@@ -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.

46
toid_costmaps/elements/elements.json Normal file
View File

@@ -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
}
]
}

46
toid_costmaps/include/toid_costmaps/element_info.hpp Normal file
View File

@@ -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<GameElement>, boost::json::value const & jv)
{
auto const& obj = jv.as_object();
return GameElement{
boost::json::value_to<double>(obj.at("x")) - 1.5,
boost::json::value_to<double>(obj.at("y")) - 1.0,
boost::json::value_to<double>(obj.at("width")),
boost::json::value_to<double>(obj.at("height"))
};
}
} // namespace toid

58
toid_costmaps/include/toid_costmaps/game_elements_layer.hpp Normal file
View File

@@ -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<GameElement> game_elements_;
std::vector<GameElement> static_elements_;
std::vector<std::string> extra_elements_;
ActiveElements::SharedPtr active_elements_;
rclcpp::Subscription<ActiveElements>::SharedPtr active_elements_sub_;
};
} // namespace toid

55
toid_costmaps/include/toid_costmaps/rival_layer.hpp Normal file
View File

@@ -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<Rivals>::SharedPtr rival_sub_;
double rival_size_ = 0.15;
};
} // namespace toid

32
toid_costmaps/package.xml Normal file
View File

@@ -0,0 +1,32 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>toid_costmaps</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="82343504+pimpest@users.noreply.github.com">Pimpest</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>angles</depend>
<depend>ament_index_cpp</depend>
<depend>boost</depend>
<depend>geometry_msgs</depend>
<depend>nav_msgs</depend>
<depend>nav2_costmap_2d</depend>
<depend>nav2_util</depend>
<depend>nav2_core</depend>
<depend>pluginlib</depend>
<depend>tf2</depend>
<depend>tf2_geometry_msgs</depend>
<depend>tf2_ros</depend>
<depend>toid_msgs</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

138
toid_costmaps/src/game_elements_layer.cpp Normal file
View File

@@ -0,0 +1,138 @@
#include "toid_costmaps/game_elements_layer.hpp"
#include <fstream>
#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<std::string>{}));
node->get_parameter(name_ + ".extra_elements", extra_elements_);
initGameElements();
active_elements_ = std::make_unique<ActiveElements>();
active_elements_->active = std::vector<bool>(game_elements_.size(), true);
using namespace std::placeholders;
active_elements_sub_ = node->create_subscription<ActiveElements>(
"/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<std::vector<GameElement>>(jv.at("game_elements"));
for (const auto & t : extra_elements_) {
std::vector<GameElement> elements = boost::json::value_to<std::vector<GameElement>>(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);

108
toid_costmaps/src/rival_layer.cpp Normal file
View File

@@ -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<Rivals>(
"/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<int>(rival_size_ / res);
int min_i = std::max(0, static_cast<int>(mx) - cell_radius);
int max_i =
std::min(static_cast<int>(grid.getSizeInCellsX()) - 1, static_cast<int>(mx) + cell_radius);
int min_j = std::max(0, static_cast<int>(my) - cell_radius);
int max_j =
std::min(static_cast<int>(grid.getSizeInCellsY()) - 1, static_cast<int>(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<double>(i) - static_cast<double>(mx);
double dj = static_cast<double>(j) - static_cast<double>(my);
double distance_sq = di * di + dj * dj;
if (distance_sq <= static_cast<double>(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);

10
toid_costmaps/toid_costmaps.xml Normal file
View File

@@ -0,0 +1,10 @@
<class_libraries>
<library path="toid_costmaps">
<class type="toid::GameElementLayer" base_class_type="nav2_costmap_2d::Layer">
<description></description>
</class>
<class type="toid::RivalLayer" base_class_type="nav2_costmap_2d::Layer">
<description></description>
</class>
</library>
</class_libraries>

22
toid_interaction/package.xml Normal file
View File

@@ -0,0 +1,22 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>toid_interaction</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="82343504+pimpest@users.noreply.github.com">pimpest</maintainer>
<license>MIT</license>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<depend>python3-serial</depend>
<depend>python3-gpiozero</depend>
<depend>toid_msgs</depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>

0
toid_interaction/resource/toid_interaction Normal file
View File

4
toid_interaction/setup.cfg Normal file
View File

@@ -0,0 +1,4 @@
[develop]
script_dir=$base/lib/toid_interaction
[install]
install_scripts=$base/lib/toid_interaction

33
toid_interaction/setup.py Normal file
View File

@@ -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'
],
},
)

25
toid_interaction/test/test_copyright.py Normal file
View File

@@ -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'

25
toid_interaction/test/test_flake8.py Normal file
View File

@@ -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)

23
toid_interaction/test/test_pep257.py Normal file
View File

@@ -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'

0
toid_interaction/toid_interaction/__init__.py Normal file
View File

117
toid_interaction/toid_interaction/interaction_node.py Normal file
View File

@@ -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()

8
toid_interaction/toid_interaction/mechanism/STservo_sdk/__init__.py Normal file
View File

@@ -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 *

151
toid_interaction/toid_interaction/mechanism/STservo_sdk/group_sync_read.py Normal file
View File

@@ -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

73
toid_interaction/toid_interaction/mechanism/STservo_sdk/group_sync_write.py Normal file
View File

@@ -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))

115
toid_interaction/toid_interaction/mechanism/STservo_sdk/port_handler.py Normal file
View File

@@ -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

530
toid_interaction/toid_interaction/mechanism/STservo_sdk/protocol_packet_handler.py Normal file
View File

@@ -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<<b)):
return -(a & ~(1<<b))
else:
return a
def sts_toscs(self, a, b):
if (a<0):
return (-a | (1<<b))
else:
return a
def sts_makeword(self, a, b):
if self.sts_end==0:
return (a & 0xFF) | ((b & 0xFF) << 8)
else:
return (b & 0xFF) | ((a & 0xFF) << 8)
def sts_makedword(self, a, b):
return (a & 0xFFFF) | (b & 0xFFFF) << 16
def sts_loword(self, l):
return l & 0xFFFF
def sts_hiword(self, h):
return (h >> 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

104
toid_interaction/toid_interaction/mechanism/STservo_sdk/scscl.py Normal file
View File

@@ -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)

7
toid_interaction/toid_interaction/mechanism/STservo_sdk/setup.py Normal file
View File

@@ -0,0 +1,7 @@
from setuptools import setup, find_packages
setup(
name='STservo_sdk',
version='0.1',
packages=find_packages(),
)

144
toid_interaction/toid_interaction/mechanism/STservo_sdk/sts.py Normal file
View File

@@ -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)

25
toid_interaction/toid_interaction/mechanism/STservo_sdk/stservo_def.py Normal file
View File

@@ -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 #

0
toid_interaction/toid_interaction/mechanism/__init__.py Normal file
View File

83
toid_interaction/toid_interaction/mechanism/motori.py Normal file
View File

@@ -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

67
toid_interaction/toid_interaction/mechanism/sekvenca_2026.py Normal file
View File

@@ -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()

167
toid_interaction/toid_interaction/mechanism/zidovi.py Normal file
View File

@@ -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()

366
toid_interaction/toid_interaction/mechanism/zidovi_load.py Normal file
View File

@@ -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()

130
toid_interaction/toid_interaction/mechanism/zupcanik.py Normal file
View File

@@ -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()

60
toid_lidar/CMakeLists.txt Normal file
View File

@@ -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()

202
toid_lidar/LICENSE Normal file
View File

@@ -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.

47
toid_lidar/include/toid_lidar/toid_lidar_node.hpp Normal file
View File

@@ -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<LaserScan>::SharedPtr scan_sub_;
rclcpp::Publisher<Rival>::SharedPtr rival_pub_;
rclcpp::Publisher<Marker>::SharedPtr marker_pub_;
std::shared_ptr<tf2_ros::Buffer> tf_buf_;
std::shared_ptr<tf2_ros::TransformListener> 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

67
toid_lidar/launch/launch.py Normal file
View File

@@ -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),
),
]
)

32
toid_lidar/package.xml Normal file
View File

@@ -0,0 +1,32 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>toid_lidar</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="82343504+pimpest@users.noreply.github.com">pimpest</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>eigen</depend>
<depend>nav2_util</depend>
<depend>sensor_msgs</depend>
<depend>tf2</depend>
<depend>tf2_eigen</depend>
<depend>tf2_ros</depend>
<depend>tf2_geometry_msgs</depend>
<depend>toid_msgs</depend>
<depend>visualization_msgs</depend>
<depend>rplidar_ros</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

11
toid_lidar/params/lidar.yaml Normal file
View File

@@ -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

Some files were not shown because too many files have changed in this diff Show More