def feedback_states_publisher(rate):
global current_joints
global current_velocity
publisher = rospy.Publisher('feedback_states', FollowJointTrajectoryFeedback, queue_size = conf_rate)
sleeper = rospy.Rate(rate)
while not rospy.is_shutdown():
msg = FollowJointTrajectoryFeedback()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = conf_base_name
msg.joint_names = conf_joint_names
msg.actual = JointTrajectoryPoint()
# -----------------------------------------------------------
# Critical Section
# -----------------------------------------------------------
with lock:
msg.actual.positions = current_joints
msg.actual.velocities = current_velocity
msg.actual.effort = current_effort
msg.actual.time_from_start = rospy.get_rostime() - trajectory_start
# -----------------------------------------------------------
publisher.publish(msg)
sleeper.sleep()
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)
def _set_current_joint_angles(self, joint_angles):
self._current_joint_angles = joint_angles
if self._trajectory_valid:
#If the specified trajectory is too far away from current joint angles, abort it
trajectory_angles = self._get_trajectory_joint_angles(
self._trajectory_t)
#TODO: improve trajectory error tracking
if (np.any(
np.abs(trajectory_angles -
joint_angles) > np.deg2rad(45))):
rospy.logerr("Trajectory aborted due to tracking error: %s",
str(np.rad2deg(trajectory_angles - joint_angles)))
self._abort_trajectory()
else:
fb = FollowJointTrajectoryFeedback()
fb.header.stamp = rospy.Time.now()
fb.desired.positions = self._get_trajectory_joint_angles(
self._trajectory_max_t)
fb.desired.time_from_start = rospy.Duration(
secs=self._trajectory_t)
fb.actual = fb.desired
gh = self._trajectory_gh
gh.publish_feedback(fb)
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
