Refactor/msg default values by MarlonRiv · Pull Request #5 · fly4future/mrs_robot_diagnostics

MarlonRiv · 2026-04-09T14:05:37Z

No description provided.

… values in msg definition NaN's are not supported yet as default values, there is a bug in ROS rosidl_generator for NaN values, so replaced with -1

Copilot

Pull request overview

This PR refactors StateMonitor diagnostics message construction to rely more on message default initialization, and extends published diagnostics with additional inputs (GNSS status, constraint/gain manager diagnostics).

Changes:

Add subscriptions/remappings for GNSS status, constraint manager diagnostics, and gain manager diagnostics, and include them in ControlInfo/SystemHealthInfo outputs.
Remove several custom “init_*” message initializers and adjust parsing functions to construct messages directly.
Enable expected_rate for PX4 in state_monitor_config.yaml.

Reviewed changes

Copilot reviewed 4 out of 4 changed files in this pull request and generated 6 comments.

File	Description
`src/state_monitor.cpp`	Adds new inputs to `ControlInfo`/`SystemHealthInfo`, refactors message initialization, updates main timer processing order
`include/mrs_robot_diagnostics/state_monitor.h`	Adds new includes/members and updates function signatures for the new diagnostics inputs
`launch/state_monitor.launch.py`	Adds/remaps new topics for GNSS status + constraint/gain diagnostics and reorders remappings/parameters
`config/state_monitor_config.yaml`	Sets `expected_rate` for the PX4 GenericSensorHandler configuration

Comments suppressed due to low confidence (1)

src/state_monitor.cpp:541

parse_general_robot_info() no longer copies data from the incoming BatteryState message (voltage/percentage/etc.). As a result, GeneralRobotInfo.battery_state will stay default-initialized and the published diagnostics will never reflect the actual battery readings. Please restore the battery field assignments (with the existing nullptr validity check) or otherwise populate msg.battery_state from battery_state.

