class MobileManipulatorControlManager(object): def __init__(self): self._tf_buffer = tf2_ros.Buffer() self._tf_listener = tf2_ros.TransformListener(self._tf_buffer) self._ope_state = OperatorState() self._ope_state_subscriber = rospy.Subscriber(rospy.get_param(PARAM_NAME_OPE_STATE_TOPIC), OperatorState, self._ope_state_callback) self._bs_controller = BlackshipController(rospy.get_param(PARAM_NAME_BS_CONTROL_METHOD), self._tf_buffer).controller self._mico_controller = MicoController(rospy.get_param(PARAM_NAME_MICO_CONTROL_METHOD), self._tf_buffer).controller self._mico_bs_controller = MicoBSController(rospy.get_param(PARAM_NAME_MICOBS_CONTROL_METHOD), self._tf_buffer).controller self._mico_bs_stop_controller = MicoBSController(rospy.get_param(PARAM_NAME_MICOBS_CTRL_METHOD_FORCE_STOP), self._tf_buffer).controller self.is_activated = False def _ope_state_callback(self, ope_state): self._ope_state = ope_state def activate(self): is_successes = [self._bs_controller.activate().is_activated, self._mico_controller.activate().is_activated, self._mico_bs_controller.activate().is_activated] self.is_activated = all(is_successes) rospy.loginfo('activated') return self def publish_input(self): if not self.is_activated: self.activate() if self._ope_state.is_manipulating: if self._mico_controller.is_reachable: self._mico_controller.update().publish_input() else: self._mico_bs_controller.update().publish_input() if self._ope_state.is_walking: self._bs_controller.update().publish_input() if self._ope_state.is_stopping: self._mico_bs_stop_controller.update().publish_input() #TODO アームは単体で動かして周り見回す return self
def __init__(self): self._tf_buffer = tf2_ros.Buffer() self._tf_listener = tf2_ros.TransformListener(self._tf_buffer) self._ope_state = OperatorState() self._ope_state_subscriber = rospy.Subscriber(rospy.get_param(PARAM_NAME_OPE_STATE_TOPIC), OperatorState, self._ope_state_callback) self._bs_controller = BlackshipController(rospy.get_param(PARAM_NAME_BS_CONTROL_METHOD), self._tf_buffer).controller self._mico_controller = MicoController(rospy.get_param(PARAM_NAME_MICO_CONTROL_METHOD), self._tf_buffer).controller self._mico_bs_controller = MicoBSController(rospy.get_param(PARAM_NAME_MICOBS_CONTROL_METHOD), self._tf_buffer).controller self._mico_bs_stop_controller = MicoBSController(rospy.get_param(PARAM_NAME_MICOBS_CTRL_METHOD_FORCE_STOP), self._tf_buffer).controller self.is_activated = False