From 0b3d7f2b2244a7bcdbf33b9b4e614908c907031a Mon Sep 17 00:00:00 2001 From: 0nhc Date: Sun, 13 Oct 2024 00:37:45 -0500 Subject: [PATCH] successfully applies segmentation input for policy class --- README.md | 14 ++++ cfg/active_grasp.rviz | 74 ++++++++++++++++---- cfg/active_grasp.yaml | 1 + src/active_grasp/active_perception_policy.py | 2 +- src/active_grasp/controller.py | 19 +++-- src/active_grasp/nbv.py | 2 +- src/active_grasp/policy.py | 4 +- 7 files changed, 92 insertions(+), 24 deletions(-) diff --git a/README.md b/README.md index ca43770..ba5e91e 100644 --- a/README.md +++ b/README.md @@ -25,6 +25,20 @@ cd /src/active_grasp/src/active_grasp/active_perception/modules pip install -e . ``` +# Updated Features +* Added our baseline: src/active_grasp/active_perception_policy.py +* Added RGB and Segmentation image publishers. The segmentation ID 1 corresponds to the grasping target object. + + + + + + + + + + + # Closed-Loop Next-Best-View Planning for Target-Driven Grasping This repository contains the implementation of our IROS 2022 submission, _"Closed-Loop Next-Best-View Planning for Target-Driven Grasping"_. [[Paper](http://arxiv.org/abs/2207.10543)][[Video](https://youtu.be/67W_VbSsAMQ)] diff --git a/cfg/active_grasp.rviz b/cfg/active_grasp.rviz index 39b8940..02be1e3 100644 --- a/cfg/active_grasp.rviz +++ b/cfg/active_grasp.rviz @@ -23,7 +23,6 @@ Panels: Name: Views Splitter Ratio: 0.5 - Class: rviz/Time - Experimental: false Name: Time SyncMode: 0 SyncSource: SceneCloud @@ -54,6 +53,8 @@ Visualization Manager: Value: true - Class: rviz/TF Enabled: false + Filter (blacklist): "" + Filter (whitelist): "" Frame Timeout: 15 Frames: All Enabled: false @@ -86,6 +87,15 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + panda_hand_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false panda_leftfinger: Alpha: 1 Show Axes: false @@ -96,46 +106,85 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + panda_link0_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true panda_link1: Alpha: 1 Show Axes: false Show Trail: false Value: true + panda_link1_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true panda_link2: Alpha: 1 Show Axes: false Show Trail: false Value: true + panda_link2_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true panda_link3: Alpha: 1 Show Axes: false Show Trail: false Value: true + panda_link3_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true panda_link4: Alpha: 1 Show Axes: false Show Trail: false Value: true + panda_link4_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true panda_link5: Alpha: 1 Show Axes: false Show Trail: false Value: true + panda_link5_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true panda_link6: Alpha: 1 Show Axes: false Show Trail: false Value: true + panda_link6_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true panda_link7: Alpha: 1 Show Axes: false Show Trail: false Value: true + panda_link7_sc: + 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 @@ -242,10 +291,7 @@ Visualization Manager: Name: Markers Namespaces: bbox: true - grasp: true - ig_views: true - path: true - views: true + roi: true Queue Size: 100 Value: true - Alpha: 1 @@ -364,7 +410,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 1.2000000476837158 + Distance: 1.3440001010894775 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -372,9 +418,9 @@ Visualization Manager: Value: false Field of View: 0.7799999713897705 Focal Point: - X: 0.44999998807907104 - Y: 0 - Z: 0.4000000059604645 + X: 0.40848347544670105 + Y: 0.02209819294512272 + Z: 0.4073639214038849 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false @@ -416,7 +462,7 @@ Window Geometry: Hide Right Dock: true InfraImage: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -425,6 +471,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1920 - X: 0 - Y: 27 + Width: 1157 + X: 1309 + Y: 90 diff --git a/cfg/active_grasp.yaml b/cfg/active_grasp.yaml index 2e442e6..7f7edc1 100644 --- a/cfg/active_grasp.yaml +++ b/cfg/active_grasp.yaml @@ -16,6 +16,7 @@ grasp_controller: frame_id: camera_depth_optical_frame info_topic: /camera/depth/camera_info depth_topic: /camera/depth/image_rect_raw + seg_topic: /camera/segmentation/image_rect_raw min_z_dist: 0.3 cartesian_velocity_controller: diff --git a/src/active_grasp/active_perception_policy.py b/src/active_grasp/active_perception_policy.py index a725153..b86887a 100644 --- a/src/active_grasp/active_perception_policy.py +++ b/src/active_grasp/active_perception_policy.py @@ -18,7 +18,7 @@ class ActivePerceptionPolicy(MultiViewPolicy): def activate(self, bbox, view_sphere): super().activate(bbox, view_sphere) - def update(self, img, x, q): + def update(self, img, seg, x, q): self.depth_image_to_ap_input(img) # if len(self.views) > self.max_views or self.best_grasp_prediction_is_stable(): # self.done = True diff --git a/src/active_grasp/controller.py b/src/active_grasp/controller.py index 3205f9c..852ae09 100644 --- a/src/active_grasp/controller.py +++ b/src/active_grasp/controller.py @@ -32,6 +32,7 @@ class GraspController: self.T_grasp_ee = Transform.from_list(rospy.get_param("~ee_grasp_offset")).inv() self.cam_frame = rospy.get_param("~camera/frame_id") self.depth_topic = rospy.get_param("~camera/depth_topic") + self.seg_topic = rospy.get_param("~camera/seg_topic") self.min_z_dist = rospy.get_param("~camera/min_z_dist") self.control_rate = rospy.get_param("~control_rate") self.linear_vel = rospy.get_param("~linear_vel") @@ -71,10 +72,14 @@ class GraspController: def init_camera_stream(self): self.cv_bridge = cv_bridge.CvBridge() - rospy.Subscriber(self.depth_topic, Image, self.sensor_cb, queue_size=1) + rospy.Subscriber(self.depth_topic, Image, self.depth_cb, queue_size=1) + rospy.Subscriber(self.seg_topic, Image, self.seg_cb, queue_size=1) - def sensor_cb(self, msg): + def depth_cb(self, msg): self.latest_depth_msg = msg + + def seg_cb(self, msg): + self.latest_seg_msg = msg def run(self): bbox = self.reset() @@ -102,8 +107,8 @@ class GraspController: timer = rospy.Timer(rospy.Duration(1.0 / self.control_rate), self.send_vel_cmd) r = rospy.Rate(self.policy_rate) while not self.policy.done: - img, pose, q = self.get_state() - self.policy.update(img, pose, q) + depth_img, seg_image, pose, q = self.get_state() + self.policy.update(depth_img, seg_image, pose, q) r.sleep() rospy.sleep(0.2) # Wait for a zero command to be sent to the robot. self.policy.deactivate() @@ -113,9 +118,11 @@ class GraspController: def get_state(self): q, _ = self.arm.get_state() msg = copy.deepcopy(self.latest_depth_msg) - img = self.cv_bridge.imgmsg_to_cv2(msg).astype(np.float32) * 0.001 + depth_img = self.cv_bridge.imgmsg_to_cv2(msg).astype(np.float32) * 0.001 + msg = copy.deepcopy(self.latest_seg_msg) + seg_img = self.cv_bridge.imgmsg_to_cv2(msg).astype(np.float32) pose = tf.lookup(self.base_frame, self.cam_frame, msg.header.stamp) - return img, pose, q + return depth_img, seg_img, pose, q def send_vel_cmd(self, event): if self.policy.x_d is None or self.policy.done: diff --git a/src/active_grasp/nbv.py b/src/active_grasp/nbv.py index f282608..902b0c5 100644 --- a/src/active_grasp/nbv.py +++ b/src/active_grasp/nbv.py @@ -84,7 +84,7 @@ class NextBestView(MultiViewPolicy): def activate(self, bbox, view_sphere): super().activate(bbox, view_sphere) - def update(self, img, x, q): + def update(self, img, seg, x, q): if len(self.views) > self.max_views or self.best_grasp_prediction_is_stable(): self.done = True else: diff --git a/src/active_grasp/policy.py b/src/active_grasp/policy.py index 0632523..f0fca95 100644 --- a/src/active_grasp/policy.py +++ b/src/active_grasp/policy.py @@ -75,7 +75,7 @@ class Policy: rospy.sleep(1.0) # Wait for tf tree to be updated self.vis.roi(self.task_frame, 0.3) - def update(self, img, x, q): + def update(self, img, seg, x, q): raise NotImplementedError def filter_grasps(self, out, q): @@ -106,7 +106,7 @@ def select_best_grasp(grasps, qualities): class SingleViewPolicy(Policy): - def update(self, img, x, q): + def update(self, img, seg, x, q): linear, _ = compute_error(self.x_d, x) if np.linalg.norm(linear) < 0.02: self.views.append(x)