00001
00002
00003 import roslib; roslib.load_manifest('clearpath_teleop')
00004 import rospy
00005
00006 from sensor_msgs.msg import Joy
00007 from geometry_msgs.msg import Twist
00008 from std_msgs.msg import String
00009
00010
00011 class Teleop:
00012 def __init__(self):
00013 rospy.init_node('clearpath_teleop')
00014
00015 self.turn_scale = rospy.get_param('~turn_scale')
00016 self.drive_scale = rospy.get_param('~drive_scale')
00017 self.deadman_button = rospy.get_param('~deadman_button', 0)
00018 self.planner_button = rospy.get_param('~planner_button', 1)
00019
00020 self.cmd = None
00021 self.joy = Joy()
00022 cmd_pub = rospy.Publisher('cmd_vel', Twist)
00023
00024 announce_pub = rospy.Publisher('/clearpath/announce/teleops',
00025 String, latch=True)
00026 announce_pub.publish(rospy.get_namespace());
00027
00028 rospy.Subscriber("joy", Joy, self.callback)
00029 rospy.Subscriber("plan_cmd_vel", Twist, self.planned_callback)
00030 self.planned_motion = Twist()
00031
00032 rate = rospy.Rate(rospy.get_param('~hz', 20))
00033
00034 while not rospy.is_shutdown():
00035 rate.sleep()
00036 if self.cmd:
00037 cmd_pub.publish(self.cmd)
00038
00039 def planned_callback(self, data):
00040 """ Handle incoming Twist command from a planner.
00041 Manually update motion planned output if the buttons
00042 are in the right state """
00043 self.planned_motion = data
00044 if self.joy.buttons[self.deadman_button] == 0 and\
00045 self.joy.buttons[self.planner_button] == 1:
00046 self.cmd = self.planned_motion
00047
00048 def callback(self, data):
00049 """ Receive joystick data, formulate Twist message.
00050 Use planner if a secondary button is pressed """
00051 self.joy = data
00052 cmd = Twist()
00053 cmd.linear.x = data.axes[1] * self.drive_scale
00054 cmd.angular.z = data.axes[0] * self.turn_scale
00055
00056 if data.buttons[self.deadman_button] == 1:
00057 self.cmd = cmd
00058 elif data.buttons[self.planner_button] == 1:
00059 self.cmd = self.planned_motion
00060 else:
00061 self.cmd = None
00062
00063 if __name__ == "__main__": Teleop()
