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 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)
def tr2_arm_follow_joint_trajectory(goal):
global tr2_arm_action_server, tr2_a0_state, tr2_a1_state, tr2_a2_state, tr2_a3_state, tr2_a4_state, tr2_a0_pub, tr2_a1_pub, tr2_a2_pub, tr2_a3_pub, tr2_a4_pub, tr2_mode_servo_pub, tr2_stop_pub
tr2_mode_servo_pub.publish(1)
tr2_stop_pub.publish(0)
time.sleep(0.050)
success = True
feedback = FollowJointTrajectoryFeedback()
result = FollowJointTrajectoryResult()
joint_names = goal.trajectory.joint_names
feedback.joint_names = joint_names
print len(goal.trajectory.points)
t_start = datetime.datetime.now()
for point in goal.trajectory.points:
tr2_a0_pub.publish(point.positions[0])
tr2_a1_pub.publish(point.positions[1])
tr2_a2_pub.publish(point.positions[2])
tr2_a3_pub.publish(point.positions[3])
tr2_a4_pub.publish(point.positions[4])
'''if tr2_arm_action_server.is_preempt_requested():
tr2_arm_action_server.set_preempted()
success = False
break'''
while (datetime.datetime.now() - t_start
).total_seconds() < point.time_from_start.nsecs / 1000000000.0:
feedback.desired = point
feedback.actual.positions = [
tr2_a0_state, tr2_a1_state, tr2_a2_state, tr2_a3_state,
tr2_a4_state
]
#feedback.error.positions
tr2_arm_action_server.publish_feedback(feedback)
tr2_arm_action_server.set_succeeded(result)
time.sleep(5)
tr2_stop_pub.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 follow_trajectory_goal(self,
traj_msg: FollowJointTrajectoryGoal,
feedback_cb: Optional[Callable] = None,
stop_cb: Optional[Callable] = None):
result = FollowJointTrajectoryResult()
# Interpolate the trajectory to a fine resolution
# if you set max_step_size to be large and position tolerance to be small, then things will be jerky
if len(traj_msg.trajectory.points) == 0:
rospy.loginfo("Ignoring empty trajectory")
result.error_code = FollowJointTrajectoryResult.SUCCESSFUL
result.error_string = "empty trajectory"
return result
# construct a list of the tolerances in order of the joint names
trajectory_joint_names = traj_msg.trajectory.joint_names
tolerance = get_ordered_tolerance_list(trajectory_joint_names, traj_msg.path_tolerance)
goal_tolerance = get_ordered_tolerance_list(trajectory_joint_names, traj_msg.goal_tolerance, is_goal=True)
interpolated_points = interpolate_joint_trajectory_points(traj_msg.trajectory.points, max_step_size=0.1)
if len(interpolated_points) == 0:
rospy.loginfo("Trajectory was empty after interpolation")
result.error_code = FollowJointTrajectoryResult.SUCCESSFUL
result.error_string = "empty trajectory"
return result
rospy.logdebug(interpolated_points)
trajectory_point_idx = 0
t0 = rospy.Time.now()
while True:
# tiny sleep lets the listeners process messages better, results in smoother following
rospy.sleep(1e-6)
desired_point = interpolated_points[trajectory_point_idx]
command_failed, command_failed_msg = self.robot.send_joint_command(trajectory_joint_names, desired_point)
# get feedback
actual_point = self.get_actual_trajectory_point(trajectory_joint_names)
# let the caller stop
if stop_cb is not None:
stop, stop_msg = stop_cb(actual_point)
else:
stop = None
stop_msg = ""
dt = rospy.Time.now() - t0
error = waypoint_error(actual_point, desired_point)
rospy.logdebug_throttle(1, f"{error} {desired_point.time_from_start.to_sec()} {dt.to_sec()}")
if desired_point.time_from_start.to_sec() > 0 and dt > desired_point.time_from_start * 5.0:
stop = True
if trajectory_point_idx == len(interpolated_points) - 1:
stop_msg = f"timeout. expected t={desired_point.time_from_start.to_sec()} but t={dt.to_sec()}." \
+ f" error to waypoint is {error}, tolerance is {goal_tolerance}"
else:
stop_msg = f"timeout. expected t={desired_point.time_from_start.to_sec()} but t={dt.to_sec()}." \
+ f" error to waypoint is {error}, tolerance is {tolerance}"
if command_failed or stop:
# command the current configuration
self.robot.send_joint_command(trajectory_joint_names, actual_point)
rospy.loginfo("Preempt requested, aborting.")
if command_failed_msg:
rospy.loginfo(command_failed_msg)
if stop_msg:
rospy.logwarn(stop_msg)
result.error_code = -10
result.error_string = stop_msg
break
# If we're close enough, advance
if trajectory_point_idx >= len(interpolated_points) - 1:
if waypoint_reached(desired_point, actual_point, goal_tolerance):
# we're done!
result.error_code = FollowJointTrajectoryResult.SUCCESSFUL
break
else:
if waypoint_reached(desired_point, actual_point, tolerance):
trajectory_point_idx += 1
if feedback_cb is not None:
feedback = FollowJointTrajectoryFeedback()
feedback.header.stamp = rospy.Time.now()
feedback.joint_names = trajectory_joint_names
feedback.desired = desired_point
feedback.actual = actual_point
feedback_cb(feedback)
return result
