1. 패키지 생성
catkin_create_pkg zetabank_teleop_new rospy geometry_msgs
2. ROS Joystick package 설치
sudo apt-get install ros-kinetic-joy
3. package.xml 파일과 다음과 같이 수정합니다.
<?xml version="1.0"?>
<package format="2">
<name>zetabank_teleop_new</name>
<version>0.1.0</version>
<description>The zetabank_teleop_new package</description>
<author email="sjyong@humanoidsystem.com">Seo, Jae Yong</author>
<maintainer email="sjyong@humanoidsystem.com">Seo, Jae Yong</maintainer>
<license>BSD</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>rospy</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>
</package>
4. CMakeLists.txt 파일을 아래의 내용으로 수정합니다.
cmake_minimum_required(VERSION 2.8.3)
project(zetabank_teleop_new)
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
rospy
)
catkin_package(
INCLUDE_DIRS
CATKIN_DEPENDS geometry_msgs rospy
)
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
catkin_install_python(PROGRAMS scripts/zetabank_teleop_key_new
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
install(DIRECTORY launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
#add_executable(${PROJECT_NAME}_node src/zetabank_teleop_new_node.cpp)
#target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
#)
5. zetabank_teleop_key_new 파일을 아래와 같이 작성합니다.
#!/usr/bin/env python # Copyright (c) 2011, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS “AS IS” # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. import rospy from geometry_msgs.msg import Twist import sys, select, termios, tty msg = “”” Control Your Turtlebot! ————————— Moving around: u i o j k l m , . q/z : increase/decrease max speeds by 10% w/x : increase/decrease only linear speed by 10% e/c : increase/decrease only angular speed by 10% space key, k : force stop anything else : stop smoothly CTRL-C to quit “”” moveBindings = { ‘i’:(1,0), ‘o’:(1,-1), ‘j’:(0,1), ‘l’:(0,-1), ‘u’:(1,1), ‘,’:(-1,0), ‘.’:(-1,1), ‘m’:(-1,-1), } speedBindings={ ‘q’:(1.1,1.1), ‘z’:(.9,.9), ‘w’:(1.1,1), ‘x’:(.9,1), ‘e’:(1,1.1), ‘c’:(1,.9), } def getKey(): tty.setraw(sys.stdin.fileno()) rlist, _, _ = select.select([sys.stdin], [], [], 0.1) if rlist: key = sys.stdin.read(1) else: key = ” termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) return key speed = .2 turn = 1 def vels(speed,turn): return “currently:\tspeed %s\tturn %s ” % (speed,turn) if __name__==”__main__”: settings = termios.tcgetattr(sys.stdin) rospy.init_node(‘zetabank_teleop’) pub = rospy.Publisher(‘cmd_vel’, Twist, queue_size=5) x = 0 th = 0 status = 0 count = 0 acc = 0.1 target_speed = 0 target_turn = 0 control_speed = 0 control_turn = 0 try: print(msg) print(vels(speed,turn)) while(1): key = getKey() if key in moveBindings.keys(): x = moveBindings[key][0] th = moveBindings[key][1] count = 0 elif key in speedBindings.keys(): speed = speed * speedBindings[key][0] turn = turn * speedBindings[key][1] count = 0 print(vels(speed,turn)) if (status == 14): print(msg) status = (status + 1) % 15 elif key == ‘ ‘ or key == ‘k’ : x = 0 th = 0 control_speed = 0 control_turn = 0 else: count = count + 1 if count > 4: x = 0 th = 0 if (key == ‘\x03‘): break target_speed = speed * x target_turn = turn * th if target_speed > control_speed: control_speed = min( target_speed, control_speed + 0.02 ) elif target_speed < control_speed: control_speed = max( target_speed, control_speed – 0.02 ) else: control_speed = target_speed if target_turn > control_turn: control_turn = min( target_turn, control_turn + 0.1 ) elif target_turn < control_turn: control_turn = max( target_turn, control_turn – 0.1 ) else: control_turn = target_turn twist = Twist() twist.linear.x = control_speed; twist.linear.y = 0; twist.linear.z = 0 twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = control_turn pub.publish(twist) #print(“loop: {0}”.format(count)) #print(“target: vx: {0}, wz: {1}”.format(target_speed, target_turn)) #print(“publihsed: vx: {0}, wz: {1}”.format(twist.linear.x, twist.angular.z)) except Exception as e: print(e) finally: twist = Twist() twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0 twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0 pub.publish(twist) termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
6. 패키지를 컴파일 합니다.
catkin_make zetabank_teleop_new
7. VirtualBox에서 USB Serial 연결
8. USB Serial 디바이스의 읽기/쓰기 속성 변경
sudo chmod a+rw /dev/ttyUSB0
9. roscore 실행
10. zetabank_robot_control 노드 실행
rosrun zetabank_robot_control zetabank_robot_control_node
11. zetabank_teleop_key_new 파일의 속성 변경
chmod +x zetabank_teleop_key_new
12. zetabank_teleop_new 노드 실행
rosrun zetabank_teleop_new zetabank_teleop_key_new
