| 16 | import time |
| 17 | |
| 18 | class RobotWriteNode(LifecycleNode): |
| 19 | |
| 20 | def __init__(self): |
| 21 | super().__init__('lifecycle_robot_write_node') |
| 22 | |
| 23 | # Initialize internal state variables |
| 24 | self.emergency_flag = True |
| 25 | self.light_state = False |
| 26 | self.obstacle_avoidance_state = False |
| 27 | self.op_mode = None |
| 28 | self.gait_type = None |
| 29 | self.sport_response = None |
| 30 | self.robot_state_response = None |
| 31 | |
| 32 | # Initialize ROS communicator handles to None. They will be created in on_configure(). |
| 33 | self.callback_group = None |
| 34 | self.publisher_ = None |
| 35 | self.robot_state_publisher_ = None |
| 36 | self.vui_publisher_ = None |
| 37 | self.obstacle_avoidance_publisher_ = None |
| 38 | self.robot_status_publisher_ = None |
| 39 | self.robot_status_timer_ = None |
| 40 | self.cmd_vel_subscription = None |
| 41 | self.sports_mode_subscription = None |
| 42 | self.sport_response_subscription = None |
| 43 | self.robot_state_response_subscription = None |
| 44 | self.vui_subscription = None |
| 45 | self.set_mode_srv = None |
| 46 | self.set_pose_srv = None |
| 47 | self.set_body_height_srv = None |
| 48 | self.set_continuous_gait_srv = None |
| 49 | self.set_euler_srv = None |
| 50 | self.set_foot_raise_height_srv = None |
| 51 | self.set_switch_gait_srv = None |
| 52 | self.set_switch_joystick_srv = None |
| 53 | self.set_speed_level_service_ = None |
| 54 | self.get_current_mode_srv = None |
| 55 | self.set_emergency_stop_srv = None |
| 56 | self.set_light_srv = None |
| 57 | self.set_obstacle_avoidance_srv = None |
| 58 | |
| 59 | self.get_logger().info("Lifecycle Mode Service Node created, in 'unconfigured' state.") |
| 60 | |
| 61 | |
| 62 | def on_configure(self, state: rclpy.lifecycle.State) -> TransitionCallbackReturn: |
| 63 | |
| 64 | self.get_logger().info("on_configure() is called.") |
| 65 | |
| 66 | self.callback_group = ReentrantCallbackGroup() |
| 67 | |
| 68 | # Publishers |
| 69 | self.publisher_ = self.create_publisher(Request, '/api/sport/request', 1) |
| 70 | self.robot_state_publisher_ = self.create_publisher(Request, '/api/robot_state/request', 1) |
| 71 | self.vui_publisher_ = self.create_publisher(Request, '/api/vui/request', 1) |
| 72 | self.obstacle_avoidance_publisher_ = self.create_publisher(Request, '/api/obstacles_avoid/request', 1) |
| 73 | self.robot_status_publisher_ = self.create_publisher(RobotStatus, 'robot_status', 1) |
| 74 | |
| 75 | # Timer (created but not started) |