diff --git a/radio-lap/src/ros_decawave/CMakeLists.txt b/radio-lap/src/ros_decawave/CMakeLists.txt new file mode 100644 index 0000000..97e0aa1 --- /dev/null +++ b/radio-lap/src/ros_decawave/CMakeLists.txt @@ -0,0 +1,203 @@ +cmake_minimum_required(VERSION 2.8.3) +project(ros_decawave) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## 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 + rospy + std_msgs + tf + message_generation +) + +## 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 + Tag.msg + Anchor.msg + AnchorArray.msg + Acc.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 + std_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 ros_decawave +# CATKIN_DEPENDS rospy std_msgs tf +# DEPENDS system_lib + CATKIN_DEPENDS message_runtime +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/ros_decawave.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## 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(${PROJECT_NAME}_node src/ros_decawave_node.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(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## 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 and/or libraries for installation +# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_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_gtest(${PROJECT_NAME}-test test/test_ros_decawave.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/radio-lap/src/ros_decawave/LICENSE b/radio-lap/src/ros_decawave/LICENSE new file mode 100644 index 0000000..7c55c1b --- /dev/null +++ b/radio-lap/src/ros_decawave/LICENSE @@ -0,0 +1,201 @@ + 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 [2019] [Paulo Rezeck - VERLAB] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/radio-lap/src/ros_decawave/README.md b/radio-lap/src/ros_decawave/README.md new file mode 100644 index 0000000..0e785ab --- /dev/null +++ b/radio-lap/src/ros_decawave/README.md @@ -0,0 +1,49 @@ +# ros_decawave +Positioning System based on Decawave's DWM1001 Ultra Wide Band transceivers. + +## Dependencies + +### Python Dependencies +``` +$ pip install pyserial +$ pip install struct +``` + +### ROS Dependencies +``` +$ apt install ros-kinetic-hector-trajectory-server +``` + +## Tutorial +### Pre-Setup +Setup the tag and the anchors using the Android app, and connected the tag using an USB cable to PC. + +### Download and Compilation +Download this repo into the ros workspace and compile: +``` +$ cd src/ +$ git clone https://github.com/verlab/ros_decawave.git +$ cd .. +$ catkin_make ## or catkin build +``` + +### Using +Run the launch: +``` +$ roslaunch ros_decawave decawave_driver.launch +``` +[![IMAGE ALT TEXT HERE](https://img.youtube.com/vi/ieaP79FDLC0/0.jpg)](https://www.youtube.com/watch?v=ieaP79FDLC0) + +### Topics +``` +$ rostopic list +/pose # tag position +/rosout +/rosout_agg +/status # anchors status +/syscommand +/tf +/tf_static +/trajectory # tag path +``` + diff --git a/radio-lap/src/ros_decawave/config/config.rviz b/radio-lap/src/ros_decawave/config/config.rviz new file mode 100644 index 0000000..d2dace9 --- /dev/null +++ b/radio-lap/src/ros_decawave/config/config.rviz @@ -0,0 +1,171 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Path1 + Splitter Ratio: 0.5 + Tree Height: 775 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 0.100000001 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 100 + Reference Frame: + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Lines + Line Width: 0.0299999993 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /trajectory + Unreliable: false + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + 1291: + Value: true + 50AA: + Value: true + 838E: + Value: true + All Enabled: true + CC8E: + Value: true + tag: + Value: true + world: + Value: true + Marker Scale: 3 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + world: + 1291: + {} + 50AA: + {} + 838E: + {} + CC8E: + {} + tag: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 13.2844334 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 0.615398467 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.21541262 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1056 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004fb0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 0 + Y: 24 diff --git a/radio-lap/src/ros_decawave/launch/decawave_driver.launch b/radio-lap/src/ros_decawave/launch/decawave_driver.launch new file mode 100644 index 0000000..3fdd3b0 --- /dev/null +++ b/radio-lap/src/ros_decawave/launch/decawave_driver.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + \ No newline at end of file diff --git a/radio-lap/src/ros_decawave/msg/Acc.msg b/radio-lap/src/ros_decawave/msg/Acc.msg new file mode 100644 index 0000000..e42a08b --- /dev/null +++ b/radio-lap/src/ros_decawave/msg/Acc.msg @@ -0,0 +1,7 @@ +# Anchor message structure. qf is the quality of this measurement and dist_qf the quality of the estimated distance. + +Header header + +float64 x ## in m/s2 +float64 y ## in m/s2 +float64 z ## in m/s2 diff --git a/radio-lap/src/ros_decawave/msg/Anchor.msg b/radio-lap/src/ros_decawave/msg/Anchor.msg new file mode 100644 index 0000000..a029238 --- /dev/null +++ b/radio-lap/src/ros_decawave/msg/Anchor.msg @@ -0,0 +1,10 @@ +# Anchor message structure. qf is the quality of this measurement and dist_qf the quality of the estimated distance. + +Header header + +float64 x ## in m +float64 y ## in m +float64 z ## in m +float64 qf ## in percent +float64 distance ## in m +float64 dist_qf ## in percent diff --git a/radio-lap/src/ros_decawave/msg/AnchorArray.msg b/radio-lap/src/ros_decawave/msg/AnchorArray.msg new file mode 100644 index 0000000..6be785e --- /dev/null +++ b/radio-lap/src/ros_decawave/msg/AnchorArray.msg @@ -0,0 +1,4 @@ +# An array of anchors +Header header + +Anchor[] anchors \ No newline at end of file diff --git a/radio-lap/src/ros_decawave/msg/Tag.msg b/radio-lap/src/ros_decawave/msg/Tag.msg new file mode 100644 index 0000000..756a7fb --- /dev/null +++ b/radio-lap/src/ros_decawave/msg/Tag.msg @@ -0,0 +1,10 @@ +# Tag message structure. qf is the quality of this measurement. + +Header header + +float64 x ## in m +float64 y ## in m +float64 z ## in m +float64 qf ## in percent +int16 n_anchors ## number of anchors + diff --git a/radio-lap/src/ros_decawave/package.xml b/radio-lap/src/ros_decawave/package.xml new file mode 100644 index 0000000..c1d08df --- /dev/null +++ b/radio-lap/src/ros_decawave/package.xml @@ -0,0 +1,69 @@ + + + ros_decawave + 0.0.0 + The ros_decawave package + + + rezeck + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + message_generation + rospy + std_msgs + tf + message_generation + rospy + std_msgs + tf + message_runtime + rospy + std_msgs + tf + + + + + + + + diff --git a/radio-lap/src/ros_decawave/script/decawave_driver.py b/radio-lap/src/ros_decawave/script/decawave_driver.py new file mode 100755 index 0000000..34a78ce --- /dev/null +++ b/radio-lap/src/ros_decawave/script/decawave_driver.py @@ -0,0 +1,192 @@ +#!/usr/bin/env python3 +import roslib; roslib.load_manifest('ros_decawave') +import rospy +import tf +import time + +import serial +import struct + +from geometry_msgs.msg import PointStamped +from ros_decawave.msg import Tag, Anchor, AnchorArray, Acc + + +class DecawaveDriver(object): + """ docstring for DecawaveDriver """ + def __init__(self): + rospy.init_node('decawave_driver', anonymous=False) + # Getting Serial Parameters + self.port_ = rospy.get_param('~port', '/dev/ttyACM0') + self.baudrate_ = int(rospy.get_param('~baudrate', '115200')) + self.tf_publisher_ = str(rospy.get_param('~tf_publisher', "True")) + self.tf_reference_ = str(rospy.get_param('~tf_reference', 'world')) + self.tag_name_ = str(rospy.get_param('~tag_name', 'tag')) + self.rate_ = int(rospy.get_param('~rate', '10')) + self.serial_timeout = rospy.Duration(0.5) + # Initiate Serial + self.ser = serial.Serial(self.port_, self.baudrate_, timeout=0.1) + rospy.loginfo("\33[96mConnected to %s at %i\33[0m", self.ser.portstr, self.baudrate_) + self.get_uart_mode() + self.switch_uart_mode() + self.get_tag_status() + self.get_tag_version() + self.anchors = AnchorArray() + self.anchors.anchors = [] + self.tag = Tag() + + + def get_uart_mode(self): + """ Check UART Mode Used """ + rospy.loginfo("\33[96mChecking which UART mode is the gateway...\33[0m") + self.mode_ = 'UNKNOWN' + self.ser.write(b'\r') # Test Mode + time.sleep(0.1) + while(self.ser.inWaiting() == 0): + pass + cc = self.ser.readline() + if cc == '\r\n' and self.ser.readline() == 'dwm> ': # SHELL MODE + rospy.loginfo("\33[96mDevice is on SHELL MODE! It must to be changed to GENERIC MODE!\33[0m") + self.mode_ = "SHELL" + elif cc == '@\x01\x01': # GENERIC MODE + rospy.loginfo("\33[96mDevice is on GENERIC MODE! Ok!\33[0m") + self.mode_ = "GENERIC" + return self.mode_ + + + def switch_uart_mode(self): + if self.mode_ == "SHELL": + rospy.loginfo("\33[96mChanging UART mode to GENERIC MODE...\33[0m") + self.ser.write(b'quit\r') # Go to Generic Mode + while(self.ser.inWaiting()==0): + pass + self.ser.readline() + rospy.loginfo("\33[96m%s\33[0m", self.ser.readline().replace('\n', '')) + elif self.mode_ == "UNKNOWN": + rospy.logerr("\33[96m%s\33[0m", "Unknown Mode Detected! Please reset the device and try again!") + + def get_tag_version(self): + self.ser.flushInput() + self.ser.write(b'\x15\x00') # Status + now = rospy.Time.now() + while(self.ser.inWaiting() < 21): + if (rospy.Time.now() - now) > self.serial_timeout: + rospy.logwarn("Malformed packet! Ignoring tag version.") + self.ser.flushInput() + return None + version = self.ser.read(21) + data_ = struct.unpack(' self.serial_timeout: + rospy.logwarn("Malformed packet! Ignoring tag location.") + self.ser.flushInput() + return None + data_ = self.ser.read(21) + data_ = struct.unpack(' self.serial_timeout: + rospy.logwarn("Malformed packet! Ignoring anchors location.") + self.ser.flushInput() + return None + self.anchors.anchors = [] ## Clean Anchors list + for i in range(self.tag.n_anchors): + + data_ = self.ser.read(self.anchor_packet_size) + data_ = struct.unpack(' rospy.Duration(0.5): + rospy.logwarn("Could not get accel data!") + cc = cc.replace('\r\n', '').replace('acc: ', '').split(',') + if len(cc) == 3: + self.accel.x = float(int(cc[0].replace('x = ', ''))>>6) * 0.04 + self.accel.y = float(int(cc[1].replace('y = ', ''))>>6) * 0.04 + self.accel.z = float(int(cc[2].replace('z = ', ''))>>6) * 0.04 + self.accel.header.frame_id = 'tag' + self.accel.header.stamp = rospy.Time.now() + + + def get_tag_location(self): + self.ser.flushInput() + self.ser.write(b'lec\r') # Test Mode + while(self.ser.inWaiting() == 0): + pass + cc = '' + t = rospy.Time.now() + while not 'DIST' in cc: + cc = self.ser.readline() + print (cc) + if rospy.Time.now() - t > rospy.Duration(0.5): + rospy.logwarn("Could not get tag data!") + self.ser.flushInput() + self.ser.write(b'\r') # Test Mode + #cc = '' + #t = rospy.Time.now() + #while not 'acc' in cc: + # cc = self.ser.readline() + # if rospy.Time.now() - t > rospy.Duration(0.5): + # rospy.logwarn("Could not get accel data!") + #cc = cc.replace('\r\n', '').replace('acc: ', '').split(',') + #if len(cc) == 3: + # self.accel.x = float(int(cc[0].replace('x = ', ''))/64.0) * 0.04 + # self.accel.y = float(int(cc[1].replace('y = ', ''))/64.0) * 0.04 + # self.accel.z = float(int(cc[2].replace('z = ', ''))/64.0) * 0.04 + # self.accel.header.frame_id = 'tag' + # self.accel.header.stamp = rospy.Time.now() + + + def tf_callback(self, timer): + if self.tf_publisher_ == 'True': + self.br.sendTransform((self.tag.x, self.tag.y, self.tag.z), + tf.transformations.quaternion_from_euler(0, 0, 0), + rospy.Time.now(), + "tag", + "world") + for anchor in self.anchors.anchors: + self.br.sendTransform((anchor.x, anchor.y, anchor.z), + tf.transformations.quaternion_from_euler(0, 0, 0), + rospy.Time.now(), + anchor.header.frame_id, + "world") + + + def run(self): + self.rate = rospy.Rate(self.rate_) + rospy.loginfo("\33[96mInitiating Driver...\33[0m") + self.tag_pub_ = rospy.Publisher('pose', Tag, queue_size=1) + self.anchors_pub_ = rospy.Publisher('status', AnchorArray, queue_size=1) + self.acc_pub_ = rospy.Publisher('accelerometer', Acc, queue_size=1) + self.timer = rospy.Timer(rospy.Duration(0.2), self.tf_callback) + self.br = tf.TransformBroadcaster() + while not rospy.is_shutdown(): + self.get_tag_acc() + self.acc_pub_.publish(self.accel) + #self.get_tag_location() + #self.tag.header.stamp = rospy.Time.now() + #self.tag_pub_.publish(self.tag) + #self.anchors.header.stamp = rospy.Time.now() + #self.anchors_pub_.publish(self.anchors) + self.rate.sleep() + +# Main function +if __name__ == '__main__': + try: + dd = DecawaveDriver() + dd.run() + except rospy.ROSInterruptException: + rospy.loginfo("[Decawave Driver]: Closed!") +