class MotomanAction(object): # create messages that are used to publish feedback/result _feedback = FollowJointTrajectoryFeedback() _result = FollowJointTrajectoryResult() def __init__(self, name): self._action_name = name self._names = ['head_hinge'] self._state = JointState(name=self._names, position=[0]*len(self._names)) # print self._state.position self._joint_limits = {'head_hinge':{'min':0,'max':1.57,'vel':.4}} # print self._joint_limits self._as = actionlib.SimpleActionServer(self._action_name, FollowJointTrajectoryAction, execute_cb=self.execute_cb, auto_start = False) self._as.start() self.servo = Controller() self.servo.setAccel(1, 0) # hinge servo self.servo.setSpeed(1, 0) # hinge servo self.set_servo_state(self._state.position[0]) # head_hinge print "@@@@@@@@@@@@@ Initial head_hinge position = ", self._state.position[0] def set_servo_state(self, angle): # zero angle in servo values = 4000; 90 degrees angle in servo value = 7600 # value in between are just an approximate... one_servo_radian = (7600 - 4000) / (math.pi / 2.0) servo_value = int(4000 + angle * one_servo_radian) # print "@@@@@@@@@@@@@ servo_value = ", servo_value, " angle = ", angle self.servo.setTarget(1, servo_value) def handle_unigripper_vacuum(self, req): if req.TurnVacuumOn == True: self.servo.setTarget(0, 7600) print "Turned vacuum on" return UnigripperVacuumOnResponse(True) else: self.servo.setTarget(0, 4000) print "Turned vacuum off" return UnigripperVacuumOnResponse(False) def execute_cb(self, goal): rospy.loginfo('Received Trajectory') ## helper variables r = rospy.Rate(100) success = True ## simulate the execution of the trajectory in goal trajectory = goal.trajectory current_point = trajectory.points[0] for x in xrange(len(trajectory.joint_names)): if abs(self._state.position[self._state.name.index(trajectory.joint_names[x])] - current_point.positions[x]) > .001: rospy.logerr('Simulator: Start State of Joint Trajectory does not correspond to true state of the robot. Aborting trajectory...') print "Current: " + str(self._state) print "Traj: " + str(current_point) print trajectory.joint_names success = False break for outer_loop in xrange(1,len(trajectory.points)): target_point = trajectory.points[outer_loop] for x in xrange(len(trajectory.joint_names)): minimum = self._joint_limits[trajectory.joint_names[x]]['min'] maximum = self._joint_limits[trajectory.joint_names[x]]['max'] if target_point.positions[x] < minimum or target_point.positions[x] > maximum: rospy.logerr('%s joint position is outside limits. Aborting...',trajectory.joint_names[x]) success = False break diff = (target_point.time_from_start - current_point.time_from_start) for x in xrange(len(trajectory.joint_names)): vel = self._joint_limits[trajectory.joint_names[x]]['vel'] if math.fabs(target_point.positions[x] - current_point.positions[x])/(diff.secs+diff.nsecs/1000000000.0) > vel: print (diff.secs+diff.nsecs/1000000000.0) print (target_point.positions[x] - current_point.positions[x]) print target_point.positions[x] print current_point.positions[x] rospy.logerr('Unigripper driver: %s requested velocity %f (given by the time_from_start value) is too large (max %f). Aborting...',trajectory.joint_names[x],(target_point.positions[x] - current_point.positions[x])/(diff.secs+diff.nsecs/1000000000.0),vel) success = False break if not success: break num_iters = int((diff.secs+diff.nsecs/1000000000.0)/.01) # print num_iters for iter_count in xrange(num_iters): if self._as.is_preempt_requested(): self._as.set_preempted() rospy.loginfo('Preempted') success = False break for x in xrange(len(trajectory.joint_names)): val = (target_point.positions[x] - current_point.positions[x])/num_iters index = self._state.name.index(trajectory.joint_names[x]) self._state.position[index] = self._state.position[index] + val self.set_servo_state(self._state.position[0]) r.sleep() current_point = target_point if success: self._as.set_succeeded(self._result) rospy.loginfo("SUCCESS!!!") else: rospy.loginfo("FAILURE :( :( :(");