Utilities Module¶

from nectar.utils.process import ProcessUtils

# Check if ROS2 node is running
if ProcessUtils.is_node_running("mavros_node"):
    print("MAVROS node is running")

# Start a ROS2 node in background
ProcessUtils.start_process(
    command="ros2 launch mavros apm.launch fcu_url:=serial:///dev/ttyUSB0:921600",
    name="mavros_node",
    gui=False  # Use tmux (headless)
)

# Wait for node to appear
if ProcessUtils.wait_for_node("mavros_node", timeout=10.0):
    print("MAVROS node started")

# Wait for a topic before connecting
if ProcessUtils.wait_for_topic("/mavros/local_position/pose", timeout=10.0):
    print("Local position topic is live")

# Check if tmux session exists
if ProcessUtils.has_process("mavros_node"):
    print("MAVROS session exists")

# Stop the process
ProcessUtils.kill_process("mavros_node")

from nectar.utils.log import OK, ERR, WARN, ARROW

print(f"{OK} Driver started")
print(f"{ERR} Connection failed")
print(f"{WARN} Retrying...")
print(f"{ARROW} Next step: arm")

from nectar.utils.gps_calculate import GPSCalculate

# Calculate distance between two GPS coordinates
dist = GPSCalculate.haversine(-27.1234, -48.4567, -27.1245, -48.4578)
print(f"Distance: {dist:.2f} m")

# Calculate bearing
bearing = GPSCalculate.bearing(-27.1234, -48.4567, -27.1245, -48.4578)
print(f"Bearing: {bearing:.1f}°")

# Calculate GPS offset
new_lat, new_lon, new_alt = GPSCalculate.calculate_gps_offset(
    x=10.0,  # 10 meters east
    y=5.0,   # 5 meters north
    z=2.0,   # 2 meters up
    latitude=-27.1234,
    longitude=-48.4567,
    altitude=100.0,
    heading=45.0  # degrees
)

# Interpolate between GPS points
midpoint = GPSCalculate.interp_geo(
    start=(-27.1234, -48.4567),
    end=(-27.1245, -48.4578),
    frac=0.5  # 50% of the way
)

# Generate grid of GPS points
vertices = (
    (-27.1234, -48.4567),  # top-left
    (-27.1234, -48.4578),  # top-right
    (-27.1245, -48.4578),  # bottom-right
    (-27.1245, -48.4567),  # bottom-left
)
grid = GPSCalculate.generate_point_grid(vertices, grid_shape=(10, 10))

from nectar.utils.position_utils import PositionUtils
from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import PositionTarget

# Calculate body frame distance
dx_body, dy_body, dz_body = PositionUtils.get_body_distance(
    target=target_position,  # PositionTarget or GeoPoseStamped
    current=current_pose,     # PoseStamped, PoseWithCovarianceStamped, or NavSatFix
    heading=45.0  # degrees (required for NavSatFix)
)

# Extract yaw from pose
yaw = PositionUtils.get_yaw_from_pose(pose_message)  # radians

# Compute yaw error (shortest path, wrapped to [-pi, pi])
import numpy as np
dyaw = PositionUtils.compute_yaw_error(target_yaw=1.0, current_yaw=0.5)  # radians
dyaw = PositionUtils.compute_yaw_error(1.0, 0.5, threshold=np.radians(3))  # with deadband

# Convert position to target message
target = PositionUtils.convert_position_to_target(
    pose=current_pose,
    heading=45.0,  # degrees, required for NavSatFix
    lidar=2.5  # optional altitude override
)

# Transform velocities from takeoff frame to body frame
vx_body, vy_body, vz_body = PositionUtils.transform_takeoff_to_body_velocities(
    vx=1.0,
    vy=0.5,
    vz=0.0,
    current_yaw=0.785,  # radians (45°)
    takeoff_yaw=0.0     # radians
)