From f361f1ee92a25842f2ba87f6e15ead41795451ee Mon Sep 17 00:00:00 2001
From: Michel Breyer <10465414+mbreyer@users.noreply.github.com>
Date: Wed, 24 Nov 2021 16:26:26 +0100
Subject: [PATCH] Add hw reset node

---
 launch/env.launch  |  5 +++++
 scripts/hw_node.py | 51 ++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 56 insertions(+)
 create mode 100755 scripts/hw_node.py

diff --git a/launch/env.launch b/launch/env.launch
index ac085c4..f4a9eea 100644
--- a/launch/env.launch
+++ b/launch/env.launch
@@ -15,6 +15,11 @@
     <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
   </group>
 
+  <!-- Real environment -->
+  <group unless="$(arg simulation)">
+    <node pkg="active_grasp" type="hw_node.py" name="hw" output="screen" />
+  </group>
+
   <!-- Launch MoveIt -->
   <node pkg="tf2_ros" type="static_transform_publisher" name="to_panda" args="0 0 0 0 0 0  world panda_link0" />
   <include file="$(find panda_moveit_config)/launch/move_group.launch" />
diff --git a/scripts/hw_node.py b/scripts/hw_node.py
new file mode 100755
index 0000000..bdc118f
--- /dev/null
+++ b/scripts/hw_node.py
@@ -0,0 +1,51 @@
+#!/usr/bin/env python3
+
+from controller_manager_msgs.srv import *
+import rospy
+
+from active_grasp.bbox import AABBox, to_bbox_msg
+from active_grasp.srv import *
+
+
+from robot_helpers.ros.moveit import MoveItClient
+
+
+class HwNode:
+    def __init__(self):
+        self.advertise_services()
+        self.switch_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)
+        rospy.Service("reset", Reset, self.reset)
+
+    def seed(self, req):
+        # Nothing to do
+        return SeedResponse()
+
+    def reset(self, req):
+        req = SwitchControllerRequest()
+        req.start_controllers = ["position_joint_trajectory_controller"]
+        req.stop_controllers = ["cartesian_velocity_controller"]
+        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")
+    HwNode()
+
+
+if __name__ == "__main__":
+    main()