Issues with align_to_aruco Launch: Warnings and No Navigation

[allen]

Hi!

I’ve been following this part of the tutorial for the align_to_aruco package, but I’m encountering issues when trying to align the Stretch 3 robot with the ArUco cube. After pointing the camera at the cube and running the command:

ros2 launch stretch_core align_to_aruco.launch.py

the robot does not navigate or align with the cube. In the logs, I don’t see any specific errors but receive several warnings like:

too few points from the depth image for plane fitting, so only using the RGB Aruco estimate. number of points = 0.

and

could not transform base_right to base_link: "base_right" passed to lookup_transform argument source_frame does not exist

When I run ros2 topic echo /aruco/marker_array, I get output like this:

markers:
- header:
    stamp:
      sec: 1720816709
      nanosec: 411620361
    frame_id: camera_color_optical_frame
  ns: ''
  id: 202
  type: 1
  action: 0
  pose:
    position:
      x: 0.14020924840773602
      y: -0.3437893926218787
      z: 0.9591698540674505
    orientation:
      x: -0.9969761559233328
      y: 0.008318439378358258
      z: 0.010848742464402412
      w: 0.07649609711340849
  scale:
    x: 0.024
    y: 0.024
    z: 0.005
  color:
    r: 0.16470588743686676
    g: 0.0
    b: 1.0
    a: 0.33000001311302185
  lifetime:
    sec: 0
    nanosec: 200000000
  frame_locked: false
  points: []
  colors: []
  texture_resource: ''
  texture:
    header:
      stamp:
        sec: 0
        nanosec: 0
      frame_id: ''
    format: ''
    data: []
  uv_coordinates: []
  text: unknown
  mesh_resource: ''
  mesh_file:
    filename: ''
    data: []
  mesh_use_embedded_materials: false

The marker data appears to be correct, but the robot is not aligning as expected.

Are there any additional steps or troubleshooting tips I can follow to resolve these issues and get the alignment working?

Thank you!

[allen]

Hi!
I’m still having issues with aligning to the ArUco example. However, I have new findings since trying the ArUco ‘base_right’ tag. It seems like it’s being detected, but the base fails to move. I wonder if there is an issue with the trajectory mode. I’ve included new output when running ros2 launch stretch_core align_to_aruco.launch.py. It seems like the align_to_aruco node is detecting the tag and the transform position, and the stretch_driver is attempting to run the joint action.

Errors and outputs when running the launch file:

Rviz output:

Here’s the output from ros2 topic echo /aruco/marker_array

Thanks!

[bshah]

Hi @allen, I’m not familiar with the Align to ArUco tutorial, but I can speak to some of the warnings/errors you’ve screenshotted.

• Stereo is NOT SUPPORTED - Can ignore this.
• [rviz2]: The link is static or has unrealistic inertia, so the equivalent inertia box will not be shown. - Can ignore this as well. This is not actually an error. Since the message doesn’t explain what links are problematic, it’s difficult to tell which URDF links are causing Rviz to print the warning. To solve this, you’d need to guess-and-check modify the inertia of each link in your URDF.
• [WARNING] [base]: Base waypoint trajectory already active - The driver will output this warning if you attempt to preempt the current trajectory incorrectly (i.e. the robot is part way through executing the trajectory, and the replacement trajectory tries to modify a waypoint in the past). The code in align to aruco is likely doing something unexpected to be causing this.
• joint_traj action: hardware failed to start trajectory - Likely related to the previous error. If you solve the previous error, this should disappear.
• Could not transform base_right to base_link: Lookup would require extrapolation into the past. - This is a pretty standard TF2 error, and means that the code is attempting to lookup a transform that doesn’t exist or didn’t exist at that point. You’d need to use TF2 tools to understand why this error is occurring.

I’d recommend executing the code line-by-line in iPython and taking each error as it appears, so you can debug what’s going on.

[ris7188]

Is there any progress yet on this topic? I am getting similar error.

[bshah]

Hey @ris7188, could you start a new thread and include the error?

[allen]

Hi @ris7188 , I think at that time there’s a issue for the driver to execute the MultiDOFJointTrajectoryPoint defined in the file. So I ended up executing the action using the send_goal which is used in here. Or you can try using hello_node to move the robot based on the calculation of the aruco tag transform. I could also maybe create a pull request of my code. I hope that helps!

[ris7188]

