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)
