Draw colored camera views in rviz

2021-08-16 17:38:17 +02:00
parent 4b4d54240b
commit ac5c490ae8
3 changed files with 68 additions and 7 deletions
--- a/active_grasp/policy.py
+++ b/active_grasp/policy.py
@@ -43,6 +43,7 @@ class BasePolicy(Policy):

        self.center = 0.5 * (bbox.min + bbox.max)
        self.T_base_task = Transform.translation(self.center - np.full(3, 0.15))
+        self.T_task_base = self.T_base_task.inv()
        tf.broadcast(self.T_base_task, self.base_frame, self.task_frame)
        rospy.sleep(1.0)  # wait for the transform to be published