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)