Fix rel_alt type collision; retire VM-era mock_publisher
- depth_node + failsafe_monitor: subscribe to real MAVROS topic /mavros/mavros/rel_alt (was /mavros/global_position/rel_alt) - failsafe_monitor: Float32 -> Float64 (resolves duplicate-type error that dropped the topic from recordings) - Remove mock_publisher (VM-era fake data, now collides with real MAVROS) - Verified: /rov/depth now publishes real data; zero duplicate-type errors
This commit is contained in:
Grant du Toit
parent
c94412ddae
commit
e36644e97e
@ -26,7 +26,7 @@ Priority order (highest wins when multiple conditions active):
|
||||
Topics subscribed:
|
||||
/mavros/battery (sensor_msgs/BatteryState)
|
||||
/mavros/state (mavros_msgs/State)
|
||||
/mavros/global_position/rel_alt (std_msgs/Float32) — altitude/depth proxy
|
||||
/mavros/mavros/rel_alt (std_msgs/Float64) — altitude/depth proxy
|
||||
/rov/mission/abort (std_msgs/Bool)
|
||||
|
||||
Topics published:
|
||||
@ -37,7 +37,7 @@ import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.duration import Duration
|
||||
from rclpy.qos import qos_profile_sensor_data
|
||||
from std_msgs.msg import Bool, Float32
|
||||
from std_msgs.msg import Bool, Float64
|
||||
from sensor_msgs.msg import BatteryState
|
||||
from mavros_msgs.msg import State
|
||||
from rov_interfaces.msg import FailsafeStatus
|
||||
@ -152,7 +152,7 @@ class FailsafeMonitor(Node):
|
||||
self._state_callback, qos_profile_sensor_data)
|
||||
|
||||
self.create_subscription(
|
||||
Float32, '/mavros/global_position/rel_alt',
|
||||
Float64, '/mavros/mavros/rel_alt',
|
||||
self._altitude_callback, qos_profile_sensor_data)
|
||||
|
||||
self.create_subscription(
|
||||
@ -216,7 +216,7 @@ class FailsafeMonitor(Node):
|
||||
self.last_heartbeat = self.get_clock().now()
|
||||
self.comms_loss_start = None
|
||||
|
||||
def _altitude_callback(self, msg: Float32):
|
||||
def _altitude_callback(self, msg: Float64):
|
||||
"""
|
||||
Update current depth from relative altitude.
|
||||
rel_alt is positive above home — negate for depth below surface.
|
||||
|
||||
@ -4,7 +4,7 @@ Depth node.
|
||||
Reads depth/pressure from ArduSub via MAVROS and republishes as Float64.
|
||||
|
||||
Topics subscribed:
|
||||
/mavros/global_position/rel_alt (std_msgs/Float64) — altitude from MAVROS
|
||||
/mavros/mavros/rel_alt (std_msgs/Float64) — altitude from MAVROS
|
||||
|
||||
Topics published:
|
||||
/rov/depth (std_msgs/Float64) — depth in metres (positive down)
|
||||
@ -23,7 +23,7 @@ class DepthNode(Node):
|
||||
super().__init__('depth_node')
|
||||
|
||||
self.sub = self.create_subscription(
|
||||
Float64, '/mavros/global_position/rel_alt',
|
||||
Float64, '/mavros/mavros/rel_alt',
|
||||
self.alt_callback, qos_profile_sensor_data)
|
||||
|
||||
self.pub = self.create_publisher(Float64, '/rov/depth', 10)
|
||||
|
||||
@ -1,322 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Mock Publisher Node — Argonaut 3 Simulation
|
||||
=============================================
|
||||
DEV ONLY — NOT FOR DEPLOYMENT TO VEHICLE HARDWARE.
|
||||
|
||||
Publishes synthetic sensor data to simulate a connected ArduSub vehicle.
|
||||
Allows failsafe_monitor and mission_executor to be tested without
|
||||
physical hardware or a live MAVROS connection.
|
||||
|
||||
Simulated topics (mimics MAVROS output):
|
||||
/mavros/state (mavros_msgs/State) — FCU connection + heartbeat
|
||||
/mavros/battery (sensor_msgs/BatteryState) — battery level
|
||||
/mavros/global_position/rel_alt (std_msgs/Float32) — depth (negative = below surface)
|
||||
/rov/state (nav_msgs/Odometry) — vehicle position + orientation
|
||||
|
||||
Scenarios (set via ROS parameter 'scenario'):
|
||||
nominal — healthy vehicle, normal battery, shallow depth
|
||||
low_battery — battery draining toward return threshold
|
||||
comms_loss — heartbeat stops after 10s to trigger comms failsafe
|
||||
depth_warning — vehicle descends toward warning depth
|
||||
all_clear — all GREEN, used to verify normal operation
|
||||
|
||||
Usage:
|
||||
ros2 run rov_simulation mock_publisher --ros-args -p scenario:=low_battery
|
||||
"""
|
||||
|
||||
import math
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import Float32, Bool, Header
|
||||
from sensor_msgs.msg import BatteryState
|
||||
from nav_msgs.msg import Odometry
|
||||
from geometry_msgs.msg import Quaternion
|
||||
from mavros_msgs.msg import State
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Available simulation scenarios
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
SCENARIOS = {
|
||||
'nominal': {
|
||||
'description': 'Healthy vehicle — all systems nominal',
|
||||
'battery_start': 0.85, # 85% — well above all thresholds
|
||||
'battery_drain': 0.0, # No drain
|
||||
'depth_start_m': 5.0, # Shallow
|
||||
'depth_rate_mps': 0.0, # Stationary
|
||||
'comms_loss_at_s': None, # No comms loss
|
||||
},
|
||||
'low_battery': {
|
||||
'description': 'Battery draining — triggers warning then return threshold',
|
||||
'battery_start': 0.30, # Start above warning (25%)
|
||||
'battery_drain': 0.001, # Drain 0.1% per second
|
||||
'depth_start_m': 10.0,
|
||||
'depth_rate_mps': 0.0,
|
||||
'comms_loss_at_s': None,
|
||||
},
|
||||
'comms_loss': {
|
||||
'description': 'Heartbeat stops after 10s — triggers comms failsafe',
|
||||
'battery_start': 0.80,
|
||||
'battery_drain': 0.0,
|
||||
'depth_start_m': 8.0,
|
||||
'depth_rate_mps': 0.0,
|
||||
'comms_loss_at_s': 10.0, # Stop heartbeat at 10s
|
||||
},
|
||||
'depth_warning': {
|
||||
'description': 'Vehicle descending toward warning depth limit',
|
||||
'battery_start': 0.75,
|
||||
'battery_drain': 0.0,
|
||||
'depth_start_m': 180.0, # Start near design depth (200m)
|
||||
'depth_rate_mps': 0.5, # Descend 0.5 m/s toward warning (250m)
|
||||
'comms_loss_at_s': None,
|
||||
},
|
||||
'all_clear': {
|
||||
'description': 'All GREEN — verify normal operation produces no failsafe',
|
||||
'battery_start': 0.95,
|
||||
'battery_drain': 0.0,
|
||||
'depth_start_m': 3.0,
|
||||
'depth_rate_mps': 0.0,
|
||||
'comms_loss_at_s': None,
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
class MockPublisher(Node):
|
||||
"""
|
||||
Publishes synthetic MAVROS-compatible sensor data for dev testing.
|
||||
Simulates a connected ArduSub vehicle without physical hardware.
|
||||
"""
|
||||
|
||||
PUBLISH_RATE_HZ = 10 # All topics published at 10 Hz
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('mock_publisher')
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Parameters
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
self.declare_parameter('scenario', 'nominal')
|
||||
scenario_name = self.get_parameter('scenario').value
|
||||
|
||||
if scenario_name not in SCENARIOS:
|
||||
self.get_logger().error(
|
||||
f'Unknown scenario: {scenario_name}. '
|
||||
f'Valid: {list(SCENARIOS.keys())}'
|
||||
)
|
||||
scenario_name = 'nominal'
|
||||
|
||||
self._scenario = SCENARIOS[scenario_name]
|
||||
self.get_logger().info(
|
||||
f'MockPublisher starting — scenario: {scenario_name} '
|
||||
f'({self._scenario["description"]})'
|
||||
)
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Simulation state
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
self._elapsed_s = 0.0
|
||||
self._battery_pct = self._scenario['battery_start']
|
||||
self._depth_m = self._scenario['depth_start_m']
|
||||
self._comms_active = True
|
||||
|
||||
# Simple position simulation — vehicle moves in XY circle for realism
|
||||
self._pos_x = 0.0
|
||||
self._pos_y = 0.0
|
||||
self._orbit_radius_m = 2.0 # metres
|
||||
self._orbit_rate_rps = 0.05 # radians per second
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Publishers (all matching MAVROS topic names and message types)
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
self._state_pub = self.create_publisher(
|
||||
State, '/mavros/state', 10)
|
||||
|
||||
self._battery_pub = self.create_publisher(
|
||||
BatteryState, '/mavros/battery', 10)
|
||||
|
||||
self._alt_pub = self.create_publisher(
|
||||
Float32, '/mavros/global_position/rel_alt', 10)
|
||||
|
||||
self._odom_pub = self.create_publisher(
|
||||
Odometry, '/rov/state', 10)
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Timer
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
self._dt = 1.0 / self.PUBLISH_RATE_HZ
|
||||
self.create_timer(self._dt, self._publish_all)
|
||||
|
||||
self.get_logger().warn(
|
||||
'*** DEV ONLY — mock_publisher is active. '
|
||||
'Do not deploy to vehicle hardware. ***'
|
||||
)
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Main publish cycle
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
def _publish_all(self):
|
||||
"""Advance simulation state and publish all topics."""
|
||||
self._advance_simulation()
|
||||
self._publish_state()
|
||||
self._publish_battery()
|
||||
self._publish_altitude()
|
||||
self._publish_odometry()
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Simulation state advancement
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
def _advance_simulation(self):
|
||||
"""Advance all simulated quantities by one timestep."""
|
||||
self._elapsed_s += self._dt
|
||||
|
||||
# Battery drain
|
||||
drain = self._scenario['battery_drain'] * self._dt
|
||||
self._battery_pct = max(0.0, self._battery_pct - drain)
|
||||
|
||||
# Depth change
|
||||
self._depth_m += self._scenario['depth_rate_mps'] * self._dt
|
||||
|
||||
# Position orbit (simple circle for realistic /rov/state output)
|
||||
angle = self._orbit_rate_rps * self._elapsed_s
|
||||
self._pos_x = self._orbit_radius_m * math.cos(angle)
|
||||
self._pos_y = self._orbit_radius_m * math.sin(angle)
|
||||
|
||||
# Comms loss scenario — stop publishing heartbeat after threshold
|
||||
loss_at = self._scenario['comms_loss_at_s']
|
||||
if loss_at is not None and self._elapsed_s >= loss_at:
|
||||
if self._comms_active:
|
||||
self.get_logger().warn(
|
||||
f'[{self._elapsed_s:.1f}s] Simulating comms loss — '
|
||||
'stopping FCU heartbeat'
|
||||
)
|
||||
self._comms_active = False
|
||||
|
||||
# Log key threshold crossings once
|
||||
self._log_threshold_crossings()
|
||||
|
||||
def _log_threshold_crossings(self):
|
||||
"""Log once when battery crosses each threshold."""
|
||||
pct = self._battery_pct
|
||||
thresholds = [
|
||||
(0.25, 'WARNING (25%)'),
|
||||
(0.20, 'RETURN threshold (20%)'),
|
||||
(0.12, 'CRITICAL (12%)'),
|
||||
(0.08, 'EMERGENCY (8%)'),
|
||||
]
|
||||
for threshold, label in thresholds:
|
||||
attr = f'_logged_battery_{int(threshold*100)}'
|
||||
if pct < threshold and not getattr(self, attr, False):
|
||||
self.get_logger().warn(
|
||||
f'[{self._elapsed_s:.1f}s] Battery crossed {label}: '
|
||||
f'{pct*100:.1f}%'
|
||||
)
|
||||
setattr(self, attr, True)
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Topic publishers
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
def _publish_state(self):
|
||||
"""
|
||||
Publish FCU connection state (mavros_msgs/State).
|
||||
Mimics the MAVROS /mavros/state topic.
|
||||
Stops publishing when comms_loss scenario activates.
|
||||
"""
|
||||
if not self._comms_active:
|
||||
# Do not publish — simulates lost heartbeat
|
||||
return
|
||||
|
||||
msg = State()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.connected = True
|
||||
msg.armed = False # Unarmed for dev testing
|
||||
msg.guided = True
|
||||
msg.mode = 'ALT_HOLD'
|
||||
msg.system_status = 3 # MAV_STATE_STANDBY
|
||||
self._state_pub.publish(msg)
|
||||
|
||||
def _publish_battery(self):
|
||||
"""
|
||||
Publish battery state (sensor_msgs/BatteryState).
|
||||
Mimics the MAVROS /mavros/battery topic.
|
||||
percentage field: 0.0–1.0.
|
||||
"""
|
||||
msg = BatteryState()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.percentage = float(self._battery_pct)
|
||||
msg.voltage = 14.8 * self._battery_pct + 12.0 # Approximate LiPo curve
|
||||
msg.current = 5.0 # Amps — nominal draw
|
||||
msg.charge = float('nan')
|
||||
msg.capacity = float('nan')
|
||||
msg.design_capacity = float('nan')
|
||||
msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
|
||||
self._battery_pub.publish(msg)
|
||||
|
||||
def _publish_altitude(self):
|
||||
"""
|
||||
Publish relative altitude (std_msgs/Float32).
|
||||
Negative value = depth below surface, matching MAVROS convention.
|
||||
"""
|
||||
msg = Float32()
|
||||
msg.data = -abs(self._depth_m) # Always negative (depth)
|
||||
self._alt_pub.publish(msg)
|
||||
|
||||
def _publish_odometry(self):
|
||||
"""
|
||||
Publish vehicle state (nav_msgs/Odometry).
|
||||
Provides position for mission_executor arrival detection and
|
||||
breadcrumb logging.
|
||||
"""
|
||||
msg = Odometry()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.header.frame_id = 'map'
|
||||
msg.child_frame_id = 'base_link'
|
||||
|
||||
# Position — orbit + depth
|
||||
msg.pose.pose.position.x = self._pos_x
|
||||
msg.pose.pose.position.y = self._pos_y
|
||||
msg.pose.pose.position.z = -abs(self._depth_m)
|
||||
|
||||
# Orientation — identity quaternion (level vehicle)
|
||||
msg.pose.pose.orientation.w = 1.0
|
||||
msg.pose.pose.orientation.x = 0.0
|
||||
msg.pose.pose.orientation.y = 0.0
|
||||
msg.pose.pose.orientation.z = 0.0
|
||||
|
||||
# Velocity — tangential to orbit
|
||||
angle = self._orbit_rate_rps * self._elapsed_s
|
||||
speed = self._orbit_radius_m * self._orbit_rate_rps
|
||||
msg.twist.twist.linear.x = -speed * math.sin(angle)
|
||||
msg.twist.twist.linear.y = speed * math.cos(angle)
|
||||
msg.twist.twist.linear.z = 0.0
|
||||
|
||||
self._odom_pub.publish(msg)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Entry point
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def main(args=None):
|
||||
"""ROS2 node entry point."""
|
||||
rclpy.init(args=args)
|
||||
node = MockPublisher()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@ -23,7 +23,6 @@ setup(
|
||||
license='Proprietary',
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'mock_publisher = rov_simulation.mock_publisher:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user