Can someone help me with a sample code snippet to use hm.HelloNode (instead of MultiDOFJointTrajectoryPoint) inside the align_to_marker function?

​

​

​​Reply

[ris7188]

I am trying this as MultiDOFJointTrajectory has issue as mentioned in previous comment. Still it isn’t working.

def align_to_marker(self, trans_base, trans_camera):
        self.node.get_logger().info("align_to_marker method called.")

        # Use the pre-fetched transforms
        phi, dist, z_rot_base = self.compute_difference(trans_base)

        

        if self.joint_state is not None:
            # Phase 1: Rotate by phi
            point1 = JointTrajectoryPoint()
            point2 = JointTrajectoryPoint()
            point1.positions = [0.0]
            point2.positions = [phi]
            point1.time_from_start = Duration(seconds=0.0).to_msg()  # Start immediately
            point2.time_from_start = Duration(seconds=1.0).to_msg()  # End after 1 second

            trajectory_goal = FollowJointTrajectory.Goal()
            trajectory_goal.trajectory.joint_names = ['position']
            trajectory_goal.trajectory.points = [point1, point2]
            # Omit header.stamp to let the action server use its default timing

            self.trajectory_client.send_goal_async(trajectory_goal)
            self.node.get_logger().info("Executing first goal (Rotation)")

            # Phase 2: Move forward by dist
            point1 = JointTrajectoryPoint()
            point2 = JointTrajectoryPoint()
            point1.positions = [0.0]
            point2.positions = [dist]
            point1.velocities = [0.2, 0.2, 2.5]
            point1.accelerations = [1.0, 1.0, 3.5]
            point1.time_from_start = Duration(seconds=0.0).to_msg()  # Start immediately
            point2.time_from_start = Duration(seconds=1.0).to_msg()  # End after 1 second

            trajectory_goal = FollowJointTrajectory.Goal()
            trajectory_goal.trajectory.joint_names = ['position']
            trajectory_goal.trajectory.points = [point1, point2]
            # Omit header.stamp to let the action server use its default timing

            self.trajectory_client.send_goal_async(trajectory_goal)
            self.node.get_logger().info("Executing second goal (Translation)")

            # Phase 3: Rotate by z_rot_base
            point1 = JointTrajectoryPoint()
            point2 = JointTrajectoryPoint()
            point1.positions = [0.0]
            point2.positions = [z_rot_base]
            point1.time_from_start = Duration(seconds=0.0).to_msg()  # Start immediately
            point2.time_from_start = Duration(seconds=1.0).to_msg()  # End after 1 second

            trajectory_goal = FollowJointTrajectory.Goal()
            trajectory_goal.trajectory.joint_names = ['position']
            trajectory_goal.trajectory.points = [point1, point2]
            # Omit header.stamp to let the action server use its default timing

            self.trajectory_client.send_goal_async(trajectory_goal)
            self.node.get_logger().info("Executing third goal (Final Rotation)")

[Mohamed_Fazil]

Hi @ris7188 , while reviewing your code which uses FollowJointTrajectory action client, the joint names seems to have been given wrong. For base rotation the joint name is rotate_mobile_base and for base translation the joint name is translate_mobile_base.
You have given the joint name as position for all the three phases of motion which is an invalid joint name and would give out the following error.

[stretch_driver-3] [INFO] [1742001087.719453884] [stretch_driver]: stretch_driver joint_traj action: New trajectory received with joint_names = ['position']
[stretch_driver-3] -------------------------------------------
[stretch_driver-3] Errored goal
[stretch_driver-3] [INFO] [1742001087.720733459] [stretch_driver]: stretch_driver joint_traj action: Received a command without any valid joint names. Received joint names = ['position']
[stretch_driver-3] [INFO] [1742001087.726502010] [stretch_driver]: Received goal request
[stretch_driver-3] [INFO] [1742001087.731458325] [stretch_driver]: stretch_driver joint_traj action: New trajectory received with joint_names = ['position']
[stretch_driver-3] -------------------------------------------
[stretch_driver-3] Errored goal
[stretch_driver-3] [INFO] [1742001087.732012072] [stretch_driver]: Received goal request
[stretch_driver-3] [INFO] [1742001087.733374391] [stretch_driver]: stretch_driver joint_traj action: Received a command without any valid joint names. Received joint names = ['position']

Please let me know if fixing this bug makes your code work.