L1.5/rosaria
6
0
Fork 0
Code Issues 1 Pull Requests Projects Releases Wiki Activity

added ros_decawave

Browse Source
This commit is contained in:
Jakub Delicat 2023-01-20 15:10:40 +01:00
parent 303eb96495
commit 47eb5e389a
12 changed files with 1124 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

203
radio-lap/src/ros_decawave/CMakeLists.txt Normal file
View File

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

201
radio-lap/src/ros_decawave/LICENSE Normal file
View File

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

49
radio-lap/src/ros_decawave/README.md Normal file
View File

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

171
radio-lap/src/ros_decawave/config/config.rviz Normal file
View File

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

13
radio-lap/src/ros_decawave/launch/decawave_driver.launch Normal file
View File

@ -0,0 +1,13 @@
<launch>
<arg name="pionier_id" default="0"/>
<group ns="/PIONIER$(arg pionier_id)/decawave">
<node pkg="ros_decawave" type="decawave_driver.py" name="decawave_driver" output="screen">
<param name="port" value="/dev/decawave"/>
<param name="baudrate" value="115200"/>
<param name="tf_publisher" value="True"/>
<param name="tf_reference" value="decawave_global_frame"/>
<param name="tag_name" value="/PIONIER$(arg pionier_id)/tag"/>
<param name="rate" value="10"/>
</node>
</group>
</launch>

7
radio-lap/src/ros_decawave/msg/Acc.msg Normal file
View File

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

10
radio-lap/src/ros_decawave/msg/Anchor.msg Normal file
View File

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

4
radio-lap/src/ros_decawave/msg/AnchorArray.msg Normal file
View File

@ -0,0 +1,4 @@
# An array of anchors
Header header
Anchor[] anchors

10
radio-lap/src/ros_decawave/msg/Tag.msg Normal file
View File

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

69
radio-lap/src/ros_decawave/package.xml Normal file
View File

@ -0,0 +1,69 @@
<?xml version="1.0"?>
<package format="2">
<name>ros_decawave</name>
<version>0.0.0</version>
<description>The ros_decawave package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<maintainer email="rezeck@dcc.ufmg.br">rezeck</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/ros_decawave</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>message_generation</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>tf</build_depend>
<build_export_depend>message_generation</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>tf</build_export_depend>
<exec_depend>message_runtime</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>tf</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

192
radio-lap/src/ros_decawave/script/decawave_driver.py Executable file
View File

@ -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('<BBBBBLBBLBBL', bytearray(version))
rospy.loginfo("\33[96m--------------------------------\33[0m")
rospy.loginfo("\33[96mFirmware Version:0x"+format(data_[5], '04X')+"\33[0m")
rospy.loginfo("\33[96mConfiguration Version:0x"+format(data_[8], '04X')+"\33[0m")
rospy.loginfo("\33[96mHardware Version:0x"+format(data_[11], '04X')+"\33[0m")
rospy.loginfo("\33[96m--------------------------------\33[0m")
def get_tag_acc(self):
# Acc is not implemented on Generic Mode
self.ser.flushInput()
self.ser.write(b'\x19\x33\x04') # Status
while(self.ser.inWaiting() == 0):
pass
data_ = self.ser.readline()
print ("%s", data_)
def get_tag_status(self):
self.ser.flushInput()
self.ser.write(b'\x32\x00') # Status
while(self.ser.inWaiting()==0):
pass
status = self.ser.readline()
data_ = struct.unpack('<BBBBBB', bytearray(status))
if data_[0] != 64 and data_[2] != 0:
rospy.logwarn("Get Status Failed! Packet does not match!")
print("%s", data_)
if data_[5] == 3:
rospy.loginfo("\33[96mTag is CONNECTED to a UWB network and LOCATION data are READY!\33[0m")
elif data_[5] == 2:
rospy.logwarn("Tag is CONNECTED to a UWB network but LOCATION data are NOT READY!")
elif data_[5] == 1:
rospy.logwarn("Tag is NOT CONNECTED to a UWB network but LOCATION data are READY!")
elif data_[5] == 0:
rospy.logwarn("Tag is NOT CONNECTED to a UWB network and LOCATION data are NOT READY!")
def get_tag_location(self):
self.ser.flushInput()
self.ser.write(b'\x0c\x00')
now = rospy.Time.now()
while (self.ser.inWaiting() < 21):
if (rospy.Time.now() - now) > self.serial_timeout:
rospy.logwarn("Malformed packet! Ignoring tag location.")
self.ser.flushInput()
return None
data_ = self.ser.read(21)
data_ = struct.unpack('<BBBBBlllBBBB', bytearray(data_))
self.tag.x = float(data_[5])/1000.0
self.tag.y = float(data_[6])/1000.0
self.tag.z = float(data_[7])/1000.0
self.tag.qf = float(data_[8])/100.0
self.tag.n_anchors = int(data_[11])
self.tag.header.frame_id = self.tag_name_
self.anchor_packet_size = 20 ## Size of anchor packet in bytes
now = rospy.Time.now()
while (self.ser.inWaiting() < self.anchor_packet_size * self.tag.n_anchors):
if (rospy.Time.now() - now) > 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('<HlBlllB', bytearray(data_))
a = Anchor()
a.header.frame_id = str(format(data_[0], '04X'))
a.header.stamp = rospy.Time.now()
a.distance = float(data_[1])/1000.0
a.dist_qf = float(data_[2])/100.0
a.x = float(data_[3])/1000.0
a.y = float(data_[4])/1000.0
a.z = float(data_[5])/1000.0
a.qf = float(data_[6])/100.0
self.anchors.anchors.append(a)
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(),
self.tag_name_,
self.tf_reference_)
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,
self.tf_reference_)
def run(self):
self.rate = rospy.Rate(self.rate_)
rospy.loginfo("\33[96mInitiating Driver...\33[0m")
self.tag_pub_ = rospy.Publisher('tag_pose', Tag, queue_size=1)
self.anchors_pub_ = rospy.Publisher('tag_status', AnchorArray, queue_size=1)
self.timer = rospy.Timer(rospy.Duration(0.1), self.tf_callback)
self.br = tf.TransformBroadcaster()
while not rospy.is_shutdown():
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!")

