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