Define custom scenes via yaml files

2021-09-08 16:50:53 +02:00 · 2021-09-08 16:50:53 +02:00 · 1fb2eaf2b6
commit 1fb2eaf2b6
parent 60443b0c2f
7 changed files with 465 additions and 244 deletions
--- a/active_grasp/baselines.py
+++ b/active_grasp/baselines.py
@ -41,7 +41,7 @@ class TopTrajectory(MultiViewPolicy):
 class CircularTrajectory(MultiViewPolicy):
    def __init__(self, rate):
        super().__init__(rate)
-        self.r = 0.12
+        self.r = 0.08
        self.h = 0.3
        self.duration = 2.0 * np.pi * self.r / self.linear_vel
        self.m = scipy.interpolate.interp1d([0.0, self.duration], [np.pi, 3.0 * np.pi])
--- a/active_grasp/controller.py
+++ b/active_grasp/controller.py
@ -50,8 +50,8 @@ class GraspController:
    def init_moveit(self):
        self.moveit = MoveItClient("panda_arm")
        rospy.sleep(1.0)  # wait for connections to be established
-        msg = to_pose_stamped_msg(Transform.t([0.4, 0, 0.4]), self.base_frame)
-        self.moveit.scene.add_box("table", msg, size=(0.5, 0.5, 0.02))
+        # msg = to_pose_stamped_msg(Transform.t([0.4, 0, 0.4]), self.base_frame)
+        # self.moveit.scene.add_box("table", msg, size=(0.5, 0.5, 0.02))

    def switch_to_cartesian_velocity_control(self):
        req = SwitchControllerRequest()
@ -80,7 +80,7 @@ class GraspController:
            grasp = self.search_grasp(bbox)

        self.switch_to_joint_trajectory_control()
-        with Timer("execution_time"):
+        with Timer("grasp_time"):
            res = self.execute_grasp(grasp)

        return self.collect_info(res)
@ -109,18 +109,13 @@ class GraspController:
    def execute_grasp(self, grasp):
        if not grasp:
            return "aborted"
-
        T_base_grasp = self.postprocess(grasp.pose)
-
        self.gripper.move(0.08)
-
        self.moveit.goto(T_base_grasp * Transform.t([0, 0, -0.05]) * self.T_grasp_ee)
        self.moveit.goto(T_base_grasp * self.T_grasp_ee)
        self.gripper.grasp()
        self.moveit.goto(Transform.t([0, 0, 0.1]) * T_base_grasp * self.T_grasp_ee)
-
        success = self.gripper.read() > 0.005
-
        return "succeeded" if success else "failed"

    def postprocess(self, T_base_grasp):
--- a/active_grasp/simulation.py
+++ b/active_grasp/simulation.py
@ -1,6 +1,7 @@
 from pathlib import Path
 import pybullet as p
 import pybullet_data
+import yaml
 import rospkg

 from active_grasp.bbox import AABBox
@ -77,19 +78,17 @@ class Simulation:

 class Scene:
    def __init__(self):
-        self.urdfs_path = Path(rospack.get_path("vgn")) / "assets/urdfs"
+        self.vgn_urdfs_dir = Path(rospack.get_path("vgn")) / "assets/urdfs"
+        self.ycb_urdfs_dir = Path(rospack.get_path("urdf_zoo")) / "models/ycb"
+        self.support_urdf = self.vgn_urdfs_dir / "setup/plane.urdf"
        self.support_uid = -1
        self.object_uids = []

-    def load_support(self, urdf, ori, pos, scale):
-        uid = p.loadURDF(str(urdf), pos, ori.as_quat(), globalScaling=scale)
-        self.support_uid = uid
-        return uid
+    def load_support(self, pos):
+        self.support_uid = p.loadURDF(str(self.support_urdf), pos, globalScaling=0.2)

    def remove_support(self):
-        if self.support_uid != -1:
-            p.removeBody(self.support_uid)
-            self.support_uid = -1
+        p.removeBody(self.support_uid)

    def load_object(self, urdf, ori, pos, scale=1.0):
        uid = p.loadURDF(str(urdf), pos, ori.as_quat(), globalScaling=scale)
@ -112,6 +111,9 @@ class Scene:
    def load(self, rng):
        raise NotImplementedError

+    def get_ycb_urdf_path(self, model_name):
+        return self.ycb_urdfs_dir / model_name / "model.urdf"
+

 def find_urdfs(root):
    # Scans a dir for URDF assets
@ -125,12 +127,10 @@ class RandomScene(Scene):
        self.center = np.r_[0.6, 0.0, 0.1]
        self.length = 0.3
        self.origin = self.center - np.r_[0.5 * self.length, 0.5 * self.length, 0.0]
-        self.object_urdfs = find_urdfs(self.urdfs_path / "packed" / "test")
-        self.support_urdf = self.urdfs_path / "setup/plane.urdf"
+        self.object_urdfs = find_urdfs(self.vgn_urdfs_dir / "packed" / "test")

    def load(self, rng, attempts=10):
-        self.load_support(self.support_urdf, Rotation.identity(), self.center, 0.3)
-
+        self.load_support(self.center)
        urdfs, scale = rng.choice(self.object_urdfs, 5), 0.8
        for urdf in urdfs:
            uid = self.load_object(urdf, Rotation.identity(), np.zeros(3), scale)
@ -158,13 +158,33 @@ class RandomScene(Scene):
        return q


-scenes = {
-    "random": RandomScene,
-}
+class CustomScene(Scene):
+    def __init__(self, config_name):
+        super().__init__()
+        config_path = Path(rospack.get_path("active_grasp")) / "cfg" / config_name
+        with config_path.open("r") as f:
+            self.scene = yaml.load(f)
+        self.center = np.asarray(self.scene["center"])
+        self.length = 0.3
+        self.origin = self.center - np.r_[0.5 * self.length, 0.5 * self.length, 0.0]
+
+    def load(self, rng):
+        self.load_support(self.center)
+        for object in self.scene["objects"]:
+            self.load_object(
+                self.get_ycb_urdf_path(object["object_id"]),
+                Rotation.from_euler("xyz", object["rpy"]),
+                self.center + np.asarray(object["xyz"]),
+            )
+
+    def sample_initial_configuration(self, rng):
+        return self.scene["configuration"]


 def get_scene(scene_id):
-    if scene_id in scenes:
-        return scenes[scene_id]()
+    if scene_id == "random":
+        return RandomScene()
+    elif scene_id.endswith(".yaml"):
+        return CustomScene(scene_id)
    else:
        raise ValueError("Unknown scene {}.".format(scene_id))
--- a/cfg/active_grasp.yaml
+++ b/cfg/active_grasp.yaml
@ -1,8 +1,8 @@
 bt_sim:
-  gui: False
+  gui: true
  cam_noise: True
  calib_error: True
-  scene: random
+  scene: test.yaml

 grasp_controller:
  base_frame_id: panda_link0
--- a/cfg/test.yaml
+++ b/cfg/test.yaml
@ -0,0 +1,9 @@
+center: [0.55, 0.0, 0.3]
+objects:
+- object_id: 003_cracker_box
+  rpy: [0.0, 0.0, 0.0]
+  xyz: [-0.08, 0.08, 0.002]
+- object_id: 010_potted_meat_can
+  rpy: [0.0, 0.0, 0.0]
+  xyz: [0.0, 0.0, 0.002]
+configuration: [0.0, -1.04, 0.17, -1.91, 0.0, 1.57, 0.79]
--- a/launch/active_grasp.rviz
+++ b/launch/active_grasp.rviz
@ -4,8 +4,7 @@ Panels:
    Name: Displays
    Property Tree Widget:
      Expanded:
-        - /PlanningScene1/Scene Geometry1
-        - /Markers1/Namespaces1
+        - /TF1/Frames1
      Splitter Ratio: 0.5
    Tree Height: 805
  - Class: rviz/Selection
@ -52,6 +51,377 @@ Visualization Manager:
      Plane Cell Count: 10
      Reference Frame: <Fixed Frame>
      Value: true
+    - Alpha: 0.20000000298023224
+      Autocompute Intensity Bounds: false
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 1
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: MapCloud
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.007499999832361937
+      Style: Boxes
+      Topic: /map_cloud
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: visualization_marker_array
+      Name: Markers
+      Namespaces:
+        bbox: true
+        best_grasp: true
+        grasps: true
+        path: true
+      Queue Size: 100
+      Value: true
+    - Acceleration_Scaling_Factor: 0.1
+      Class: moveit_rviz_plugin/MotionPlanning
+      Enabled: false
+      Move Group Namespace: ""
+      MoveIt_Allow_Approximate_IK: false
+      MoveIt_Allow_External_Program: false
+      MoveIt_Allow_Replanning: false
+      MoveIt_Allow_Sensor_Positioning: false
+      MoveIt_Planning_Attempts: 10
+      MoveIt_Planning_Time: 5
+      MoveIt_Use_Cartesian_Path: false
+      MoveIt_Use_Constraint_Aware_IK: false
+      MoveIt_Warehouse_Host: 127.0.0.1
+      MoveIt_Warehouse_Port: 33829
+      MoveIt_Workspace:
+        Center:
+          X: 0
+          Y: 0
+          Z: 0
+        Size:
+          X: 2
+          Y: 2
+          Z: 2
+      Name: MotionPlanning
+      Planned Path:
+        Color Enabled: false
+        Interrupt Display: false
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          camera_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          panda_hand:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_leftfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link8:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_rightfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+        Loop Animation: false
+        Robot Alpha: 0.5
+        Robot Color: 150; 50; 150
+        Show Robot Collision: false
+        Show Robot Visual: true
+        Show Trail: false
+        State Display Time: 0.05 s
+        Trail Step Size: 1
+        Trajectory Topic: /move_group/display_planned_path
+      Planning Metrics:
+        Payload: 1
+        Show Joint Torques: false
+        Show Manipulability: false
+        Show Manipulability Index: false
+        Show Weight Limit: false
+        TextHeight: 0.07999999821186066
+      Planning Request:
+        Colliding Link Color: 255; 0; 0
+        Goal State Alpha: 1
+        Goal State Color: 250; 128; 0
+        Interactive Marker Size: 0
+        Joint Violation Color: 255; 0; 255
+        Planning Group: panda_arm
+        Query Goal State: true
+        Query Start State: false
+        Show Workspace: false
+        Start State Alpha: 1
+        Start State Color: 0; 255; 0
+      Planning Scene Topic: move_group/monitored_planning_scene
+      Robot Description: robot_description
+      Scene Geometry:
+        Scene Alpha: 0.8999999761581421
+        Scene Color: 50; 230; 50
+        Scene Display Time: 0.009999999776482582
+        Show Scene Geometry: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
+      Scene Robot:
+        Attached Body Color: 150; 50; 150
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          camera_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          panda_hand:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_leftfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link8:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_rightfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+        Robot Alpha: 1
+        Show Robot Collision: false
+        Show Robot Visual: true
+      Value: false
+      Velocity_Scaling_Factor: 0.1
+    - Class: moveit_rviz_plugin/PlanningScene
+      Enabled: false
+      Move Group Namespace: ""
+      Name: PlanningScene
+      Planning Scene Topic: move_group/monitored_planning_scene
+      Robot Description: robot_description
+      Scene Geometry:
+        Scene Alpha: 0.8999999761581421
+        Scene Color: 50; 230; 50
+        Scene Display Time: 0.009999999776482582
+        Show Scene Geometry: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
+      Scene Robot:
+        Attached Body Color: 150; 50; 150
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          camera_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          panda_hand:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_leftfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link8:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_rightfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+        Robot Alpha: 1
+        Show Robot Collision: false
+        Show Robot Visual: false
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: false
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 138; 226; 52
+      Max Intensity: 1
+      Min Color: 239; 41; 41
+      Min Intensity: 0
+      Name: Quality
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.007499999832361937
+      Style: Boxes
+      Topic: /quality
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: false
+      Value: false
    - Alpha: 1
      Class: rviz/RobotModel
      Collision Enabled: false
@ -62,6 +432,10 @@ Visualization Manager:
        Expand Link Details: false
        Expand Tree: false
        Link Tree Style: Links in Alphabetic Order
+        camera_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
        panda_hand:
          Alpha: 1
          Show Axes: false
@ -128,114 +502,34 @@ Visualization Manager:
      Update Interval: 0
      Value: true
      Visual Enabled: true
-    - Acceleration_Scaling_Factor: 0.1
-      Class: moveit_rviz_plugin/MotionPlanning
-      Enabled: false
-      Move Group Namespace: ""
-      MoveIt_Allow_Approximate_IK: false
-      MoveIt_Allow_External_Program: false
-      MoveIt_Allow_Replanning: false
-      MoveIt_Allow_Sensor_Positioning: false
-      MoveIt_Planning_Attempts: 10
-      MoveIt_Planning_Time: 5
-      MoveIt_Use_Cartesian_Path: false
-      MoveIt_Use_Constraint_Aware_IK: false
-      MoveIt_Warehouse_Host: 127.0.0.1
-      MoveIt_Warehouse_Port: 33829
-      MoveIt_Workspace:
-        Center:
-          X: 0
-          Y: 0
-          Z: 0
-        Size:
-          X: 2
-          Y: 2
-          Z: 2
-      Name: MotionPlanning
-      Planned Path:
-        Color Enabled: false
-        Interrupt Display: false
-        Links:
-          All Links Enabled: true
-          Expand Joint Details: false
-          Expand Link Details: false
-          Expand Tree: false
-          Link Tree Style: Links in Alphabetic Order
-        Loop Animation: false
-        Robot Alpha: 0.5
-        Robot Color: 150; 50; 150
-        Show Robot Collision: false
-        Show Robot Visual: true
-        Show Trail: false
-        State Display Time: 0.05 s
-        Trail Step Size: 1
-        Trajectory Topic: /move_group/display_planned_path
-      Planning Metrics:
-        Payload: 1
-        Show Joint Torques: false
-        Show Manipulability: false
-        Show Manipulability Index: false
-        Show Weight Limit: false
-        TextHeight: 0.07999999821186066
-      Planning Request:
-        Colliding Link Color: 255; 0; 0
-        Goal State Alpha: 1
-        Goal State Color: 250; 128; 0
-        Interactive Marker Size: 0
-        Joint Violation Color: 255; 0; 255
-        Planning Group: panda_arm
-        Query Goal State: true
-        Query Start State: false
-        Show Workspace: false
-        Start State Alpha: 1
-        Start State Color: 0; 255; 0
-      Planning Scene Topic: move_group/monitored_planning_scene
-      Robot Description: robot_description
-      Scene Geometry:
-        Scene Alpha: 0.8999999761581421
-        Scene Color: 50; 230; 50
-        Scene Display Time: 0.009999999776482582
-        Show Scene Geometry: true
-        Voxel Coloring: Z-Axis
-        Voxel Rendering: Occupied Voxels
-      Scene Robot:
-        Attached Body Color: 150; 50; 150
-        Links:
-          All Links Enabled: true
-          Expand Joint Details: false
-          Expand Link Details: false
-          Expand Tree: false
-          Link Tree Style: Links in Alphabetic Order
-        Robot Alpha: 1
-        Show Robot Collision: false
-        Show Robot Visual: true
-      Value: false
-      Velocity_Scaling_Factor: 0.1
-    - Class: moveit_rviz_plugin/PlanningScene
-      Enabled: false
-      Move Group Namespace: ""
-      Name: PlanningScene
-      Planning Scene Topic: move_group/monitored_planning_scene
-      Robot Description: robot_description
-      Scene Geometry:
-        Scene Alpha: 0.8999999761581421
-        Scene Color: 50; 230; 50
-        Scene Display Time: 0.009999999776482582
-        Show Scene Geometry: true
-        Voxel Coloring: Z-Axis
-        Voxel Rendering: Occupied Voxels
-      Scene Robot:
-        Attached Body Color: 150; 50; 150
-        Links:
-          All Links Enabled: true
-          Expand Joint Details: false
-          Expand Link Details: false
-          Expand Tree: false
-          Link Tree Style: Links in Alphabetic Order
-        Robot Alpha: 1
-        Show Robot Collision: false
-        Show Robot Visual: false
-      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: SceneCloud
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Spheres
+      Topic: /scene_cloud
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
    - Class: rviz/TF
      Enabled: true
      Frame Timeout: 15
