From 41ec14b2b9b4fcf92c371d2cb03bd21606b1c5cb Mon Sep 17 00:00:00 2001 From: Marcin Pilch Date: Tue, 20 Apr 2021 14:25:33 +0200 Subject: [PATCH] Initial commit - transfer v1.2.0 --- .gitignore | 3 + CMakeLists.txt | 293 +++++++++++++++++++++ LICENSE | 21 ++ README.md | 168 ++++++++++++ config/rtls_anchor_0.yaml | 14 + config/rtls_anchor_1.yaml | 14 + config/rtls_anchor_2.yaml | 14 + config/rtls_anchor_3.yaml | 14 + config/rtls_tracker.yaml | 14 + examples/example_follow_me_subscriber.cpp | 19 ++ include/rtls_configurator_ros.hpp | 34 +++ launch/configure_device.launch | 8 + launch/tracker_reader.launch | 8 + msg/FollowMeDriverConfig.msg | 4 + msg/FollowMeDriverRS485Config.msg | 3 + msg/FollowMeRemoteControlConfig.msg | 2 + msg/PolarPoint2D.msg | 3 + msg/RtlsAnchorData.msg | 3 + msg/RtlsTrackerFrame.msg | 3 + package.xml | 47 ++++ roswiki | 144 +++++++++++ src/follow_me_master_beacon.cpp | 242 ++++++++++++++++++ src/follow_me_remote_control.cpp | 128 ++++++++++ src/rtls_configurator_ros.cpp | 296 ++++++++++++++++++++++ src/rtls_device_config_node.cpp | 51 ++++ src/rtls_tracker_node.cpp | 121 +++++++++ 26 files changed, 1671 insertions(+) create mode 100644 .gitignore create mode 100644 CMakeLists.txt create mode 100644 LICENSE create mode 100644 README.md create mode 100644 config/rtls_anchor_0.yaml create mode 100644 config/rtls_anchor_1.yaml create mode 100644 config/rtls_anchor_2.yaml create mode 100644 config/rtls_anchor_3.yaml create mode 100644 config/rtls_tracker.yaml create mode 100644 examples/example_follow_me_subscriber.cpp create mode 100644 include/rtls_configurator_ros.hpp create mode 100644 launch/configure_device.launch create mode 100644 launch/tracker_reader.launch create mode 100644 msg/FollowMeDriverConfig.msg create mode 100644 msg/FollowMeDriverRS485Config.msg create mode 100644 msg/FollowMeRemoteControlConfig.msg create mode 100644 msg/PolarPoint2D.msg create mode 100644 msg/RtlsAnchorData.msg create mode 100644 msg/RtlsTrackerFrame.msg create mode 100644 package.xml create mode 100644 roswiki create mode 100644 src/follow_me_master_beacon.cpp create mode 100644 src/follow_me_remote_control.cpp create mode 100644 src/rtls_configurator_ros.cpp create mode 100644 src/rtls_device_config_node.cpp create mode 100644 src/rtls_tracker_node.cpp diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..1b550c7 --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +build +commit_msg + diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..7b9327a --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,293 @@ +cmake_minimum_required(VERSION 3.10.2) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -O -g") + +project(positioning_systems_ros) + +option(BUILD_EXAMPLES "Build short example binaries" ON) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages + +find_package(catkin REQUIRED COMPONENTS + roscpp + geometry_msgs + message_generation + tf2_ros +) +find_package(positioning_systems_api REQUIRED) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +add_message_files( + FILES + PolarPoint2D.msg + FollowMeDriverConfig.msg + FollowMeDriverRS485Config.msg + FollowMeRemoteControlConfig.msg + RtlsAnchorData.msg + RtlsTrackerFrame.msg +) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + geometry_msgs + std_msgs # Or other packages containing msgs +) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS roscpp message_runtime +) + +########### +## Build ## +########### + +include_directories(include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +add_executable(follow_me_master_beacon + src/follow_me_master_beacon.cpp +) + +add_executable(follow_me_remote_control + src/follow_me_remote_control.cpp +) + +add_executable(rtls_tracker_node + src/rtls_tracker_node.cpp +) + +add_executable(rtls_device_config_node + src/rtls_device_config_node.cpp + src/rtls_configurator_ros.cpp +) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +add_dependencies(follow_me_master_beacon + positioning_systems_ros_generate_messages_cpp + ${follow_me_master_beacon_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) + +add_dependencies(follow_me_remote_control + positioning_systems_ros_generate_messages_cpp + ${follow_me_remote_control_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) + +add_dependencies(rtls_tracker_node + positioning_systems_ros_generate_messages_cpp + ${rtls_tracker_node_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) + +add_dependencies(rtls_device_config_node + positioning_systems_ros_generate_messages_cpp + ${rtls_device_config_node_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) + +## Specify libraries to link a library or executable target against +target_link_libraries(follow_me_master_beacon + ${catkin_LIBRARIES} + positioning_systems_api::follow_me_driver +) + +target_link_libraries(follow_me_remote_control + ${catkin_LIBRARIES} + positioning_systems_api::follow_me_driver +) + +target_link_libraries(rtls_tracker_node + ${catkin_LIBRARIES} + positioning_systems_api::rtls_driver +) + +target_link_libraries(rtls_device_config_node + ${catkin_LIBRARIES} + positioning_systems_api::rtls_driver +) + +if(BUILD_EXAMPLES) + + add_executable(example_follow_me_subscriber + examples/example_follow_me_subscriber.cpp + ) + + add_dependencies(example_follow_me_subscriber + positioning_systems_ros_generate_messages_cpp + ${follow_me_remote_control_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} + ) + + target_link_libraries(example_follow_me_subscriber + ${catkin_LIBRARIES} + positioning_systems_api::follow_me_driver + ) + +endif() + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +install(TARGETS follow_me_master_beacon + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + PUBLIC_HEADER DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +install(TARGETS follow_me_remote_control + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + PUBLIC_HEADER DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +#catkin_add_gmock(${PROJECT_NAME}-test +# test/test_follow_me_driver.cpp +#) + +#if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test +# ${catkin_LIBRARIES} +# follow_me_driver +# ) +#endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..79f74f3 --- /dev/null +++ b/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2017 pl-kabaradjian + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/README.md b/README.md new file mode 100644 index 0000000..1e0b08e --- /dev/null +++ b/README.md @@ -0,0 +1,168 @@ +# ROS package for Terabee Follow-Me and Terabee Robot Positioning System + +This package is a ROS wrapper for `positioning_systems_api` contained in [positioning_systems_api](https://github.com/Terabee/positioning_systems_api) package. It enables easy use of Terabee Follow-Me system in ROS environment. + +## Supported hardware +This package works with Terabee Follow-Me and Terabee Robot Positioning System. + +## Dependencies + +This package depends on [positioning_systems_api](https://github.com/Terabee/positioning_systems_api) version 1.5.6 or newer. + +Please follow the instructions in positioning_systems_api README file to build and install. + +## Building the packages +Clone the repository into your workspace: + +* If you have ssh key setup for your github account: +``` +cd ~/ros_ws/src +git clone git@github.com:Terabee/positioning_systems_ros.git +``` +* If you prefer to use https use this set of commands: +``` +cd ~/ros_ws/src +git clone https://github.com/Terabee/positioning_systems_ros.git +``` +Navigate to your workspace and build: +``` +cd ~/ros_ws +catkin build +source devel/setup.bash +``` +## Nodes +### follow_me_master_beacon + +With this node you can receive data (distance *in meters* and heading *in degrees*) from the system. It is intended to use with master beacon connected to the computer. + +It provides following configuration options of the system: +* switching between text and binary printout modes, +* swapping beacons, +* setting *Exponential Moving Average* filter number of samples (window size), +* setting span between the beacons (in millimeters), +* settings parameters for RS485 connection (Modbus slave id, baudrate, parity). + +#### Running the node +After your workspace is built and sourced, you can run the node (set `_portname` to the actual port name where the master beacon is connected: +``` +rosrun positioning_systems_ros follow_me_master_beacon _portname:=/dev/ttyACM0 +``` +#### Subscribed topics +This node subscribes to the following topics: + +* `/follow_me_master_beacon/follow_me_autocalibrate` : publishing to this topic activates span autocalibration mode +* `/follow_me_master_beacon/follow_me_config` : publishing to this topic sets parameters: printout mode, swap beacons, EMA filter window, span between the beacons +* `/follow_me_master_beacon/follow_me_rs485_config` : publishing to this topic sets slave id, baudrate and parity of RS485 interface +* `/follow_me_master_beacon/follow_me_test_cmd` : publishing to this topic triggers test command which returns actual configuration of the device + +Example 1 of usage: +``` +rostopic pub /follow_me_master_beacon/follow_me_config positioning_systems_ros/FollowMeDriverConfig "printout_mode: 'Binary' +swap_beacons: true +ema_window: 10 +beacons_span: 540" +``` +Sets binary printout mode, swaps beacons, sets EMA filter to 10 samples and beacons span to 540 mm. + +Example 2 of usage: +``` +rostopic pub /follow_me_master_beacon/follow_me_rs485_config positioning_systems_ros/FollowMeDriverRS485Config "rs485_slave_id: 3 +rs485_baudrate: 19200 +rs485_parity: 2" +``` +Sets Modbus RTU slave id to 3, baud rate to 19200 and parity to 2 (Even). + +To see list of valid values for each parameter, open respective \*.msg file. + +#### Published topics +This node publishes to the topic: +* `/follow_me_master_beacon/follow_me_polar_point_2d` : provides distance and heading of the remote control with respect to the beacons + +### follow_me_remote_control + +This node is intended to use with remote control connected to the computer. + +It provides following configuration options of the remote control: +* setting button operation mode (hold, toggle) +* setting buzzer operation (enabled, disabled) + +#### Running the node +After your workspace is built and sourced, you can run the node (set `_portname` to the actual port name where the master beacon is connected: +``` +rosrun positioning_systems_ros follow_me_remote_control _portname:=/dev/ttyUSB0 +``` +#### Subscribed topics +This node subscribes to the following topics: +* `/follow_me_remote_control/follow_me_get_config` : publishing to this topic triggers command which returns actual configuration of the remote control +* `/follow_me_remote_control/follow_me_set_config` : publishing to this topic sets remote control configuration (button mode and buzzer state) + +Example of usage: +``` +rostopic pub /follow_me_remote_control/follow_me_set_config positioning_systems_ros/FollowMeRemoteControlConfig "button_mode: 'Hold' +buzzer_active: false" +``` +Sets button mode to *Hold* and deactivates buzzer. + +To see list of valid values for each parameter, open respective \*.msg file. + +### rtls_configurator_ros + +With this node and provided launch file `configure_device.launch` you can configure connected device using one of the provided YAML files, for example: +``` +roslaunch positioning_systems_ros configure_device.launch file:=config/rtls_anchor_0.yaml +``` +will configure the device as the initiator anchor with priority number set to 0. + +### rtls_tracker_node + +With this node you can read the tracker position output when the tracker device is connected. + +#### Published topics +This node publishes to the topic: + +`/rtls_tracker_node/rtls_tracker_frame` + +which provides a custom message: +``` +RtlsAnchorData[] anchors +geometry_msgs/Point position +bool is_valid_position +``` + +with list of anchors data as embedded message: +``` +uint16 id // anchor id +geometry_msgs/Point position // position of anchor +float64 distance // distance from anchor to tracker +``` +position of tracker: `geometry_msgs/Point position` +and validity of message: `bool is_valid_position` set to false if unable to parse tracker output or the device was unable to provide valid tracker position. + +#### Transform broadcast +The node broadcasts `tf2` transforms with positions of anchors and tracker, based on the data read from the tracker. + +#### Launch file parameters + +Provided launch file `tracker_reader.launch` has already predefined following parameters: + +- portname - name of serial port device, e.g. `/dev/ttyUSB0` +- publish_tf - whether transforms of anchors and tracker should be broadcasted +- ref_frame - reference frame name for the broadcasted transforms + +## Example + +A basic subscriber is available in subdirectory `examples`. + +In order to use the package in your own node, do the following: + +In `CMakeLists.txt` add the package name `positioning_systems_ros` to `find_package` and `catkin_package` commands. + +In `package.xml` add: + +``` +positioning_systems_ros +``` +and +``` +positioning_systems_ros +``` diff --git a/config/rtls_anchor_0.yaml b/config/rtls_anchor_0.yaml new file mode 100644 index 0000000..668b18e --- /dev/null +++ b/config/rtls_anchor_0.yaml @@ -0,0 +1,14 @@ +device_type: "Anchor" +is_initiator: True +priority: 0 +label: 0x0000 +network_id: 0x0000 +update_time_ms: 100 +is_auto_anchor_positioning: True +anchor_height: 0 +is_tracker_stream_mode: False +tracker_message_mode: "Long" +is_led_enabled: True +x: 0 +y: 0 +z: 0 diff --git a/config/rtls_anchor_1.yaml b/config/rtls_anchor_1.yaml new file mode 100644 index 0000000..fdb1b3e --- /dev/null +++ b/config/rtls_anchor_1.yaml @@ -0,0 +1,14 @@ +device_type: "Anchor" +is_initiator: False +priority: 1 +label: 0x0000 +network_id: 0x0000 +update_time_ms: 100 +is_auto_anchor_positioning: True +anchor_height: 0 +is_tracker_stream_mode: False +tracker_message_mode: "Long" +is_led_enabled: True +x: 0 +y: 0 +z: 0 diff --git a/config/rtls_anchor_2.yaml b/config/rtls_anchor_2.yaml new file mode 100644 index 0000000..fc85121 --- /dev/null +++ b/config/rtls_anchor_2.yaml @@ -0,0 +1,14 @@ +device_type: "Anchor" +is_initiator: False +priority: 2 +label: 0x0000 +network_id: 0x0000 +update_time_ms: 100 +is_auto_anchor_positioning: True +anchor_height: 0 +is_tracker_stream_mode: False +tracker_message_mode: "Long" +is_led_enabled: True +x: 0 +y: 0 +z: 0 diff --git a/config/rtls_anchor_3.yaml b/config/rtls_anchor_3.yaml new file mode 100644 index 0000000..1316ecd --- /dev/null +++ b/config/rtls_anchor_3.yaml @@ -0,0 +1,14 @@ +device_type: "Anchor" +is_initiator: False +priority: 3 +label: 0x0000 +network_id: 0x0000 +update_time_ms: 100 +is_auto_anchor_positioning: True +anchor_height: 0 +is_tracker_stream_mode: False +tracker_message_mode: "Long" +is_led_enabled: True +x: 0 +y: 0 +z: 0 diff --git a/config/rtls_tracker.yaml b/config/rtls_tracker.yaml new file mode 100644 index 0000000..b912198 --- /dev/null +++ b/config/rtls_tracker.yaml @@ -0,0 +1,14 @@ +device_type: "Tracker" # Tracker, Anchor +is_initiator: False +priority: 0 +label: 0x0000 +network_id: 0x0000 +update_time_ms: 100 +is_auto_anchor_positioning: False +anchor_height: 0 +is_tracker_stream_mode: True +tracker_message_mode: "Long" # Long, Short +is_led_enabled: True +x: 0 +y: 0 +z: 0 diff --git a/examples/example_follow_me_subscriber.cpp b/examples/example_follow_me_subscriber.cpp new file mode 100644 index 0000000..fcfed6e --- /dev/null +++ b/examples/example_follow_me_subscriber.cpp @@ -0,0 +1,19 @@ +#include +#include "positioning_systems_ros/PolarPoint2D.h" + +void chatterCallback(const positioning_systems_ros::PolarPoint2DConstPtr& point) +{ + ROS_INFO_STREAM("Distance: " << point->distance << "\tHeading: " << point->heading); +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "example_follow_me_subscriber"); + ros::NodeHandle nh; + + ros::Subscriber sub = nh.subscribe("follow_me_master_beacon/follow_me_polar_point_2d", 1, chatterCallback); + + ros::spin(); + + return 0; +} diff --git a/include/rtls_configurator_ros.hpp b/include/rtls_configurator_ros.hpp new file mode 100644 index 0000000..4a48c3b --- /dev/null +++ b/include/rtls_configurator_ros.hpp @@ -0,0 +1,34 @@ +#ifndef RTLS_CONFIGURATOR_ROS_HPP +#define RTLS_CONFIGURATOR_ROS_HPP + +#include +#include "rtls_driver/rtls_driver.hpp" + +class RtlsConfiguratorROS +{ +public: + explicit RtlsConfiguratorROS(std::shared_ptr nh, + std::shared_ptr rtls_device); + ~RtlsConfiguratorROS() = default; + RtlsConfiguratorROS() = delete; + RtlsConfiguratorROS(const RtlsConfiguratorROS&) = delete; + RtlsConfiguratorROS(RtlsConfiguratorROS &&) = delete; + + bool setDevice(); + bool setTrackerMessage(); + bool setLabel(); + bool setNetworkId(); + bool setUpdateTime(); + bool setAnchorPosition(); + bool setAnchorHeightForAutoPositioning(); + bool setAnchorInitiator(); + bool setAutoAnchorPositioning(); + bool setLED(); + bool setTrackerStream(); + +private: + std::shared_ptr rtls_device_; + std::shared_ptr nh_; +}; + +#endif // RTLS_CONFIGURATOR_ROS_HPP diff --git a/launch/configure_device.launch b/launch/configure_device.launch new file mode 100644 index 0000000..2feb8ce --- /dev/null +++ b/launch/configure_device.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/launch/tracker_reader.launch b/launch/tracker_reader.launch new file mode 100644 index 0000000..8e38d12 --- /dev/null +++ b/launch/tracker_reader.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/msg/FollowMeDriverConfig.msg b/msg/FollowMeDriverConfig.msg new file mode 100644 index 0000000..8d59b5e --- /dev/null +++ b/msg/FollowMeDriverConfig.msg @@ -0,0 +1,4 @@ +string printout_mode # Text, Binary +bool swap_beacons # true, false +uint8 ema_window # 0-255 +uint16 beacons_span # 100-3000 diff --git a/msg/FollowMeDriverRS485Config.msg b/msg/FollowMeDriverRS485Config.msg new file mode 100644 index 0000000..7ac0fb1 --- /dev/null +++ b/msg/FollowMeDriverRS485Config.msg @@ -0,0 +1,3 @@ +uint8 rs485_slave_id # 1-247 +uint32 rs485_baudrate # 1200, 2400, 4800, 9600, 14400, 19200, 28800, 38400, 56000, 57600, 115200, 128000, 256000 +uint8 rs485_parity # 0 = None, 1 = Odd, 2 = Even diff --git a/msg/FollowMeRemoteControlConfig.msg b/msg/FollowMeRemoteControlConfig.msg new file mode 100644 index 0000000..299d817 --- /dev/null +++ b/msg/FollowMeRemoteControlConfig.msg @@ -0,0 +1,2 @@ +string button_mode # Hold, Toggle +bool buzzer_active # true, false diff --git a/msg/PolarPoint2D.msg b/msg/PolarPoint2D.msg new file mode 100644 index 0000000..50d12fb --- /dev/null +++ b/msg/PolarPoint2D.msg @@ -0,0 +1,3 @@ +Header header +float64 distance +float64 heading diff --git a/msg/RtlsAnchorData.msg b/msg/RtlsAnchorData.msg new file mode 100644 index 0000000..ea6a135 --- /dev/null +++ b/msg/RtlsAnchorData.msg @@ -0,0 +1,3 @@ +uint16 id +geometry_msgs/Point position +float64 distance diff --git a/msg/RtlsTrackerFrame.msg b/msg/RtlsTrackerFrame.msg new file mode 100644 index 0000000..72ede9e --- /dev/null +++ b/msg/RtlsTrackerFrame.msg @@ -0,0 +1,3 @@ +RtlsAnchorData[] anchors +geometry_msgs/Point position +bool is_valid_position diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..3b54cb0 --- /dev/null +++ b/package.xml @@ -0,0 +1,47 @@ + + + positioning_systems_ros + 1.2.0 + ROS wrapper for the Terabee Follow-Me and Robot Positioning System. + + Marcin Pilch + MIT + + + + + http://wiki.ros.org/follow_me_driver_ros + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + message_generation + geometry_msgs + positioning_systems_api + message_runtime + + + + + + + diff --git a/roswiki b/roswiki new file mode 100644 index 0000000..8ce6c99 --- /dev/null +++ b/roswiki @@ -0,0 +1,144 @@ +## Repository: https://github.com/Terabee/positioning_systems_ros +<> <> + +Github: *https://github.com/Terabee/positioning_systems_ros + +This package is a ROS wrapper for `follow_me_driver` contained in [[https://github.com/Terabee/positioning_systems_api|positioning_systems_api]] package. It enables easy use of Terabee Follow-Me system in ROS environment. + +== Supported hardware == +This package works with Terabee Follow-Me system + +{{attachment:follow_me_picture.png}} + +== Dependencies == +This package depends on [[https://github.com/Terabee/positioning_systems_api|positioning_systems_api]]. Please clone the repository into you workspace: + * If you have ssh key setup for your github account: +{{{ +cd ~/ros_ws/src +git clone git@github.com:Terabee/positioning_systems_api.git +}}} + + * If you prefer to use https use this set of commands: +{{{ +cd ~/ros_ws/src +git clone https://github.com/Terabee/positioning_systems_api.git +}}} + +== Building the packages == +Clone the repository into your workspace: + * If you have ssh key setup for your github account: +{{{ +cd ~/ros_ws/src +git clone git@github.com:Terabee/positioning_systems_ros.git +}}} + + * If you prefer to use https use this set of commands: +{{{ +cd ~/ros_ws/src +git clone https://github.com/Terabee/positioning_systems_ros.git +}}} + +Navigate to your workspace and build: +{{{ +cd ~/ros_ws +catkin build +source devel/setup.bash +}}} + +== Nodes == +=== follow_me_master_beacon === +With this node you can receive data (distance ''in meters'' and heading ''in degrees'') from the system. It is intended to use with master beacon connected to the computer. + +It provides following configuration options of the system: + + * switching between text and binary printout modes, + * swapping beacons, + * setting ''Exponential Moving Average'' filter number of samples (window size), + * setting span between the beacons (in millimeters), + * settings parameters for RS485 connection (Modbus slave id, baudrate, parity). + +==== Running the node ==== +After your workspace is built and sourced, you can run the node (set `_portname` to the actual port name where the master beacon is connected: +{{{ +rosrun positioning_systems_ros follow_me_master_beacon _portname:=/dev/ttyACM0 +}}} + +==== Subscribed topics ==== +This node subscribes to the following topics: + + * `/follow_me_master_beacon/follow_me_autocalibrate` : publishing to this topic activates span autocalibration mode + * `/follow_me_master_beacon/follow_me_config` : publishing to this topic sets parameters: printout mode, swap beacons, EMA filter window, span between the beacons + * `/follow_me_master_beacon/follow_me_rs485_config` : publishing to this topic sets slave id, baudrate and parity of RS485 interface + * `/follow_me_master_beacon/follow_me_test_cmd` : publishing to this topic triggers test command which returns actual configuration of the device + +Example 1 of usage: +{{{ +rostopic pub /follow_me_master_beacon/follow_me_config positioning_systems_ros/FollowMeDriverConfig "printout_mode: 'Binary' +swap_beacons: true +ema_window: 10 +beacons_span: 540" +}}} +Sets binary printout mode, swaps beacons, sets EMA filter to 10 samples and beacons span to 540 mm. + +Example 2 of usage: +{{{ +rostopic pub /follow_me_master_beacon/follow_me_rs485_config positioning_systems_ros/FollowMeDriverRS485Config "rs485_slave_id: 3 +rs485_baudrate: 19200 +rs485_parity: 2" +}}} +Sets Modbus RTU slave id to 3, baud rate to 19200 and parity to 2 (Even). + +To see list of valid values for each parameter, open respective *.msg file. + +==== Published topics ==== +This node publishes to the topic: + + * `/follow_me_master_beacon/follow_me_polar_point_2d` : provides distance and heading of the remote control with respect to the beacons + +=== follow_me_remote_control === +This node is intended to use with remote control connected to the computer. + +It provides following configuration options of the remote control: + + * setting button operation mode (hold, toggle) + * setting buzzer operation (enabled, disabled) + +==== Running the node ==== +After your workspace is built and sourced, you can run the node (set `_portname` to the actual port name where the remote control is connected: +{{{ +rosrun positioning_systems_ros follow_me_remote_control _portname:=/dev/ttyUSB0 +}}} + +==== Subscribed topics ==== +This node subscribes to the following topics: + + * `/follow_me_remote_control/follow_me_get_config` : publishing to this topic triggers command which returns actual configuration of the remote control + * `/follow_me_remote_control/follow_me_set_config` : publishing to this topic sets remote control configuration (button mode and buzzer state) + +Example of usage: +{{{ +rostopic pub /follow_me_remote_control/follow_me_set_config positioning_systems_ros/FollowMeRemoteControlConfig "button_mode: 'Hold' +buzzer_active: false" +}}} +Sets button mode to ''Hold'' and deactivates buzzer. + +To see list of valid values for each parameter, open respective *.msg file. +== Example == +A basic subscriber is available in subdirectory `examples`. + +In order to use the package in your own node, do the following: + +In `CMakeLists.txt` add the package name `positioning_systems_ros` to `find_package` and `catkin_package` commands. + +In `package.xml` add: + +{{{ +positioning_systems_ros +}}} +and +{{{ +positioning_systems_ros +}}} + +## AUTOGENERATED DON'T DELETE +## CategoryPackage diff --git a/src/follow_me_master_beacon.cpp b/src/follow_me_master_beacon.cpp new file mode 100644 index 0000000..2400fe5 --- /dev/null +++ b/src/follow_me_master_beacon.cpp @@ -0,0 +1,242 @@ +#include +#include +#include "follow_me_driver/follow_me_driver.hpp" +#include "positioning_systems_ros/PolarPoint2D.h" +#include "positioning_systems_ros/FollowMeDriverConfig.h" +#include "positioning_systems_ros/FollowMeDriverRS485Config.h" +#include "serial_communication/serial.hpp" + +namespace terabee { + +class FollowMeMasterBeaconROS +{ +public: + FollowMeMasterBeaconROS(ros::NodeHandle& nh, + std::shared_ptr serialIf); + + void spin(); + void start() { is_started_ = true; } + void stop() { is_started_ = false; } + +private: + FollowMeMasterBeacon master_beacon_; + ros::NodeHandle& nodeHandle_; + bool is_started_ = false; + double time_step_; + + std::string portname_; + int baudrate_; + int serial_timeout_ms_; + std::unique_ptr rate_; + + ros::Publisher point_publisher_; + ros::Subscriber follow_me_config_sub_; + ros::Subscriber follow_me_rs485_config_sub_; + ros::Subscriber follow_me_autocalibrate_sub_; + ros::Subscriber follow_me_test_cmd_; + + void updateParametersCallback(const positioning_systems_ros::FollowMeDriverConfig &config); + void updateParametersRS485Callback(const positioning_systems_ros::FollowMeDriverRS485Config &config); + void autoCalibrateCallback(std_msgs::Empty) const; + void testCommandCallback(std_msgs::Empty) const; + bool processTestCommand(const std::string &test_result) const; +}; + +FollowMeMasterBeaconROS::FollowMeMasterBeaconROS(ros::NodeHandle& nh, + std::shared_ptr serialIf): + master_beacon_(serialIf), + nodeHandle_(nh) +{ + nodeHandle_.param("time_step", time_step_, 0.01); + nodeHandle_.param("portname", portname_, "/dev/ttyACM0"); + nodeHandle_.param("baudrate", baudrate_, 115200); + nodeHandle_.param("serial_timeout_ms", serial_timeout_ms_, 200); + + serialIf->setPortName(portname_); + serialIf->setBaudrate(baudrate_); + serialIf->setTimeout(std::chrono::milliseconds(serial_timeout_ms_)); + + serialIf->open(); + + if(serialIf->isOpen()){ + ROS_INFO_STREAM("Serial port open: " << portname_.c_str()); + } + else + { + ROS_ERROR_STREAM("Could not open: " << portname_.c_str()); + ros::shutdown(); + } + master_beacon_.printoutModeBin(); + + rate_ = std::make_unique(1.0/time_step_); + + point_publisher_ = nodeHandle_.advertise("follow_me_polar_point_2d", 1); + follow_me_config_sub_ = + nodeHandle_.subscribe("follow_me_config", 1, + &FollowMeMasterBeaconROS::updateParametersCallback, this); + + follow_me_rs485_config_sub_ = + nodeHandle_.subscribe("follow_me_rs485_config", 1, + &FollowMeMasterBeaconROS::updateParametersRS485Callback, this); + + follow_me_autocalibrate_sub_ = + nodeHandle_.subscribe("follow_me_autocalibrate", 1, + &FollowMeMasterBeaconROS::autoCalibrateCallback, this); + + follow_me_test_cmd_ = + nodeHandle_.subscribe("follow_me_test_cmd", 1, + &FollowMeMasterBeaconROS::testCommandCallback, this); + + ROS_INFO("follow_me_master_beacon launched successfully."); +} + +void FollowMeMasterBeaconROS::spin() +{ + while(ros::ok() && is_started_) + { + PolarPoint2D recv_point; + if (master_beacon_.process(recv_point)) + { + positioning_systems_ros::PolarPoint2D point; + point.header.stamp = ros::Time::now(); + point.distance = recv_point.distance; + point.heading = recv_point.heading; + point_publisher_.publish(point); + } + + ros::spinOnce(); + rate_->sleep(); + } +} + +void FollowMeMasterBeaconROS::autoCalibrateCallback(std_msgs::Empty) const +{ + master_beacon_.spanAutoCalibrate(); +} + +void FollowMeMasterBeaconROS::testCommandCallback(std_msgs::Empty) const +{ + if(!processTestCommand(master_beacon_.retrieveDeviceData())) + { + ROS_WARN("Error while processing test command"); + } +} + +bool FollowMeMasterBeaconROS::processTestCommand(const std::string &test_result) const +{ + if (test_result.empty() || test_result.find("ID TB-FM") == std::string::npos) + { + ROS_INFO("Failed to retrieve parameters from master beacon"); + return false; + } + size_t rs485_slave_id_pos = test_result.find("RS485@ ") + 7; + size_t rs485_baud_pos = test_result.find(" ", rs485_slave_id_pos) + 1; + size_t rs485_parity_pos = test_result.find(" ", rs485_baud_pos) + 1; + size_t rs485_parity_pos_end = test_result.find(" UWB", rs485_parity_pos); + size_t span_pos = test_result.find("D:", rs485_parity_pos_end) + 2; + size_t ema_window_pos = test_result.find(":", span_pos) + 1; + size_t swap_beacons_pos = test_result.find(":", ema_window_pos) + 1; + size_t print_out_mode_pos = test_result.find(":", swap_beacons_pos) + 1; + + int rs485_slave_id = std::stoi(test_result.substr(rs485_slave_id_pos, rs485_baud_pos - rs485_slave_id_pos - 1)); + int span = std::stoi(test_result.substr(span_pos, ema_window_pos - span_pos - 1)); + int ema_window = std::stoi(test_result.substr(ema_window_pos, swap_beacons_pos - ema_window_pos - 1)); + bool swap_beacons = std::stoi(test_result.substr(swap_beacons_pos).c_str()); + FollowMeMasterBeacon::printout_mode printout_mode = + static_cast(std::stoi(test_result.substr(print_out_mode_pos).c_str())); + FollowMeMasterBeacon::rs485_parity rs485_parity = + static_cast(std::stoi(test_result.substr(rs485_parity_pos, rs485_parity_pos_end - rs485_parity_pos))); + int rs485_baudrate = std::stoi(test_result.substr(rs485_baud_pos, rs485_parity_pos - rs485_baud_pos - 1)); + + std::string parity_str; + switch (rs485_parity) + { + case FollowMeMasterBeacon::rs485_parity::rs485_parity_none: + parity_str = "None"; + break; + case FollowMeMasterBeacon::rs485_parity::rs485_parity_odd: + parity_str = "Odd"; + break; + case FollowMeMasterBeacon::rs485_parity::rs485_parity_even: + parity_str = "Even"; + break; + } + ROS_INFO_STREAM("Parameters:" + "\n\tRS485 slave id:\t" << rs485_slave_id << + "\n\tRS485 baudrate:\t" << rs485_baudrate << + "\n\tRS485 parity:\t" << parity_str << + "\n\tPrintout mode:\t" << (printout_mode == FollowMeMasterBeacon::printout_mode::text ? "Text" : "Binary") << + "\n\tBeacons span:\t" << span << + "\n\tSwap beacons:\t" << (swap_beacons ? "Enabled" : "Disabled") << + "\n\tEMA window:\t" << ema_window); + return true; +} + +void FollowMeMasterBeaconROS::updateParametersCallback(const positioning_systems_ros::FollowMeDriverConfig &config) +{ + if (!master_beacon_.setEMAWindow(config.ema_window)) + { + ROS_WARN("Failed to set ema_window parameter!"); + } + if (!master_beacon_.setBeaconsSpan(config.beacons_span)) + { + ROS_WARN("Failed to set beacons_span parameter!"); + } + if (!master_beacon_.swapBeacons(config.swap_beacons)) + { + ROS_WARN("Failed to set swap_beacons parameter!"); + } + + if (config.printout_mode == "Binary") + { + if (!master_beacon_.printoutModeBin()) + { + ROS_WARN("Failed to set printout_mode: Binary!"); + } + } + else if (config.printout_mode == "Text") + { + if (!master_beacon_.printoutModeText()) + { + ROS_WARN("Failed to set printout_mode: Text!"); + } + } + else + { + ROS_WARN("Unknown printout mode"); + } +} + +void FollowMeMasterBeaconROS::updateParametersRS485Callback(const positioning_systems_ros::FollowMeDriverRS485Config &config) +{ + if (!master_beacon_.setRS485_SlaveId(config.rs485_slave_id)) + { + ROS_WARN("Failed to set RS485 slave ID"); + } + if (!master_beacon_.setRS485_Parameters(config.rs485_baudrate, static_cast(config.rs485_parity))) + { + ROS_WARN("Failed to set RS485 parameters"); + } + + if (!master_beacon_.setRS485_SlaveId(config.rs485_slave_id)) + { + ROS_WARN("Failed to set Modbus Slave ID"); + } +} + +} // namespace terabee + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "follow_me_master_beacon"); + ros::NodeHandle nh("~"); + + std::shared_ptr serialInterface = + std::make_shared("/dev/ttyACM0"); + + terabee::FollowMeMasterBeaconROS followMeDriver(nh, serialInterface); + + followMeDriver.start(); + followMeDriver.spin(); + return 0; +} diff --git a/src/follow_me_remote_control.cpp b/src/follow_me_remote_control.cpp new file mode 100644 index 0000000..14d1932 --- /dev/null +++ b/src/follow_me_remote_control.cpp @@ -0,0 +1,128 @@ +#include +#include +#include "follow_me_driver/follow_me_driver.hpp" +#include "positioning_systems_ros/FollowMeRemoteControlConfig.h" +#include "serial_communication/serial.hpp" + +namespace terabee { + +class FollowMeRemoteControlROS +{ +public: + FollowMeRemoteControlROS(ros::NodeHandle& nh, + std::shared_ptr serialIf); + + void spin(); + void start() { is_started_ = true; } + void stop() { is_started_ = false; } + +private: + FollowMeRemoteControl remote_control_; + ros::NodeHandle& nodeHandle_; + bool is_started_ = false; + double time_step_; + + std::string portname_; + int baudrate_; + int serial_timeout_ms_; + + std::unique_ptr rate_; + + ros::Subscriber follow_me_set_remote_config_sub_; + ros::Subscriber follow_me_get_remote_config_sub_; + + void updateParametersCallback(const positioning_systems_ros::FollowMeRemoteControlConfig &config); + void getParametersCallback(std_msgs::Empty) const; +}; + +FollowMeRemoteControlROS::FollowMeRemoteControlROS(ros::NodeHandle &nh, + std::shared_ptr serialIf): + remote_control_(serialIf), + nodeHandle_(nh) +{ + nodeHandle_.param("time_step", time_step_, 0.01); + nodeHandle_.param("portname", portname_, "/dev/ttyUSB0"); + nodeHandle_.param("baudrate", baudrate_, 115200); + nodeHandle_.param("serial_timeout_ms", serial_timeout_ms_, 800); + + serialIf->setPortName(portname_); + serialIf->setBaudrate(baudrate_); + serialIf->setTimeout(std::chrono::milliseconds(serial_timeout_ms_)); + + serialIf->open(); + + if(serialIf->isOpen()){ + ROS_INFO_STREAM("Serial port open: " << portname_.c_str()); + } + else + { + ROS_ERROR_STREAM("Could not open: " << portname_.c_str()); + ros::shutdown(); + } + + rate_ = std::make_unique(1.0/time_step_); + + follow_me_set_remote_config_sub_ = + nodeHandle_.subscribe("follow_me_set_config", 1, + &FollowMeRemoteControlROS::updateParametersCallback, this); + + follow_me_get_remote_config_sub_ = + nodeHandle_.subscribe("follow_me_get_config", 1, + &FollowMeRemoteControlROS::getParametersCallback, this); + + ROS_INFO("follow_me_remote_control launched successfully."); +} + +void FollowMeRemoteControlROS::spin() +{ + while (ros::ok() && is_started_) + { + ros::spinOnce(); + rate_->sleep(); + } +} + +void FollowMeRemoteControlROS::updateParametersCallback(const positioning_systems_ros::FollowMeRemoteControlConfig &config) +{ + if (config.button_mode == "Toggle") + { + remote_control_.setButtonMode(FollowMeRemoteControl::button_mode::toggle); + } + else if (config.button_mode == "Hold") + { + remote_control_.setButtonMode(FollowMeRemoteControl::button_mode::hold); + } + else + { + ROS_WARN("Unknown button mode"); + } + remote_control_.setBuzzer(config.buzzer_active); +} + +void FollowMeRemoteControlROS::getParametersCallback(std_msgs::Empty) const +{ + FollowMeRemoteControl::button_mode btn_mode; + bool buzzer_activated; + if (remote_control_.retrieveRemoteParameters(btn_mode, buzzer_activated)) + { + ROS_INFO_STREAM("Buzzer " << (buzzer_activated ? "activated" : "deactivated") << + "; Button " << (btn_mode == FollowMeRemoteControl::button_mode::toggle ? "toggle" : "hold") << " mode" + ); + } +} + +} // namespace terabee + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "follow_me_remote_control"); + ros::NodeHandle nh("~"); + + std::shared_ptr serialInterface = + std::make_shared("/dev/ttyUSB0"); + + terabee::FollowMeRemoteControlROS followMeDriver(nh, serialInterface); + + followMeDriver.start(); + followMeDriver.spin(); +} diff --git a/src/rtls_configurator_ros.cpp b/src/rtls_configurator_ros.cpp new file mode 100644 index 0000000..7436ec6 --- /dev/null +++ b/src/rtls_configurator_ros.cpp @@ -0,0 +1,296 @@ +#include "rtls_configurator_ros.hpp" + +RtlsConfiguratorROS::RtlsConfiguratorROS(std::shared_ptr nh, std::shared_ptr rtls_device): + rtls_device_(rtls_device), + nh_(nh) +{ +} + +bool RtlsConfiguratorROS::setDevice() +{ + bool success = false; + std::string device_type_str; + int priority = 0; + + if (nh_->getParam("device_type", device_type_str) && + nh_->getParam("priority", priority)) + { + terabee::RtlsDevice::device_type device_type; + if (device_type_str == "Anchor") + device_type = terabee::RtlsDevice::device_type::anchor; + else if (device_type_str == "Tracker") + device_type = terabee::RtlsDevice::device_type::tracker; + else + throw std::out_of_range("Invalid device type: " + device_type_str); + + success = rtls_device_->setDevice(device_type, static_cast(priority)); + if (success) + { + ROS_INFO_STREAM("Device type set to: " << device_type_str << " priority: " << priority); + } + else + { + ROS_ERROR("Failed to set device type and priority!"); + } + } + return success; +} + +bool RtlsConfiguratorROS::setLabel() +{ + bool success = false; + int label = 0; + + if (nh_->getParam("label", label)) + { + success = rtls_device_->setLabel(static_cast(label)); + if (success) + { + ROS_INFO_STREAM("Label set to: " << std::hex << label); + } + else + { + ROS_ERROR("Failed to configure device label!"); + } + } + + return success; +} + +bool RtlsConfiguratorROS::setTrackerMessage() +{ + bool success = false; + std::string tracker_message_mode; + + if (nh_->getParam("tracker_message_mode", tracker_message_mode)) + { + if (tracker_message_mode == "Short") + success = rtls_device_->setTrackerMessageShort(); + else if (tracker_message_mode == "Long") + success = rtls_device_->setTrackerMessageLong(); + else + throw std::out_of_range("Invalid tracker mode: " + tracker_message_mode); + + if (success) + { + ROS_INFO_STREAM("Tracker message type set to: " << tracker_message_mode); + } + else + { + ROS_ERROR("Failed to set tracker message type!"); + } + } + + return success; +} + +bool RtlsConfiguratorROS::setNetworkId() +{ + bool success = false; + int network_id = 0; + + if (nh_->getParam("network_id", network_id)) + { + success = rtls_device_->setNetworkId(static_cast(network_id)); + if (success) + { + ROS_INFO_STREAM("Network ID set to: " << std::hex << network_id); + } + else + { + ROS_ERROR("Failed to configure network ID!"); + } + } + + return success; +} + +bool RtlsConfiguratorROS::setUpdateTime() +{ + bool success = false; + int update_time_ms = 0; + + if (nh_->getParam("update_time_ms", update_time_ms)) + { + success = rtls_device_->setUpdateTime(static_cast(update_time_ms/100)); + if (success) + { + ROS_INFO_STREAM("Update time set to: " << update_time_ms); + } + else + { + ROS_ERROR("Failed to configure update time!"); + } + } + + return success; +} + +bool RtlsConfiguratorROS::setAnchorPosition() +{ + bool success = false; + int x = 0; + int y = 0; + int z = 0; + + if (nh_->getParam("x", x) && + nh_->getParam("y", y) && + nh_->getParam("z", z)) + { + success = rtls_device_->setAnchorPosition(x, y, z); + if (success) + { + ROS_INFO_STREAM("Position set to: (" << x << ", " << y << ", " << z << ")."); + } + else + { + ROS_ERROR("Failed to configure anchor position!"); + } + } + + return success; +} + +bool RtlsConfiguratorROS::setAnchorHeightForAutoPositioning() +{ + bool success = false; + int anchor_height = 0; + + if (!nh_->getParam("anchor_height", anchor_height)) + { + ROS_WARN("Parameter 'anchor_height' not specified."); + } + else + { + success = rtls_device_->setAnchorHeightForAutoPositioning(anchor_height); + if (success) + { + ROS_INFO_STREAM("Anchor height set to: " << anchor_height); + } + else + { + ROS_ERROR("Failed to configure anchor height!"); + } + } + + return success; +} + +bool RtlsConfiguratorROS::setAnchorInitiator() +{ + bool success = false; + bool is_initiator = false; + + if (!nh_->getParam("is_initiator", is_initiator)) + { + ROS_WARN("Parameter 'is_initiator' not specified."); + } + else + { + is_initiator ? + success = rtls_device_->enableAnchorInitiator() : + success = rtls_device_->disableAnchorInitiator(); + if (success) + { + ROS_INFO_STREAM("Initiator mode " << (is_initiator ? "enabled." : "disabled.")); + } + else + { + ROS_ERROR("Failed to configure initiator mode!"); + } + } + + return success; +} + +bool RtlsConfiguratorROS::setAutoAnchorPositioning() +{ + bool success = false; + bool is_auto_anchor_positioning = false; + + if (!nh_->getParam("is_auto_anchor_positioning", is_auto_anchor_positioning)) + { + ROS_WARN("Parameter 'is_auto_anchor_positioning' not specified."); + } + else + { + is_auto_anchor_positioning ? + success = rtls_device_->enableAutoAnchorPositioning() : + success = rtls_device_->disableAutoAnchorPositioning(); + + if (success) + { + ROS_INFO_STREAM("Auto anchor positioning " << + (is_auto_anchor_positioning ? "enabled." : "disabled.")); + } + else + { + ROS_ERROR("Failed to configure auto anchor positioning!"); + } + } + + return success; +} + +bool RtlsConfiguratorROS::setLED() +{ + bool success = false; + bool is_led_enabled = false; + + if (!nh_->getParam("is_led_enabled", is_led_enabled)) + { + ROS_WARN("Parameter 'is_led_enabled' not specified."); + } + else + { + is_led_enabled ? + success = rtls_device_->enableLED() : + success = rtls_device_->disableLED(); + + if (success) + { + ROS_INFO_STREAM("LEDs " << (is_led_enabled ? "enabled." : "disabled.")); + } + else + { + ROS_ERROR("Failed to configure LEDs mode!"); + } + } + + return success; +} + +bool RtlsConfiguratorROS::setTrackerStream() +{ + bool success = false; + bool is_tracker_stream_mode = false; + + if (!nh_->getParam("is_tracker_stream_mode", is_tracker_stream_mode)) + { + ROS_WARN("Parameter 'is_tracker_stream_mode' not specified."); + } + else + { + if (is_tracker_stream_mode) + { + success = rtls_device_->enableTrackerStream(); + } + else + { + rtls_device_->disableTrackerStream(); + success = true; + } + + if (success) + { + ROS_INFO_STREAM("Tracker stream mode " << + (is_tracker_stream_mode ? "enabled." : "disabled.")); + } + else + { + ROS_ERROR("Failed to configure tracker stream mode!"); + } + } + + return success; +} diff --git a/src/rtls_device_config_node.cpp b/src/rtls_device_config_node.cpp new file mode 100644 index 0000000..ba9a7e6 --- /dev/null +++ b/src/rtls_device_config_node.cpp @@ -0,0 +1,51 @@ +#include +#include "serial_communication/serial.hpp" +#include "rtls_driver/rtls_driver.hpp" +#include "rtls_configurator_ros.hpp" + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "rtls_device_config_node"); + std::shared_ptr nh = std::make_shared("~"); + + std::string portname = nh->param("portname", std::string("/dev/ttyUSB0")); + + constexpr int serial_timeout = 800; + std::shared_ptr serial_port = + std::make_shared(portname); + + serial_port->setBaudrate(115200); + serial_port->setTimeout(std::chrono::milliseconds(serial_timeout)); + + serial_port->open(); + if (!serial_port->isOpen()) + { + ROS_ERROR("Failed to open serial port!"); + return 0; + } + + auto rtls_device = std::make_shared(serial_port); + RtlsConfiguratorROS rtls_configurator(nh, rtls_device); + + rtls_device->disableTrackerStream(); + serial_port->flushInput(); + + rtls_configurator.setDevice(); + rtls_configurator.setTrackerMessage(); + rtls_configurator.setLabel(); + rtls_configurator.setNetworkId(); + rtls_configurator.setUpdateTime(); + rtls_configurator.setAnchorPosition(); + rtls_configurator.setAnchorHeightForAutoPositioning(); + rtls_configurator.setAnchorInitiator(); + rtls_configurator.setAutoAnchorPositioning(); + rtls_configurator.setLED(); + rtls_configurator.setTrackerStream(); + + serial_port->flushInput(); + rtls_device->requestConfig(); + auto rtls_config = rtls_device->getConfig(); + std::string config = rtls_device->serializeConfig(); + ROS_INFO_STREAM(config); + return 0; +} diff --git a/src/rtls_tracker_node.cpp b/src/rtls_tracker_node.cpp new file mode 100644 index 0000000..439b832 --- /dev/null +++ b/src/rtls_tracker_node.cpp @@ -0,0 +1,121 @@ +#include +#include +#include "rtls_driver/rtls_driver.hpp" +#include "serial_communication/serial.hpp" +#include "positioning_systems_ros/RtlsTrackerFrame.h" +#include "positioning_systems_ros/RtlsAnchorData.h" + +constexpr double MM_TO_M_FACTOR = 0.001; + +positioning_systems_ros::RtlsTrackerFrame createTrackerMsgROS( + const terabee::RtlsDevice::tracker_msg_t& tracker_msg) +{ + positioning_systems_ros::RtlsTrackerFrame msg; + for (auto anchor : tracker_msg.anchors_data) + { + positioning_systems_ros::RtlsAnchorData anchor_msg; + anchor_msg.id = anchor.id; + anchor_msg.position.x = MM_TO_M_FACTOR*static_cast(anchor.pos_x); + anchor_msg.position.y = MM_TO_M_FACTOR*static_cast(anchor.pos_y); + anchor_msg.position.z = MM_TO_M_FACTOR*static_cast(anchor.pos_z); + anchor_msg.distance = MM_TO_M_FACTOR*static_cast(anchor.distance); + msg.anchors.push_back(anchor_msg); + } + + msg.position.x = MM_TO_M_FACTOR*static_cast(tracker_msg.tracker_position_xyz.at(0)); + msg.position.y = MM_TO_M_FACTOR*static_cast(tracker_msg.tracker_position_xyz.at(1)); + msg.position.z = MM_TO_M_FACTOR*static_cast(tracker_msg.tracker_position_xyz.at(2)); + msg.is_valid_position = tracker_msg.is_valid_position; + + return msg; +} + +void publishTransform(const std::string& ref_frame, + const std::string& frame, + const geometry_msgs::Point& point) +{ + static tf2_ros::TransformBroadcaster br; + geometry_msgs::TransformStamped transform_st; + transform_st.header.stamp = ros::Time::now(); + transform_st.header.frame_id = ref_frame; + transform_st.child_frame_id = frame; + transform_st.transform.translation.x = point.x; + transform_st.transform.translation.y = point.y; + transform_st.transform.translation.z = point.z; + transform_st.transform.rotation.x = 0.0; + transform_st.transform.rotation.y = 0.0; + transform_st.transform.rotation.z = 0.0; + transform_st.transform.rotation.w = 1.0; + + br.sendTransform(transform_st); +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "rtls_tracker_node"); + ros::NodeHandle nh("~"); + + bool publish_tf = nh.param("publish_tf", true); + std::string ref_frame = nh.param("ref_frame", std::string("map")); + std::string portname = nh.param("portname", std::string("/dev/ttyUSB0")); + + constexpr int serial_timeout = 800; + std::shared_ptr serial_port = + std::make_shared(portname); + + serial_port->setBaudrate(115200); + serial_port->setTimeout(std::chrono::milliseconds(serial_timeout)); + + serial_port->open(); + + if (!serial_port->isOpen()) + { + ROS_ERROR("Failed to open serial port!"); + return 0; + } + + terabee::RtlsDevice rtls_device(serial_port); + ros::Publisher rtls_publisher = + nh.advertise("rtls_tracker_frame", 1); + + rtls_device.registerOnDistanceDataCaptureCallback( + [&rtls_publisher, + &publish_tf, + &ref_frame](const terabee::RtlsDevice::tracker_msg_t& tracker_msg) + { + ROS_INFO_STREAM("T: " << tracker_msg.timestamp); + positioning_systems_ros::RtlsTrackerFrame msg; + msg = createTrackerMsgROS(tracker_msg); + rtls_publisher.publish(msg); + + if (publish_tf) + { + publishTransform(ref_frame, "tracker", msg.position); + + for (auto anchor : msg.anchors) + { + std::stringstream anchor_id_hex; + anchor_id_hex << "0x" << std::setfill ('0') + << std::setw(4) << std::hex + << anchor.id; + publishTransform(ref_frame, anchor_id_hex.str(), anchor.position); + } + } + }); + + rtls_device.disableTrackerStream(); + serial_port->flushInput(); + rtls_device.requestConfig(); + if (rtls_device.getConfig().type != terabee::RtlsDevice::device_type::tracker) + { + ROS_ERROR("Device is not configured as tracker!"); + return 0; + } + rtls_device.enableTrackerStream(); + rtls_device.startReadingStream(); + + ros::spin(); + rtls_device.stopReadingStream(); + + return 0; +}