Add hw reset node

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" />

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()