I'm new to ROS. I wrote the code below to execute a trajectory using SimpleActionClient. I need to know when each point in the trajectory was executed while the trajectory is being executed. I thought to use the feedback callback from action client's send_goal method, but it's never called. The done_cb and active_cb are called, but not feedback_cb. Then, I thought since each point has time_from_start field I could listen to /feedback_states topic (see list of topics below) and check the the time_from_start field (see the feedback_state output below). But, it's always 0.

What am I doing wrong? Or, even better, is this how to get what I need or is there another better way?

UPDATE1

Just to clarify, I want to control the Yaskawa hc10 robot from ROS AND be able to know what trajectory point it's currently executing. After the research I saw that I should use motoman driver. I found some python example and continued from there. So, the first part seems to work. But, now I need to make the second part to work. To do that I decided to check the time_from_start field from the message from /feedback_states topic since I assign time_from_start to each point when I build the trajectory object. But, when I do rostopic echo /joint_states I can see that time_from_start is always 0 and doesn't have the values I assigned.

Therefore, I decided a different approach - listen to joint_states topic and do the following:

• I consider 6-joint values as a vector.
• Now, I have the target point (6-joint values) that I want the robot to reach and current positions that I receive from joint_states topic. Therefore,

For each received point:

• Calculate distance between current point and target point (numpy.norm(current_point - target_point))
• As the arm approaches the target point, the distance (norm) value should decrease. If current distance is greater than previous distance, I consider that it has reached the target point.

Does this make sense? I'll post the issues I have with this approach if needed, unless you already know what can be the issues.

Code

client = actionlib.SimpleActionClient('/joint_trajectory_action', FollowJointTrajectoryAction)
        if not client.wait_for_server(rospy.Duration(5.0)):
            raise RuntimeError("Action server not available!")

def _create_trajpoint(self, joint_positions, time_from_start, velocities) -> JointTrajectoryPoint:
        point = JointTrajectoryPoint()
        point.positions = joint_positions
        point.time_from_start = rospy.Duration(time_from_start)
        point.velocities = velocities

        return point

def _execute_trajectory(self, traj):
        self._send_trajectory_to_controller(traj)
        self._wait_for_traj_to_complete()

    def _send_trajectory_to_controller(self, traj):
        print("Sending trajectory")

        goal = FollowJointTrajectoryGoal()
        goal.trajectory = traj

        self._action_client.send_goal(goal, feedback_cb=my_feedback_cb, done_cb=my_done_cb, active_cb=my_active_cb)
        self._action_client.wait_for_result(rospy.Duration(2.0))

        print("Trajectory sent")

    def _wait_for_traj_to_complete(self):
        print('Waiting for trajectory to complete')

        wait_time = 0.1
        no_motion_total = 0
        no_motion_timeout = 1
        traj_state = self._action_client.get_state()
        while traj_state == actionlib.GoalStatus.ACTIVE:
            rospy.sleep(wait_time)

            traj_state = self._action_client.get_state()
            robot_status = self.get_robot_status()
            if robot_status.in_motion.val == 0: 
                no_motion_total += wait_time
                if no_motion_total > no_motion_timeout:
                    raise RuntimeError(f'Robot hasn\'t moved for {no_motion_timeout} sec. TrajState={traj_state}')
            else:
                no_motion_total = 0

        if traj_state == actionlib.GoalStatus.PREEMPTED:
            raise OperationCanceledExepction('Trajectory goal was canceled')
        if traj_state != actionlib.GoalStatus.SUCCEEDED:
            raise RuntimeError(f"Failed to complete trajectory. TrajState={traj_state}")

feedback_states

rostopic echo /feedback_states

header: 
  seq: 265925
  stamp: 
    secs: 1687457226
    nsecs:  28453125
  frame_id: ''
joint_names: 
  - joint_1_s
  - joint_2_l
  - joint_3_u
  - joint_4_r
  - joint_5_b
  - joint_6_t
desired: 
  positions: []
  velocities: []
  accelerations: []
  effort: []
  time_from_start: 
    secs: 0
    nsecs:         0
actual: 
  positions: [-0.5130592584609985, 0.45683741569519043, 0.9317458271980286, -1.008545994758606, -1.255672812461853, -1.8192806243896484]
  velocities: []
  accelerations: []
  effort: []

Topics

/feedback_states
/joint_path_command
/joint_states
/joint_trajectory_action/cancel
/joint_trajectory_action/feedback
/joint_trajectory_action/goal
/joint_trajectory_action/result
/joint_trajectory_action/status
/robot_status
/rosout
/rosout_agg
/tf
/tf_static