将旧仓库代码迁移至新代码仓库中

README.md

@@ -1,3 +1,19 @@
 # ros_control_handle
-
-该仓库中代码是为了DIY的ROS控制手柄所开发的代码
+---------------------------------------------------------------------------------
+## Copyright: www.corvin.cn ROS小课堂 2016-2018
+---------------------------------------------------------------------------------
+## Author: corvin
+---------------------------------------------------------------------------------
+## Description:
+-    该代码仓库是为DIY的控制手柄准备的，包含arduino部分的代码和ros驱动部分的代码.
+-
+---------------------------------------------------------------------------------
+## Usage:
+-  0:首先需要根据代码提示将arduino部分代码烧写进arduino板中。
+-  1:然后启动ROS部分的驱动代码，这样就可以来解析arduino的输出数据，将其组装成
+-    geometry_msgs::Twist消息格式，然后将其发送到指定的控制移动的cmd_vel话题中.
+-
+---------------------------------------------------------------------------------
+## History:
+-  20181024: init this README.txt.
+---------------------------------------------------------------------------------

arduino_code/arduino_handle/arduino_handle.ino

@@ -0,0 +1,69 @@
+/********************************************************************
+   Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+   Author: corvin
+   Description:
+     ROS控制手柄的arduino部分代码,主要就是获取各传感器信息.将其组装好后通过串口
+     发送给上位机部分的ROS python代码来解析.
+   History:
+     20181024:initial this code.
+     20181030:修改为发送各模块的原始数据到上位机,数据解析处理在上位机进行,尽可能
+         保持该固件代码结构简单,易于调试维护.
+*********************************************************************/
+
+#define BAUD_RATE     57600
+#define ROTATE_RANGE  100
+#define SEND_RATE     10
+
+/*
+   define sensors pin
+*/
+int self_lock_pin = 11;
+int handle_x_pin  = A2;
+int handle_y_pin  = A1;
+int handle_z_pin  = 10;
+int rotation_pin  = A0;
+
+
+void setup() {
+  pinMode(self_lock_pin, INPUT);
+  pinMode(rotation_pin,  INPUT);
+  pinMode(handle_z_pin,  INPUT_PULLUP);
+
+  Serial.begin(BAUD_RATE);
+}
+
+void loop() {
+  int lock_status    = digitalRead(self_lock_pin);
+  int rotation_value = analogRead(rotation_pin);
+  int rotate_percent = map(rotation_value, 1023, 0, 0, ROTATE_RANGE);
+
+  //turn left or right
+  int handle_x = analogRead(handle_x_pin);
+
+  //move forward or back
+  int handle_y = analogRead(handle_y_pin);
+
+  //click handle
+  int handle_z = digitalRead(handle_z_pin);
+
+  //output all value by serial port
+  outPutData(lock_status, handle_x, handle_y, handle_z, rotate_percent);
+  delay(1000 / SEND_RATE);
+}
+
+/*
+   formate output data
+*/
+void outPutData(int lock_status, int handle_x, int handle_y, int handle_z, int rotate_percent)
+{
+  Serial.print(lock_status, DEC);
+  Serial.print(" ");
+  Serial.print(handle_x, DEC);
+  Serial.print(" ");
+  Serial.print(handle_y, DEC);
+  Serial.print(" ");
+  Serial.print(handle_z, DEC);
+  Serial.print(" ");
+  Serial.println(rotate_percent, DEC);
+}
+

ros_code/.catkin_workspace

@@ -0,0 +1 @@
+# This file currently only serves to mark the location of a catkin workspace for tool integration

ros_code/src/CMakeLists.txt

@@ -0,0 +1 @@
+/opt/ros/indigo/share/catkin/cmake/toplevel.cmake

ros_code/src/ros_handle_python/CMakeLists.txt

@@ -0,0 +1,200 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ros_handle_python)
+
+## 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
+  geometry_msgs
+  rospy
+  dynamic_reconfigure
+)
+
+## 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
+#   geometry_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/handle_params.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_handle_python
+#  CATKIN_DEPENDS geometry_msgs rospy
+#  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}/ros_handle_python.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}
+#    ${PROJECT_NAME}_gencfg)
+
+
+## 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_handle_python_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_handle_python.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)

