optimize code
This commit is contained in:
@@ -139,18 +139,18 @@ class CtrlGuiNode(Node):
|
||||
self.messages_sent += 1
|
||||
self.file_logger.info(f"Published message #{self.messages_sent} to /led_cmd: {str(text)}")
|
||||
|
||||
def rebuild_now(self, type: str, config: str, param: str) -> None:
|
||||
self.rebuild_requests += 1
|
||||
self.file_logger.info('Rebuild BehaviorTree now')
|
||||
self.file_logger.info(f'Total rebuild requests: {str(self.rebuild_requests)}')
|
||||
# def rebuild_now(self, type: str, config: str, param: str) -> None:
|
||||
# self.rebuild_requests += 1
|
||||
# self.file_logger.info('Rebuild BehaviorTree now')
|
||||
# self.file_logger.info(f'Total rebuild requests: {str(self.rebuild_requests)}')
|
||||
|
||||
request = BtRebuild.Request()
|
||||
request.type = type #"Remote"
|
||||
request.config = config #"MoveWaist"
|
||||
request.param = param #f"move_pitch_degree: 10\nmove_yaw_degree: 10\n"
|
||||
# request = BtRebuild.Request()
|
||||
# request.type = type #"Remote"
|
||||
# request.config = config #"MoveWaist"
|
||||
# request.param = param #f"move_pitch_degree: 10\nmove_yaw_degree: 10\n"
|
||||
|
||||
future = self.run_trigger_.call_async(request)
|
||||
future.add_done_callback(self._on_rebuild_response)
|
||||
# future = self.run_trigger_.call_async(request)
|
||||
# future.add_done_callback(self._on_rebuild_response)
|
||||
|
||||
# Log to file
|
||||
self.file_logger.info(f"Rebuild requested. Total rebuild requests: {str(self.rebuild_requests)}")
|
||||
@@ -206,21 +206,8 @@ def build_ui(node: CtrlGuiNode) -> None:
|
||||
dialog.open()
|
||||
|
||||
with ui.row():
|
||||
# rebuild button
|
||||
with ui.column():
|
||||
with ui.row():
|
||||
pass
|
||||
def rebuild() -> None:
|
||||
try:
|
||||
node.file_logger.info('Running rebuild_now...')
|
||||
node.rebuild_now("Trigger", "SwitchSchedule", "")
|
||||
except Exception as e:
|
||||
node.file_logger.error(f'Failed to trigger rebuild: {str(e)}')
|
||||
|
||||
ui.element().style('height: 40px')
|
||||
ui.button('Switch Schedule', on_click=lambda: show_confirm_dialog('Confirm Switch Schedule?', rebuild)).classes('self-end')
|
||||
|
||||
# send action package
|
||||
# send action package
|
||||
def send_action_package(action_name: str, params: str) -> None:
|
||||
"""Send an action package to the robot."""
|
||||
try:
|
||||
@@ -236,26 +223,67 @@ def build_ui(node: CtrlGuiNode) -> None:
|
||||
node.file_logger.error(f'Failed to trigger bt_action_send_goal: {str(e)}')
|
||||
ui.notify(f'Failed to send action: {str(e)}', color='negative')
|
||||
|
||||
#move arm
|
||||
# def _on_rebuild_response(future):
|
||||
# """Callback for rebuild_now service response."""
|
||||
# try:
|
||||
# response = future.result()
|
||||
# if response.success:
|
||||
# node.file_logger.info('Rebuild request successful')
|
||||
# ui.notify('Rebuild request successful')
|
||||
# else:
|
||||
# node.file_logger.warning(f'Rebuild request failed: {response.message}')
|
||||
# ui.notify(f'Rebuild request failed: {response.message}', color='negative')
|
||||
# except Exception as e:
|
||||
# node.file_logger.error(f'Rebuild request exception: {str(e)}')
|
||||
# ui.notify(f'Rebuild request exception: {str(e)}', color='negative')
|
||||
|
||||
def rebuild_now(type: str, config: str, param: str) -> None:
|
||||
try:
|
||||
node.rebuild_requests += 1
|
||||
node.file_logger.info('Rebuild BehaviorTree now')
|
||||
node.file_logger.info(f'Total rebuild requests: {str(node.rebuild_requests)}')
|
||||
|
||||
request = BtRebuild.Request()
|
||||
request.type = type #"Remote"
|
||||
request.config = config #"MoveWaist"
|
||||
request.param = param #f"move_pitch_degree: 10\nmove_yaw_degree: 10\n"
|
||||
|
||||
future = node.run_trigger_.call_async(request)
|
||||
future.add_done_callback(node._on_rebuild_response)
|
||||
ui.notify(f'rebuild_now: type={type}, config={config}, param={param}')
|
||||
except Exception as e:
|
||||
node.file_logger.error(f'Failed to rebuild_now: type={type}, config={config}, {str(e)}')
|
||||
ui.notify(f'Failed to rebuild_now: type={type}, config={config}, {str(e)}', color='negative')
|
||||
|
||||
# rebuild button
|
||||
with ui.column():
|
||||
with ui.row():
|
||||
left_arm_position_pose = ui.input('LEFT_ARM_POSE: x, y, z, rx, ry, rz, w', value='0, 0, 0, 0, 0, 0, 0').style('width: 550px')
|
||||
pass
|
||||
def rebuild() -> None:
|
||||
try:
|
||||
node.file_logger.info('Running rebuild_now...')
|
||||
rebuild_now("Trigger", "SwitchSchedule", "")
|
||||
except Exception as e:
|
||||
node.file_logger.error(f'Failed to trigger rebuild: {str(e)}')
|
||||
|
||||
ui.element().style('height: 40px')
|
||||
ui.button('Switch Schedule', on_click=lambda: show_confirm_dialog('Confirm Switch Schedule?', rebuild)).classes('self-end')
|
||||
|
||||
#move home
|
||||
with ui.column():
|
||||
with ui.row():
|
||||
right_arm_position_pose = ui.input('RIGHT_ARM_POSE: x, y, z, rx, ry, rz, w', value='0, 0, 0, 0, 0, 0, 0').style('width: 550px')
|
||||
|
||||
# TODO check input data
|
||||
|
||||
def move_arm_() -> None:
|
||||
node.file_logger.info('Running move_arm_...')
|
||||
action_name = "Arm"
|
||||
text = f"body_id: 0\ndata_type: 3\ndata_length: 14\ncommand_id: 0\nframe_time_stamp: {node.get_clock().now().to_msg().sec}\ndata_array: [{left_arm_position_pose.value}, {right_arm_position_pose.value}]\n"
|
||||
pass
|
||||
def move_home_() -> None:
|
||||
node.file_logger.info('Running move_home_...')
|
||||
action_name = "MoveHome"
|
||||
text = ""
|
||||
# send_action_package(action_name, text)
|
||||
node.rebuild_now("Remote", action_name, text)
|
||||
rebuild_now("Remote", action_name, text)
|
||||
|
||||
ui.button('Arm Control', on_click=lambda: show_confirm_dialog(
|
||||
f'Confirm Move Arm operation?\nleft_arm_pose: {left_arm_position_pose.value}\nright_arm_pose: {right_arm_position_pose.value}\n',
|
||||
move_arm_
|
||||
ui.element().style('height: 40px')
|
||||
ui.button('Move Home', on_click=lambda: show_confirm_dialog(
|
||||
f'Confirm Move Home operation?\n',
|
||||
move_home_
|
||||
)).classes('self-end')
|
||||
|
||||
#move waist
|
||||
@@ -271,7 +299,7 @@ def build_ui(node: CtrlGuiNode) -> None:
|
||||
action_name = "MoveWaist"
|
||||
text = f"move_pitch_degree: {move_waist_input_move_pitch_degree.value}\nmove_yaw_degree: {move_waist_input_move_yaw_degree.value}\n"
|
||||
# send_action_package(action_name, text)
|
||||
node.rebuild_now("Remote", action_name, text)
|
||||
rebuild_now("Remote", action_name, text)
|
||||
|
||||
ui.button('Move Waist', on_click=lambda: show_confirm_dialog(
|
||||
f'Confirm Move Waist operation?\nmove_pitch_degree: {move_waist_input_move_pitch_degree.value}\nmove_yaw_degree: {move_waist_input_move_yaw_degree.value}',
|
||||
@@ -291,7 +319,7 @@ def build_ui(node: CtrlGuiNode) -> None:
|
||||
action_name = "MoveWheel"
|
||||
text = f"move_distance: {move_wheel_input_move_forward.value}\nmove_angle: {move_wheel_input_move_turn.value}\n"
|
||||
# send_action_package(action_name, text)
|
||||
node.rebuild_now("Remote", action_name, text)
|
||||
rebuild_now("Remote", action_name, text)
|
||||
|
||||
ui.button('Move Wheel', on_click=lambda: show_confirm_dialog(
|
||||
f'Confirm Move Wheel operation?\nmove_distance: {move_wheel_input_move_forward.value}\nmove_angle: {move_wheel_input_move_turn.value}',
|
||||
@@ -310,7 +338,7 @@ def build_ui(node: CtrlGuiNode) -> None:
|
||||
action_name = "MoveLeg"
|
||||
text = f"move_up_distance: {move_leg_input_move_distance.value}\n"
|
||||
# send_action_package(action_name, text)
|
||||
node.rebuild_now("Remote", action_name, text)
|
||||
rebuild_now("Remote", action_name, text)
|
||||
|
||||
ui.button('Move Leg', on_click=lambda: show_confirm_dialog(
|
||||
f'Confirm Move Leg operation?\nmove_up_distance: {move_leg_input_move_distance.value}',
|
||||
@@ -337,6 +365,28 @@ def build_ui(node: CtrlGuiNode) -> None:
|
||||
f'Confirm LED control operation?\nColor: {color_input.value}',
|
||||
led_control_
|
||||
)).classes('self-end')
|
||||
|
||||
#move arm
|
||||
with ui.column():
|
||||
with ui.row():
|
||||
left_arm_position_pose = ui.input('LEFT_ARM_POSE: x, y, z, rx, ry, rz, w', value='0, 0, 0, 0, 0, 0, 0').style('width: 550px')
|
||||
|
||||
with ui.row():
|
||||
right_arm_position_pose = ui.input('RIGHT_ARM_POSE: x, y, z, rx, ry, rz, w', value='0, 0, 0, 0, 0, 0, 0').style('width: 550px')
|
||||
|
||||
# TODO check input data
|
||||
|
||||
def move_arm_() -> None:
|
||||
node.file_logger.info('Running move_arm_...')
|
||||
action_name = "Arm"
|
||||
text = f"body_id: 0\ndata_type: 3\ndata_length: 14\ncommand_id: 0\nframe_time_stamp: {node.get_clock().now().to_msg().sec}\ndata_array: [{left_arm_position_pose.value}, {right_arm_position_pose.value}]\n"
|
||||
# send_action_package(action_name, text)
|
||||
rebuild_now("Remote", action_name, text)
|
||||
|
||||
ui.button('Arm Control', on_click=lambda: show_confirm_dialog(
|
||||
f'Confirm Move Arm operation?\nleft_arm_pose: {left_arm_position_pose.value}\nright_arm_pose: {right_arm_position_pose.value}\n',
|
||||
move_arm_
|
||||
)).classes('self-end')
|
||||
|
||||
# Add statistics display
|
||||
ui.separator()
|
||||
|
||||
Reference in New Issue
Block a user