From d8ea9683db6572576a8b070ea4f8b4cc0e80d7dc Mon Sep 17 00:00:00 2001
From: Michel Breyer <10465414+mbreyer@users.noreply.github.com>
Date: Fri, 3 Dec 2021 13:52:07 +0100
Subject: [PATCH] Load scene config

---
 cfg/active_grasp.yaml          |  4 ++++
 cfg/hw/scene01.yaml            |  4 ++++
 launch/env.launch              |  6 ++---
 scripts/hw_node.py             | 40 ++++++++++++++++++++++++----------
 src/active_grasp/controller.py |  2 ++
 5 files changed, 41 insertions(+), 15 deletions(-)
 create mode 100644 cfg/hw/scene01.yaml

diff --git a/cfg/active_grasp.yaml b/cfg/active_grasp.yaml
index 18f82cc..aeb99ca 100644
--- a/cfg/active_grasp.yaml
+++ b/cfg/active_grasp.yaml
@@ -4,6 +4,10 @@ bt_sim:
   gripper_force: 10
   scene: random
 
+hw:
+  roi_calib_file: $(find active_grasp)/cfg/hw/T_base_tag.txt
+  scene_file: $(find active_grasp)/cfg/hw/scene01.yaml
+
 grasp_controller:
   base_frame_id: panda_link0
   ee_grasp_offset: [0.0, 0.0, -0.383, 0.924, 0.0, 0.0, 0.065]  # offset to panda_link8
diff --git a/cfg/hw/scene01.yaml b/cfg/hw/scene01.yaml
new file mode 100644
index 0000000..a0d12b5
--- /dev/null
+++ b/cfg/hw/scene01.yaml
@@ -0,0 +1,4 @@
+target:
+  min: [0.1, 0.1, 0.0]
+  max: [0.2, 0.2, 0.1]
+q0: [0.0, -0.79, 0.0, -2.356, 0.0, 1.57, 0.79]
diff --git a/launch/env.launch b/launch/env.launch
index c40f708..6447b62 100644
--- a/launch/env.launch
+++ b/launch/env.launch
@@ -1,6 +1,6 @@
 <?xml version="1.0" ?>
 <launch>
-  <arg name="simulation" default="true" />
+  <arg name="sim" default="true" />
   <arg name="launch_rviz" default="false" />
 
   <!-- Load parameters -->
@@ -11,14 +11,14 @@
   <include file="$(find panda_moveit_config)/launch/move_group.launch" />
 
   <!-- Simulated environment -->
-  <group if="$(arg simulation)">
+  <group if="$(arg sim)">
     <param name="robot_description" command="$(find xacro)/xacro $(find active_grasp)/assets/franka/panda_arm_hand.urdf.xacro" />
     <node pkg="active_grasp" type="bt_sim_node.py" name="bt_sim" output="screen" />
     <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
   </group>
 
   <!-- Real environment -->
-  <group unless="$(arg simulation)">
+  <group unless="$(arg sim)">
     <node pkg="active_grasp" type="hw_node.py" name="hw" output="screen" />
   </group>
 
diff --git a/scripts/hw_node.py b/scripts/hw_node.py
index bdc118f..0daba1d 100755
--- a/scripts/hw_node.py
+++ b/scripts/hw_node.py
@@ -1,24 +1,34 @@
 #!/usr/bin/env python3
 
 from controller_manager_msgs.srv import *
+import numpy as np
 import rospy
 
 from active_grasp.bbox import AABBox, to_bbox_msg
 from active_grasp.srv import *
-
-
+from robot_helpers.io import load_yaml
 from robot_helpers.ros.moveit import MoveItClient
+from robot_helpers.ros.panda import PandaGripperClient
+from robot_helpers.spatial import Transform
 
 
 class HwNode:
     def __init__(self):
+        self.load_parameters()
+        self.init_robot_connection()
         self.advertise_services()
-        self.switch_controller = rospy.ServiceProxy(
+        rospy.spin()
+
+    def load_parameters(self):
+        cfg = rospy.get_param("hw")
+        self.T_base_roi = Transform.from_matrix(np.loadtxt(cfg["roi_calib_file"]))
+        self.scene_config = load_yaml(cfg["scene_file"])
+
+    def init_robot_connection(self):
+        self.switch_to_joint_trajectory_controller = rospy.ServiceProxy(
             "controller_manager/switch_controller", SwitchController
         )
-
         self.moveit = MoveItClient("panda_arm")
-        rospy.spin()
 
     def advertise_services(self):
         rospy.Service("seed", Seed, self.seed)
@@ -29,18 +39,24 @@ class HwNode:
         return SeedResponse()
 
     def reset(self, req):
+        # Move to the initial configuration
+        self.switch_to_joint_trajectory_controller()
+        self.moveit.goto(self.scene_config["q0"])
+
+        # Construct bounding box
+        bbox_min = self.T_base_roi.apply(self.scene_config["target"]["min"])
+        bbox_max = self.T_base_roi.apply(self.scene_config["target"]["max"])
+        bbox = AABBox(bbox_min, bbox_max)
+
+        return ResetResponse(to_bbox_msg(bbox))
+
+    def switch_to_joint_trajectory_controller(self):
         req = SwitchControllerRequest()
         req.start_controllers = ["position_joint_trajectory_controller"]
         req.stop_controllers = ["cartesian_velocity_controller"]
+        req.strictness = 1
         self.switch_controller(req)
 
-        # Move to the initial configuration
-        self.moveit.goto([0.0, -0.79, 0.0, -2.356, 0.0, 1.57, 0.79])
-
-        # Detect target
-        bbox = AABBox([0.4, -0.1, 0.0], [0.5, 0.1, 0.1])
-        return ResetResponse(to_bbox_msg(bbox))
-
 
 def main():
     rospy.init_node("hw")
diff --git a/src/active_grasp/controller.py b/src/active_grasp/controller.py
index 7185881..68125a0 100644
--- a/src/active_grasp/controller.py
+++ b/src/active_grasp/controller.py
@@ -59,12 +59,14 @@ class GraspController:
         req = SwitchControllerRequest()
         req.start_controllers = ["cartesian_velocity_controller"]
         req.stop_controllers = ["position_joint_trajectory_controller"]
+        req.strictness = 1
         self.switch_controller(req)
 
     def switch_to_joint_trajectory_control(self):
         req = SwitchControllerRequest()
         req.start_controllers = ["position_joint_trajectory_controller"]
         req.stop_controllers = ["cartesian_velocity_controller"]
+        req.strictness = 1
         self.switch_controller(req)
 
     def init_camera_stream(self):