AMBF client not updating the pose of cameras and Camera frame about surgical_robotics_challenge

[adnanmunawar]

Hi Juan,

Thanks for sending the example script. This may be a bug, resulting from some optimization logic. Can you try this updated script and check if the values are updated?

Also, the pose of cameraL and cameraR should not change if you only move CameraFrame, as they are parented to the CameraFrame, and thus their reported pose should always be relative to the parent. However, if you move them individually, then the pose should change.

from surgical_robotics_challenge.scene import Scene
from surgical_robotics_challenge.camera import Camera
from ambf_client import Client
import rospy
import time
import numpy as np
import tf_conversions.posemath as pm

np.set_printoptions(precision=3, suppress=True, sign=" ")

rospy.init_node("dataset_collection")
c = Client("juanclient")
c.connect()

# Create clients
scene = Scene(c)
ambf_cam_l = Camera(c, "cameraL")
ambf_cam_r = Camera(c, "cameraR")
ambf_cam_frame = Camera(c, "CameraFrame")

#Query camera pose.
counter = 0
flag = True
try:
    while flag:
        ambf_cam_l.set_pose_changed() # Addition
        ambf_cam_frame.set_pose_changed() # Addition
        T_FC = pm.toMatrix(ambf_cam_l.get_T_c_w())
        T_WF = pm.toMatrix(ambf_cam_frame.get_T_c_w())
        print(f"pose at step {counter}")
        print(f"camera to frame\n {T_FC}")
        print(f"frame to world\n {T_WF}")
        print("\n\n\n")
        counter += 1
        time.sleep(1.5)
        if counter > 20:
            break

except Exception as e:
    print("Exit code")

[jabarragann]

Hi Adnan! Thanks that fixed the issue! Will you be adding the set_pose_changed() call inside the get_T_c_w function? Or should I add the set_pose_changed() to my scripts?

[adnanmunawar]

Hi Juan, glad that we found what the issue is. I think we should fix it properly rather than requiring to add the set_pose_changed method. I shall try to create a fix today.

[adnanmunawar]

Closing this as the above commit should fix the issue.

[jabarragann]

Hi Adnan, just wanted to confirm that the issue was solved!