mrs_msgs::msg::GeneralRobotInfo StateMonitor::parse_general_robot_info(sensor_msgs::msg::BatteryState::ConstSharedPtr battery_state) {
  mrs_msgs::msg::GeneralRobotInfo msg;
  msg.stamp            = clock_->now();
  msg.robot_name       = _robot_name_;
  msg.robot_type       = static_cast<int>(robot_type_);
  msg.robot_ip_address = robot_ip_address_;

  const bool autostart_running = sh_automatic_start_can_takeoff_.getNumPublishers();
  const bool autostart_ready   = sh_automatic_start_can_takeoff_.hasMsg() && sh_automatic_start_can_takeoff_.getMsg()->data;

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

Copilot · 2026-04-09T14:10:34Z

src/state_monitor.cpp

  if (mpc_tracker_diagnostics.hasNewMessage)
    last_collision_avoidance_info_ = parse_collision_avoidance_info(mpc_tracker_diagnostics.message);

  if (hw_api_status.hasNewMessage || uav_status.hasNewMessage || mass_nominal.hasNewMessage | mass_estimate.hasNewMessage)


This condition uses bitwise OR (|) instead of logical OR (||): mass_nominal.hasNewMessage | mass_estimate.hasNewMessage. While it works on bools, it prevents short-circuiting and is easy to misread as a typo. Please change it to || for consistency with the rest of the expression.

Suggested change

if (hw_api_status.hasNewMessage || uav_status.hasNewMessage || mass_nominal.hasNewMessage | mass_estimate.hasNewMessage)

if (hw_api_status.hasNewMessage || uav_status.hasNewMessage || mass_nominal.hasNewMessage || mass_estimate.hasNewMessage)

Copilot · 2026-04-09T14:10:35Z

src/state_monitor.cpp

+  const auto       now                             = clock_->now();
+  const auto       battery_state                   = processIncomingMessage(sh_battery_state_);
+  const auto       control_manager_diagnostics     = processIncomingMessage(sh_control_manager_diagnostics_);
+  const auto       control_manager_heading         = processIncomingMessage(sh_control_manager_heading_);
+  const auto       control_manager_thrust          = processIncomingMessage(sh_control_manager_thrust_);
+  const auto       contstraint_manager_diagnostics = processIncomingMessage(sh_constraint_manager_diagnostics_);
+  const auto       gain_manager_diagnostics        = processIncomingMessage(sh_gain_manager_diagnostics_);
+  const auto       estimation_diagnostics          = processIncomingMessage(sh_estimation_diagnostics_);
+  const auto       hw_api_gnss                     = processIncomingMessage(sh_hw_api_gnss_);
+  const auto       hw_api_gnss_status              = processIncomingMessage(sh_hw_api_gnss_status_);


Typo in the local variable name contstraint_manager_diagnostics (extra 't') makes the code harder to read and propagates into the parse_control_info() call. Please rename it to constraint_manager_diagnostics for clarity.

Suggested change

const auto now = clock_->now();

const auto battery_state = processIncomingMessage(sh_battery_state_);

const auto control_manager_diagnostics = processIncomingMessage(sh_control_manager_diagnostics_);

const auto control_manager_heading = processIncomingMessage(sh_control_manager_heading_);

const auto control_manager_thrust = processIncomingMessage(sh_control_manager_thrust_);

const auto contstraint_manager_diagnostics = processIncomingMessage(sh_constraint_manager_diagnostics_);

const auto gain_manager_diagnostics = processIncomingMessage(sh_gain_manager_diagnostics_);

const auto estimation_diagnostics = processIncomingMessage(sh_estimation_diagnostics_);

const auto hw_api_gnss = processIncomingMessage(sh_hw_api_gnss_);

const auto hw_api_gnss_status = processIncomingMessage(sh_hw_api_gnss_status_);

const auto now = clock_->now();

const auto battery_state = processIncomingMessage(sh_battery_state_);

const auto control_manager_diagnostics = processIncomingMessage(sh_control_manager_diagnostics_);

const auto control_manager_heading = processIncomingMessage(sh_control_manager_heading_);

const auto control_manager_thrust = processIncomingMessage(sh_control_manager_thrust_);

const auto constraint_manager_diagnostics = processIncomingMessage(sh_constraint_manager_diagnostics_);

const auto gain_manager_diagnostics = processIncomingMessage(sh_gain_manager_diagnostics_);

const auto estimation_diagnostics = processIncomingMessage(sh_estimation_diagnostics_);

const auto hw_api_gnss = processIncomingMessage(sh_hw_api_gnss_);

const auto hw_api_gnss_status = processIncomingMessage(sh_hw_api_gnss_status_);

Copilot · 2026-04-09T14:10:35Z

src/state_monitor.cpp

@@ -686,12 +686,22 @@ mrs_msgs::msg::ControlInfo StateMonitor::parse_control_info(mrs_msgs::msg::Contr
  if (is_thrust_valid)
    msg.thrust = thrust->data;



parse_control_info() constructs mrs_msgs::msg::ControlInfo msg; without applying the previous sentinel defaults (e.g., "unknown" controller/tracker, NaN thrust). When any of the inputs are nullptr (including timeouts), the published fields will now default to empty strings / 0.0, which is ambiguous and can be misinterpreted as valid data. Consider reintroducing explicit sentinel initialization for fields that are unknown/unavailable, or ensure the message definitions provide the intended defaults.

Copilot · 2026-04-09T14:10:36Z

include/mrs_robot_diagnostics/state_monitor.h

-  mrs_msgs::msg::ControlInfo parse_control_info(mrs_msgs::msg::ControlManagerDiagnostics::ConstSharedPtr control_manager_diagnostics,
-                                                std_msgs::msg::Float64::ConstSharedPtr                   thrust);
+  mrs_msgs::msg::ControlInfo parse_control_info(mrs_msgs::msg::ControlManagerDiagnostics::ConstSharedPtr    control_manager_diagnostics,
+                                                mrs_msgs::msg::ConstraintManagerDiagnostics::ConstSharedPtr contstraint_manager_diagnostics,


Parameter name typo in the parse_control_info() declaration: contstraint_manager_diagnostics (extra 't'). Even though it doesn't affect the signature, it reduces readability and makes grepping harder. Please rename it to constraint_manager_diagnostics to match the type and the .cpp definition.

Suggested change

mrs_msgs::msg::ConstraintManagerDiagnostics::ConstSharedPtr contstraint_manager_diagnostics,

mrs_msgs::msg::ConstraintManagerDiagnostics::ConstSharedPtr constraint_manager_diagnostics,

Copilot · 2026-04-09T14:10:36Z

include/mrs_robot_diagnostics/state_monitor.h

  /** @brief Build SystemHealthInfo from UAV status, GNSS, magnetometer, RC RSSI, and WiFi. */
-  mrs_msgs::msg::SystemHealthInfo parse_system_health_info(mrs_msgs::msg::UavStatus::ConstSharedPtr        uav_status,
-                                                           sensor_msgs::msg::NavSatFix::ConstSharedPtr     gnss,
+  mrs_msgs::msg::SystemHealthInfo parse_system_health_info(mrs_msgs::msg::UavStatus::ConstSharedPtr    uav_status,
+                                                           sensor_msgs::msg::NavSatFix::ConstSharedPtr gnss, mrs_msgs::msg::GpsInfo::ConstSharedPtr gnss_status,
                                                           sensor_msgs::msg::MagneticField::ConstSharedPtr magnetic_field,
                                                           mrs_msgs::msg::HwApiRcRssi::ConstSharedPtr      rc_rssi);


The docstring says parse_system_health_info builds SystemHealthInfo from "UAV status, GNSS, magnetometer, RC RSSI, and WiFi", but the function now also takes gnss_status (mrs_msgs::msg::GpsInfo). Please update the comment to reflect the new input so the header stays accurate for readers.

Copilot · 2026-04-09T14:10:36Z

src/state_monitor.cpp

 mrs_msgs::msg::SystemHealthInfo StateMonitor::parse_system_health_info(mrs_msgs::msg::UavStatus::ConstSharedPtr        uav_status,
                                                                       sensor_msgs::msg::NavSatFix::ConstSharedPtr     gnss,
+                                                                       mrs_msgs::msg::GpsInfo::ConstSharedPtr          gnss_status,
                                                                       sensor_msgs::msg::MagneticField::ConstSharedPtr magnetic_field,
                                                                       mrs_msgs::msg::HwApiRcRssi::ConstSharedPtr      rc_rssi) {
-  mrs_msgs::msg::SystemHealthInfo msg = init_system_health_info();
+  mrs_msgs::msg::SystemHealthInfo msg;

  const bool is_uav_status_valid     = uav_status != nullptr;
  const bool is_gnss_valid           = gnss != nullptr;
+  const bool is_gnss_status_valid    = gnss_status != nullptr;
  const bool is_magnetic_field_valid = magnetic_field != nullptr;



parse_system_health_info() now default-constructs SystemHealthInfo instead of applying the previous NaN/-1 sentinel initialization. When uav_status/gnss/magnetic_field are nullptr (including timeouts), fields like cpu_load, rates, gnss_uncertainty, etc. will remain at their default (typically 0), which is ambiguous and can look like valid data. Consider restoring explicit sentinel defaults for "unknown/unavailable" fields, or confirm that the message definition provides appropriate defaults.

MarlonRiv added 3 commits April 8, 2026 17:46

feat: Added gain and constraints to control info

f609464

feat: Added gnss_status for the gps fix and satellites

a1a33c6

refactor: removed factory methods for messages, replaced with default…

4b8425e

… values in msg definition NaN's are not supported yet as default values, there is a bug in ROS rosidl_generator for NaN values, so replaced with -1

MarlonRiv requested a review from Copilot April 9, 2026 14:05

Copilot started reviewing on behalf of MarlonRiv April 9, 2026 14:06 View session

Copilot AI reviewed Apr 9, 2026

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Refactor/msg default values#5

Refactor/msg default values#5
MarlonRiv wants to merge 3 commits intoros2from
refactor/msg-default-values

MarlonRiv commented Apr 9, 2026

Uh oh!

Copilot AI left a comment

Uh oh!

Copilot AI Apr 9, 2026

Uh oh!

Copilot AI Apr 9, 2026

Uh oh!

Copilot AI Apr 9, 2026

Uh oh!

Copilot AI Apr 9, 2026

Uh oh!

Copilot AI Apr 9, 2026

Uh oh!

Copilot AI Apr 9, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

	if (hw_api_status.hasNewMessage \|\| uav_status.hasNewMessage \|\| mass_nominal.hasNewMessage \| mass_estimate.hasNewMessage)
	if (hw_api_status.hasNewMessage \|\| uav_status.hasNewMessage \|\| mass_nominal.hasNewMessage \|\| mass_estimate.hasNewMessage)

		@@ -686,12 +686,22 @@ mrs_msgs::msg::ControlInfo StateMonitor::parse_control_info(mrs_msgs::msg::Contr
		if (is_thrust_valid)
		msg.thrust = thrust->data;

	mrs_msgs::msg::ConstraintManagerDiagnostics::ConstSharedPtr contstraint_manager_diagnostics,
	mrs_msgs::msg::ConstraintManagerDiagnostics::ConstSharedPtr constraint_manager_diagnostics,

Conversation

MarlonRiv commented Apr 9, 2026

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Copilot AI Apr 9, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 9, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 9, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 9, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 9, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 9, 2026

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants