Define custom scenes via yaml files

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])

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)
+        # 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))
+        # 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):

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):
+    def load_support(self, pos):
-        uid = p.loadURDF(str(urdf), pos, ori.as_quat(), globalScaling=scale)
+        self.support_uid = p.loadURDF(str(self.support_urdf), pos, globalScaling=0.2)
-        self.support_uid = uid
-        return uid
 
     def remove_support(self):
-        if self.support_uid != -1:
+        p.removeBody(self.support_uid)
-            p.removeBody(self.support_uid)
-            self.support_uid = -1
 
     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.object_urdfs = find_urdfs(self.vgn_urdfs_dir / "packed" / "test")
-        self.support_urdf = self.urdfs_path / "setup/plane.urdf"
 
     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 = {
+class CustomScene(Scene):
-    "random": RandomScene,
+    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:
+    if scene_id == "random":
-        return scenes[scene_id]()
+        return RandomScene()
+    elif scene_id.endswith(".yaml"):
+        return CustomScene(scene_id)
     else:
         raise ValueError("Unknown scene {}.".format(scene_id))

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

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]

launch/active_grasp.rviz

@@ -4,8 +4,7 @@ Panels:
     Name: Displays
     Property Tree Widget:
       Expanded:
-        - /PlanningScene1/Scene Geometry1
+        - /TF1/Frames1
-        - /Markers1/Namespaces1
       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
+    - Alpha: 1
-      Class: moveit_rviz_plugin/MotionPlanning
+      Autocompute Intensity Bounds: true
-      Enabled: false
+      Autocompute Value Bounds:
-      Move Group Namespace: ""
+        Max Value: 10
-      MoveIt_Allow_Approximate_IK: false
+        Min Value: -10
-      MoveIt_Allow_External_Program: false
+        Value: true
-      MoveIt_Allow_Replanning: false
+      Axis: Z
-      MoveIt_Allow_Sensor_Positioning: false
+      Channel Name: intensity
-      MoveIt_Planning_Attempts: 10
+      Class: rviz/PointCloud2
-      MoveIt_Planning_Time: 5
+      Color: 255; 255; 255
-      MoveIt_Use_Cartesian_Path: false
+      Color Transformer: Intensity
-      MoveIt_Use_Constraint_Aware_IK: false
+      Decay Time: 0
-      MoveIt_Warehouse_Host: 127.0.0.1
+      Enabled: true
-      MoveIt_Warehouse_Port: 33829
+      Invert Rainbow: false
-      MoveIt_Workspace:
+      Max Color: 255; 255; 255
-        Center:
+      Min Color: 0; 0; 0
-          X: 0
+      Name: SceneCloud
-          Y: 0
+      Position Transformer: XYZ
-          Z: 0
+      Queue Size: 10
-        Size:
+      Selectable: true
-          X: 2
+      Size (Pixels): 3
-          Y: 2
+      Size (m): 0.009999999776482582
-          Z: 2
+      Style: Spheres
-      Name: MotionPlanning
+      Topic: /scene_cloud
-      Planned Path:
+      Unreliable: false
-        Color Enabled: false
+      Use Fixed Frame: true
-        Interrupt Display: false
+      Use rainbow: true
-        Links:
+      Value: true
-          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
     - 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
+  X: 1039
-  Y: 41
+  Y: 114

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()