From fcc5045671963098847b0266379adad6847cacee Mon Sep 17 00:00:00 2001
From: Michel Breyer <michel.p.breyer@gmail.com>
Date: Fri, 8 Oct 2021 15:41:50 +0200
Subject: [PATCH] Read nbv params from the rosparam server

---
 cfg/active_grasp.yaml          | 13 ++++++++++---
 src/active_grasp/controller.py |  2 +-
 src/active_grasp/nbv.py        |  4 ++--
 src/active_grasp/policy.py     |  5 ++++-
 4 files changed, 17 insertions(+), 7 deletions(-)

diff --git a/cfg/active_grasp.yaml b/cfg/active_grasp.yaml
index 68e4527..aad4ec8 100644
--- a/cfg/active_grasp.yaml
+++ b/cfg/active_grasp.yaml
@@ -1,6 +1,6 @@
 bt_sim:
   gui: False
-  cam_noise: True
+  cam_noise: False
   scene: kitchen.yaml
 
 grasp_controller:
@@ -8,14 +8,21 @@ grasp_controller:
   ee_grasp_offset: [0.0, 0.0, -0.383, 0.924, 0.0, 0.0, 0.065]  # offset to panda_link8
   control_rate: 30
   linear_vel: 0.05
-  policy_rate: 2
   camera:
     frame_id: camera_depth_optical_frame
     info_topic: /camera/depth/camera_info
     depth_topic: /camera/depth/image_rect_raw
     min_z_dist: 0.3
-    
+
 vgn:
   model: $(find vgn)/assets/models/vgn_conv.pth
   finger_depth: 0.05
   qual_threshold: 0.9
+
+policy:
+  rate: 2
+  window_size: 6
+
+nbv_grasp:
+  max_views: 30
+  min_gain: 0.0
diff --git a/src/active_grasp/controller.py b/src/active_grasp/controller.py
index 54536fd..84521fc 100644
--- a/src/active_grasp/controller.py
+++ b/src/active_grasp/controller.py
@@ -34,7 +34,7 @@ class GraspController:
         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")
-        self.policy_rate = rospy.get_param("~policy_rate")
+        self.policy_rate = rospy.get_param("policy/rate")
 
     def init_service_proxies(self):
         self.reset_env = rospy.ServiceProxy("reset", Reset)
diff --git a/src/active_grasp/nbv.py b/src/active_grasp/nbv.py
index a3b6181..24964c6 100644
--- a/src/active_grasp/nbv.py
+++ b/src/active_grasp/nbv.py
@@ -57,8 +57,8 @@ class NextBestView(MultiViewPolicy):
     def __init__(self):
         super().__init__()
         self.min_z_dist = rospy.get_param("~camera/min_z_dist")
-        self.max_views = 50
-        self.min_gain = 0.0
+        self.max_views = rospy.get_param("nbv_grasp/max_views")
+        self.min_gain = rospy.get_param("nbv_grasp/min_gain")
 
     def activate(self, bbox, view_sphere):
         super().activate(bbox, view_sphere)
diff --git a/src/active_grasp/policy.py b/src/active_grasp/policy.py
index add82ca..adf1af1 100644
--- a/src/active_grasp/policy.py
+++ b/src/active_grasp/policy.py
@@ -118,9 +118,12 @@ class SingleViewPolicy(Policy):
 
 
 class MultiViewPolicy(Policy):
+    def __init__(self):
+        super().__init__()
+        self.T = rospy.get_param("policy/window_size")
+
     def activate(self, bbox, view_sphere):
         super().activate(bbox, view_sphere)
-        self.T = 10  # Window size of grasp prediction history
         self.qual_hist = np.zeros((self.T,) + (40,) * 3, np.float32)
 
     def integrate(self, img, x, q):