Camera Sim Update
This commit is contained in:
@@ -123,22 +123,40 @@ class Controller:
|
||||
self._drain_messages()
|
||||
print("[UAV] Mode -> GUIDED_NOGPS")
|
||||
|
||||
def arm(self, retries: int = 10):
|
||||
def arm(self, retries: int = 30):
|
||||
self.set_mode_guided()
|
||||
|
||||
for attempt in range(1, retries + 1):
|
||||
print(f"[UAV] Arm attempt {attempt}/{retries}...")
|
||||
self.conn.arducopter_arm()
|
||||
for i in range(retries):
|
||||
print(f"[UAV] Arm attempt {i+1}/{retries}...")
|
||||
|
||||
t0 = time.time()
|
||||
while time.time() - t0 < 5:
|
||||
# Force arm: param2=21196 bypasses pre-arm checks
|
||||
# (equivalent to MAVProxy's "arm throttle force")
|
||||
self.conn.mav.command_long_send(
|
||||
self.conn.target_system,
|
||||
self.conn.target_component,
|
||||
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
|
||||
0,
|
||||
1, # param1: 1 = arm
|
||||
21196, # param2: force arm magic number
|
||||
0, 0, 0, 0, 0)
|
||||
|
||||
# Wait up to 4 seconds for armed status instead of
|
||||
# blocking forever with motors_armed_wait()
|
||||
armed = False
|
||||
for _ in range(20):
|
||||
self._drain_messages()
|
||||
if self.armed:
|
||||
print("[UAV] Armed")
|
||||
return True
|
||||
sleep(0.2)
|
||||
if self.conn.motors_armed():
|
||||
armed = True
|
||||
break
|
||||
time.sleep(0.2)
|
||||
|
||||
self._drain_messages()
|
||||
if armed:
|
||||
self.armed = True
|
||||
print("[UAV] Armed")
|
||||
return True
|
||||
|
||||
print(f"[UAV] Arm attempt {i+1} failed, retrying...")
|
||||
time.sleep(2)
|
||||
|
||||
print("[UAV] FAILED to arm after all retries")
|
||||
return False
|
||||
@@ -156,8 +174,12 @@ class Controller:
|
||||
print(f"[UAV] Takeoff -> {altitude}m")
|
||||
|
||||
def land(self):
|
||||
self.conn.set_mode_rtl()
|
||||
print("[UAV] Landing (RTL)")
|
||||
self.conn.mav.command_long_send(
|
||||
self.conn.target_system,
|
||||
self.conn.target_component,
|
||||
MAV_CMD_DO_SET_MODE,
|
||||
0, 89, 9, 0, 0, 0, 0, 0)
|
||||
print("[UAV] Landing (LAND mode)")
|
||||
|
||||
def land_at(self, lat: int, lon: int):
|
||||
self.conn.mav.command_long_send(
|
||||
|
||||
173
src/main.py
173
src/main.py
@@ -4,28 +4,56 @@ import sys
|
||||
import os
|
||||
import time
|
||||
import argparse
|
||||
import yaml
|
||||
from time import sleep
|
||||
from pathlib import Path
|
||||
|
||||
sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
|
||||
|
||||
from control.uav_controller import Controller, HOLD_ALT
|
||||
from control.uav_controller import Controller
|
||||
from control.ugv_controller import UGVController
|
||||
from utils.helpers import clamp, distance_2d, PIDController, LowPassFilter
|
||||
from utils.transforms import normalize_angle, body_to_world, world_to_body
|
||||
|
||||
PROJECT_DIR = Path(__file__).resolve().parent.parent
|
||||
CONFIG_DIR = PROJECT_DIR / "config"
|
||||
|
||||
def mission_hover(ctrl: Controller, altitude: float = HOLD_ALT,
|
||||
duration: float = 30.0):
|
||||
print("\n" + "=" * 50)
|
||||
print(f" HOVER MISSION: {altitude}m for {duration}s")
|
||||
print("=" * 50 + "\n")
|
||||
|
||||
def load_config(name: str) -> dict:
|
||||
path = CONFIG_DIR / name
|
||||
if not path.exists():
|
||||
print(f"[WARN] Config not found: {path}")
|
||||
return {}
|
||||
with open(path, 'r') as f:
|
||||
return yaml.safe_load(f) or {}
|
||||
|
||||
|
||||
def load_mission(name: str) -> dict:
|
||||
path = CONFIG_DIR / "missions" / f"{name}.yaml"
|
||||
if not path.exists():
|
||||
print(f"[WARN] Mission not found: {path}")
|
||||
return {}
|
||||
with open(path, 'r') as f:
|
||||
return yaml.safe_load(f) or {}
|
||||
|
||||
|
||||
def setup_ardupilot(ctrl: Controller):
|
||||
ctrl.set_param('ARMING_CHECK', 0)
|
||||
ctrl.set_param('SCHED_LOOP_RATE', 200)
|
||||
ctrl.set_param('FS_THR_ENABLE', 0)
|
||||
ctrl.set_param('FS_GCS_ENABLE', 0)
|
||||
sleep(2)
|
||||
|
||||
|
||||
def mission_hover(ctrl: Controller, uav_cfg: dict, mission_cfg: dict):
|
||||
altitude = mission_cfg.get('altitude', uav_cfg['flight']['takeoff_altitude'])
|
||||
duration = mission_cfg.get('duration', 30.0)
|
||||
|
||||
print("\n" + "=" * 50)
|
||||
print(f" HOVER MISSION: {altitude}m for {duration}s")
|
||||
print("=" * 50 + "\n")
|
||||
|
||||
setup_ardupilot(ctrl)
|
||||
ctrl.wait_for_gps()
|
||||
|
||||
if not ctrl.arm():
|
||||
@@ -46,29 +74,18 @@ def mission_hover(ctrl: Controller, altitude: float = HOLD_ALT,
|
||||
|
||||
print("\n[UAV] Hover complete")
|
||||
ctrl.land()
|
||||
|
||||
print("[UAV] Waiting for landing ...")
|
||||
t0 = time.time()
|
||||
while time.time() - t0 < 30:
|
||||
ctrl.update_state()
|
||||
if ctrl.altitude < 0.3:
|
||||
break
|
||||
sleep(0.5)
|
||||
print("[UAV] Landed!")
|
||||
wait_for_landing(ctrl)
|
||||
|
||||
|
||||
def mission_square(ctrl: Controller, altitude: float = HOLD_ALT,
|
||||
side: float = 5.0):
|
||||
def mission_square(ctrl: Controller, uav_cfg: dict, mission_cfg: dict):
|
||||
altitude = mission_cfg.get('altitude', uav_cfg['flight']['takeoff_altitude'])
|
||||
side = mission_cfg.get('side_length', 5.0)
|
||||
|
||||
print("\n" + "=" * 50)
|
||||
print(f" SQUARE MISSION: {side}m sides at {altitude}m")
|
||||
print("=" * 50 + "\n")
|
||||
|
||||
ctrl.set_param('ARMING_CHECK', 0)
|
||||
ctrl.set_param('SCHED_LOOP_RATE', 200)
|
||||
ctrl.set_param('FS_THR_ENABLE', 0)
|
||||
ctrl.set_param('FS_GCS_ENABLE', 0)
|
||||
sleep(2)
|
||||
|
||||
setup_ardupilot(ctrl)
|
||||
ctrl.wait_for_gps()
|
||||
|
||||
if not ctrl.arm():
|
||||
@@ -92,29 +109,23 @@ def mission_square(ctrl: Controller, altitude: float = HOLD_ALT,
|
||||
|
||||
print("[UAV] Square complete")
|
||||
ctrl.land()
|
||||
|
||||
print("[UAV] Waiting for landing ...")
|
||||
t0 = time.time()
|
||||
while time.time() - t0 < 30:
|
||||
ctrl.update_state()
|
||||
if ctrl.altitude < 0.3:
|
||||
break
|
||||
sleep(0.5)
|
||||
print("[UAV] Landed!")
|
||||
wait_for_landing(ctrl)
|
||||
|
||||
|
||||
def mission_search(ctrl: Controller, ugv: UGVController,
|
||||
altitude: float = HOLD_ALT):
|
||||
uav_cfg: dict, mission_cfg: dict):
|
||||
altitude = mission_cfg.get('altitude', uav_cfg['flight']['takeoff_altitude'])
|
||||
search_cfg = mission_cfg.get('search', {})
|
||||
initial_leg = search_cfg.get('initial_leg', 3.0)
|
||||
leg_increment = search_cfg.get('leg_increment', 2.0)
|
||||
max_legs = search_cfg.get('max_legs', 12)
|
||||
detection_radius = search_cfg.get('detection_radius', 2.0)
|
||||
|
||||
print("\n" + "=" * 50)
|
||||
print(f" SEARCH MISSION at {altitude}m")
|
||||
print("=" * 50 + "\n")
|
||||
|
||||
ctrl.set_param('ARMING_CHECK', 0)
|
||||
ctrl.set_param('SCHED_LOOP_RATE', 200)
|
||||
ctrl.set_param('FS_THR_ENABLE', 0)
|
||||
ctrl.set_param('FS_GCS_ENABLE', 0)
|
||||
sleep(2)
|
||||
|
||||
setup_ardupilot(ctrl)
|
||||
ctrl.wait_for_gps()
|
||||
|
||||
if not ctrl.arm():
|
||||
@@ -127,30 +138,23 @@ def mission_search(ctrl: Controller, ugv: UGVController,
|
||||
ugv_pos = ugv.get_position()
|
||||
print(f"[UAV] UGV target at ({ugv_pos['x']:.1f}, {ugv_pos['y']:.1f})")
|
||||
|
||||
distance_step = 3.0
|
||||
increment = 2.0
|
||||
distance_step = initial_leg
|
||||
travel_x = True
|
||||
direction = 1
|
||||
MAX_LEGS = 12
|
||||
|
||||
for leg in range(MAX_LEGS):
|
||||
for leg in range(max_legs):
|
||||
ctrl.update_state()
|
||||
uav_pos = ctrl.get_local_position()
|
||||
dist_to_ugv = distance_2d(
|
||||
(uav_pos['x'], uav_pos['y']),
|
||||
(ugv_pos['x'], ugv_pos['y'])
|
||||
)
|
||||
print(f"[UAV] Spiral leg {leg+1}/{MAX_LEGS} dist_to_ugv: {dist_to_ugv:.1f}m")
|
||||
print(f"[UAV] Spiral leg {leg+1}/{max_legs} dist_to_ugv: {dist_to_ugv:.1f}m")
|
||||
|
||||
if dist_to_ugv < 2.0:
|
||||
print("[UAV] UGV found! Landing nearby.")
|
||||
if dist_to_ugv < detection_radius:
|
||||
print("[UAV] UGV found! Landing.")
|
||||
ctrl.land()
|
||||
t0 = time.time()
|
||||
while time.time() - t0 < 30:
|
||||
ctrl.update_state()
|
||||
if ctrl.altitude < 0.3:
|
||||
break
|
||||
sleep(0.5)
|
||||
wait_for_landing(ctrl)
|
||||
print("[UAV] Landed on UGV!")
|
||||
return
|
||||
|
||||
@@ -159,55 +163,82 @@ def mission_search(ctrl: Controller, ugv: UGVController,
|
||||
else:
|
||||
ctrl.move_pos_rel(0, distance_step * direction, 0)
|
||||
direction *= -1
|
||||
distance_step += increment
|
||||
distance_step += leg_increment
|
||||
|
||||
travel_x = not travel_x
|
||||
sleep(4)
|
||||
|
||||
print("[UAV] Search complete - UGV not found, landing")
|
||||
ctrl.land()
|
||||
wait_for_landing(ctrl)
|
||||
|
||||
|
||||
def wait_for_landing(ctrl: Controller, timeout: float = 60.0):
|
||||
print("[UAV] Waiting for landing ...")
|
||||
t0 = time.time()
|
||||
while time.time() - t0 < 30:
|
||||
while time.time() - t0 < timeout:
|
||||
ctrl.update_state()
|
||||
elapsed = int(time.time() - t0)
|
||||
print(f"\r[UAV] Descending: {ctrl.altitude:.1f}m ({elapsed}s) ",
|
||||
end='', flush=True)
|
||||
if ctrl.altitude < 0.5 and not ctrl.armed:
|
||||
break
|
||||
if ctrl.altitude < 0.3:
|
||||
break
|
||||
sleep(0.5)
|
||||
print("[UAV] Landed!")
|
||||
print(f"\n[UAV] Landed! (alt: {ctrl.altitude:.2f}m)")
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description='UAV-UGV Simulation')
|
||||
parser.add_argument('--device', default='sim', choices=['sim', 'real'])
|
||||
parser.add_argument('--connection', default=None)
|
||||
parser.add_argument('--mission', default='hover', choices=['hover', 'square', 'search'])
|
||||
parser.add_argument('--altitude', type=float, default=HOLD_ALT)
|
||||
parser.add_argument('--duration', type=float, default=30.0)
|
||||
parser.add_argument('--ugv-x', type=float, default=10.0)
|
||||
parser.add_argument('--ugv-y', type=float, default=5.0)
|
||||
parser.add_argument('--ugv-connection', default=None)
|
||||
parser.add_argument('--mission', default='hover')
|
||||
parser.add_argument('--altitude', type=float, default=None)
|
||||
parser.add_argument('--duration', type=float, default=None)
|
||||
args = parser.parse_args()
|
||||
|
||||
uav_cfg = load_config('uav.yaml')
|
||||
ugv_cfg = load_config('ugv.yaml')
|
||||
props = load_config('properties.yaml')
|
||||
mission_cfg = load_mission(args.mission)
|
||||
|
||||
if args.altitude is not None:
|
||||
mission_cfg['altitude'] = args.altitude
|
||||
if args.duration is not None:
|
||||
mission_cfg['duration'] = args.duration
|
||||
|
||||
if args.connection:
|
||||
conn_str = args.connection
|
||||
elif args.device == 'real':
|
||||
conn_str = '/dev/ttyAMA0'
|
||||
conn_str = uav_cfg.get('connection', {}).get('real', '/dev/ttyAMA0')
|
||||
else:
|
||||
conn_str = 'tcp:127.0.0.1:5760'
|
||||
conn_str = uav_cfg.get('connection', {}).get('sim', 'tcp:127.0.0.1:5760')
|
||||
|
||||
ugv_pos = ugv_cfg.get('position', {})
|
||||
ugv = UGVController(
|
||||
connection_string=args.ugv_connection,
|
||||
static_pos=(args.ugv_x, args.ugv_y),
|
||||
connection_string=ugv_cfg.get('connection', {}).get('sim'),
|
||||
static_pos=(ugv_pos.get('x', 10.0), ugv_pos.get('y', 5.0)),
|
||||
)
|
||||
|
||||
ctrl = Controller(conn_str)
|
||||
|
||||
if args.mission == 'hover':
|
||||
mission_hover(ctrl, altitude=args.altitude, duration=args.duration)
|
||||
elif args.mission == 'square':
|
||||
mission_square(ctrl, altitude=args.altitude)
|
||||
elif args.mission == 'search':
|
||||
mission_search(ctrl, ugv, altitude=args.altitude)
|
||||
print(f"[MAIN] Config loaded from {CONFIG_DIR}")
|
||||
print(f"[MAIN] Mission: {args.mission}")
|
||||
|
||||
missions = {
|
||||
'hover': lambda: mission_hover(ctrl, uav_cfg, mission_cfg),
|
||||
'square': lambda: mission_square(ctrl, uav_cfg, mission_cfg),
|
||||
'search': lambda: mission_search(ctrl, ugv, uav_cfg, mission_cfg),
|
||||
}
|
||||
|
||||
runner = missions.get(args.mission)
|
||||
if runner:
|
||||
runner()
|
||||
else:
|
||||
print(f"[MAIN] Unknown mission: {args.mission}")
|
||||
print(f"[MAIN] Available: {list(missions.keys())}")
|
||||
return
|
||||
|
||||
print("[MAIN] Mission finished.")
|
||||
|
||||
|
||||
@@ -1,227 +1,152 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Camera Processor Node
|
||||
Handles camera feed processing for GPS-denied navigation
|
||||
Camera Processor - Handles Gazebo camera feeds for GPS-denied navigation.
|
||||
Subscribes to gz-transport topics, processes frames, and displays via OpenCV.
|
||||
Downstream modules (ArUco detector, optical flow) register as callbacks.
|
||||
"""
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import Image, CameraInfo
|
||||
from std_msgs.msg import Header
|
||||
from cv_bridge import CvBridge
|
||||
import cv2
|
||||
import sys
|
||||
import signal
|
||||
import time
|
||||
import numpy as np
|
||||
import cv2
|
||||
from gz.transport13 import Node
|
||||
from gz.msgs10.image_pb2 import Image
|
||||
|
||||
# PixelFormatType enum values (from gz.msgs10 Image proto)
|
||||
PF_RGB_INT8 = 3
|
||||
PF_BGR_INT8 = 8
|
||||
PF_R_FLOAT32 = 13
|
||||
|
||||
|
||||
class CameraProcessor(Node):
|
||||
"""
|
||||
Processes camera feeds for visual odometry and obstacle detection.
|
||||
Outputs processed images for downstream nodes.
|
||||
"""
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('camera_processor')
|
||||
|
||||
self.bridge = CvBridge()
|
||||
|
||||
# Camera parameters
|
||||
self.camera_matrix = None
|
||||
self.dist_coeffs = None
|
||||
self.image_size = None
|
||||
|
||||
# Image processing parameters
|
||||
self.declare_parameter('undistort', True)
|
||||
self.declare_parameter('grayscale_output', True)
|
||||
self.declare_parameter('histogram_equalization', True)
|
||||
self.declare_parameter('resize_factor', 1.0)
|
||||
|
||||
self.undistort = self.get_parameter('undistort').value
|
||||
self.grayscale_output = self.get_parameter('grayscale_output').value
|
||||
self.histogram_eq = self.get_parameter('histogram_equalization').value
|
||||
self.resize_factor = self.get_parameter('resize_factor').value
|
||||
|
||||
# Undistort maps (computed once)
|
||||
self.map1 = None
|
||||
self.map2 = None
|
||||
|
||||
# Subscribers - Forward camera
|
||||
self.forward_image_sub = self.create_subscription(
|
||||
Image,
|
||||
'/uav/camera/forward/image_raw',
|
||||
self.forward_image_callback,
|
||||
10
|
||||
)
|
||||
|
||||
self.forward_info_sub = self.create_subscription(
|
||||
CameraInfo,
|
||||
'/uav/camera/forward/camera_info',
|
||||
self.camera_info_callback,
|
||||
10
|
||||
)
|
||||
|
||||
# Subscribers - Downward camera
|
||||
self.downward_image_sub = self.create_subscription(
|
||||
Image,
|
||||
'/uav/camera/downward/image_raw',
|
||||
self.downward_image_callback,
|
||||
10
|
||||
)
|
||||
|
||||
# Publishers - Processed images
|
||||
self.forward_processed_pub = self.create_publisher(
|
||||
Image,
|
||||
'/uav/camera/forward/image_processed',
|
||||
10
|
||||
)
|
||||
|
||||
self.downward_processed_pub = self.create_publisher(
|
||||
Image,
|
||||
'/uav/camera/downward/image_processed',
|
||||
10
|
||||
)
|
||||
|
||||
# Debug visualization
|
||||
self.debug_pub = self.create_publisher(
|
||||
Image,
|
||||
'/uav/camera/debug',
|
||||
10
|
||||
)
|
||||
|
||||
self.get_logger().info('Camera Processor Node Started')
|
||||
|
||||
def camera_info_callback(self, msg: CameraInfo):
|
||||
"""Extract and store camera calibration parameters."""
|
||||
if self.camera_matrix is None:
|
||||
self.camera_matrix = np.array(msg.k).reshape(3, 3)
|
||||
self.dist_coeffs = np.array(msg.d)
|
||||
self.image_size = (msg.width, msg.height)
|
||||
|
||||
# Compute undistortion maps
|
||||
if self.undistort and self.dist_coeffs is not None:
|
||||
new_camera_matrix, _ = cv2.getOptimalNewCameraMatrix(
|
||||
self.camera_matrix,
|
||||
self.dist_coeffs,
|
||||
self.image_size,
|
||||
alpha=0
|
||||
class CameraProcessor:
|
||||
def __init__(self, topics=None, show_gui=True):
|
||||
self.node = Node()
|
||||
self.show_gui = show_gui
|
||||
self.frames = {}
|
||||
self.callbacks = {}
|
||||
self.running = True
|
||||
|
||||
if topics is None:
|
||||
topics = {
|
||||
"downward": "/uav/camera/downward",
|
||||
"gimbal": "/world/uav_ugv_search/model/iris_with_gimbal/model/gimbal/link/pitch_link/sensor/camera/image",
|
||||
}
|
||||
|
||||
self.topics = topics
|
||||
|
||||
for name, topic in self.topics.items():
|
||||
self.callbacks[name] = []
|
||||
ok = self.node.subscribe(Image, topic, self._make_gz_callback(name))
|
||||
if ok:
|
||||
print(f"[CAM] Subscribed: {name} -> {topic}")
|
||||
else:
|
||||
print(f"[CAM] Failed: {topic}")
|
||||
|
||||
signal.signal(signal.SIGINT, lambda s, f: self.stop())
|
||||
|
||||
def _make_gz_callback(self, name):
|
||||
def cb(msg):
|
||||
fmt = msg.pixel_format_type
|
||||
if fmt == PF_RGB_INT8:
|
||||
arr = np.frombuffer(msg.data, dtype=np.uint8).reshape(
|
||||
msg.height, msg.width, 3
|
||||
)
|
||||
self.map1, self.map2 = cv2.initUndistortRectifyMap(
|
||||
self.camera_matrix,
|
||||
self.dist_coeffs,
|
||||
None,
|
||||
new_camera_matrix,
|
||||
self.image_size,
|
||||
cv2.CV_16SC2
|
||||
bgr = cv2.cvtColor(arr, cv2.COLOR_RGB2BGR)
|
||||
elif fmt == PF_BGR_INT8:
|
||||
arr = np.frombuffer(msg.data, dtype=np.uint8).reshape(
|
||||
msg.height, msg.width, 3
|
||||
)
|
||||
|
||||
self.get_logger().info(
|
||||
f'Camera calibration received: {self.image_size[0]}x{self.image_size[1]}'
|
||||
)
|
||||
|
||||
def process_image(self, image: np.ndarray) -> np.ndarray:
|
||||
"""
|
||||
Apply image processing pipeline.
|
||||
|
||||
Args:
|
||||
image: Input BGR image
|
||||
|
||||
Returns:
|
||||
Processed image
|
||||
"""
|
||||
bgr = arr
|
||||
elif fmt == PF_R_FLOAT32:
|
||||
arr = np.frombuffer(msg.data, dtype=np.float32).reshape(
|
||||
msg.height, msg.width
|
||||
)
|
||||
normalized = cv2.normalize(arr, None, 0, 255, cv2.NORM_MINMAX)
|
||||
bgr = cv2.cvtColor(normalized.astype(np.uint8), cv2.COLOR_GRAY2BGR)
|
||||
else:
|
||||
return
|
||||
|
||||
processed = self.process_image(bgr)
|
||||
self.frames[name] = processed
|
||||
|
||||
for fn in self.callbacks.get(name, []):
|
||||
fn(name, processed)
|
||||
|
||||
return cb
|
||||
|
||||
def process_image(self, image):
|
||||
processed = image.copy()
|
||||
|
||||
# 1. Undistort if calibration available
|
||||
if self.undistort and self.map1 is not None:
|
||||
processed = cv2.remap(processed, self.map1, self.map2, cv2.INTER_LINEAR)
|
||||
|
||||
# 2. Resize if needed
|
||||
if self.resize_factor != 1.0:
|
||||
new_size = (
|
||||
int(processed.shape[1] * self.resize_factor),
|
||||
int(processed.shape[0] * self.resize_factor)
|
||||
)
|
||||
processed = cv2.resize(processed, new_size, interpolation=cv2.INTER_AREA)
|
||||
|
||||
# 3. Convert to grayscale if requested
|
||||
if self.grayscale_output:
|
||||
if len(processed.shape) == 3:
|
||||
processed = cv2.cvtColor(processed, cv2.COLOR_BGR2GRAY)
|
||||
|
||||
# 4. Histogram equalization for better feature detection
|
||||
if self.histogram_eq:
|
||||
if len(processed.shape) == 2:
|
||||
# CLAHE for better results than standard histogram equalization
|
||||
clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8))
|
||||
processed = clahe.apply(processed)
|
||||
else:
|
||||
# For color images, apply to L channel in LAB
|
||||
lab = cv2.cvtColor(processed, cv2.COLOR_BGR2LAB)
|
||||
clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8))
|
||||
lab[:, :, 0] = clahe.apply(lab[:, :, 0])
|
||||
processed = cv2.cvtColor(lab, cv2.COLOR_LAB2BGR)
|
||||
|
||||
clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8))
|
||||
lab = cv2.cvtColor(processed, cv2.COLOR_BGR2LAB)
|
||||
lab[:, :, 0] = clahe.apply(lab[:, :, 0])
|
||||
processed = cv2.cvtColor(lab, cv2.COLOR_LAB2BGR)
|
||||
return processed
|
||||
|
||||
def forward_image_callback(self, msg: Image):
|
||||
"""Process forward camera images for visual odometry."""
|
||||
try:
|
||||
# Convert ROS Image to OpenCV
|
||||
cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
|
||||
|
||||
# Process image
|
||||
processed = self.process_image(cv_image)
|
||||
|
||||
# Convert back to ROS Image
|
||||
if len(processed.shape) == 2:
|
||||
encoding = 'mono8'
|
||||
else:
|
||||
encoding = 'bgr8'
|
||||
|
||||
processed_msg = self.bridge.cv2_to_imgmsg(processed, encoding)
|
||||
processed_msg.header = msg.header
|
||||
|
||||
# Publish processed image
|
||||
self.forward_processed_pub.publish(processed_msg)
|
||||
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Forward image processing error: {e}')
|
||||
|
||||
def downward_image_callback(self, msg: Image):
|
||||
"""Process downward camera images for optical flow."""
|
||||
try:
|
||||
# Convert ROS Image to OpenCV
|
||||
cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
|
||||
|
||||
# Process image
|
||||
processed = self.process_image(cv_image)
|
||||
|
||||
# Convert back to ROS Image
|
||||
if len(processed.shape) == 2:
|
||||
encoding = 'mono8'
|
||||
else:
|
||||
encoding = 'bgr8'
|
||||
|
||||
processed_msg = self.bridge.cv2_to_imgmsg(processed, encoding)
|
||||
processed_msg.header = msg.header
|
||||
|
||||
# Publish processed image
|
||||
self.downward_processed_pub.publish(processed_msg)
|
||||
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Downward image processing error: {e}')
|
||||
|
||||
def register_callback(self, camera_name, fn):
|
||||
if camera_name in self.callbacks:
|
||||
self.callbacks[camera_name].append(fn)
|
||||
print(f"[CAM] Registered callback for {camera_name}: {fn.__name__}")
|
||||
|
||||
def stop(self):
|
||||
self.running = False
|
||||
|
||||
def spin(self):
|
||||
print("[CAM] Running. Press 'q' to quit, 's' to screenshot.")
|
||||
while self.running:
|
||||
for name, frame in list(self.frames.items()):
|
||||
cv2.imshow(name, frame)
|
||||
|
||||
key = cv2.waitKey(33) & 0xFF
|
||||
if key == ord("q"):
|
||||
break
|
||||
elif key == ord("s"):
|
||||
for name, frame in self.frames.items():
|
||||
fname = f"{name}_{int(time.time())}.png"
|
||||
cv2.imwrite(fname, frame)
|
||||
print(f"[CAM] Saved {fname}")
|
||||
|
||||
cv2.destroyAllWindows()
|
||||
print("[CAM] Stopped.")
|
||||
|
||||
def spin_headless(self):
|
||||
print("[CAM] Running headless (no GUI).")
|
||||
while self.running:
|
||||
time.sleep(0.1)
|
||||
print("[CAM] Stopped.")
|
||||
|
||||
def get_frame(self, camera_name):
|
||||
return self.frames.get(camera_name)
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = CameraProcessor()
|
||||
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
def main():
|
||||
cameras = "both"
|
||||
show_gui = True
|
||||
|
||||
if len(sys.argv) > 1:
|
||||
cameras = sys.argv[1].lower()
|
||||
if "--headless" in sys.argv:
|
||||
show_gui = False
|
||||
|
||||
all_topics = {
|
||||
"downward": "/uav/camera/downward",
|
||||
"gimbal": "/world/uav_ugv_search/model/iris_with_gimbal/model/gimbal/link/pitch_link/sensor/camera/image",
|
||||
}
|
||||
|
||||
if cameras == "down":
|
||||
topics = {"downward": all_topics["downward"]}
|
||||
elif cameras == "gimbal":
|
||||
topics = {"gimbal": all_topics["gimbal"]}
|
||||
else:
|
||||
topics = all_topics
|
||||
|
||||
proc = CameraProcessor(topics=topics, show_gui=show_gui)
|
||||
|
||||
if show_gui:
|
||||
proc.spin()
|
||||
else:
|
||||
proc.spin_headless()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
||||
@@ -1,132 +1,126 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Object Detector - Visual landmark and obstacle detection."""
|
||||
"""
|
||||
Object Detector - ArUco marker and feature detection.
|
||||
Registers as a callback on CameraProcessor to receive processed frames.
|
||||
"""
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import Image
|
||||
from geometry_msgs.msg import PoseArray, Pose
|
||||
from std_msgs.msg import Float32MultiArray
|
||||
from cv_bridge import CvBridge
|
||||
import cv2
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
|
||||
class ObjectDetector(Node):
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('object_detector')
|
||||
|
||||
self.bridge = CvBridge()
|
||||
|
||||
self.declare_parameter('detection_method', 'ArUco')
|
||||
self.declare_parameter('marker_size', 0.15)
|
||||
self.declare_parameter('camera_matrix', [500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0])
|
||||
|
||||
self.detection_method = self.get_parameter('detection_method').value
|
||||
self.marker_size = self.get_parameter('marker_size').value
|
||||
camera_matrix_flat = self.get_parameter('camera_matrix').value
|
||||
self.camera_matrix = np.array(camera_matrix_flat).reshape(3, 3)
|
||||
class ObjectDetector:
|
||||
def __init__(self, marker_size=0.15, camera_matrix=None, detection_method="ArUco"):
|
||||
self.detection_method = detection_method
|
||||
self.marker_size = marker_size
|
||||
|
||||
if camera_matrix is not None:
|
||||
self.camera_matrix = np.array(camera_matrix).reshape(3, 3)
|
||||
else:
|
||||
self.camera_matrix = np.array(
|
||||
[[500.0, 0.0, 320.0], [0.0, 500.0, 240.0], [0.0, 0.0, 1.0]]
|
||||
)
|
||||
self.dist_coeffs = np.zeros(5)
|
||||
|
||||
if self.detection_method == 'ArUco':
|
||||
|
||||
if self.detection_method == "ArUco":
|
||||
self.aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
|
||||
self.aruco_params = cv2.aruco.DetectorParameters()
|
||||
self.aruco_detector = cv2.aruco.ArucoDetector(self.aruco_dict, self.aruco_params)
|
||||
|
||||
self.image_sub = self.create_subscription(
|
||||
Image, '/uav/camera/forward/image_raw', self.image_callback, 10)
|
||||
|
||||
self.landmarks_pub = self.create_publisher(PoseArray, '/uav/landmarks/poses', 10)
|
||||
self.marker_ids_pub = self.create_publisher(Float32MultiArray, '/uav/landmarks/ids', 10)
|
||||
self.debug_image_pub = self.create_publisher(Image, '/uav/landmarks/debug', 10)
|
||||
|
||||
self.get_logger().info(f'Object Detector Started - {self.detection_method}')
|
||||
|
||||
def image_callback(self, msg):
|
||||
try:
|
||||
frame = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
|
||||
|
||||
if self.detection_method == 'ArUco':
|
||||
self.detect_aruco(frame, msg.header)
|
||||
else:
|
||||
self.detect_orb_features(frame, msg.header)
|
||||
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Detection error: {e}')
|
||||
|
||||
def detect_aruco(self, frame, header):
|
||||
self.aruco_detector = cv2.aruco.ArucoDetector(
|
||||
self.aruco_dict, self.aruco_params
|
||||
)
|
||||
|
||||
self.last_detections = []
|
||||
self.on_detection = None
|
||||
|
||||
print(f"[DET] Object Detector initialized ({self.detection_method})")
|
||||
|
||||
def detect(self, camera_name, frame):
|
||||
if self.detection_method == "ArUco":
|
||||
detections, annotated = self.detect_aruco(frame)
|
||||
else:
|
||||
detections, annotated = self.detect_orb(frame)
|
||||
|
||||
self.last_detections = detections
|
||||
|
||||
if detections and self.on_detection:
|
||||
self.on_detection(detections)
|
||||
|
||||
if annotated is not None:
|
||||
cv2.imshow(f"{camera_name} [detections]", annotated)
|
||||
|
||||
return detections
|
||||
|
||||
def detect_aruco(self, frame):
|
||||
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
|
||||
corners, ids, rejected = self.aruco_detector.detectMarkers(gray)
|
||||
|
||||
pose_array = PoseArray()
|
||||
pose_array.header = header
|
||||
pose_array.header.frame_id = 'camera_link'
|
||||
|
||||
id_array = Float32MultiArray()
|
||||
|
||||
|
||||
detections = []
|
||||
annotated = frame.copy()
|
||||
|
||||
if ids is not None:
|
||||
rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
|
||||
corners, self.marker_size, self.camera_matrix, self.dist_coeffs)
|
||||
|
||||
corners, self.marker_size, self.camera_matrix, self.dist_coeffs
|
||||
)
|
||||
|
||||
for i, marker_id in enumerate(ids.flatten()):
|
||||
pose = Pose()
|
||||
pose.position.x = float(tvecs[i][0][0])
|
||||
pose.position.y = float(tvecs[i][0][1])
|
||||
pose.position.z = float(tvecs[i][0][2])
|
||||
|
||||
rotation_matrix, _ = cv2.Rodrigues(rvecs[i])
|
||||
from scipy.spatial.transform import Rotation
|
||||
tvec = tvecs[i][0]
|
||||
rvec = rvecs[i][0]
|
||||
|
||||
rotation_matrix, _ = cv2.Rodrigues(rvec)
|
||||
r = Rotation.from_matrix(rotation_matrix)
|
||||
quat = r.as_quat()
|
||||
pose.orientation.x = quat[0]
|
||||
pose.orientation.y = quat[1]
|
||||
pose.orientation.z = quat[2]
|
||||
pose.orientation.w = quat[3]
|
||||
|
||||
pose_array.poses.append(pose)
|
||||
id_array.data.append(float(marker_id))
|
||||
|
||||
debug_frame = cv2.aruco.drawDetectedMarkers(frame.copy(), corners, ids)
|
||||
|
||||
center = np.mean(corners[i][0], axis=0)
|
||||
|
||||
detection = {
|
||||
"id": int(marker_id),
|
||||
"position": tvec.tolist(),
|
||||
"orientation_quat": quat.tolist(),
|
||||
"rvec": rvec.tolist(),
|
||||
"tvec": tvec.tolist(),
|
||||
"corners": corners[i][0].tolist(),
|
||||
"center_px": center.tolist(),
|
||||
"distance": float(np.linalg.norm(tvec)),
|
||||
}
|
||||
detections.append(detection)
|
||||
|
||||
cv2.aruco.drawDetectedMarkers(annotated, corners, ids)
|
||||
for i in range(len(rvecs)):
|
||||
cv2.drawFrameAxes(debug_frame, self.camera_matrix, self.dist_coeffs,
|
||||
rvecs[i], tvecs[i], self.marker_size * 0.5)
|
||||
|
||||
debug_msg = self.bridge.cv2_to_imgmsg(debug_frame, 'bgr8')
|
||||
debug_msg.header = header
|
||||
self.debug_image_pub.publish(debug_msg)
|
||||
|
||||
self.landmarks_pub.publish(pose_array)
|
||||
self.marker_ids_pub.publish(id_array)
|
||||
|
||||
def detect_orb_features(self, frame, header):
|
||||
cv2.drawFrameAxes(
|
||||
annotated,
|
||||
self.camera_matrix,
|
||||
self.dist_coeffs,
|
||||
rvecs[i],
|
||||
tvecs[i],
|
||||
self.marker_size * 0.5,
|
||||
)
|
||||
|
||||
d = detections[i]
|
||||
label = f"ID:{d['id']} d:{d['distance']:.2f}m"
|
||||
pos = (int(d["center_px"][0]), int(d["center_px"][1]) - 10)
|
||||
cv2.putText(
|
||||
annotated, label, pos, cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2
|
||||
)
|
||||
|
||||
return detections, annotated
|
||||
|
||||
def detect_orb(self, frame):
|
||||
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
|
||||
orb = cv2.ORB_create(nfeatures=500)
|
||||
keypoints, descriptors = orb.detectAndCompute(gray, None)
|
||||
|
||||
pose_array = PoseArray()
|
||||
pose_array.header = header
|
||||
|
||||
|
||||
detections = []
|
||||
for kp in keypoints[:50]:
|
||||
pose = Pose()
|
||||
pose.position.x = float(kp.pt[0])
|
||||
pose.position.y = float(kp.pt[1])
|
||||
pose.position.z = 0.0
|
||||
pose_array.poses.append(pose)
|
||||
|
||||
self.landmarks_pub.publish(pose_array)
|
||||
detections.append(
|
||||
{
|
||||
"type": "feature",
|
||||
"center_px": [kp.pt[0], kp.pt[1]],
|
||||
"size": kp.size,
|
||||
"response": kp.response,
|
||||
}
|
||||
)
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = ObjectDetector()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
annotated = cv2.drawKeypoints(
|
||||
frame, keypoints[:50], None, color=(0, 255, 0), flags=0
|
||||
)
|
||||
return detections, annotated
|
||||
|
||||
@@ -1,128 +1,85 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Optical Flow - Velocity estimation from downward camera."""
|
||||
"""
|
||||
Optical Flow - Velocity estimation from downward camera.
|
||||
Lucas-Kanade sparse optical flow for GPS-denied velocity estimation.
|
||||
"""
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import Image, Range
|
||||
from geometry_msgs.msg import TwistStamped
|
||||
from cv_bridge import CvBridge
|
||||
import time
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
|
||||
class OpticalFlowNode(Node):
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('optical_flow_node')
|
||||
|
||||
self.bridge = CvBridge()
|
||||
|
||||
class OpticalFlowEstimator:
|
||||
def __init__(self, focal_length=500.0, min_altitude=0.3, max_altitude=10.0):
|
||||
self.focal_length = focal_length
|
||||
self.min_altitude = min_altitude
|
||||
self.max_altitude = max_altitude
|
||||
|
||||
self.prev_frame = None
|
||||
self.prev_points = None
|
||||
self.current_altitude = 1.0
|
||||
|
||||
self.declare_parameter('window_size', 15)
|
||||
self.declare_parameter('max_level', 3)
|
||||
self.declare_parameter('min_altitude', 0.3)
|
||||
self.declare_parameter('max_altitude', 10.0)
|
||||
self.declare_parameter('focal_length', 500.0)
|
||||
|
||||
self.window_size = self.get_parameter('window_size').value
|
||||
self.max_level = self.get_parameter('max_level').value
|
||||
self.min_altitude = self.get_parameter('min_altitude').value
|
||||
self.max_altitude = self.get_parameter('max_altitude').value
|
||||
self.focal_length = self.get_parameter('focal_length').value
|
||||
|
||||
self.lk_params = dict(
|
||||
winSize=(self.window_size, self.window_size),
|
||||
maxLevel=self.max_level,
|
||||
criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03)
|
||||
)
|
||||
|
||||
self.feature_params = dict(
|
||||
maxCorners=100,
|
||||
qualityLevel=0.3,
|
||||
minDistance=7,
|
||||
blockSize=7
|
||||
)
|
||||
|
||||
self.velocity = np.zeros(2)
|
||||
self.prev_time = None
|
||||
|
||||
self.image_sub = self.create_subscription(
|
||||
Image, '/uav/camera/downward/image_raw', self.image_callback, 10)
|
||||
|
||||
self.altitude_sub = self.create_subscription(
|
||||
Range, '/uav/rangefinder/range', self.altitude_callback, 10)
|
||||
|
||||
self.velocity_pub = self.create_publisher(
|
||||
TwistStamped, '/uav/optical_flow/velocity', 10)
|
||||
|
||||
self.get_logger().info('Optical Flow Node Started')
|
||||
|
||||
def altitude_callback(self, msg):
|
||||
if msg.range > self.min_altitude and msg.range < self.max_altitude:
|
||||
self.current_altitude = msg.range
|
||||
|
||||
def image_callback(self, msg):
|
||||
try:
|
||||
frame = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
|
||||
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
|
||||
|
||||
current_time = self.get_clock().now()
|
||||
|
||||
if self.prev_frame is not None and self.prev_points is not None:
|
||||
new_points, status, error = cv2.calcOpticalFlowPyrLK(
|
||||
self.prev_frame, gray, self.prev_points, None, **self.lk_params)
|
||||
|
||||
if new_points is not None:
|
||||
good_new = new_points[status == 1]
|
||||
good_old = self.prev_points[status == 1]
|
||||
|
||||
if len(good_new) > 10:
|
||||
flow = good_new - good_old
|
||||
avg_flow = np.mean(flow, axis=0)
|
||||
|
||||
if self.prev_time is not None:
|
||||
dt = (current_time - self.prev_time).nanoseconds / 1e9
|
||||
if dt > 0:
|
||||
pixel_velocity = avg_flow / dt
|
||||
|
||||
self.velocity[0] = pixel_velocity[0] * self.current_altitude / self.focal_length
|
||||
self.velocity[1] = pixel_velocity[1] * self.current_altitude / self.focal_length
|
||||
|
||||
self.publish_velocity()
|
||||
|
||||
self.prev_points = cv2.goodFeaturesToTrack(gray, **self.feature_params)
|
||||
self.prev_frame = gray
|
||||
self.prev_time = current_time
|
||||
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Optical flow error: {e}')
|
||||
|
||||
def publish_velocity(self):
|
||||
vel_msg = TwistStamped()
|
||||
vel_msg.header.stamp = self.get_clock().now().to_msg()
|
||||
vel_msg.header.frame_id = 'base_link'
|
||||
|
||||
vel_msg.twist.linear.x = float(self.velocity[0])
|
||||
vel_msg.twist.linear.y = float(self.velocity[1])
|
||||
vel_msg.twist.linear.z = 0.0
|
||||
|
||||
self.velocity_pub.publish(vel_msg)
|
||||
self.current_altitude = 1.0
|
||||
|
||||
self.velocity = np.zeros(2)
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = OpticalFlowNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
self.lk_params = dict(
|
||||
winSize=(15, 15),
|
||||
maxLevel=3,
|
||||
criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03),
|
||||
)
|
||||
|
||||
self.feature_params = dict(
|
||||
maxCorners=100, qualityLevel=0.3, minDistance=7, blockSize=7
|
||||
)
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
self.on_velocity = None
|
||||
|
||||
print("[OF] Optical Flow Estimator initialized")
|
||||
|
||||
def set_altitude(self, altitude):
|
||||
if self.min_altitude < altitude < self.max_altitude:
|
||||
self.current_altitude = altitude
|
||||
|
||||
def process_frame(self, camera_name, frame):
|
||||
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
|
||||
current_time = time.time()
|
||||
|
||||
if self.prev_frame is not None and self.prev_points is not None:
|
||||
new_points, status, error = cv2.calcOpticalFlowPyrLK(
|
||||
self.prev_frame, gray, self.prev_points, None, **self.lk_params
|
||||
)
|
||||
|
||||
if new_points is not None:
|
||||
good_new = new_points[status.flatten() == 1]
|
||||
good_old = self.prev_points[status.flatten() == 1]
|
||||
|
||||
if len(good_new) > 10:
|
||||
flow = good_new - good_old
|
||||
avg_flow = np.mean(flow, axis=0)
|
||||
|
||||
if self.prev_time is not None:
|
||||
dt = current_time - self.prev_time
|
||||
if dt > 0:
|
||||
pixel_velocity = avg_flow / dt
|
||||
self.velocity[0] = (
|
||||
pixel_velocity[0]
|
||||
* self.current_altitude
|
||||
/ self.focal_length
|
||||
)
|
||||
self.velocity[1] = (
|
||||
pixel_velocity[1]
|
||||
* self.current_altitude
|
||||
/ self.focal_length
|
||||
)
|
||||
|
||||
if self.on_velocity:
|
||||
self.on_velocity(self.velocity)
|
||||
|
||||
self.prev_points = cv2.goodFeaturesToTrack(gray, **self.feature_params)
|
||||
self.prev_frame = gray
|
||||
self.prev_time = current_time
|
||||
|
||||
return self.velocity
|
||||
|
||||
def get_velocity(self):
|
||||
return self.velocity.copy()
|
||||
|
||||
@@ -1,173 +1,119 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Visual Odometry - Camera-based position estimation without GPS."""
|
||||
"""
|
||||
Visual Odometry - Camera-based position estimation without GPS.
|
||||
ORB/SIFT feature matching with essential matrix decomposition.
|
||||
"""
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import Image, CameraInfo
|
||||
from geometry_msgs.msg import PoseStamped, TwistStamped
|
||||
from cv_bridge import CvBridge
|
||||
import time
|
||||
import cv2
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
|
||||
class VisualOdometryNode(Node):
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('visual_odometry_node')
|
||||
|
||||
self.bridge = CvBridge()
|
||||
|
||||
self.prev_frame = None
|
||||
self.prev_keypoints = None
|
||||
self.prev_descriptors = None
|
||||
|
||||
self.camera_matrix = None
|
||||
self.dist_coeffs = None
|
||||
|
||||
self.current_pose = np.eye(4)
|
||||
self.position = np.zeros(3)
|
||||
self.orientation = np.eye(3)
|
||||
self.prev_time = None
|
||||
self.velocity = np.zeros(3)
|
||||
|
||||
self.detector_type = self.declare_parameter('detector_type', 'ORB').value
|
||||
self.min_features = self.declare_parameter('min_features', 100).value
|
||||
self.feature_quality = self.declare_parameter('feature_quality', 0.01).value
|
||||
|
||||
if self.detector_type == 'ORB':
|
||||
class VisualOdometry:
|
||||
def __init__(self, camera_matrix=None, detector_type="ORB", min_features=100):
|
||||
self.detector_type = detector_type
|
||||
self.min_features = min_features
|
||||
|
||||
if camera_matrix is not None:
|
||||
self.camera_matrix = np.array(camera_matrix).reshape(3, 3)
|
||||
else:
|
||||
self.camera_matrix = np.array(
|
||||
[[500.0, 0.0, 320.0], [0.0, 500.0, 240.0], [0.0, 0.0, 1.0]]
|
||||
)
|
||||
self.dist_coeffs = np.zeros(5)
|
||||
|
||||
if detector_type == "ORB":
|
||||
self.detector = cv2.ORB_create(nfeatures=500)
|
||||
self.matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=False)
|
||||
elif self.detector_type == 'SIFT':
|
||||
elif detector_type == "SIFT":
|
||||
self.detector = cv2.SIFT_create(nfeatures=500)
|
||||
self.matcher = cv2.BFMatcher(cv2.NORM_L2, crossCheck=False)
|
||||
else:
|
||||
self.detector = cv2.ORB_create(nfeatures=500)
|
||||
self.matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=False)
|
||||
|
||||
self.image_sub = self.create_subscription(
|
||||
Image, '/uav/camera/forward/image_raw', self.image_callback, 10)
|
||||
|
||||
self.camera_info_sub = self.create_subscription(
|
||||
CameraInfo, '/uav/camera/forward/camera_info', self.camera_info_callback, 10)
|
||||
|
||||
self.pose_pub = self.create_publisher(PoseStamped, '/uav/visual_odometry/pose', 10)
|
||||
self.velocity_pub = self.create_publisher(TwistStamped, '/uav/visual_odometry/velocity', 10)
|
||||
|
||||
self.timer = self.create_timer(0.033, self.publish_pose)
|
||||
|
||||
self.get_logger().info(f'Visual Odometry Node Started - {self.detector_type} detector')
|
||||
|
||||
def camera_info_callback(self, msg):
|
||||
if self.camera_matrix is None:
|
||||
self.camera_matrix = np.array(msg.k).reshape(3, 3)
|
||||
self.dist_coeffs = np.array(msg.d)
|
||||
self.get_logger().info('Camera calibration received')
|
||||
|
||||
def image_callback(self, msg):
|
||||
try:
|
||||
frame = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
|
||||
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
|
||||
|
||||
keypoints, descriptors = self.detector.detectAndCompute(gray, None)
|
||||
|
||||
current_time = self.get_clock().now()
|
||||
|
||||
if self.prev_frame is not None and len(keypoints) >= self.min_features:
|
||||
matches = self.match_features(self.prev_descriptors, descriptors)
|
||||
|
||||
if len(matches) >= self.min_features:
|
||||
self.estimate_motion(self.prev_keypoints, keypoints, matches, current_time)
|
||||
|
||||
self.prev_frame = gray
|
||||
self.prev_keypoints = keypoints
|
||||
self.prev_descriptors = descriptors
|
||||
self.prev_time = current_time
|
||||
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Visual odometry error: {e}')
|
||||
|
||||
def match_features(self, desc1, desc2):
|
||||
|
||||
self.prev_frame = None
|
||||
self.prev_keypoints = None
|
||||
self.prev_descriptors = None
|
||||
self.prev_time = None
|
||||
|
||||
self.position = np.zeros(3)
|
||||
self.orientation = np.eye(3)
|
||||
self.velocity = np.zeros(3)
|
||||
|
||||
self.on_pose = None
|
||||
|
||||
print(f"[VO] Visual Odometry initialized ({detector_type})")
|
||||
|
||||
def process_frame(self, camera_name, frame):
|
||||
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
|
||||
keypoints, descriptors = self.detector.detectAndCompute(gray, None)
|
||||
current_time = time.time()
|
||||
|
||||
if (
|
||||
self.prev_frame is not None
|
||||
and self.prev_descriptors is not None
|
||||
and len(keypoints) >= self.min_features
|
||||
):
|
||||
matches = self._match_features(self.prev_descriptors, descriptors)
|
||||
if len(matches) >= self.min_features:
|
||||
self._estimate_motion(
|
||||
self.prev_keypoints, keypoints, matches, current_time
|
||||
)
|
||||
|
||||
self.prev_frame = gray
|
||||
self.prev_keypoints = keypoints
|
||||
self.prev_descriptors = descriptors
|
||||
self.prev_time = current_time
|
||||
|
||||
def _match_features(self, desc1, desc2):
|
||||
if desc1 is None or desc2 is None:
|
||||
return []
|
||||
|
||||
|
||||
matches = self.matcher.knnMatch(desc1, desc2, k=2)
|
||||
|
||||
good_matches = []
|
||||
for match_pair in matches:
|
||||
if len(match_pair) == 2:
|
||||
m, n = match_pair
|
||||
good = []
|
||||
for pair in matches:
|
||||
if len(pair) == 2:
|
||||
m, n = pair
|
||||
if m.distance < 0.7 * n.distance:
|
||||
good_matches.append(m)
|
||||
|
||||
return good_matches
|
||||
|
||||
def estimate_motion(self, prev_kp, curr_kp, matches, current_time):
|
||||
if self.camera_matrix is None or len(matches) < 5:
|
||||
good.append(m)
|
||||
return good
|
||||
|
||||
def _estimate_motion(self, prev_kp, curr_kp, matches, current_time):
|
||||
if len(matches) < 5:
|
||||
return
|
||||
|
||||
|
||||
pts1 = np.float32([prev_kp[m.queryIdx].pt for m in matches])
|
||||
pts2 = np.float32([curr_kp[m.trainIdx].pt for m in matches])
|
||||
|
||||
|
||||
E, mask = cv2.findEssentialMat(
|
||||
pts1, pts2, self.camera_matrix,
|
||||
method=cv2.RANSAC, prob=0.999, threshold=1.0)
|
||||
|
||||
pts1, pts2, self.camera_matrix, method=cv2.RANSAC, prob=0.999, threshold=1.0
|
||||
)
|
||||
|
||||
if E is None:
|
||||
return
|
||||
|
||||
|
||||
_, R, t, mask = cv2.recoverPose(E, pts1, pts2, self.camera_matrix, mask=mask)
|
||||
|
||||
|
||||
scale = 1.0
|
||||
translation = scale * t.flatten()
|
||||
|
||||
|
||||
if self.prev_time is not None:
|
||||
dt = (current_time - self.prev_time).nanoseconds / 1e9
|
||||
dt = current_time - self.prev_time
|
||||
if dt > 0:
|
||||
self.velocity = translation / dt
|
||||
|
||||
|
||||
self.position += self.orientation @ translation
|
||||
self.orientation = R @ self.orientation
|
||||
|
||||
def publish_pose(self):
|
||||
pose_msg = PoseStamped()
|
||||
pose_msg.header.stamp = self.get_clock().now().to_msg()
|
||||
pose_msg.header.frame_id = 'odom'
|
||||
|
||||
pose_msg.pose.position.x = float(self.position[0])
|
||||
pose_msg.pose.position.y = float(self.position[1])
|
||||
pose_msg.pose.position.z = float(self.position[2])
|
||||
|
||||
rotation = Rotation.from_matrix(self.orientation)
|
||||
quat = rotation.as_quat()
|
||||
pose_msg.pose.orientation.x = quat[0]
|
||||
pose_msg.pose.orientation.y = quat[1]
|
||||
pose_msg.pose.orientation.z = quat[2]
|
||||
pose_msg.pose.orientation.w = quat[3]
|
||||
|
||||
self.pose_pub.publish(pose_msg)
|
||||
|
||||
vel_msg = TwistStamped()
|
||||
vel_msg.header.stamp = self.get_clock().now().to_msg()
|
||||
vel_msg.header.frame_id = 'odom'
|
||||
vel_msg.twist.linear.x = float(self.velocity[0])
|
||||
vel_msg.twist.linear.y = float(self.velocity[1])
|
||||
vel_msg.twist.linear.z = float(self.velocity[2])
|
||||
|
||||
self.velocity_pub.publish(vel_msg)
|
||||
|
||||
if self.on_pose:
|
||||
r = Rotation.from_matrix(self.orientation)
|
||||
self.on_pose(self.position.copy(), r.as_quat())
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = VisualOdometryNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
def get_pose(self):
|
||||
r = Rotation.from_matrix(self.orientation)
|
||||
return self.position.copy(), r.as_quat()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
def get_velocity(self):
|
||||
return self.velocity.copy()
|
||||
|
||||
@@ -1,135 +1,76 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Visual Servoing - Vision-based control for precision positioning."""
|
||||
"""
|
||||
Visual Servoing - Vision-based control for precision landing on ArUco marker.
|
||||
Uses ObjectDetector detections + pymavlink to send velocity commands.
|
||||
"""
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import Image
|
||||
from geometry_msgs.msg import PoseStamped, TwistStamped, Pose
|
||||
from std_msgs.msg import Bool
|
||||
from cv_bridge import CvBridge
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
|
||||
class VisualServoing(Node):
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('visual_servoing')
|
||||
|
||||
self.bridge = CvBridge()
|
||||
|
||||
self.declare_parameter('target_marker_id', 0)
|
||||
self.declare_parameter('desired_distance', 1.0)
|
||||
self.declare_parameter('kp_linear', 0.5)
|
||||
self.declare_parameter('kp_angular', 0.3)
|
||||
self.declare_parameter('max_velocity', 1.0)
|
||||
|
||||
self.target_marker_id = self.get_parameter('target_marker_id').value
|
||||
self.desired_distance = self.get_parameter('desired_distance').value
|
||||
self.kp_linear = self.get_parameter('kp_linear').value
|
||||
self.kp_angular = self.get_parameter('kp_angular').value
|
||||
self.max_velocity = self.get_parameter('max_velocity').value
|
||||
|
||||
class VisualServoing:
|
||||
def __init__(self, controller, target_marker_id=0, desired_distance=1.0):
|
||||
self.controller = controller
|
||||
self.target_marker_id = target_marker_id
|
||||
self.desired_distance = desired_distance
|
||||
|
||||
self.kp_xy = 0.5
|
||||
self.kp_z = 0.3
|
||||
self.max_velocity = 1.0
|
||||
|
||||
self.enabled = False
|
||||
self.target_pose = None
|
||||
self.target_acquired = False
|
||||
self.image_center = (320, 240)
|
||||
|
||||
self.camera_matrix = np.array([
|
||||
[500.0, 0.0, 320.0],
|
||||
[0.0, 500.0, 240.0],
|
||||
[0.0, 0.0, 1.0]
|
||||
])
|
||||
self.dist_coeffs = np.zeros(5)
|
||||
|
||||
self.aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
|
||||
self.aruco_params = cv2.aruco.DetectorParameters()
|
||||
self.aruco_detector = cv2.aruco.ArucoDetector(self.aruco_dict, self.aruco_params)
|
||||
|
||||
self.image_sub = self.create_subscription(
|
||||
Image, '/uav/camera/forward/image_raw', self.image_callback, 10)
|
||||
|
||||
self.enable_sub = self.create_subscription(
|
||||
Bool, '/visual_servoing/enable', self.enable_callback, 10)
|
||||
|
||||
self.target_sub = self.create_subscription(
|
||||
Pose, '/visual_servoing/target', self.target_callback, 10)
|
||||
|
||||
self.velocity_pub = self.create_publisher(
|
||||
TwistStamped, '/uav/visual_servoing/cmd_vel', 10)
|
||||
|
||||
self.status_pub = self.create_publisher(
|
||||
Bool, '/visual_servoing/target_acquired', 10)
|
||||
|
||||
self.get_logger().info('Visual Servoing Node Started')
|
||||
|
||||
def enable_callback(self, msg):
|
||||
self.enabled = msg.data
|
||||
self.get_logger().info(f'Visual servoing {"enabled" if self.enabled else "disabled"}')
|
||||
|
||||
def target_callback(self, msg):
|
||||
self.target_pose = msg
|
||||
|
||||
def image_callback(self, msg):
|
||||
|
||||
self.last_detection = None
|
||||
|
||||
print(f"[VS] Visual Servoing initialized (target marker: {target_marker_id})")
|
||||
|
||||
def enable(self):
|
||||
self.enabled = True
|
||||
print("[VS] Enabled")
|
||||
|
||||
def disable(self):
|
||||
self.enabled = False
|
||||
print("[VS] Disabled")
|
||||
|
||||
def on_detections(self, detections):
|
||||
if not self.enabled:
|
||||
return
|
||||
|
||||
try:
|
||||
frame = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
|
||||
self.image_center = (frame.shape[1] // 2, frame.shape[0] // 2)
|
||||
|
||||
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
|
||||
corners, ids, _ = self.aruco_detector.detectMarkers(gray)
|
||||
|
||||
target_acquired = Bool()
|
||||
target_acquired.data = False
|
||||
|
||||
if ids is not None and self.target_marker_id in ids:
|
||||
idx = np.where(ids == self.target_marker_id)[0][0]
|
||||
target_corners = corners[idx]
|
||||
|
||||
rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
|
||||
[target_corners], 0.15, self.camera_matrix, self.dist_coeffs)
|
||||
|
||||
target_pos = tvecs[0][0]
|
||||
self.compute_control(target_pos, target_corners)
|
||||
|
||||
target_acquired.data = True
|
||||
|
||||
self.status_pub.publish(target_acquired)
|
||||
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Visual servoing error: {e}')
|
||||
|
||||
def compute_control(self, target_pos, corners):
|
||||
marker_center = np.mean(corners[0], axis=0)
|
||||
|
||||
error_x = self.image_center[0] - marker_center[0]
|
||||
error_y = self.image_center[1] - marker_center[1]
|
||||
error_z = target_pos[2] - self.desired_distance
|
||||
|
||||
vel_msg = TwistStamped()
|
||||
vel_msg.header.stamp = self.get_clock().now().to_msg()
|
||||
vel_msg.header.frame_id = 'base_link'
|
||||
|
||||
vel_msg.twist.linear.x = np.clip(self.kp_linear * error_z, -self.max_velocity, self.max_velocity)
|
||||
vel_msg.twist.linear.y = np.clip(self.kp_linear * error_x / 100.0, -self.max_velocity, self.max_velocity)
|
||||
vel_msg.twist.linear.z = np.clip(self.kp_linear * error_y / 100.0, -self.max_velocity, self.max_velocity)
|
||||
vel_msg.twist.angular.z = np.clip(self.kp_angular * error_x / 100.0, -1.0, 1.0)
|
||||
|
||||
self.velocity_pub.publish(vel_msg)
|
||||
|
||||
target = None
|
||||
for d in detections:
|
||||
if d.get("id") == self.target_marker_id:
|
||||
target = d
|
||||
break
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = VisualServoing()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
if target is None:
|
||||
self.target_acquired = False
|
||||
return
|
||||
|
||||
self.target_acquired = True
|
||||
self.last_detection = target
|
||||
self.compute_control(target)
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
def compute_control(self, detection):
|
||||
center_px = detection["center_px"]
|
||||
distance = detection["distance"]
|
||||
position = detection["position"]
|
||||
|
||||
error_x = self.image_center[0] - center_px[0]
|
||||
error_y = self.image_center[1] - center_px[1]
|
||||
error_z = distance - self.desired_distance
|
||||
|
||||
vx = np.clip(self.kp_xy * error_y / 100.0, -self.max_velocity, self.max_velocity)
|
||||
vy = np.clip(self.kp_xy * error_x / 100.0, -self.max_velocity, self.max_velocity)
|
||||
vz = np.clip(-self.kp_z * error_z, -self.max_velocity, self.max_velocity)
|
||||
|
||||
self.controller.send_velocity(vx, vy, vz)
|
||||
|
||||
print(
|
||||
f"\r[VS] Target ID:{detection['id']} "
|
||||
f"d:{distance:.2f}m "
|
||||
f"err:({error_x:.0f},{error_y:.0f}) "
|
||||
f"vel:({vx:.2f},{vy:.2f},{vz:.2f})",
|
||||
end="",
|
||||
flush=True,
|
||||
)
|
||||
|
||||
Reference in New Issue
Block a user