From bad907be558123cca942901f5a4e8067a9dc6745 Mon Sep 17 00:00:00 2001 From: Federica Di Lauro Date: Tue, 25 Feb 2020 14:35:39 +0100 Subject: [PATCH] move serial transmission scripts to new package --- .../src/serial_bridge/CMakeLists.txt | 205 ++++++++++++++++++ utils/catkin_ws/src/serial_bridge/package.xml | 65 ++++++ .../scripts/otto_communication_pb2.py | 0 .../scripts/serial_receiver.py | 16 +- .../scripts/serial_transmitter.py} | 22 +- 5 files changed, 290 insertions(+), 18 deletions(-) create mode 100644 utils/catkin_ws/src/serial_bridge/CMakeLists.txt create mode 100644 utils/catkin_ws/src/serial_bridge/package.xml rename utils/catkin_ws/src/{joypad_bridge => serial_bridge}/scripts/otto_communication_pb2.py (100%) rename utils/catkin_ws/src/{joypad_bridge => serial_bridge}/scripts/serial_receiver.py (91%) rename utils/catkin_ws/src/{joypad_bridge/scripts/cmd_vel_transmitter.py => serial_bridge/scripts/serial_transmitter.py} (63%) diff --git a/utils/catkin_ws/src/serial_bridge/CMakeLists.txt b/utils/catkin_ws/src/serial_bridge/CMakeLists.txt new file mode 100644 index 0000000..0e5584b --- /dev/null +++ b/utils/catkin_ws/src/serial_bridge/CMakeLists.txt @@ -0,0 +1,205 @@ +cmake_minimum_required(VERSION 2.8.3) +project(serial_bridge) + +## 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 +) + +## 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 +# 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 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 serial_bridge +# CATKIN_DEPENDS rospy std_msgs +# DEPENDS system_lib +) + +########### +## 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}/serial_bridge.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/serial_bridge_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_serial_bridge.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/utils/catkin_ws/src/serial_bridge/package.xml b/utils/catkin_ws/src/serial_bridge/package.xml new file mode 100644 index 0000000..a52368b --- /dev/null +++ b/utils/catkin_ws/src/serial_bridge/package.xml @@ -0,0 +1,65 @@ + + + serial_bridge + 0.0.0 + The serial_bridge package + + + + + fdila + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + rospy + std_msgs + rospy + std_msgs + rospy + std_msgs + + + + + + + + diff --git a/utils/catkin_ws/src/joypad_bridge/scripts/otto_communication_pb2.py b/utils/catkin_ws/src/serial_bridge/scripts/otto_communication_pb2.py similarity index 100% rename from utils/catkin_ws/src/joypad_bridge/scripts/otto_communication_pb2.py rename to utils/catkin_ws/src/serial_bridge/scripts/otto_communication_pb2.py diff --git a/utils/catkin_ws/src/joypad_bridge/scripts/serial_receiver.py b/utils/catkin_ws/src/serial_bridge/scripts/serial_receiver.py similarity index 91% rename from utils/catkin_ws/src/joypad_bridge/scripts/serial_receiver.py rename to utils/catkin_ws/src/serial_bridge/scripts/serial_receiver.py index c9e46f4..0a4a659 100755 --- a/utils/catkin_ws/src/joypad_bridge/scripts/serial_receiver.py +++ b/utils/catkin_ws/src/serial_bridge/scripts/serial_receiver.py @@ -24,16 +24,17 @@ ser = serial.Serial( exclusive=False) def serial_receiver(): - while(ser.is_open == False): + rospy.init_node('serial_receiver', anonymous=True) + serial_port = rospy.get_param("serial_port") + while(ser.is_open == False and not rospy.is_shutdown()): try: - ser.port = '/dev/ttyUSB0' + ser.port = serial_port ser.open() except SerialException: - print('couldnt open /dev/ttyUSB0') + rospy.logerr('Couldn\'t open ' + serial_port) time.sleep(2) - print('ttyUSB0 opened') - rospy.init_node('serial_receiver', anonymous=True) + rospy.loginfo(serial_port + ' opened') odom_pub = rospy.Publisher('/odom', Odometry, queue_size=10) odom_broadcaster = tf.TransformBroadcaster() @@ -59,8 +60,9 @@ def serial_receiver(): try: otto_status.ParseFromString(encoded_buffer) - print(otto_status) + rospy.logdebug(otto_status) + # 3 = RUNNING if (otto_status.status == 3): lin_vel = otto_status.linear_velocity ang_vel = otto_status.angular_velocity @@ -106,7 +108,7 @@ def serial_receiver(): odom_pub.publish(odom) except DecodeError: - print("Decode Error") + rospy.logerr("Decode Error") ser.reset_input_buffer() diff --git a/utils/catkin_ws/src/joypad_bridge/scripts/cmd_vel_transmitter.py b/utils/catkin_ws/src/serial_bridge/scripts/serial_transmitter.py similarity index 63% rename from utils/catkin_ws/src/joypad_bridge/scripts/cmd_vel_transmitter.py rename to utils/catkin_ws/src/serial_bridge/scripts/serial_transmitter.py index 4724092..64df0d2 100755 --- a/utils/catkin_ws/src/joypad_bridge/scripts/cmd_vel_transmitter.py +++ b/utils/catkin_ws/src/serial_bridge/scripts/serial_transmitter.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -import rospy, serial, struct, time +import rospy, serial import otto_communication_pb2 from geometry_msgs.msg import Twist from serial import SerialException @@ -14,31 +14,31 @@ ser = serial.Serial( exclusive=None) def callback(data): - linear = -data.linear.x + linear = data.linear.x angular = -data.angular.z #da fixare? my_velocity = otto_communication_pb2.VelocityCommand() my_velocity.linear_velocity = linear; my_velocity.angular_velocity = angular; - rospy.loginfo('I heard %f %f', linear, angular) + rospy.logdebug('Cmd vel transmitted %f %f', linear, angular) out_buffer = my_velocity.SerializeToString() - print(len(out_buffer)) ser.write(out_buffer) ser.reset_output_buffer() - time.sleep(0.002) def listener(): - while(ser.is_open == False): + rospy.init_node('serial_transmitter', anonymous=True) + + serial_port = rospy.get_param("serial_port") + while(ser.is_open == False and not rospy.is_shutdown()): try: - ser.port = '/dev/ttyUSB0' + ser.port = serial_port ser.open() except SerialException: - print('couldnt open /dev/ttyUSB0') + rospy.logerr('Couldn\'t open ' + serial_port) time.sleep(2) - print('ttyUSB0 opened') - rospy.init_node('cmd_vel_transmitter', anonymous=True) + rospy.loginfo(serial_port + ' opened') - rospy.Subscriber('/cmd_vel', Twist, callback) + rospy.Subscriber('/cmd_vel_throttled', Twist, callback) rospy.spin() -- 2.52.0