havoc420ubuntu
1d18ec1e56
- Comment out angle correction logic in go_straight function - Adjust go_straight parameters in task_1 - Add success message for horizontal line calibration - Update main function to include task_test - Modify marker request response format
60 lines
1.8 KiB
Python
Executable File
60 lines
1.8 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
|
|
import rclpy
|
|
from rclpy.node import Node
|
|
from cyberdog_marker.srv import PlaceMarker
|
|
|
|
class MarkerClient(Node):
|
|
def __init__(self):
|
|
super().__init__('marker_client')
|
|
self.client = self.create_client(PlaceMarker, 'place_marker')
|
|
while not self.client.wait_for_service(timeout_sec=1.0):
|
|
self.get_logger().info('服务不可用，等待...')
|
|
self.req = PlaceMarker.Request()
|
|
|
|
def send_request(self, x, y, z, color):
|
|
self.req.x = x
|
|
self.req.y = y
|
|
self.req.z = 1.0 # INFO 作为标志，防止阻碍 Robot
|
|
self.req.color = color
|
|
self.future = self.client.call_async(self.req)
|
|
rclpy.spin_until_future_complete(self, self.future)
|
|
return self.future.result()
|
|
|
|
|
|
class MarkerRunner:
|
|
def __init__(self, pass_flag=False):
|
|
self.marker_client = None
|
|
self.pass_flag = pass_flag
|
|
|
|
def run(self):
|
|
self.marker_client = MarkerClient()
|
|
|
|
def destroy(self):
|
|
if self.marker_client:
|
|
self.marker_client.destroy_node()
|
|
|
|
def send_request(self, x, y, z, color, observe=False):
|
|
if self.pass_flag:
|
|
return {
|
|
'success': True,
|
|
'message': "Pass"
|
|
}
|
|
response = self.marker_client.send_request(x, y, z, color)
|
|
if observe:
|
|
self.marker_client.get_logger().info(
|
|
f'结果: {response.success}, 消息: {response.message}')
|
|
return response
|
|
|
|
|
|
def main(args=None):
|
|
rclpy.init(args=args)
|
|
client = MarkerClient()
|
|
response = client.send_request(1.0, 1.0, 0.0, 'blue')
|
|
client.get_logger().info(
|
|
f'结果: {response.success}, 消息: {response.message}')
|
|
client.destroy_node()
|
|
|
|
if __name__ == '__main__':
|
|
main()
|