@ -272,11 +566,11 @@ Visualization Manager:
        world:
          Value: false
      Marker Alpha: 1
-      Marker Scale: 0.4000000059604645
+      Marker Scale: 0.30000001192092896
      Name: TF
      Show Arrows: false
      Show Axes: true
-      Show Names: true
+      Show Names: false
      Tree:
        world:
          panda_link0:
@ -299,105 +593,6 @@ Visualization Manager:
              {}
      Update Interval: 0
      Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: Intensity
-      Decay Time: 0
-      Enabled: true
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Min Color: 0; 0; 0
-      Name: SceneCloud
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.009999999776482582
-      Style: Spheres
-      Topic: /scene_cloud
-      Unreliable: false
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: true
-    - Alpha: 0.20000000298023224
-      Autocompute Intensity Bounds: false
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: Intensity
-      Decay Time: 0
-      Enabled: false
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 1
-      Min Color: 0; 0; 0
-      Min Intensity: 0
-      Name: MapCloud
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.007499999832361937
-      Style: Boxes
-      Topic: /map_cloud
-      Unreliable: false
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: false
-    - Alpha: 1
-      Autocompute Intensity Bounds: false
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: Intensity
-      Decay Time: 0
-      Enabled: false
-      Invert Rainbow: false
-      Max Color: 138; 226; 52
-      Max Intensity: 1
-      Min Color: 239; 41; 41
-      Min Intensity: 0
-      Name: Quality
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.007499999832361937
-      Style: Boxes
-      Topic: /quality
-      Unreliable: false
-      Use Fixed Frame: true
-      Use rainbow: false
-      Value: false
-    - Class: rviz/MarkerArray
-      Enabled: true
-      Marker Topic: visualization_marker_array
-      Name: Markers
-      Namespaces:
-        bbox: true
-        best_grasp: true
-        grasps: true
-        path: true
-      Queue Size: 100
-      Value: true
  Enabled: true
  Global Options:
    Background Color: 48; 48; 48
@ -426,7 +621,7 @@ Visualization Manager:
  Views:
    Current:
      Class: rviz/Orbit
-      Distance: 1.379216194152832
+      Distance: 1.5005543231964111
      Enable Stereo Rendering:
        Stereo Eye Separation: 0.05999999865889549
        Stereo Focal Distance: 1
@ -442,9 +637,9 @@ Visualization Manager:
      Invert Z Axis: false
      Name: Current View
      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.3597939908504486
+      Pitch: 0.39479339122772217
      Target Frame: <Fixed Frame>
-      Yaw: 5.247860431671143
+      Yaw: 5.172863483428955
    Saved: ~
 Window Geometry:
  Displays:
@ -466,5 +661,5 @@ Window Geometry:
  Views:
    collapsed: true
  Width: 1516
-  X: 866
-  Y: 41
+  X: 1039
+  Y: 114
--- a/scripts/bt_sim_node.py
+++ b/scripts/bt_sim_node.py
@ -335,6 +335,8 @@ def apply_noise(img, k=1000, theta=0.001, sigma=0.005, l=4.0):
 def main():
    rospy.init_node("bt_sim")
    server = BtSimNode()
+    # server.seed(SeedRequest(1))
+    # server.reset(ResetRequest())
    server.run()