195
radio-lap/src/ros_decawave/script/decawave_driver_shell.py Executable file
View File

@ -0,0 +1,195 @@
#!/usr/bin/env python
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_ = rospy.get_param('tf_publisher', 'True')
self.rate_ = int(rospy.get_param('rate', '10'))
# 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()
self.accel = Acc()
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.flushInput()
self.ser.write(b'\r') # Test Mode
time.sleep(0.1)
while(self.ser.inWaiting() == 0):
pass
cc = self.ser.readline()
if cc == '@\x01\x01': # GENERIC MODE
rospy.loginfo("\33[96mDevice is on GENERIC MODE! It must to be changed to SHELL MODE!\33[0m")
self.mode_ = "GENERIC"
else: # SHELL MODE
rospy.loginfo("\33[96mDevice is on SHELL MODE! Ok!\33[0m")
self.mode_ = "SHELL"
return self.mode_
def switch_uart_mode(self):
self.ser.flushInput()
if self.mode_ == "GENERIC":
rospy.loginfo("\33[96mChanging UART mode to SHELL MODE...\33[0m")
self.ser.write(b'\r\r') # Go to Shell Mode
while(self.ser.inWaiting()==0):
pass
time.sleep(1.0)
self.ser.flushInput()
rospy.loginfo("\33[96m%s\33[0m", self.ser.readline().replace('\n', ''))
elif self.mode_ == "UNKNOWN":
rospy.logerr("%s", "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
while(self.ser.inWaiting() < 21):
pass
version = self.ser.read(21)
data_ = struct.unpack('<BBBBBLBBLBBL', bytearray(version))
rospy.loginfo("\33[96m--------------------------------\33[0m")
rospy.loginfo("\33[96mFirmware Version:0x"+format(data_[5], '04X')+"\33[0m")
rospy.loginfo("\33[96mConfiguration Version:0x"+format(data_[8], '04X')+"\33[0m")
rospy.loginfo("\33[96mHardware Version:0x"+format(data_[11], '04X')+"\33[0m")
rospy.loginfo("\33[96m--------------------------------\33[0m")
#def get_tag_status(self):
# self.ser.flushInput()
# self.ser.write(b'\x32\x00') # Status
# while(self.ser.inWaiting()==0):
# pass
# status = self.ser.readline()
# data_ = struct.unpack('<BBBBBB', bytearray(status))
# if data_[0] != 64 and data_[2] != 0:
# rospy.logwarn("Get Status Failed! Packet does not match!")
# print("%s", data_)
# if data_[5] == 3:
# rospy.loginfo("\33[96mTag is CONNECTED to a UWB network and LOCATION data are READY!\33[0m")
# elif data_[5] == 2:
# rospy.logwarn("Tag is CONNECTED to a UWB network but LOCATION data are NOT READY!")
# elif data_[5] == 1:
# rospy.logwarn("Tag is NOT CONNECTED to a UWB network but LOCATION data are READY!")
# elif data_[5] == 0:
# rospy.logwarn("Tag is NOT CONNECTED to a UWB network and LOCATION data are NOT READY!")
def get_tag_acc(self):
""" Read Acc Value: The values are raw values. So to convert them to g you first have to divide the
values by 2^6 ( as it is shifted) and then multiply it into 0.004 (assuming you are using the
+-2g scale). With regards to the getting the accelerometer readings to the UART, I have written
specific functions to read the data . I could put the github link up if you want."""
self.ser.flushInput()
self.ser.write(b'av\r') # Test Mode
while(self.ser.inWaiting() == 0):
pass
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 = ', ''))>>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!")