237 lines
8.0 KiB
Python
237 lines
8.0 KiB
Python
#!/usr/bin/env python3
|
|
"""Mission Planner - Coordinates missions using local waypoints."""
|
|
|
|
import rclpy
|
|
from rclpy.node import Node
|
|
from geometry_msgs.msg import PoseStamped
|
|
from nav_msgs.msg import Path
|
|
from std_msgs.msg import String, Bool
|
|
import yaml
|
|
from enum import Enum
|
|
|
|
|
|
class MissionState(Enum):
|
|
IDLE = 0
|
|
LOADING = 1
|
|
EXECUTING = 2
|
|
PAUSED = 3
|
|
COMPLETED = 4
|
|
ABORTED = 5
|
|
|
|
|
|
class MissionPlanner(Node):
|
|
|
|
def __init__(self):
|
|
super().__init__('mission_planner')
|
|
|
|
self.declare_parameter('mission_file', '')
|
|
|
|
self.mission = []
|
|
self.current_action_idx = 0
|
|
self.state = MissionState.IDLE
|
|
|
|
self.uav_goal_reached = False
|
|
self.ugv_goal_reached = False
|
|
|
|
self.command_sub = self.create_subscription(
|
|
String, '/mission/command', self.command_callback, 10)
|
|
|
|
self.uav_status_sub = self.create_subscription(
|
|
String, '/uav/controller/status', self.uav_status_callback, 10)
|
|
|
|
self.ugv_status_sub = self.create_subscription(
|
|
String, '/ugv/controller/status', self.ugv_status_callback, 10)
|
|
|
|
self.uav_reached_sub = self.create_subscription(
|
|
Bool, '/uav/waypoint_follower/waypoint_reached', self.uav_reached_callback, 10)
|
|
|
|
self.ugv_reached_sub = self.create_subscription(
|
|
Bool, '/ugv/goal_reached', self.ugv_reached_callback, 10)
|
|
|
|
self.uav_cmd_pub = self.create_publisher(String, '/uav/controller/command', 10)
|
|
self.ugv_cmd_pub = self.create_publisher(String, '/ugv/controller/command', 10)
|
|
self.uav_waypoint_pub = self.create_publisher(PoseStamped, '/uav/setpoint_position', 10)
|
|
self.ugv_goal_pub = self.create_publisher(PoseStamped, '/ugv/goal_pose', 10)
|
|
self.uav_path_pub = self.create_publisher(Path, '/waypoint_follower/set_path', 10)
|
|
self.status_pub = self.create_publisher(String, '/mission/status', 10)
|
|
|
|
self.mission_timer = self.create_timer(0.5, self.mission_loop)
|
|
|
|
self.get_logger().info('Mission Planner Started - LOCAL coordinates only')
|
|
|
|
def command_callback(self, msg):
|
|
cmd = msg.data.lower().split(':')
|
|
action = cmd[0]
|
|
|
|
if action == 'load':
|
|
if len(cmd) > 1:
|
|
self.load_mission_file(cmd[1])
|
|
else:
|
|
self.load_demo_mission()
|
|
elif action == 'start':
|
|
self.start_mission()
|
|
elif action == 'pause':
|
|
self.state = MissionState.PAUSED
|
|
elif action == 'resume':
|
|
self.state = MissionState.EXECUTING
|
|
elif action == 'abort':
|
|
self.abort_mission()
|
|
elif action == 'clear':
|
|
self.mission = []
|
|
self.current_action_idx = 0
|
|
self.state = MissionState.IDLE
|
|
|
|
def uav_status_callback(self, msg):
|
|
pass
|
|
|
|
def ugv_status_callback(self, msg):
|
|
pass
|
|
|
|
def uav_reached_callback(self, msg):
|
|
if msg.data:
|
|
self.uav_goal_reached = True
|
|
|
|
def ugv_reached_callback(self, msg):
|
|
if msg.data:
|
|
self.ugv_goal_reached = True
|
|
|
|
def load_demo_mission(self):
|
|
self.mission = [
|
|
{'type': 'uav_command', 'command': 'arm'},
|
|
{'type': 'uav_command', 'command': 'takeoff'},
|
|
{'type': 'wait', 'duration': 3.0},
|
|
{'type': 'uav_waypoint', 'x': 10.0, 'y': 0.0, 'z': 5.0},
|
|
{'type': 'uav_waypoint', 'x': 10.0, 'y': 10.0, 'z': 5.0},
|
|
{'type': 'ugv_goal', 'x': 5.0, 'y': 5.0},
|
|
{'type': 'wait_for_vehicles'},
|
|
{'type': 'uav_waypoint', 'x': 0.0, 'y': 0.0, 'z': 5.0},
|
|
{'type': 'ugv_goal', 'x': 0.0, 'y': 0.0},
|
|
{'type': 'wait_for_vehicles'},
|
|
{'type': 'uav_command', 'command': 'land'},
|
|
]
|
|
self.current_action_idx = 0
|
|
self.state = MissionState.IDLE
|
|
self.get_logger().info(f'Demo mission loaded: {len(self.mission)} actions')
|
|
|
|
def load_mission_file(self, filepath):
|
|
try:
|
|
with open(filepath, 'r') as f:
|
|
data = yaml.safe_load(f)
|
|
self.mission = data.get('mission', [])
|
|
self.current_action_idx = 0
|
|
self.state = MissionState.IDLE
|
|
self.get_logger().info(f'Mission loaded from {filepath}')
|
|
except Exception as e:
|
|
self.get_logger().error(f'Failed to load mission: {e}')
|
|
|
|
def start_mission(self):
|
|
if len(self.mission) == 0:
|
|
self.get_logger().warn('No mission loaded')
|
|
return
|
|
|
|
self.current_action_idx = 0
|
|
self.state = MissionState.EXECUTING
|
|
self.uav_goal_reached = False
|
|
self.ugv_goal_reached = False
|
|
self.get_logger().info('Mission started')
|
|
|
|
def abort_mission(self):
|
|
self.state = MissionState.ABORTED
|
|
|
|
stop_msg = String()
|
|
stop_msg.data = 'hold'
|
|
self.uav_cmd_pub.publish(stop_msg)
|
|
|
|
stop_msg.data = 'stop'
|
|
self.ugv_cmd_pub.publish(stop_msg)
|
|
|
|
self.get_logger().warn('Mission aborted')
|
|
|
|
def mission_loop(self):
|
|
status_msg = String()
|
|
status_msg.data = f'{self.state.name}|{self.current_action_idx}/{len(self.mission)}'
|
|
self.status_pub.publish(status_msg)
|
|
|
|
if self.state != MissionState.EXECUTING:
|
|
return
|
|
|
|
if self.current_action_idx >= len(self.mission):
|
|
self.state = MissionState.COMPLETED
|
|
self.get_logger().info('Mission completed!')
|
|
return
|
|
|
|
action = self.mission[self.current_action_idx]
|
|
completed = self.execute_action(action)
|
|
|
|
if completed:
|
|
self.current_action_idx += 1
|
|
self.uav_goal_reached = False
|
|
self.ugv_goal_reached = False
|
|
|
|
def execute_action(self, action):
|
|
action_type = action.get('type', '')
|
|
|
|
if action_type == 'uav_command':
|
|
cmd_msg = String()
|
|
cmd_msg.data = action['command']
|
|
self.uav_cmd_pub.publish(cmd_msg)
|
|
return True
|
|
|
|
elif action_type == 'ugv_command':
|
|
cmd_msg = String()
|
|
cmd_msg.data = action['command']
|
|
self.ugv_cmd_pub.publish(cmd_msg)
|
|
return True
|
|
|
|
elif action_type == 'uav_waypoint':
|
|
pose = PoseStamped()
|
|
pose.header.stamp = self.get_clock().now().to_msg()
|
|
pose.header.frame_id = 'odom'
|
|
pose.pose.position.x = float(action['x'])
|
|
pose.pose.position.y = float(action['y'])
|
|
pose.pose.position.z = float(action['z'])
|
|
pose.pose.orientation.w = 1.0
|
|
self.uav_waypoint_pub.publish(pose)
|
|
return self.uav_goal_reached
|
|
|
|
elif action_type == 'ugv_goal':
|
|
pose = PoseStamped()
|
|
pose.header.stamp = self.get_clock().now().to_msg()
|
|
pose.header.frame_id = 'odom'
|
|
pose.pose.position.x = float(action['x'])
|
|
pose.pose.position.y = float(action['y'])
|
|
pose.pose.orientation.w = 1.0
|
|
self.ugv_goal_pub.publish(pose)
|
|
return self.ugv_goal_reached
|
|
|
|
elif action_type == 'wait':
|
|
if not hasattr(self, '_wait_start'):
|
|
self._wait_start = self.get_clock().now()
|
|
|
|
elapsed = (self.get_clock().now() - self._wait_start).nanoseconds / 1e9
|
|
if elapsed >= action['duration']:
|
|
delattr(self, '_wait_start')
|
|
return True
|
|
return False
|
|
|
|
elif action_type == 'wait_for_vehicles':
|
|
return self.uav_goal_reached and self.ugv_goal_reached
|
|
|
|
return True
|
|
|
|
|
|
def main(args=None):
|
|
rclpy.init(args=args)
|
|
node = MissionPlanner()
|
|
try:
|
|
rclpy.spin(node)
|
|
except KeyboardInterrupt:
|
|
pass
|
|
finally:
|
|
node.destroy_node()
|
|
rclpy.shutdown()
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|