diff --git a/bt_sim_node.py b/bt_sim_node.py index 0742e2a..c8fee84 100755 --- a/bt_sim_node.py +++ b/bt_sim_node.py @@ -47,7 +47,7 @@ class BtSimNode: self.step_cnt = 0 self.reset_requested = False self.move_server.start() - T_base_task = Transform(Rotation.identity(), np.r_[0.2, -0.1, 0.1]) + T_base_task = Transform(Rotation.identity(), np.r_[0.2, -0.15, 0.1]) self.tf_tree.broadcast_static(T_base_task, "panda_link0", "task") def reset(self, req): @@ -97,6 +97,7 @@ class BtSimNode: def publish_cam_imgs(self): _, depth = self.sim.camera.update() depth_msg = self.cv_bridge.cv2_to_imgmsg(depth) + depth_msg.header.stamp = rospy.Time.now() self.depth_pub.publish(depth_msg) def move(self, goal): diff --git a/simulation.py b/simulation.py index c09b657..d63eb7b 100644 --- a/simulation.py +++ b/simulation.py @@ -7,6 +7,8 @@ from robot_utils.spatial import Rotation, Transform class BtPandaEnv(BtBaseEnv): def __init__(self, gui=True, sleep=True): super().__init__(gui, sleep) + p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) + p.resetDebugVisualizerCamera(1.4, 50, -35, [0.0, 0.0, 0.6]) self.arm = BtPandaArm() self.gripper = BtPandaGripper(self.arm) self.camera = BtCamera(