From 1fb2eaf2b64ed25d1d892371dde4b14e55183b51 Mon Sep 17 00:00:00 2001 From: Michel Breyer Date: Wed, 8 Sep 2021 16:50:53 +0200 Subject: [PATCH] Define custom scenes via yaml files --- active_grasp/baselines.py | 2 +- active_grasp/controller.py | 11 +- active_grasp/simulation.py | 54 +++- cfg/active_grasp.yaml | 4 +- cfg/test.yaml | 9 + launch/active_grasp.rviz | 627 ++++++++++++++++++++++++------------- scripts/bt_sim_node.py | 2 + 7 files changed, 465 insertions(+), 244 deletions(-) create mode 100644 cfg/test.yaml diff --git a/active_grasp/baselines.py b/active_grasp/baselines.py index a3f56c6..95f716e 100644 --- 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]) diff --git a/active_grasp/controller.py b/active_grasp/controller.py index 885f82c..73e48b8 100644 --- 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): diff --git a/active_grasp/simulation.py b/active_grasp/simulation.py index 229920e..09854d1 100644 --- 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)) diff --git a/cfg/active_grasp.yaml b/cfg/active_grasp.yaml index 4e4993f..b3158df 100644 --- 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 diff --git a/cfg/test.yaml b/cfg/test.yaml new file mode 100644 index 0000000..e7f091f --- /dev/null +++ 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] diff --git a/launch/active_grasp.rviz b/launch/active_grasp.rviz index e226729..65dea62 100644 --- 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: 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: - 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 diff --git a/scripts/bt_sim_node.py b/scripts/bt_sim_node.py index 736c7c2..4ead748 100755 --- 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()