ROS 2: Library with dynamic modes API

Tools/msg/px_generate_uorb_topic_helper.py

@@ -171,6 +171,61 @@ def get_children_fields(base_type, search_path):
     return spec_temp.parsed_fields()
 
 
+def get_message_fields_str_for_message_hash(msg_fields, search_path):
+    """
+    Get all fields (including for nested types) in the form of:
+'''
+uint64 timestamp
+uint8 esc_count
+uint8 esc_online_flags
+EscReport[8] esc
+uint64 timestamp
+uint32 esc_errorcount
+int32 esc_rpm
+float32 esc_voltage
+uint16 failures
+int8 esc_power
+'''
+    """
+    all_fields_str = ''
+    for field in msg_fields:
+        if field.is_header:
+            continue
+
+        type_name = field.type
+        # detect embedded types
+        sl_pos = type_name.find('/')
+        if sl_pos >= 0:
+            type_name = type_name[sl_pos + 1:]
+
+        all_fields_str += type_name + ' ' + field.name + '\n'
+
+        if sl_pos >= 0: # nested type, add all nested fields
+            children_fields = get_children_fields(field.base_type, search_path)
+            all_fields_str += get_message_fields_str_for_message_hash(children_fields, search_path)
+
+    return all_fields_str
+
+
+def hash_32_fnv1a(data: str):
+    hash_val = 0x811c9dc5
+    prime = 0x1000193
+    for i in range(len(data)):
+        value = ord(data[i])
+        hash_val = hash_val ^ value
+        hash_val *= prime
+        hash_val &= 0xffffffff
+    return hash_val
+
+
+def get_message_hash(msg_fields, search_path):
+    """
+    Get a 32 bit message hash over all fields
+    """
+    all_fields_str = get_message_fields_str_for_message_hash(msg_fields, search_path)
+    return hash_32_fnv1a(all_fields_str)
+
+
 def add_padding_bytes(fields, search_path):
     """
     Add padding fields before the embedded types, at the end and calculate the

Tools/msg/templates/ucdr/msg.h.em

@@ -124,7 +124,7 @@ static inline constexpr int ucdr_topic_size_@(topic)()
 	return @(struct_size);
 }
 
-bool ucdr_serialize_@(topic)(const void* data, ucdrBuffer& buf, int64_t time_offset = 0)
+static inline bool ucdr_serialize_@(topic)(const void* data, ucdrBuffer& buf, int64_t time_offset = 0)
 {
 	const @(uorb_struct)& topic = *static_cast<const @(uorb_struct)*>(data);
 @{
@@ -153,7 +153,7 @@ for field_type, field_name, field_size, padding in fields:
 	return true;
 }
 
-bool ucdr_deserialize_@(topic)(ucdrBuffer& buf, @(uorb_struct)& topic, int64_t time_offset = 0)
+static inline bool ucdr_deserialize_@(topic)(ucdrBuffer& buf, @(uorb_struct)& topic, int64_t time_offset = 0)
 {
 @{
 for field_type, field_name, field_size, padding in fields:

Tools/msg/templates/uorb/msg.cpp.em

@@ -58,6 +58,7 @@ from px_generate_uorb_topic_helper import * # this is in Tools/
 
 uorb_struct = '%s_s'%name_snake_case
 
+message_hash = get_message_hash(spec.parsed_fields(), search_path)
 sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
 struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
 }@
@@ -74,7 +75,7 @@ struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
 
 @[for topic in topics]@
 static_assert(static_cast<orb_id_size_t>(ORB_ID::@topic) == @(all_topics.index(topic)), "ORB_ID index mismatch");
-ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), static_cast<orb_id_size_t>(ORB_ID::@topic));
+ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), @(message_hash)u, static_cast<orb_id_size_t>(ORB_ID::@topic));
 @[end for]
 
 void print_message(const orb_metadata *meta, const @uorb_struct& message)

Tools/px4events/srcparser.py

@@ -250,6 +250,22 @@ def parse_message(s):
                             event.group = "arming_check"
                         event.prepend_arguments([('navigation_mode_group_t', 'modes'),
                                 ('uint8_t', 'health_component_index')])
+                    elif call in ['reporter.healthFailureExt', 'reporter.armingCheckFailureExt']: # from ROS2
+                        assert len(args_split) == num_args + 3, \
+                            "Unexpected Number of arguments for: {:}, {:}".format(args_split, num_args)
+                        m = self.re_event_id.search(args_split[0])
+                        if m:
+                            _, event_name = m.group(1, 2)
+                        else:
+                            raise Exception("Could not extract event ID from {:}".format(args_split[0]))
+                        event.name = event_name
+                        event.message = args_split[2][1:-1]
+                        if 'health' in call:
+                            event.group = "health"
+                        else:
+                            event.group = "arming_check"
+                        event.prepend_arguments([('navigation_mode_group_t', 'modes'),
+                                ('uint8_t', 'health_component_index')])
                     else:
                         raise Exception("unknown event method call: {}, args: {}".format(call, args))
 

boards/px4/fmu-v5x/default.px4board

@@ -111,3 +111,4 @@ CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
+CONFIG_MAVLINK_DIALECT="development"

msg/ArmingCheckReply.msg

@@ -0,0 +1,34 @@
+uint64 timestamp # time since system start (microseconds)
+
+uint8 request_id
+uint8 registration_id
+
+uint8 HEALTH_COMPONENT_INDEX_NONE = 0
+uint8 HEALTH_COMPONENT_INDEX_AVOIDANCE = 19
+
+uint8 health_component_index      # HEALTH_COMPONENT_INDEX_*
+bool health_component_is_present
+bool health_component_warning
+bool health_component_error
+
+bool can_arm_and_run              # whether arming is possible, and if it's a navigation mode, if it can run
+
+uint8 num_events
+
+Event[5] events
+
+# Mode requirements
+bool mode_req_angular_velocity
+bool mode_req_attitude
+bool mode_req_local_alt
+bool mode_req_local_position
+bool mode_req_local_position_relaxed
+bool mode_req_global_position
+bool mode_req_mission
+bool mode_req_home_position
+bool mode_req_prevent_arming
+bool mode_req_manual_control
+
+
+uint8 ORB_QUEUE_LENGTH = 4
+

msg/ArmingCheckRequest.msg

@@ -0,0 +1,7 @@
+uint64 timestamp # time since system start (microseconds)
+
+# broadcast message to request all registered arming checks to be reported
+
+uint8 request_id
+
+

msg/CMakeLists.txt

@@ -49,6 +49,8 @@ set(msg_files
 	Airspeed.msg
 	AirspeedValidated.msg
 	AirspeedWind.msg
+	ArmingCheckReply.msg
+	ArmingCheckRequest.msg
 	AutotuneAttitudeControlStatus.msg
 	BatteryStatus.msg
 	ButtonEvent.msg
@@ -58,6 +60,7 @@ set(msg_files
 	CellularStatus.msg
 	CollisionConstraints.msg
 	CollisionReport.msg
+	ConfigOverrides.msg
 	ControlAllocatorStatus.msg
 	Cpuload.msg
 	DatamanRequest.msg
@@ -130,6 +133,8 @@ set(msg_files
 	ManualControlSwitches.msg
 	MavlinkLog.msg
 	MavlinkTunnel.msg
+	MessageFormatRequest.msg
+	MessageFormatResponse.msg
 	Mission.msg
 	MissionResult.msg
 	MountOrientation.msg
@@ -161,6 +166,8 @@ set(msg_files
 	RateCtrlStatus.msg
 	RcChannels.msg
 	RcParameterMap.msg
+	RegisterExtComponentReply.msg
+	RegisterExtComponentRequest.msg
 	Rpm.msg
 	RtlTimeEstimate.msg
 	SatelliteInfo.msg
@@ -198,6 +205,7 @@ set(msg_files
 	UavcanParameterValue.msg
 	UlogStream.msg
 	UlogStreamAck.msg
+	UnregisterExtComponent.msg
 	VehicleAcceleration.msg
 	VehicleAirData.msg
 	VehicleAngularAccelerationSetpoint.msg

msg/ConfigOverrides.msg

@@ -0,0 +1,19 @@
+# Configurable overrides by (external) modes or mode executors
+uint64 timestamp		# time since system start (microseconds)
+
+bool disable_auto_disarm         # Prevent the drone from automatically disarming after landing (if configured)
+
+bool defer_failsafes             # Defer all failsafes that can be deferred (until the flag is cleared)
+int16 defer_failsafes_timeout_s  # Maximum time a failsafe can be deferred. 0 = system default, -1 = no timeout
+
+
+int8 SOURCE_TYPE_MODE = 0
+int8 SOURCE_TYPE_MODE_EXECUTOR = 1
+int8 source_type
+
+uint8 source_id                  # ID depending on source_type
+
+uint8 ORB_QUEUE_LENGTH = 4
+
+# TOPICS config_overrides config_overrides_request
+

msg/MessageFormatRequest.msg

@@ -0,0 +1,10 @@
+uint64 timestamp # time since system start (microseconds)
+
+# Request to PX4 to get the hash of a message, to check for message compatibility
+
+uint16 LATEST_PROTOCOL_VERSION = 1 # Current version of this protocol. Increase this whenever the MessageFormatRequest or MessageFormatResponse changes.
+
+uint16 protocol_version           # Must be set to LATEST_PROTOCOL_VERSION. Do not change this field, it must be the first field after the timestamp
+
+char[50] topic_name  # E.g. /fmu/in/vehicle_command
+

msg/MessageFormatResponse.msg

@@ -0,0 +1,11 @@
+uint64 timestamp # time since system start (microseconds)
+
+# Response from PX4 with the format of a message
+
+uint16 protocol_version           # Must be set to LATEST_PROTOCOL_VERSION. Do not change this field, it must be the first field after the timestamp
+
+char[50] topic_name  # E.g. /fmu/in/vehicle_command
+
+bool success
+uint32 message_hash # hash over all message fields
+

msg/RegisterExtComponentReply.msg

@@ -0,0 +1,14 @@
+uint64 timestamp # time since system start (microseconds)
+
+uint64 request_id          # ID from the request
+char[25] name              # name from the request
+
+uint16 px4_ros2_api_version
+
+bool success
+int8 arming_check_id      # arming check registration ID (-1 if invalid)
+int8 mode_id              # assigned mode ID (-1 if invalid)
+int8 mode_executor_id     # assigned mode executor ID (-1 if invalid)
+
+uint8 ORB_QUEUE_LENGTH = 2
+

msg/RegisterExtComponentRequest.msg

@@ -0,0 +1,21 @@
+# Request to register an external component
+uint64 timestamp # time since system start (microseconds)
+
+uint64 request_id                  # ID, set this to a random value
+char[25] name                      # either the requested mode name, or component name
+
+uint16 LATEST_PX4_ROS2_API_VERSION = 1 # API version compatibility. Increase this on a breaking semantic change. Changes to any message field are detected separately and do not require an API version change.
+
+uint16 px4_ros2_api_version   # Set to LATEST_PX4_ROS2_API_VERSION
+
+# Components to be registered
+bool register_arming_check
+bool register_mode                 # registering a mode also requires arming_check to be set
+bool register_mode_executor        # registering an executor also requires a mode to be registered (which is the owned mode by the executor)
+
+bool enable_replace_internal_mode  # set to true if an internal mode should be replaced
+uint8 replace_internal_mode        # vehicle_status::NAVIGATION_STATE_*
+bool activate_mode_immediately     # switch to the registered mode (can only be set in combination with an executor)
+
+
+uint8 ORB_QUEUE_LENGTH = 2

msg/UnregisterExtComponent.msg

@@ -0,0 +1,10 @@
+uint64 timestamp # time since system start (microseconds)
+
+char[25] name                      # either the mode name, or component name
+
+int8 arming_check_id      # arming check registration ID (-1 if not registered)
+int8 mode_id              # assigned mode ID (-1 if not registered)
+int8 mode_executor_id     # assigned mode executor ID (-1 if not registered)
+
+
+

msg/VehicleCommand.msg

@@ -71,6 +71,7 @@ uint16 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243		# UAVCAN configuration. If param 1 ==
 uint16 VEHICLE_CMD_PREFLIGHT_STORAGE = 245		# Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246	# Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_OBLIQUE_SURVEY=260			# Mission command to set a Camera Auto Mount Pivoting Oblique Survey for this flight|Camera trigger distance (meters)| Shutter integration time (ms)| Camera minimum trigger interval| Number of positions| Roll| Pitch| Empty|
+uint16 VEHICLE_CMD_DO_SET_STANDARD_MODE=262		# Enable the specified standard MAVLink mode |MAV_STANDARD_MODE|
 uint16 VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION = 283	# Command to ask information about a low level gimbal
 
 uint16 VEHICLE_CMD_MISSION_START = 300			# start running a mission |first_item: the first mission item to run| last_item:  the last mission item to run (after this item is run, the mission ends)|
@@ -103,6 +104,7 @@ uint16 VEHICLE_CMD_DO_WINCH = 42600			# Command to operate winch.
 # PX4 vehicle commands (beyond 16 bit mavlink commands)
 uint32 VEHICLE_CMD_PX4_INTERNAL_START    = 65537        # start of PX4 internal only vehicle commands (> UINT16_MAX)
 uint32 VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN = 100000       # Sets the GPS coordinates of the vehicle local origin (0,0,0) position. |Empty|Empty|Empty|Empty|Latitude|Longitude|Altitude|
+uint32 VEHICLE_CMD_SET_NAV_STATE = 100001               # Change mode by specifying nav_state directly. |nav_state|Empty|Empty|Empty|Empty|Empty|Empty|
 
 uint8 VEHICLE_MOUNT_MODE_RETRACT = 0			# Load and keep safe position (Roll,Pitch,Yaw) from permanent memory and stop stabilization |
 uint8 VEHICLE_MOUNT_MODE_NEUTRAL = 1			# Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. |
@@ -174,8 +176,10 @@ uint32 command			# Command ID
 uint8 target_system		# System which should execute the command
 uint8 target_component		# Component which should execute the command, 0 for all components
 uint8 source_system		# System sending the command
-uint8 source_component		# Component sending the command
+uint16 source_component	# Component / mode executor sending the command
 uint8 confirmation		# 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)
 bool from_external
 
-# TOPICS vehicle_command gimbal_v1_command
+uint16 COMPONENT_MODE_EXECUTOR_START = 1000
+
+# TOPICS vehicle_command gimbal_v1_command vehicle_command_mode_executor

msg/VehicleCommandAck.msg

@@ -28,6 +28,6 @@ uint8 result						# Command result
 uint8 result_param1					# Also used as progress[%], it can be set with the reason why the command was denied, or the progress percentage when result is MAV_RESULT_IN_PROGRESS
 int32 result_param2					# Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied.
 uint8 target_system
-uint8 target_component
+uint16 target_component 				# Target component / mode executor
 
 bool from_external					# Indicates if the command came from an external source

msg/VehicleControlMode.msg

@@ -15,3 +15,8 @@ bool flag_control_altitude_enabled		# true if altitude is controlled
 bool flag_control_climb_rate_enabled		# true if climb rate is controlled
 bool flag_control_termination_enabled		# true if flighttermination is enabled
 bool flag_control_allocation_enabled		# true if control allocation is enabled
+
+# TODO: use dedicated topic for external requests
+uint8 source_id                  # Mode ID (nav_state)
+
+# TOPICS vehicle_control_mode config_control_setpoints

msg/VehicleStatus.msg

@@ -52,7 +52,20 @@ uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19  # Auto Follow
 uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20       # Precision land with landing target
 uint8 NAVIGATION_STATE_ORBIT = 21               # Orbit in a circle
 uint8 NAVIGATION_STATE_AUTO_VTOL_TAKEOFF = 22   # Takeoff, transition, establish loiter
-uint8 NAVIGATION_STATE_MAX = 23
+uint8 NAVIGATION_STATE_EXTERNAL1 = 23
+uint8 NAVIGATION_STATE_EXTERNAL2 = 24
+uint8 NAVIGATION_STATE_EXTERNAL3 = 25
+uint8 NAVIGATION_STATE_EXTERNAL4 = 26
+uint8 NAVIGATION_STATE_EXTERNAL5 = 27
+uint8 NAVIGATION_STATE_EXTERNAL6 = 28
+uint8 NAVIGATION_STATE_EXTERNAL7 = 29
+uint8 NAVIGATION_STATE_EXTERNAL8 = 30
+uint8 NAVIGATION_STATE_MAX = 31
+
+uint8 executor_in_charge                        # Current mode executor in charge (0=Autopilot)
+
+uint32 valid_nav_states_mask                    # Bitmask for all valid nav_state values
+uint32 can_set_nav_states_mask                  # Bitmask for all modes that a user can select
 
 # Bitmask of detected failures
 uint16 failure_detector_status
@@ -78,8 +91,13 @@ uint8 VEHICLE_TYPE_FIXED_WING = 2
 uint8 VEHICLE_TYPE_ROVER = 3
 uint8 VEHICLE_TYPE_AIRSHIP = 4
 
+uint8 FAILSAFE_DEFER_STATE_DISABLED = 0
+uint8 FAILSAFE_DEFER_STATE_ENABLED = 1
+uint8 FAILSAFE_DEFER_STATE_WOULD_FAILSAFE = 2 # Failsafes deferred, but would trigger a failsafe
+
 bool failsafe # true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...)
 bool failsafe_and_user_took_over # true if system is in failsafe state but the user took over control
+uint8 failsafe_defer_state # one of FAILSAFE_DEFER_STATE_*
 
 # Link loss
 bool gcs_connection_lost              # datalink to GCS lost