Stm

.gitignore

@@ -0,0 +1,3 @@
+*.pyc
+*.idea
+*.swp

eurobot_camera/CMakeLists.txt → eurobot_stm/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 2.8.3)
-project(eurobot_camera)
+project(eurobot_stm)
 
 ## Compile as C++11, supported in ROS Kinetic and newer
 # add_compile_options(-std=c++11)
@@ -104,7 +104,7 @@ find_package(catkin REQUIRED COMPONENTS
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
 #  INCLUDE_DIRS include
-#  LIBRARIES eurobot_camera
+#  LIBRARIES eurobot_stm
 #  CATKIN_DEPENDS roscpp rospy std_msgs
 #  DEPENDS system_lib
 )
@@ -122,7 +122,7 @@ include_directories(
 
 ## Declare a C++ library
 # add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/eurobot_camera.cpp
+#   src/${PROJECT_NAME}/eurobot_stm.cpp
 # )
 
 ## Add cmake target dependencies of the library
@@ -133,7 +133,7 @@ include_directories(
 ## 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_camera_node.cpp)
+# 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
@@ -190,7 +190,7 @@ include_directories(
 #############
 
 ## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_eurobot_camera.cpp)
+# 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()

eurobot_stm/configs/stm.yaml

@@ -0,0 +1 @@
+ODOM_RATE: 40

eurobot_stm/launch/STM_node.launch

@@ -0,0 +1,9 @@
+<?xml version="1.0"?>
+<launch>
+  <rosparam file="$(find eurobot_stm)/configs/stm.yaml" command="load" ns="secondary_robot"/>
+  <group ns = "secondary_robot">
+	<node name="stm_node" pkg="eurobot_stm" type="STM.py" output="screen">
+	</node>
+  </group>
+</launch>
+

eurobot_camera/package.xml → eurobot_stm/package.xml

@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <package format="2">
-  <name>eurobot_camera</name>
+  <name>eurobot_stm</name>
   <version>0.0.0</version>
-  <description>The eurobot_camera package</description>
+  <description>The eurobot_stm package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->
   <!-- Example:  -->
@@ -19,7 +19,7 @@
   <!-- 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/eurobot_camera</url> -->
+  <!-- <url type="website">http://wiki.ros.org/eurobot_stm</url> -->
 
 
   <!-- Author tags are optional, multiple are allowed, one per tag -->

eurobot_stm/scripts/STM.py

@@ -0,0 +1,50 @@
+#!/usr/bin/env python
+import rospy
+from std_msgs.msg import String
+
+import itertools
+
+from STM_protocol import STMprotocol
+from odometry import Odometry
+from manipulator import Manipulator
+
+ODOM_RATE = rospy.get_param("ODOM_RATE")
+
+class STM():
+    def __init__(self, serial_port, baudrate=115200):
+        rospy.init_node('stm_node', anonymous=True)
+        rospy.Subscriber("stm_command", String, self.stm_command_callback)
+        self.response = rospy.Publisher("stm_response", String, queue_size=50)
+
+        self.stm_protocol = STMprotocol(serial_port, baudrate)
+        self.odometry = Odometry(self.stm_protocol, ODOM_RATE)
+
+
+    def stm_command_callback(self, data):
+        id, cmd, args = self.parse_data(data)
+        successfully, values = self.stm_protocol.send(cmd, args)
+        st = "".join(values)
+        response = str(id) + " " + st
+        print ("RESPONSE=", values)
+        self.response.publish(response)
+
+    def parse_data(self, data):
+        data_splitted = data.data.split()
+        print (data_splitted)
+        id = data_splitted[0]
+        try:
+            cmd = int(data_splitted[1])
+        except ValueError as e:
+            cmd = int(data_splitted[1], 16)
+        args_dict = {'c': str, 'B': int, 'f': float}
+        args = [args_dict[t](s) for t, s in itertools.izip(self.stm_protocol.pack_format[cmd][1:], data_splitted[2:])]
+        return id, cmd, args
+
+if __name__ == '__main__':
+
+
+    #TODO::search for ports
+
+    serial_port = "/dev/ttyUSB0"
+    stm = STM(serial_port)
+    rospy.spin()