ros_code/src/ros_handle_python/cfg/handle_params.cfg

@@ -0,0 +1,23 @@
+#!/usr/bin/env python
+# _*_ coding:utf-8 _*_
+
+"""
+ Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+ Author: corvin
+ Description:
+    实现dynamic_reconfigure功能的需要的配置文件源码.
+ History:
+    20181116: initial this comment.
+"""
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+PACKAGE = "ros_handle_python"
+gen = ParameterGenerator()
+
+gen.add("cmd_topic_name",  str_t,    0, "set cmd topic name",  "/turtle1/cmd_vel")
+gen.add("cmd_public_rate", int_t,    0, "send cmd rate",       20, 0, 20)
+gen.add("angular_z",       double_t, 0, "set angular_z param", 1.0, 0, 2.0)
+gen.add("linear_x",        double_t, 0, "set linear_x param",  1.0, 0, 2.0)
+
+exit(gen.generate(PACKAGE, "handle_dynamic_reconfig", "handle"))

ros_code/src/ros_handle_python/cfg/handle_params.yaml

@@ -0,0 +1,26 @@
+# Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+# Author: corvin
+# Description:
+#   该文件为手柄的配置参数,包括:端口号,波特率,话题发布频率等.
+#   下面来依次介绍下各参数的意义:
+#   port:arduino板的连接端口号.
+#   baud:连接arduino的波特率,这个是在arduino代码中设置的.
+#   timeout:读取数据时超时时间.
+#   public_rate:发布话题的频率.
+#   cmd_topic:发布控制移动的话题名称.
+#   angular_z:发送角速度的最大值.
+#   linear_x:发送线速度的最大值.
+#   send_cmd_delay:话题中命令执行的延迟,最小需要设置为1,不能为0.
+#       最大无上限,一般不超过10.这里默认设置为1,即以最快速度执行话题中命令.
+# History:
+#   20181024: initial this config file.
+
+port: /dev/ttyACM0
+baud: 57600
+timeout: 0.5
+public_rate: 20
+cmd_topic: /turtle1/cmd_vel
+angular_z: 1.0
+linear_x: 1.0
+cmd_delay: 1
+

ros_code/src/ros_handle_python/launch/ros_handle.launch

@@ -0,0 +1,19 @@
+<!--
+  Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+  Author: corvin
+  Description:
+    启动ROS控制手柄的launch文件，用来解析下位机arduino的各传感器数据，然后组装
+    并发布控制命令.
+  History:
+    20181024: init this launch file.
+    20181105: add dynamic server node.
+-->
+<launch>
+  <!--startup ros control handle dynamic server node -->
+  <node name="handle_dynamic_reconfig" pkg="ros_handle_python" type="dynamic_server.py" output="screen" />
+
+  <!-- startup contorl handle client node -->
+  <node name="handle_python_node" pkg="ros_handle_python" type="handle_node.py" output="screen">
+    <rosparam file="$(find ros_handle_python)/cfg/handle_params.yaml" command="load"/>
+  </node>
+</launch>

ros_code/src/ros_handle_python/package.xml

@@ -0,0 +1,67 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ros_handle_python</name>
+  <version>0.0.0</version>
+  <description>The ros_handle_python package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="corvin@todo.todo">corvin</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_handle_python</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>geometry_msgs</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
+  <build_export_depend>geometry_msgs</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>dynamic_reconfigure</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

ros_code/src/ros_handle_python/src/arduino_driver.py

