Hello, i am a student from UMass Lowell who is using the Zed2 camera in conjunction with a SPOT robot from Boston Dynamics to do research in HRI. We are working together with Georgia Tech on this project, who had started it before we joined it. Initially we joined this project around January of 2024, i built the latest available commit of the zed-ros2-wrapper and it didn’t work because some of the frames were different and this caused all sorts of issues. I fixed this issue by just checking out an older commit (21e4866) which is the commit that the Georgia Tech people used.
Recently, i had to reinstall Ubuntu, which meant that i had to rebuild all of our ros2 packages. I knew that the most recent commit of ZED-Ros2-wrapper would give us the same issues that the one we tried in January gave us, so i tried to build the commit i used before (21e4866). Unfortunately it won’t build because it relies on the zed_components package, which i saw was very recently changed to zed_msgs. For the last couple weeks i have been trying to make the most recent version of ZED-Ros2-wrapper work. I changed the frames in the URDF files to match those of the old commit, and everything seemed fine until i turned on RVIZ.
This is the setup for the experiment, the ZED camera is in front of the SPOT robot, the Apriltag glued to the table is “map”
As you can see in this picture, the ZED camera in RVIZ is below the robot, it seems like it is flipped on some axis.
This is the same issue we had when we initially tried to build in January. Im not sure what changed from the old commit to then, but it is messing up our transforms.
Over the last week we implemented a fix by negating the XYZ values of the static transform from the camera to map, as well as flipping the RPY values over the Y-axis. This solution looked fine in RVIZ, and i was able to use SPOT’s arm to grab various objects with Apriltags on them. But it was messing something up with our LFD (“Learning From Demonstration”) algorithm.
Here is a snippet of code we added to the launch file to add the static transform from “Zed2_base_link” to the Apriltag on the table.
Convert ‘cam_pose’ parameter
cam_pose_str = cam_pose.perform(context)
cam_pose_array = parse_array_param(cam_pose_str)
Coordinates received from “Calibrate.py (XYZ RPY)”
cam_pose_array = [‘-0.0194’, ‘0.4003’, ‘0.4244’, ‘-0.0477’, ‘0.7354’, ‘-1.6362’]
Robot State Publisher node
rsp_node = Node(
condition=IfCondition(publish_urdf),
package='robot_state_publisher',
namespace=camera_name_val,
executable='robot_state_publisher',
name='zed_state_publisher',
output='screen',
parameters=[{
'robot_description': Command(
[
'xacro', ' ', xacro_path, ' ',
'camera_name:=', camera_name_val, ' ',
'camera_model:=', camera_model_val, ' ',
'base_frame:=', base_frame, ' ',
'gnss_frame:=', gnss_frame, ' ',
'cam_pos_x:=', cam_pose_array[0], ' ',
'cam_pos_y:=', cam_pose_array[1], ' ',
'cam_pos_z:=', cam_pose_array[2], ' ',
'cam_roll:=', cam_pose_array[3], ' ',
'cam_pitch:=', cam_pose_array[4], ' ',
'cam_yaw:=', cam_pose_array[5]
])
}]
)
And here is the the code from “calibrate.py” that does the transform calculation.
def __init__(self):
super().__init__("calibration_node")
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
self.timer = self.create_timer(0.1, self.run)
self.T = []
self.R = []
self.BASE_FRAME = "tag36h11:0"
self.CAM_FRAME = "zed2_base_link"
self.FRAME_COUNT = 10
# self.counter = 0
def run(self):
if self.tf_buffer.can_transform(self.BASE_FRAME, self.CAM_FRAME, rclpy.time.Time()):
msg = self.tf_buffer.lookup_transform(self.BASE_FRAME, self.CAM_FRAME, rclpy.time.Time())
# self.get_logger().info(str(msg))
trans = [msg.transform.translation.x,
msg.transform.translation.y,
msg.transform.translation.z, ]
rot = [msg.transform.rotation.x,
msg.transform.rotation.y,
msg.transform.rotation.z,
msg.transform.rotation.w ]
rot = euler_from_quaternion(rot)
self.T.append(trans)
self.R.append(rot)
self.FRAME_COUNT -= 1
# self.get_logger().info(f"Found valid transform!")
# self.get_logger().info(f"{self.counter}")
# self.counter += 1
if self.FRAME_COUNT < 0:
trans = np.array(self.T).mean(axis=0)
rot = np.array(self.R).mean(axis=0)
self.get_logger().info(f"['{trans[0]:.4f}', '{trans[1]:.4f}', '{trans[2]:.4f}', '{rot[0]:.4f}', '{rot[1]:.4f}', '{rot[2]:.4f}']")
I guess my question is: what changed from commit (21e4866) to now that flips the camera relative to the robot and is it possible rebuild this commit?
Please let me know if there are any other details i can provide such as a TF tree.
Thanks!
-Boris