diff --git a/ros/kxr_controller/CMakeLists.txt b/ros/kxr_controller/CMakeLists.txt index 3f276218..9f805bc0 100644 --- a/ros/kxr_controller/CMakeLists.txt +++ b/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( diff --git a/ros/kxr_controller/action/ControlAirBoard.action b/ros/kxr_controller/action/ControlAirBoard.action new file mode 100644 index 00000000..a07fe6d4 --- /dev/null +++ b/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 diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py index 533780f5..ac124225 100644 --- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py +++ b/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 diff --git a/ros/kxr_controller/python/kxr_controller/kxr_interface.py b/ros/kxr_controller/python/kxr_controller/kxr_interface.py index 26940c26..75622bc0 100644 --- a/ros/kxr_controller/python/kxr_controller/kxr_interface.py +++ b/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"