Reliability & Setpoint types

px4_ros2_cpp/include/px4_ros2/common/setpoint_base.hpp

@@ -8,7 +8,8 @@
 #include <cstdint>
 #include <functional>
 #include <memory>
-#include <px4_msgs/msg/vehicle_control_mode.hpp>
+#include <px4_msgs/msg/setpoint_config.hpp>
+#include <px4_msgs/msg/setpoint_config_reply.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include "context.hpp"
@@ -19,30 +20,7 @@ namespace px4_ros2 {
 class SetpointBase : public std::enable_shared_from_this<SetpointBase> {
  public:
   using ShouldActivateCB = std::function<void()>;
-
-  struct Configuration {
-    void fillControlMode(px4_msgs::msg::VehicleControlMode& control_mode)
-    {
-      control_mode.flag_control_rates_enabled = rates_enabled;
-      control_mode.flag_control_attitude_enabled = attitude_enabled;
-      control_mode.flag_control_acceleration_enabled = acceleration_enabled;
-      control_mode.flag_control_velocity_enabled = velocity_enabled;
-      control_mode.flag_control_position_enabled = position_enabled;
-      control_mode.flag_control_altitude_enabled = altitude_enabled;
-      control_mode.flag_control_allocation_enabled = control_allocation_enabled;
-      control_mode.flag_control_climb_rate_enabled = climb_rate_enabled;
-    }
-
-    bool control_allocation_enabled{true};
-    bool rates_enabled{true};
-    bool attitude_enabled{true};
-    bool altitude_enabled{true};
-    bool acceleration_enabled{true};
-    bool velocity_enabled{true};
-    bool position_enabled{true};
-    bool local_position_is_optional{false};
-    bool climb_rate_enabled{false};
-  };
+  using SetpointType = decltype(px4_msgs::msg::SetpointConfig::type);
 
   explicit SetpointBase(Context& context) { context.addSetpointType(this); }
 
@@ -58,7 +36,21 @@ class SetpointBase : public std::enable_shared_from_this<SetpointBase> {
     return {};
   }
 
-  virtual Configuration getConfiguration() = 0;
+  /**
+   * Returns one of px4_msgs::msg::SetpointType::TYPE_*
+   */
+  virtual SetpointType getSetpointType() = 0;
+
+  /**
+   * Allows a setpoint class to clear an optional requirement. This is for setpoint types that
+   * support multiple variations, for example some that require local position and others that do
+   * not.
+   *
+   * @param setpoint_config_reply input and output config
+   */
+  virtual void clearOptionalRequirements(px4_msgs::msg::SetpointConfigReply& setpoint_config_reply)
+  {
+  }
 
   virtual float desiredUpdateRateHz() { return 50.f; }
 

px4_ros2_cpp/include/px4_ros2/components/message_compatibility_check.hpp

@@ -15,7 +15,6 @@ using namespace std::chrono_literals;  // NOLINT
   {"fmu/in/actuator_servos"}, \
   {"fmu/in/arming_check_reply"}, \
   {"fmu/in/aux_global_position"}, \
-  {"fmu/in/config_control_setpoints", "VehicleControlMode"}, \
   {"fmu/in/config_overrides_request", "ConfigOverrides"}, \
   {"fmu/in/fixed_wing_lateral_setpoint"}, \
   {"fmu/in/fixed_wing_longitudinal_setpoint"}, \
@@ -30,6 +29,7 @@ using namespace std::chrono_literals;  // NOLINT
   {"fmu/in/rover_speed_setpoint"}, \
   {"fmu/in/rover_steering_setpoint"}, \
   {"fmu/in/rover_throttle_setpoint"}, \
+  {"fmu/in/setpoint_config"}, \
   {"fmu/in/trajectory_setpoint"}, \
   {"fmu/in/unregister_ext_component"}, \
   {"fmu/in/vehicle_attitude_setpoint"}, \
@@ -40,10 +40,12 @@ using namespace std::chrono_literals;  // NOLINT
   {"fmu/out/airspeed_validated"}, \
   {"fmu/out/arming_check_request"}, \
   {"fmu/out/battery_status"}, \
+  {"fmu/out/config_overrides_confirm", "ConfigOverrides"}, \
   {"fmu/out/home_position"}, \
   {"fmu/out/manual_control_setpoint"}, \
   {"fmu/out/mode_completed"}, \
   {"fmu/out/register_ext_component_reply"}, \
+  {"fmu/out/setpoint_config_reply"}, \
   {"fmu/out/vehicle_attitude"}, \
   {"fmu/out/vehicle_angular_velocity"}, \
   {"fmu/out/vehicle_command_ack"}, \

px4_ros2_cpp/include/px4_ros2/components/mode.hpp

@@ -7,7 +7,8 @@
 
 #include <cstdint>
 #include <px4_msgs/msg/mode_completed.hpp>
-#include <px4_msgs/msg/vehicle_control_mode.hpp>
+#include <px4_msgs/msg/setpoint_config.hpp>
+#include <px4_msgs/msg/setpoint_config_reply.hpp>
 #include <px4_msgs/msg/vehicle_status.hpp>
 #include <px4_ros2/common/context.hpp>
 #include <px4_ros2/common/setpoint_base.hpp>
@@ -179,6 +180,7 @@ class ModeBase : public Context {
 
  protected:
   void setSkipMessageCompatibilityCheck() { _skip_message_compatibility_check = true; }
+  void setSkipSetpointCheck() { _skip_setpoint_check = true; }
   void overrideRegistration(const std::shared_ptr<Registration>& registration);
 
   void disableWatchdogTimer() { _health_and_arming_checks.disableWatchdogTimer(); }
@@ -203,21 +205,22 @@ class ModeBase : public Context {
 
   void updateSetpointUpdateTimer();
 
-  void updateModeRequirementsFromSetpoints();
+  void checkSetpointCompatibilityAndRequirements();
   void setSetpointUpdateRateFromSetpointTypes();
-  void publishSetpointConfig(SetpointBase& setpoint);
-  void activateSetpointType(SetpointBase& setpoint);
+  void activateSetpointType(const std::shared_ptr<SetpointBase>& setpoint);
   void deactivateAllSetpointTypes();
 
   std::shared_ptr<Registration> _registration;
 
   const Settings _settings;
   bool _skip_message_compatibility_check{false};
+  bool _skip_setpoint_check{false};  ///< Skip setpoint checks on startup. Only for unit tests.
 
   HealthAndArmingChecks _health_and_arming_checks;
 
   rclcpp::Publisher<px4_msgs::msg::ModeCompleted>::SharedPtr _mode_completed_pub;
-  rclcpp::Publisher<px4_msgs::msg::VehicleControlMode>::SharedPtr _config_control_setpoints_pub;
+  rclcpp::Publisher<px4_msgs::msg::SetpointConfig>::SharedPtr _setpoint_config_pub;
+  SharedSubscriptionCallbackInstance _setpoint_config_reply_sub_cb;
 
   SharedSubscriptionCallbackInstance _vehicle_status_sub_cb;
 
@@ -234,6 +237,7 @@ class ModeBase : public Context {
   std::vector<std::shared_ptr<SetpointBase>> _setpoint_types;
   std::vector<SetpointBase*>
       _new_setpoint_types;  ///< This stores new setpoints during initialization, until registration
+  std::shared_ptr<SetpointBase> _current_activating_setpoint;  ///< Setpoint waiting for a reply
 };
 
 /** @}*/

px4_ros2_cpp/include/px4_ros2/components/overrides.hpp

@@ -8,6 +8,8 @@
 #include <px4_msgs/msg/config_overrides.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include "shared_subscription.hpp"
+
 namespace px4_ros2 {
 
 class ModeBase;
@@ -34,6 +36,9 @@ class ConfigOverrides {
   rclcpp::Publisher<px4_msgs::msg::ConfigOverrides>::SharedPtr _config_overrides_pub;
   bool _is_setup{false};
   bool _require_update_after_setup{false};
+
+  rclcpp::TimerBase::SharedPtr _confirm_timer;
+  SharedSubscriptionCallbackInstance _config_overrides_confirm_sub;
 };
 
 }  // namespace px4_ros2

px4_ros2_cpp/include/px4_ros2/control/setpoint_types/direct_actuators.hpp

@@ -27,7 +27,11 @@ class DirectActuatorsSetpointType : public SetpointBase {
 
   ~DirectActuatorsSetpointType() override = default;
 
-  Configuration getConfiguration() override;
+  SetpointType getSetpointType() override
+  {
+    return px4_msgs::msg::SetpointConfig::TYPE_DIRECT_ACTUATORS;
+  }
+
   float desiredUpdateRateHz() override { return 200.f; }
 
   /**

px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/attitude.hpp

@@ -23,7 +23,8 @@ class AttitudeSetpointType : public SetpointBase {
 
   ~AttitudeSetpointType() override = default;
 
-  Configuration getConfiguration() override;
+  SetpointType getSetpointType() override { return px4_msgs::msg::SetpointConfig::TYPE_ATTITUDE; }
+
   float desiredUpdateRateHz() override { return 100.f; }
 
   void update(const Eigen::Quaternionf& attitude_setpoint,

px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rates.hpp

@@ -23,7 +23,8 @@ class RatesSetpointType : public SetpointBase {
 
   ~RatesSetpointType() override = default;
 
-  Configuration getConfiguration() override;
+  SetpointType getSetpointType() override { return px4_msgs::msg::SetpointConfig::TYPE_RATES; }
+
   float desiredUpdateRateHz() override { return 200.f; }
 
   void update(const Eigen::Vector3f& rate_setpoints_frd_rad,

px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/position.hpp

@@ -23,7 +23,11 @@ class RoverPositionSetpointType : public SetpointBase {
 
   ~RoverPositionSetpointType() override = default;
 
-  Configuration getConfiguration() override;
+  SetpointType getSetpointType() override
+  {
+    return px4_msgs::msg::SetpointConfig::TYPE_ROVER_POSITION;
+  }
+
   float desiredUpdateRateHz() override { return 30.f; }
 
   /**

px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/speed_attitude.hpp

@@ -23,7 +23,11 @@ class RoverSpeedAttitudeSetpointType : public SetpointBase {
 
   ~RoverSpeedAttitudeSetpointType() override = default;
 
-  Configuration getConfiguration() override;
+  SetpointType getSetpointType() override
+  {
+    return px4_msgs::msg::SetpointConfig::TYPE_ROVER_SPEED_ATTITUDE;
+  }
+
   float desiredUpdateRateHz() override { return 30.f; }
 
   /**

px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/speed_rate.hpp

@@ -23,7 +23,11 @@ class RoverSpeedRateSetpointType : public SetpointBase {
 
   ~RoverSpeedRateSetpointType() override = default;
 
-  Configuration getConfiguration() override;
+  SetpointType getSetpointType() override
+  {
+    return px4_msgs::msg::SetpointConfig::TYPE_ROVER_SPEED_RATE;
+  }
+
   float desiredUpdateRateHz() override { return 30.f; }
 
   /**

px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/speed_steering.hpp

@@ -23,7 +23,11 @@ class RoverSpeedSteeringSetpointType : public SetpointBase {
 
   ~RoverSpeedSteeringSetpointType() override = default;
 
-  Configuration getConfiguration() override;
+  SetpointType getSetpointType() override
+  {
+    return px4_msgs::msg::SetpointConfig::TYPE_ROVER_SPEED_STEERING;
+  }
+
   float desiredUpdateRateHz() override { return 30.f; }
 
   /**

px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/throttle_attitude.hpp

@@ -23,7 +23,11 @@ class RoverThrottleAttitudeSetpointType : public SetpointBase {
 
   ~RoverThrottleAttitudeSetpointType() override = default;
 
-  Configuration getConfiguration() override;
+  SetpointType getSetpointType() override
+  {
+    return px4_msgs::msg::SetpointConfig::TYPE_ROVER_THROTTLE_ATTITUDE;
+  }
+
   float desiredUpdateRateHz() override { return 30.f; }
 
   /**

px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/throttle_rate.hpp

@@ -23,7 +23,11 @@ class RoverThrottleRateSetpointType : public SetpointBase {
 
   ~RoverThrottleRateSetpointType() override = default;
 
-  Configuration getConfiguration() override;
+  SetpointType getSetpointType() override
+  {
+    return px4_msgs::msg::SetpointConfig::TYPE_ROVER_THROTTLE_RATE;
+  }
+
   float desiredUpdateRateHz() override { return 30.f; }
 
   /**

px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/throttle_steering.hpp

@@ -23,7 +23,11 @@ class RoverThrottleSteeringSetpointType : public SetpointBase {
 
   ~RoverThrottleSteeringSetpointType() override = default;
 
-  Configuration getConfiguration() override;
+  SetpointType getSetpointType() override
+  {
+    return px4_msgs::msg::SetpointConfig::TYPE_ROVER_THROTTLE_STEERING;
+  }
+
   float desiredUpdateRateHz() override { return 30.f; }
 
   /**

px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/trajectory.hpp

@@ -32,7 +32,10 @@ class TrajectorySetpointType : public SetpointBase {
 
   ~TrajectorySetpointType() override = default;
 
-  Configuration getConfiguration() override;
+  SetpointType getSetpointType() override { return px4_msgs::msg::SetpointConfig::TYPE_TRAJECTORY; }
+
+  void clearOptionalRequirements(
+      px4_msgs::msg::SetpointConfigReply& setpoint_config_reply) override;
 
   void update(const Eigen::Vector3f& velocity_ned_m_s,
               const std::optional<Eigen::Vector3f>& acceleration_ned_m_s2 = {},

px4_ros2_cpp/include/px4_ros2/control/setpoint_types/fixedwing/lateral_longitudinal.hpp

@@ -37,7 +37,13 @@ class FwLateralLongitudinalSetpointType : public SetpointBase {
 
   ~FwLateralLongitudinalSetpointType() override = default;
 
-  Configuration getConfiguration() override;
+  SetpointType getSetpointType() override
+  {
+    return px4_msgs::msg::SetpointConfig::TYPE_FIXEDWING_LATERAL_LONGITUDINAL;
+  }
+
+  void clearOptionalRequirements(
+      px4_msgs::msg::SetpointConfigReply& setpoint_config_reply) override;
 
   /**
    * @brief Update the setpoint with full flexibility by passing a FwLateralLongitudinalSetpoint
@@ -103,6 +109,8 @@ class FwLateralLongitudinalSetpointType : public SetpointBase {
   float desiredUpdateRateHz() override { return 30.f; }
 
  private:
+  void publishConfigurationIfNeeded();
+
   rclcpp::Node& _node;
   bool _local_position_is_optional;
   rclcpp::Publisher<px4_msgs::msg::FixedWingLateralSetpoint>::SharedPtr _fw_lateral_sp_pub;
@@ -113,6 +121,11 @@ class FwLateralLongitudinalSetpointType : public SetpointBase {
       _lateral_control_configuration_pub;
   rclcpp::Publisher<px4_msgs::msg::LongitudinalControlConfiguration>::SharedPtr
       _longitudinal_control_configuration_pub;
+
+  rclcpp::Time _last_config_published_time{};
+  std::optional<px4_msgs::msg::LateralControlConfiguration> _current_lateral_configuration;
+  std::optional<px4_msgs::msg::LongitudinalControlConfiguration>
+      _current_longitudinal_configuration;
 };
 
 struct FwLateralLongitudinalSetpoint {

px4_ros2_cpp/include/px4_ros2/control/setpoint_types/multicopter/goto.hpp

@@ -25,7 +25,10 @@ class MulticopterGotoSetpointType : public SetpointBase {
 
   ~MulticopterGotoSetpointType() override = default;
 
-  Configuration getConfiguration() override;
+  SetpointType getSetpointType() override
+  {
+    return px4_msgs::msg::SetpointConfig::TYPE_MULTICOPTER_GOTO;
+  }
 
   /**
    * @brief Go-to setpoint update

px4_ros2_cpp/include/px4_ros2/mission/mission_executor.hpp

@@ -167,6 +167,11 @@ class MissionExecutor {
       ModeBase::disableWatchdogTimer();
     }
 
+    void setSkipSetpointCheck()  // NOLINT we just want to change the methods visibility
+    {
+      ModeBase::setSkipSetpointCheck();
+    }
+
    private:
     MissionExecutor& _mission_executor;
   };