Slam 작성을 위한 키보드 조종하려 하는데
roslaunch omo_r1mini_teleop omo_r1mini_teleop_key.launch
코드 입력시 사진과 같이 오류가 발생합니다.
헤당 오류 위치에 로그 파일 내용 입니다.
import rospy
from geometry_msgs.msg import Twist
import sys, select, os
if os.name == ‘nt’:
import msvcrt
else:
import tty, termios
목록 항목
from omo_r1mini_bringup.srv import Onoff, OnoffResponse
from omo_r1mini_bringup.srv import SaveColor, ColorResponse
from omo_r1mini_bringup.srv import Battery
msg = “”"
Control Your OMO Robot!
Moving around:
w
a s d
c
w/x : increase/decrease linear velocity (~ 1.2 m/s)
a/d : increase/decrease angular velocity (~ 1.8)
space key, s : force stop
b : Buzzer turn On/Off
h : Headlight turn On/Off
c : Change RGB led Rainbow color
CTRL-C to quit
“”"
e = “”"
Communications Failed
“”"
def getKey():
if os.name == ‘nt’:
return msvcrt.getch()
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
def vels(target_linear_vel, target_angular_vel):
return "currently:\tlinear vel %s\t angular vel %s " % (target_linear_vel,target_angular_vel)
def makeSimpleProfile(output, input, slop):
if input > output:
output = min( input, output + slop )
elif input < output:
output = max( input, output - slop )
else:
output = input
return output
def constrain(input, low, high):
if input < low:
input = low
elif input > high:
input = high
else:
input = input
return input
def checkLinearLimitVelocity(vel, min, max):
vel = constrain(vel, min, max)
return vel
def checkAngularLimitVelocity(vel, max):
vel = constrain(vel, -max, max)
return vel
Service call
def set_headlight_onOff(headOnOff):
rospy.wait_for_service(‘set_headlight’)
try:
srv_headOnOff = rospy.ServiceProxy(’/set_headlight’, Onoff)
OnoffResponse = srv_headOnOff(headOnOff)
except rospy.ServiceException as e:
print("Service call failed: %s"%e)
def set_buzzer_onOff(buzzerOnOff):
rospy.wait_for_service(‘set_headlight’)
try:
srv_buzzerOnOff = rospy.ServiceProxy(’/set_buzzer’, Onoff)
OnoffResponse = srv_buzzerOnOff(buzzerOnOff)
except rospy.ServiceException as e:
print("Service call failed: %s"%e)
def set_ledColor(red, grn, blu):
rospy.wait_for_service(‘save_led_color’)
try:
srv_saveLedColor = rospy.ServiceProxy(’/save_led_color’, SaveColor)
SaveColorResponse = srv_saveLedColor(red, grn, blu)
except rospy.ServiceException as e:
print("Service call failed: %s"%e)
def get_BatteryPower():
rospy.wait_for_service(‘battery_status’)
try:
srv_Battery = rospy.ServiceProxy(’/battery_status’, Battery)
res = srv_Battery()
print(“Battery V: %.2f, SOC: %.1f, A: %.2f” % (res.volt, res.SOC, res.current) )
except rospy.ServiceException as e:
print("Service call failed: %s"%e)
if name==“main”:
if os.name != ‘nt’:
settings = termios.tcgetattr(sys.stdin)
rospy.init_node('omo_r1mini_teleop')
tf_prefix = rospy.get_param("~tf_prefix", "")
print("tf_prefix:"+tf_prefix)
max_lin_vel = rospy.get_param("~max_lin_vel") #OMO_R1mini_MAX_LIN_VEL = 1.20
min_lin_vel = rospy.get_param("~min_lin_vel")
max_ang_vel = rospy.get_param("~max_ang_vel") #OMO_R1mini_MAX_ANG_VEL = 1.80
lin_vel_step_size = rospy.get_param("~lin_vel_step") #LIN_VEL_STEP_SIZE = 0.05
ang_vel_step_size = rospy.get_param("~ang_vel_step") #ANG_VEL_STEP_SIZE = 0.1
ang_vel_rev = rospy.get_param("~ang_vel_reverse") #1 for reversed
pub = rospy.Publisher(tf_prefix+'/cmd_vel', Twist, queue_size=10)
status = 0
target_linear_vel = 0.0
target_angular_vel = 0.0
control_linear_vel = 0.0
control_angular_vel = 0.0
headlightOn = False
buzzerOn = False
colors = [ [255, 0, 0], [255,50, 0], [255,255,0], [0,255,0],
[0,0,255], [0,5,255], [100,0,255], [255,255,255] ]
colorIdx = 0
try:
print(msg)
while(1):
key = getKey()
if key == 'w' :
target_linear_vel = checkLinearLimitVelocity(target_linear_vel + lin_vel_step_size, min_lin_vel, max_lin_vel)
status = status + 1
print(vels(target_linear_vel,target_angular_vel))
elif key == 'x' :
target_linear_vel = checkLinearLimitVelocity(target_linear_vel - lin_vel_step_size, min_lin_vel, max_lin_vel)
status = status + 1
print(vels(target_linear_vel,target_angular_vel))
elif key == 'a' :
if ang_vel_rev == 1:
target_angular_vel = checkAngularLimitVelocity(target_angular_vel - ang_vel_step_size, max_ang_vel)
else:
target_angular_vel = checkAngularLimitVelocity(target_angular_vel + ang_vel_step_size, max_ang_vel)
status = status + 1
print(vels(target_linear_vel,target_angular_vel))
elif key == 'd' :
if ang_vel_rev == 1:
target_angular_vel = checkAngularLimitVelocity(target_angular_vel + ang_vel_step_size, max_ang_vel)
else:
target_angular_vel = checkAngularLimitVelocity(target_angular_vel - ang_vel_step_size, max_ang_vel)
status = status + 1
print(vels(target_linear_vel,target_angular_vel))
elif key == ' ' or key == 's' :
target_linear_vel = 0.0
control_linear_vel = 0.0
target_angular_vel = 0.0
control_angular_vel = 0.0
print(vels(target_linear_vel, target_angular_vel))
elif key == 'b' :
if buzzerOn == True:
buzzerOn = False
set_buzzer_onOff(False)
print("Buzzer: OFF")
else :
buzzerOn = True
set_buzzer_onOff(True)
print("Buzzer: ON")
elif key == 'h' :
if headlightOn == True:
headlightOn = False
set_headlight_onOff(False)
print("Headlight: OFF")
else :
headlightOn = True
set_headlight_onOff(True)
print("Headlight: ON")
elif key == 'p' :
get_BatteryPower()
elif key == 'c' :
rospy.loginfo("Color Set: %s, %s, %s", colors[colorIdx][0], colors[colorIdx][1],colors[colorIdx][2])
set_ledColor(colors[colorIdx][0], colors[colorIdx][1],colors[colorIdx][2])
colorIdx += 1
if colorIdx == len(colors):
colorIdx = 0
else:
if (key == '\x03'):
break
if status == 20 :
print(msg)
status = 0
twist = Twist()
control_linear_vel = makeSimpleProfile(control_linear_vel, target_linear_vel, (lin_vel_step_size/2.0))
twist.linear.x = control_linear_vel; twist.linear.y = 0.0; twist.linear.z = 0.0
control_angular_vel = makeSimpleProfile(control_angular_vel, target_angular_vel, (ang_vel_step_size/2.0))
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = control_angular_vel
pub.publish(twist)
except:
print(e)
finally:
twist = Twist()
twist.linear.x = 0.0; twist.linear.y = 0.0; twist.linear.z = 0.0
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = 0.0
pub.publish(twist)
if os.name != 'nt':
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)