def usrmove(forward, left, pub, twist):
global mode
global forward_HIGH, forward_init
rospy.loginfo(rospy.get_caller_id() + "I get mode usr: %d", mode)
while (mode == 2):
if mode != 2:
break
rospy.loginfo("before getch")
keyPress = getch.getArrow()
rospy.loginfo("after getch")
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
rospy.init_node('userToMower')
twist = Twist()
if (keyPress == 65):
forward = forward_HIGH
elif (keyPress == 66):
forward = forward_init
elif (keyPress == 68):
left += 5
elif (keyPress == 67):
left -= 5
elif (keyPress == 100):
mode = 0
break
# max backward/forward speed is 1.2 m/s
twist.linear.x = forward
twist.angular.z = left
pub.publish(twist)
MAX_FORWARD = 1.2
MAX_LEFT = 0.4
MIN_FORWARD = -1.2
MIN_LEFT = -0.4
forward = 0.0
left = 0.0
keyPress = 0
while(keyPress != USER_QUIT):
pub = rospy.Publisher('/RosAria/cmd_vel', Twist)
rospy.init_node('userToRosAria')
twist = Twist()
keyPress = getch.getArrow()
if((keyPress == KEY_UP) and (forward <= MAX_FORWARD)):
forward += 0.05
elif((keyPress == KEY_DOWN) and (forward >= MIN_FORWARD)):
forward -= 0.05
elif((keyPress == KEY_LEFT) and (left <= MAX_LEFT)):
left += 0.05
elif((keyPress == KEY_RIGHT) and (left >= MIN_LEFT)):
left -= 0.05
# max backward/forward speed is 1.2 m/s
if(forward > 1.2):
forward = 1.2
elif(forward < -1.2):
forward = -1.2
forward = 0.0
left = 0.0
keyPress = 0
if __name__ == '__main__':
rospy.init_node('aras_teleop')
while keyPress != USER_QUIT:
pub1 = rospy.Publisher('/aras_visual_servo/joint2_position_controller/command', Float64, queue_size=1)
pub2 = rospy.Publisher('/aras_visual_servo/gantry_position_controller/command', Float64, queue_size=1)
_gantry = Float64()
_joint1 = Float64()
keyPress = getch.getArrow()
if (keyPress == KEY_UP) and (forward <= MAX_FORWARD):
forward += 0.05
elif (keyPress == KEY_DOWN) and (forward >= MIN_FORWARD):
forward -= 0.05
elif (keyPress == KEY_LEFT) and (left <= MAX_LEFT):
left += 0.05
elif (keyPress == KEY_RIGHT) and (left >= MIN_LEFT):
left -= 0.05
_gantry.data = left
_joint1.data = forward
pub1.publish(_joint1)
pub2.publish(_gantry)
MAX_VELOCITY = 1.1
MAX_ANGLE = 0.3
MIN_VELOCITY = -1.1
MIN_ANGLE = -0.3
forward = 0.0
angle = 0.0
PressedKey = 0
while(PressedKey != QUIT):
pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
rospy.init_node('p3dx_mover')
twist = Twist()
PressedKey = getch.getArrow()
if((PressedKey == UP) and (forward <= MAX_VELOCITY)):
forward += 0.1
elif((PressedKey == DOWN) and (forward >= MIN_VELOCITY)):
forward -= 0.1
elif((PressedKey == LEFT) and (angle >= MIN_ANGLE )):
angle += 0.1
elif((PressedKey == RIGHT) and (angle <= MAX_ANGLE)):
angle -= 0.1
twist.linear.x = forward
twist.angular.z = angle
pub.publish(twist)
pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)