@@ -0,0 +1,91 @@
+#!/usr/bin/env python
+# _*_ coding:utf-8 _*_
+
+"""
+ Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+ Author: corvin
+ Description:
+    A Python driver for the Arduino microcontroller
+ History:
+    20181024: initial this source code file.
+"""
+
+import os
+import time
+import sys, traceback
+from serial.serialutil import SerialException
+from serial import Serial
+
+
+class Arduino:
+    ''' Configuration Arduino Board Parameters
+    '''
+    def __init__(self, port="/dev/ttyACM0", baudrate=57600, timeout=0.5):
+        self.port     = port
+        self.baudrate = baudrate
+        self.timeout  = timeout
+        self.interCharTimeout = timeout/30.
+
+    # connect arduino board
+    def connect(self):
+        try:
+            self.port = Serial(port=self.port, baudrate=self.baudrate, timeout=self.timeout)
+            # give the firmware time to wake up.
+            time.sleep(1)
+        except SerialException:
+            print "Serial Exception:"
+            print sys.exc_info()
+            print "Traceback follows:"
+            traceback.print_exc(file=sys.stdout)
+            print "Cannot connect to Arduino!"
+            os._exit(2)
+
+    def close(self):
+        ''' Close the serial port.
+        '''
+        self.port.close()
+
+    def recv(self, timeout=0.5):
+        timeout = min(timeout, self.timeout)
+        ''' This command should not be used on its own: it is called by the execute commands
+            below in a thread safe manner.  Note: we use read() instead of readline() since
+            readline() tends to return garbage characters from the Arduino
+        '''
+        c = ''
+        value = ''
+        attempts = 0
+        while c != '\r':
+            c = self.port.read(1)
+            value += c
+            attempts += 1
+            if attempts * self.interCharTimeout > timeout:
+                return None
+
+        value = value.strip('\r')
+        return value
+
+    def recv_int_array(self):
+        try:
+            values = self.recv(self.timeout).split()
+            return map(int, values)
+        except:
+            return []
+
+""" Basic test for connectivity """
+if __name__ == "__main__":
+    portName = "/dev/ttyACM0"
+    baudRate = 57600
+
+    myArduino = Arduino(port=portName, baudrate=baudRate, timeout=0.5)
+    print "Ready to connect arduino serial port..."
+    myArduino.connect()
+
+    print "Ready to read data ..."
+    time.sleep(1)
+    Data = []
+    Data = myArduino.recv_int_array()
+    print Data
+
+    myArduino.close()
+    print "Now disconnect arduino serial port..."
+

ros_code/src/ros_handle_python/src/dynamic_server.py

@@ -0,0 +1,26 @@
+#!/usr/bin/env python
+# _*_ coding:utf-8 _*_
+
+'''
+ Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+ Author: corvin
+ Description:
+    用于手柄可以使用dynamic_reconfigure,这样就可以动态的调整手柄的参数.
+ History:
+    20181115: initial this comment.
+'''
+
+import rospy
+
+from dynamic_reconfigure.server import Server
+from ros_handle_python.cfg import handleConfig
+
+def callback(config, level):
+    rospy.loginfo("Dynamic_reconfig_server:topic_name:{cmd_topic_name},public_rate:{cmd_public_rate},angular_z:{angular_z},linear_x:{linear_x}".format(**config))
+    return config
+
+if __name__ == "__main__":
+    rospy.init_node("handle_dynamic_reconfig", anonymous=True)
+
+    srv = Server(handleConfig, callback)
+    rospy.spin()

ros_code/src/ros_handle_python/src/handle_node.py

