From 979b46400ab77cc95512f97bbe84d70c34ce464c Mon Sep 17 00:00:00 2001 From: Michel Breyer <10465414+mbreyer@users.noreply.github.com> Date: Tue, 8 Feb 2022 11:32:42 +0100 Subject: [PATCH] Minor cleanup --- cfg/active_grasp.rviz | 87 +---------------------------------------- scripts/bt_sim_node.py | 2 +- src/active_grasp/nbv.py | 10 +---- 3 files changed, 3 insertions(+), 96 deletions(-) diff --git a/cfg/active_grasp.rviz b/cfg/active_grasp.rviz index bf77bd9..39b8940 100644 --- a/cfg/active_grasp.rviz +++ b/cfg/active_grasp.rviz @@ -147,87 +147,6 @@ 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_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: hand - 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: "" @@ -497,11 +416,7 @@ Window Geometry: Hide Right Dock: true InfraImage: collapsed: false - MotionPlanning: - collapsed: false - MotionPlanning - Trajectory Slider: - collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: diff --git a/scripts/bt_sim_node.py b/scripts/bt_sim_node.py index e87cb66..768b115 100755 --- a/scripts/bt_sim_node.py +++ b/scripts/bt_sim_node.py @@ -294,7 +294,7 @@ class CameraPlugin(Plugin): super().__init__(rate) self.camera = camera self.name = name - self.cam_noise = rospy.get_param("~cam_noise", True) + self.cam_noise = rospy.get_param("~cam_noise", False) self.cv_bridge = cv_bridge.CvBridge() self.init_publishers() diff --git a/src/active_grasp/nbv.py b/src/active_grasp/nbv.py index 713eef8..f282608 100644 --- a/src/active_grasp/nbv.py +++ b/src/active_grasp/nbv.py @@ -125,7 +125,7 @@ class NextBestView(MultiViewPolicy): view_candidates.append(view) return view_candidates - def ig_fn(self, view, downsample, vis=False): + def ig_fn(self, view, downsample): tsdf_grid, voxel_size = self.tsdf.get_grid(), self.tsdf.voxel_size tsdf_grid = -1.0 + 2.0 * tsdf_grid # Open3D maps tsdf to [0,1] @@ -178,14 +178,6 @@ class NextBestView(MultiViewPolicy): tsdfs = tsdf_grid[i, j, k] ig = np.logical_and(tsdfs > -1.0, tsdfs < 0.0).sum() - if vis: - dirs = [] - for u in range(u_min, u_max): - for v in range(v_min, v_max): - d = np.asarray([(u - cx) / fx, (v - cy) / fy, 1.0]) - dirs.append(ori @ (d / np.linalg.norm(d))) - self.vis.rays(self.task_frame, pos, dirs, t_max) - return ig def cost_fn(self, view):