Publish data to ROS topic

CMakeLists.txt

@@ -1,202 +1,49 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(decawave_ros)
 
-## 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)
-
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
+find_package( catkin REQUIRED COMPONENTS 
+              message_generation 
+              std_msgs 
+              geometry_msgs)
 
-
-## 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()
+##################
+## Run setup.py ##
+##################
+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
-#   Message1.msg
-#   Message2.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 messages in the 'msg' folder
+add_message_files(
+  DIRECTORY msg
+  FILES
+  uwb_distance.msg 
+)
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   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_messages(
+  DEPENDENCIES
+  std_msgs
+)
 
-## 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
+
+# Declare things to be passed to dependent projects
 catkin_package(
-#  INCLUDE_DIRS include
-#  LIBRARIES decawave_ros
-#  CATKIN_DEPENDS other_catkin_pkg
-#  DEPENDS system_lib
+  CATKIN_DEPENDS 
+  message_runtime
+  std_msgs
 )
 
 ###########
 ## 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}/decawave_ros.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/decawave_ros_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 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}
-# )
-
-## 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_gtest(${PROJECT_NAME}-test test/test_decawave_ros.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)
+# Specify additional locations of header files
+include_directories()

launch/read_uwb.launch

@@ -2,7 +2,7 @@
 
     <!-- Launch the DecaWave Driver node for T0 -->
     <node pkg="decawave_ros" type="DecawaveNode" name="DecawaveNode" output="screen">
-        <param name="port" value="/dev/ttyACM2"/>
+        <param name="port" value="/dev/ttyACM0"/>
         <param name="baud" value="9600" type="int"/>
     </node>
 

nodes/DecawaveNode

@@ -1,33 +1,69 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
 import rospy
 import serial
+from decawave_ros import DecaParser
+from decawave_ros.msg import uwb_distance
 
 class DecawaveNode:
     def __init__(self):
-        rospy.init_node('decawave_driver')
+        rospy.init_node('Decawave')
+        self.port = serial.Serial()
+        self.deca_pub = rospy.Publisher('Decawave', uwb_distance, queue_size=10)
+
         try:
-            self.ser = serial.Serial(
-                port        = rospy.get_param('~port','/dev/ttyACM0s'),
+            self.port = serial.Serial(
+                port        = rospy.get_param('~port','/dev/ttyACM0'),
                 baudrate    = rospy.get_param('~baud',9600),
                 timeout     = 10
             )
         except serial.SerialException:
             print("Serial Port Connection Failure")
 
     def run(self):
-        try:
-            while(not rospy.is_shutdown()):
-                data = self.ser.readline().split()
-                if (data[0] == 'mc' and int(data[1],16) & 0x01):
-                    uwbID = str(data[9][1])
-                    dist = int(data[2],16)/1000.0
-                    print("Tag: " + uwbID + "    Range: " + str(dist))
 
-        except serial.SerialException:
-            self.ser.close()
+        parser = DecaParser(self.port)
+        uwb_msg = uwb_distance()
+
+        while( not rospy.is_shutdown() ):
+            try:
+                msg = parser.receive()
+                if(msg):
+                    if(not msg.r0 == 0):
+                        uwb_msg.anc = 0
+                        uwb_msg.tag = msg.tag
+                        uwb_msg.dist = msg.r0
+                        self.deca_pub.publish(uwb_msg)
+
+                    if(not msg.r1 == 0):
+                        uwb_msg.anc = 1
+                        uwb_msg.tag = msg.tag
+                        uwb_msg.dist = msg.r1
+                        self.deca_pub.publish(uwb_msg)
+
+                    if(not msg.r2 == 0):
+                        uwb_msg.anc = 2
+                        uwb_msg.tag = msg.tag
+                        uwb_msg.dist = msg.r2
+                        self.deca_pub.publish(uwb_msg)
+
+                    if(not msg.r3 == 0):
+                        uwb_msg.anc = 3
+                        uwb_msg.tag = msg.tag
+                        uwb_msg.dist = msg.r3
+                        self.deca_pub.publish(uwb_msg)
+
+
+            except (ValueError, IOError) as err:
+                print(err)
+
+
+        if(self.port.isOpen() == True):
+            self.port.close()
+
+
 
 
 if __name__ == "__main__":
     dw = DecawaveNode()
-    dw.run()
+    dw.run()

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>decawave_ros</name>
-  <version>1.0.0</version>
+  <version>0.1.0</version>
   <description>The decawave_ros package</description>
 
   <maintainer email="[email protected]">Jacob Hartzer</maintainer>

setup.py

@@ -9,15 +9,7 @@ def readme():
         return f.read()
 
 setup_args = generate_distutils_setup(
-    name='decawave_ros',
-    version='0.1.0',
-    description='A ROS package for interfacing with the Decawave TREK1000 UWB modules',
-    long_description=readme(),
-    url='https://github.com/unmannedlab/decawave_ros.git',
-    author='Jacob Hartzer',
-    author_email='[email protected]',
-    license='MIT',
-    packages='decawave_ros',
+    packages=['decawave_ros'],
     package_dir={'' : 'src'},
     )