@@ -0,0 +1,156 @@
+#!/usr/bin/env python
+# _*_ coding:utf-8 _*_
+
+'''
+ Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+ Author: corvin
+ Description:
+    ROS解析代码,用来通过串口来获取从arduino发来的数据.然后组装成控制移动的twist命令,
+    通过话题发布出来.
+ History:
+    20181024: initial this code.
+'''
+
+import os
+import rospy
+import dynamic_reconfigure.client
+from arduino_driver import Arduino
+from geometry_msgs.msg import Twist
+from serial.serialutil import SerialException
+
+
+class HandleROS():
+    def __init__(self):
+        rospy.init_node('ros_handle_node', log_level=rospy.INFO)
+        client = dynamic_reconfigure.client.Client("handle_dynamic_reconfig",timeout=30,config_callback=self.callback)
+
+        # Get the name in case it is set in the launch file
+        self.name = rospy.get_name()
+        rospy.on_shutdown(self.shutdown)
+
+        self.getConfigParam()
+
+        # Connect arduino board
+        self.controller = Arduino(self.port, self.baud, self.timeout)
+        self.controller.connect()
+        rospy.loginfo("Connected to handle on port " + self.port + " at " +
+                      str(self.baud) + " baud!" +" cmd_topic name:" + self.cmd_topic)
+
+        while not rospy.is_shutdown():
+            dynamic_param  = client.get_configuration(timeout=3)
+            self.dynamicReconfig(dynamic_param)
+
+            self.cmd_vel_pub = rospy.Publisher(self.cmd_topic, Twist, queue_size=self.cmd_delay)
+            loop_rate = rospy.Rate(self.rate)
+
+            Data = []
+            Data = self.controller.recv_int_array()
+            if len(Data) == 5:
+                map(float, Data)
+                #rospy.loginfo("lock_status:" + str(Data[0])+" handle_x:" + str(Data[1]) + " handle_y:"+str(Data[2])
+                #              + " handle_z:"+str(Data[3])+" rotate_percent:"+str(Data[4]))
+
+                if (Data[0] == 1):
+                  linear_x, angular_z = self.parseSerialData(Data[1], Data[2], Data[3], Data[4])
+                  self.sendCmdTopic(linear_x, angular_z)
+
+            loop_rate.sleep()
+
+
+    def dynamicReconfig(self, paramList):
+        self.cmd_topic = paramList['cmd_topic_name']
+        self.rate = paramList['cmd_public_rate']
+        self.angular_z = paramList['angular_z']
+        self.linear_x  = paramList['linear_x']
+
+
+    def callback(self, config):
+        rospy.loginfo("{cmd_topic_name},{cmd_public_rate},{angular_z},{linear_x}".format(**config))
+
+
+    def parseSerialData(self, handle_x, handle_y, handle_z, rotate_percent):
+        if handle_z == 1:
+            linear_x = self.linear_x
+            linear_x *= (rotate_percent/100.0)
+            self.cmd_linear_x = linear_x
+        else:
+            angular_z = self.angular_z
+            angular_z *= (rotate_percent/100.0)
+            self.cmd_angular_z = angular_z
+
+        # config angular_z
+        angular_z = self.cmd_angular_z
+        if handle_x > 700:
+            angular_z *= -1
+        elif handle_x < 300:
+            angular_z *= 1
+        else:
+            angular_z = 0.0
+
+        # config linear x
+        linear_x = self.cmd_linear_x
+        if handle_y > 700:
+            linear_x *= 1
+        elif handle_y < 300:
+            linear_x *= -1
+        else:
+            linear_x = 0.0
+
+        #rospy.loginfo("Send linear_x: " + str(linear_x)+" angular_z:"+str(angular_z))
+        return linear_x, angular_z
+
+
+    def sendCmdTopic(self, linear_x, angular_z):
+        '''Send cmd to topic
+        '''
+        cmd_msg = Twist()
+        cmd_msg.linear.x = linear_x
+        cmd_msg.linear.y = 0
+        cmd_msg.linear.z = 0
+
+        cmd_msg.angular.x = 0
+        cmd_msg.angular.y = 0
+        cmd_msg.angular.z = angular_z
+        self.cmd_vel_pub.publish(cmd_msg)
+
+
+    def getConfigParam(self):
+        '''Get parameter from config file
+        '''
+        self.port = rospy.get_param("~port", "/dev/ttyACM0")
+        self.baud = int(rospy.get_param("~baud", 57600))
+        self.timeout = rospy.get_param("~timeout", 0.7)
+        self.cmd_topic = rospy.get_param("~cmd_topic", "/cmd_vel")
+        self.angular_z = rospy.get_param("~angular_z", 1.0)
+        self.linear_x  = rospy.get_param("~linear_x",  1.0)
+        self.rate = int(rospy.get_param("~public_rate", 20))
+        self.cmd_delay = int(rospy.get_param("~cmd_delay", 1))
+        self.cmd_linear_x  = 1.0
+        self.cmd_angular_z = 1.0
+
+
+    def shutdown(self):
+        '''Shutdown the handle
+        '''
+        rospy.logwarn("Shutting down ros handle node...")
+        try:
+            self.cmd_vel_pub.publish(Twist())
+            rospy.sleep(2)
+        except:
+            pass
+        try:
+            self.controller.close()
+        except:
+            pass
+        finally:
+            rospy.logwarn("Serial port closed")
+            os._exit(0)
+
+
+if __name__ == '__main__':
+    try:
+        myHandle = HandleROS()
+    except SerialException:
+        rospy.logerr("Serial exception trying to open port !")
+        os._exit(1)
+