From f5bbce398e6a6422e67ce11fbc34f0d43cb30889 Mon Sep 17 00:00:00 2001 From: alexey-kashapov Date: Mon, 19 Nov 2018 18:52:15 +0300 Subject: [PATCH] create stm_node --- eurobot_stm/CMakeLists.txt | 199 ++++++++++++++++++++++++++++ eurobot_stm/package.xml | 68 ++++++++++ eurobot_stm/scripts/STM_node.py | 65 +++++++++ eurobot_stm/scripts/STM_protocol.py | 119 +++++++++++++++++ 4 files changed, 451 insertions(+) create mode 100644 eurobot_stm/CMakeLists.txt create mode 100644 eurobot_stm/package.xml create mode 100755 eurobot_stm/scripts/STM_node.py create mode 100755 eurobot_stm/scripts/STM_protocol.py diff --git a/eurobot_stm/CMakeLists.txt b/eurobot_stm/CMakeLists.txt new file mode 100644 index 0000000..efcd9ac --- /dev/null +++ b/eurobot_stm/CMakeLists.txt @@ -0,0 +1,199 @@ +cmake_minimum_required(VERSION 2.8.3) +project(eurobot_stm) + +## 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 + roscpp + 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 eurobot_stm +# CATKIN_DEPENDS roscpp 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}/eurobot_stm.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/eurobot_stm_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_eurobot_stm.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/eurobot_stm/package.xml b/eurobot_stm/package.xml new file mode 100644 index 0000000..69922ee --- /dev/null +++ b/eurobot_stm/package.xml @@ -0,0 +1,68 @@ + + + eurobot_stm + 0.0.0 + The eurobot_stm package + + + + + alexey + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + + + + + + + + diff --git a/eurobot_stm/scripts/STM_node.py b/eurobot_stm/scripts/STM_node.py new file mode 100755 index 0000000..433b23e --- /dev/null +++ b/eurobot_stm/scripts/STM_node.py @@ -0,0 +1,65 @@ +#!/usr/bin/env python +import rospy +from std_msgs.msg import String +from threading import Lock +from geometry_msgs.msg import Twist, TransformStamped +from std_msgs.msg import Int32MultiArray +import numpy as np +import time +import struct +import serial +from STM_protocol import STMprotocol + +class STMnode(STMprotocol): + def __init__(self, serial_port, baudrate=115200): + # Init ROS + rospy.init_node('stm_command_node', anonymous=True) + # ROS subscribers + rospy.Subscriber("stm_command", String, self.stm_command_callback) + + super(STMnode, self).__init__(serial_port, baudrate) + self.mutex = Lock() + + def stm_command_callback(self, data): + cmd, args = self.parse_data(data) + successfully = self.send(cmd, args) + + def parse_data(self, data): + data_splitted = data.data.split() + cmd = int(data_splitted[0]) + args = [float(num) for num in data_splitted[1:]] + return cmd, args + + def send(self, cmd, args): + self.mutex.acquire() + successfully, values = self.send_command(cmd, args) + self.mutex.release() + rospy.loginfo('Got response args: ' + str(values)) + return successfully + + def square_test(self): + self.pure_send_command(8, (0.2,0,0)) + time.sleep(3) + self.pure_send_command(8, (0,0.2,0)) + time.sleep(3) + self.pure_send_command(8, (-0.2,0,0)) + time.sleep(3) + self.pure_send_command(8, (0,-0.2,0)) + time.sleep(3) + self.pure_send_command(8, (0,0,0)) + + def triangle(self): + #self.pure_send_command(8, (0,0,3.14)) + #time.sleep(1) + self.pure_send_command(8, (0,0.8,0)) + time.sleep(3) + self.pure_send_command(8, (0.8,0,0)) + time.sleep(3) + self.pure_send_command(8,(-0.8,-0.8,0)) + time.sleep(3) + self.pure_send_command(8, (0,0,0)) + +if __name__ == '__main__': + serial_port = "/dev/ttyUSB0" + stm = STMnode(serial_port) + rospy.spin() diff --git a/eurobot_stm/scripts/STM_protocol.py b/eurobot_stm/scripts/STM_protocol.py new file mode 100755 index 0000000..a318129 --- /dev/null +++ b/eurobot_stm/scripts/STM_protocol.py @@ -0,0 +1,119 @@ +#/usr/bin/env python + +import rospy +import serial +import struct +import time +import datetime + +class STMprotocol(object): + def __init__(self,serial_port, baudrate=115200): + self.ser = serial.Serial(serial_port,baudrate=baudrate, timeout = 0.01) + self.pack_format = { + 0x01: "=BBBB", + 0x03: "=Bf", + 0x04: "=B", + 0x05: "=B", + 0x06: "=Bffff", + 0x08: "=fff", + 0x09: "=", + 0x0a: "=", + 0x0b: "=BH", + 0x0c: "=B", + 0x0d: "=B", + 0xa0: "=", + 0xa1: "=B", + 0xb0: "=B", + 0xb1: "=B", + 0xb2: "=BB", + 0xb3: "=B", + 0xb4: "=B", + 0xb5: "=B", + 0xb6: "=B", + 0x0e: "=fff", + 0x0f: "=", + 0xa2: "=ffffff", + 0xc0: "=", + 0xc1: "=B", + 0xc2: "=B", + 0xc3: "=B", + 0xc4: "=BB", + 0xd0: "=", + 0xd1: "=", + 0xa3: "=", + 0xe0: "=B", + 256: "=H", + 0xa4: "", + 0xb7: "=B" + } + + self.unpack_format = { + 0x01: "=BBBB", + 0x03: "=BB", + 0x04: "=BB", + 0x05: "=BB", + 0x06: "=BB", + 0x08: "cc", + 0x09: "=fff", + 0x0a: "=fff", + 0x0b: "=BB", + 0x0c: "=f", + 0x0d: "=BB", + 0xa0: "=B", + 0xa1: "=B", + 0xb0: "=BB", + 0xb1: "=BB", + 0xb2: "=BB", + 0xb3: "=BB", + 0xb4: "=BB", + 0xb5: "=BB", + 0xb6: "=BB", + 0x0e: "=BB", + 0x0f: "=fff", + 0xa2: "=BB", + 0xc0: "=B", + 0xc1: "=BB", + 0xc2: "=BB", + 0xc3: "=BB", + 0xc4: "=BB", + 0xd0: "=BBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBB", + 0xd1: "=" + "B" * 16, + # 0xd0: "=BBBBBB", + 0xa3: "=B", + 0xe0: "=BB", + 0xa4: "=BB", + 0xb7: "=BB" + } + + def pure_send_command(self, cmd, args): + # Clear buffer + self.ser.reset_output_buffer() + self.ser.reset_input_buffer() + # Sending command + parameters = bytearray(struct.pack(self.pack_format[cmd], *args)) + print ('cmd=', cmd) + msg = bytearray([cmd]) + parameters + print ('msg = ' + str(struct.unpack('=c' + self.pack_format[cmd][1:],msg))) + self.ser.write(msg) + + response = self.ser.readline() + print(response) + if len(response) == 0: + raise Exception("No data received") + values = struct.unpack(self.unpack_format[cmd], response) + + return True, values + + + def send_command(self, cmd, args, n_repeats=5): + # print (cmd, args) + for i in range(n_repeats): + try: + return self.pure_send_command(cmd, args) + except Exception as exc: + if i == n_repeats - 1: + rospy.logerr('Exception:\t %s', str(exc)) + rospy.logerr('At time:\t %s', str(datetime.datetime.now())) + rospy.logerr('cmd: %s args: %s',str(cmd), str(args)) + print('--------------------------') + return False, None