Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016 from __future__ import print_function
00017
00018 import time
00019
00020 import rospy
00021
00022 from osrf_gear.msg import Goal
00023 from sensor_msgs.msg import JointState
00024 from std_srvs.srv import Trigger
00025 from trajectory_msgs.msg import JointTrajectory
00026 from trajectory_msgs.msg import JointTrajectoryPoint
00027
00028
00029 def start_competition():
00030 rospy.loginfo("Waiting for competition to be ready...")
00031 rospy.wait_for_service('/ariac/start_competition')
00032 rospy.loginfo("Competition is now ready.")
00033 rospy.loginfo("Requesting competition start...")
00034
00035 try:
00036 start = rospy.ServiceProxy('/ariac/start_competition', Trigger)
00037 response = start()
00038 except rospy.ServiceException as exc:
00039 rospy.logerr("Failed to start the competition: %s" % exc)
00040 return
00041 if not response.success:
00042 rospy.logerr("Failed to start the competition: %s" % response)
00043 else:
00044 rospy.loginfo("Competition started!")
00045
00046
00047 class MyCompetitionClass:
00048 def __init__(self):
00049 self.joint_trajectory_publisher = \
00050 rospy.Publisher("/ariac/arm/command", JointTrajectory, queue_size=10)
00051 self.received_goals = []
00052 self.current_joint_state = None
00053 self.last_joint_state_print = time.time()
00054 self.has_been_zeroed = False
00055
00056 def goal_callback(self, msg):
00057 rospy.loginfo("Received goal:\n" + str(msg))
00058
00059 def joint_state_callback(self, msg):
00060 if time.time() - self.last_joint_state_print >= 10:
00061 rospy.loginfo("Current Joint States (throttled to 0.1 Hz):\n" + str(msg))
00062 self.last_joint_state_print = time.time()
00063 self.current_joint_state = msg
00064 if not self.has_been_zeroed:
00065 self.has_been_zeroed = True
00066 rospy.loginfo("Sending arm to zero joint positions...")
00067 self.send_arm_to_zero_state()
00068
00069 def send_arm_to_zero_state(self):
00070 msg = JointTrajectory()
00071 msg.joint_names = self.current_joint_state.name
00072 point = JointTrajectoryPoint()
00073 point.positions = [0] * len(msg.joint_names)
00074 point.time_from_start = rospy.Duration(1.0)
00075 msg.points = [point]
00076 rospy.loginfo("Sending command:\n" + str(msg))
00077 self.joint_trajectory_publisher.publish(msg)
00078
00079
00080 def main():
00081 rospy.init_node("gear_example_node")
00082
00083 comp_class = MyCompetitionClass()
00084 goal_sub = rospy.Subscriber("/ariac/orders", Goal, comp_class.goal_callback)
00085 joint_state_sub = rospy.Subscriber("/joint_states", JointState, comp_class.joint_state_callback)
00086
00087 rospy.loginfo("Setup complete.")
00088 start_competition()
00089 rospy.spin()
00090
00091 if __name__ == '__main__':
00092 main()