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.