def execute_cb(self, goal): rospy.loginfo("Received new goal:\n%s"%goal) success = True start_time = rospy.Time.now() i = 1 previouspoint = None for point in goal.trajectory.points: time_to_wait = start_time + point.time_from_start - rospy.Time.now() #previouspoint=point if time_to_wait > rospy.Duration(0): #rospy.loginfo("Sleeping for %s seconds..."%time_to_wait.to_sec()) rospy.loginfo("Sleeping for 0.3 second") rospy.sleep(0.3) # only publish feedback if we have received some position recently if previouspoint and self.latestpositions: fb = FollowJointTrajectoryFeedback() fb.desired = previouspoint fb.actual = JointTrajectoryPoint() fb.actual.positions = self.latestpositions fb.error = JointTrajectoryPoint() fb.error.positions = map(sub, fb.desired.positions, fb.actual.positions) self.action_server.publish_feedback(fb) # only use positions and velocities (velocities are always positive) point.velocities = map(abs, point.velocities) point.accelerations = [] point.effort = [] rospy.loginfo("Go to point %d:\n%s"%(i, point)) serial_command=Float64MultiArray() previous_position=Float64MultiArray() #serial_command.data= tuple(numpy.subtract(point.positions,previouspoint.positions)) serial_command.data=point.positions self.pub_command.publish(serial_command) self.latestpositions = None self.pub_trajpoint.publish(point) previouspoint = point i += 1 if success: rospy.loginfo('%s: Succeeded' % self.action_name) self.result = FollowJointTrajectoryResult() self.result.error_code = FollowJointTrajectoryResult.SUCCESSFUL self.action_server.set_succeeded(self.result) self.traj_complete.publish(1)
def execute_cb(self, goal): rospy.loginfo("Received new goal:\n%s"%goal) success = True start_time = rospy.Time.now() i = 1 previouspoint = None for point in goal.trajectory.points: time_to_wait = start_time + point.time_from_start - rospy.Time.now() if time_to_wait > rospy.Duration(0): rospy.loginfo("Sleeping for %s seconds..."%time_to_wait.to_sec()) rospy.sleep(time_to_wait) # only publish feedback if we have received some position recently if previouspoint and self.latestpositions: fb = FollowJointTrajectoryFeedback() fb.desired = previouspoint fb.actual = JointTrajectoryPoint() fb.actual.positions = self.latestpositions fb.error = JointTrajectoryPoint() fb.error.positions = map(sub, fb.desired.positions, fb.actual.positions) self.action_server.publish_feedback(fb) # only use positions and velocities (velocities are always positive) point.velocities = map(abs, point.velocities) point.accelerations = [] point.effort = [] rospy.loginfo("Go to point %d:\n%s"%(i, point)) self.latestpositions = None self.pub_trajpoint.publish(point) previouspoint = point i += 1 if success: rospy.loginfo('%s: Succeeded' % self.action_name) self.result = FollowJointTrajectoryResult() self.result.error_code = FollowJointTrajectoryResult.SUCCESSFUL self.action_server.set_succeeded(self.result)
def do_trajectory(goal): joint_names = goal.trajectory.joint_names trajectory_points = goal.trajectory.points print("GOAL TRAJECTORY:") print(goal.trajectory) # publish normally??? left_arm_pub = rospy.Publisher('r2d2_left_arm_controller/command', JointTrajectory, queue_size=10) left_arm_pub.publish(goal.trajectory) # print("Published") start_time = time.time() print(time.time() - start_time) while (time.time() - start_time < 10): print(time.time() - start_time) feedback = FollowJointTrajectoryFeedback() feedback.desired = JointTrajectoryPoint() feedback.desired = goal.trajectory.points[0] # get actual positions from joint state or robot publisher ... feedback.actual = JointTrajectoryPoint() feedback.actual.positions = [0, 0, 0] feedback.error = JointTrajectoryPoint() feedback.error.positions = numpy.subtract(feedback.desired.positions, feedback.actual.positions) server.publish_feedback(feedback) time.sleep(1.0) result = FollowJointTrajectoryResult() server.set_succeeded(result, "Trajectory completed successfully") print(result.SUCCESSFUL)