Add control air board action

ros/kxr_controller/CMakeLists.txt

@@ -66,6 +66,7 @@ catkin_python_setup()
 add_action_files(
   DIRECTORY action
   FILES ServoOnOff.action AdjustAngleVector.action Stretch.action PressureControl.action
+  ControlAirBoard.action
 )
 
 add_message_files(

ros/kxr_controller/action/ControlAirBoard.action

@@ -0,0 +1,5 @@
+uint16 board_idx  # Do not use board id? (Currently, bus-connected air board is not supported)
+string action_name
+---
+---
+bool succeeded

ros/kxr_controller/node_scripts/rcb4_ros_bridge.py

@@ -19,6 +19,8 @@
 from kxr_controller.cfg import KXRParameteresConfig as Config
 from kxr_controller.msg import AdjustAngleVectorAction
 from kxr_controller.msg import AdjustAngleVectorResult
+from kxr_controller.msg import ControlAirBoardAction
+from kxr_controller.msg import ControlAirBoardResult
 from kxr_controller.msg import PressureControl
 from kxr_controller.msg import PressureControlAction
 from kxr_controller.msg import PressureControlResult
@@ -354,6 +356,15 @@ def setup_stretch_and_pressure_control_servers(self):
                 queue_size=1,
             )
 
+            self.control_air_board_server = actionlib.SimpleActionServer(
+                self.base_namespace + "/fullbody_controller/control_air_board_interface",
+                ControlAirBoardAction,
+                execute_cb=self.control_air_board_callback,
+                auto_start=False,
+            )
+            rospy.sleep(0.1)
+            self.control_air_board_server.start()
+
             rospy.set_param(self.base_namespace + "/air_board_ids", self.air_board_ids)
             self.pressure_control_state = {}
             for idx in self.air_board_ids:
@@ -918,6 +929,23 @@ def pressure_control_callback(self, goal):
         self.pressure_control_thread.start()
         return self.pressure_control_server.set_succeeded(PressureControlResult())
 
+    def control_air_board_callback(self, goal):
+        if not self.interface.is_opened():
+            return self.control_air_board_server.set_aborted(
+                ControlAirBoardResult())
+        if hasattr(self.interface, goal.action_name):
+            success = serial_call_with_retry(
+                getattr(self.interface, goal.action_name),
+                board_idx=goal.board_idx)
+            if success is None:
+                return self.control_air_board_server.set_aborted(
+                    ControlAirBoardResult())
+        else:
+            rospy.logwarn(f'Could not find {goal.action_name} and control air board.')
+            return self.control_air_board_server.set_aborted(
+                ControlAirBoardResult())
+        return self.control_air_board_server.set_succeeded(ControlAirBoardResult())
+
     def publish_imu_message(self):
         if self.publish_imu is False or self.imu_publisher.get_num_connections() == 0:
             return

ros/kxr_controller/python/kxr_controller/kxr_interface.py

@@ -10,6 +10,8 @@
 
 from kxr_controller.msg import AdjustAngleVectorAction
 from kxr_controller.msg import AdjustAngleVectorGoal
+from kxr_controller.msg import ControlAirBoardAction
+from kxr_controller.msg import ControlAirBoardGoal
 from kxr_controller.msg import PressureControlAction
 from kxr_controller.msg import PressureControlGoal
 from kxr_controller.msg import ServoOnOffAction
@@ -65,6 +67,15 @@ def __init__(self, *args, **kwargs):
                 self.pressure_topic_name_base = (
                     namespace + "/fullbody_controller/pressure/"
                 )
+
+                self.control_air_board_client = actionlib.SimpleActionClient(
+                    namespace + "/fullbody_controller/control_air_board_interface",
+                    ControlAirBoardAction,
+                )
+                self.enabled_control_air_board = True
+                if not self.control_air_board_client.wait_for_server(timeout):
+                    rospy.logerr("ControlAirBoard action server not available.")
+                    self.enabled_control_air_board = False
             # Adjust angle vector client
             self.adjust_angle_vector_client = actionlib.SimpleActionClient(
                 namespace + "/fullbody_controller/adjust_angle_vector_interface",
@@ -198,6 +209,21 @@ def read_pressure(self, board_idx):
             self.pressure_topic_name_base + f"{board_idx}", std_msgs.msg.Float32
         )
 
+    def control_air_board(self, board_idx, action_name):
+        if not self.enabled_control_air_board:
+            rospy.logerr("ControlAirBoard action server not available.")
+            return
+        goal = ControlAirBoardGoal(
+            board_idx=board_idx,
+            action_name=action_name,
+        )
+        client = self.control_air_board_client
+        if client.get_state() == actionlib_msgs.msg.GoalStatus.ACTIVE:
+            client.cancel_goal()
+            client.wait_for_result(timeout=rospy.Duration(10))
+        client.send_goal(goal)
+        return client
+
     @property
     def fullbody_controller(self):
         cont_name = "fullbody_controller"