From b22362eec7e30412de3b6a436de004564b4a6ea6 Mon Sep 17 00:00:00 2001 From: Tomas Baca Date: Tue, 7 Nov 2023 14:15:00 +0100 Subject: [PATCH] more doxygenating --- include/mrs_uav_hw_api/api.h | 136 +++++++++++++++++++++-- include/mrs_uav_hw_api/common_handlers.h | 25 ++++- include/mrs_uav_hw_api/publishers.h | 91 ++++++++++++--- 3 files changed, 221 insertions(+), 31 deletions(-) diff --git a/include/mrs_uav_hw_api/api.h b/include/mrs_uav_hw_api/api.h index a1b9f92..c3e8128 100644 --- a/include/mrs_uav_hw_api/api.h +++ b/include/mrs_uav_hw_api/api.h @@ -27,38 +27,154 @@ namespace mrs_uav_hw_api { +/** + * @brief Interface for the HW API Plugin. + */ class MrsUavHwApi { public: - virtual ~MrsUavHwApi() = 0; - + /** + * @brief The initialization method. This will be called only once at the beginning of the lifetime of the plugin (after initialize()). + * + * @param parent_nh Node handle of the HW API plugin manager. Use this to create subscibers, publisher, etc. + * @param common_handlers Structure pre-filled with useful variables, methods and objects that the plugin can use. + */ virtual void initialize(const ros::NodeHandle& parent_nh, std::shared_ptr common_handlers) = 0; - virtual mrs_msgs::HwApiStatus getStatus() = 0; + /** + * @brief Method for acquiring the HW API plugin's status. This method will be called repeatedly to obtain the status, which will be publisher by the plugin + * manager. + * + * @return The status ROS message. + */ + virtual mrs_msgs::HwApiStatus getStatus() = 0; + + /** + * @brief Method for obtaining the capabilities of the HW API Plugin. This method will be called repeatedly (after initialize()). + * + * @return The capabilities ROS message. + */ virtual mrs_msgs::HwApiCapabilities getCapabilities() = 0; - // | --------------------- topic callbacks -------------------- | - + // -------------------------------------------------------------- + // | topic callbacks | + // -------------------------------------------------------------- + + /** + * @brief Callback for the incoming ActuatorCmd control reference. + * + * @param msg incoming ROS message + * + * @return success (true if processed). + */ virtual bool callbackActuatorCmd(const mrs_msgs::HwApiActuatorCmd::ConstPtr msg) = 0; + + /** + * @brief Callback for the incoming ControlGroupCmd control reference. + * + * @param msg incoming ROS message + * + * @return success (true if processed). + */ virtual bool callbackControlGroupCmd(const mrs_msgs::HwApiControlGroupCmd::ConstPtr msg) = 0; + + /** + * @brief Callback for the incoming AttitudeRate+ThrottleCmd control reference. + * + * @param msg incoming ROS message + * + * @return success (true if processed). + */ virtual bool callbackAttitudeRateCmd(const mrs_msgs::HwApiAttitudeRateCmd::ConstPtr msg) = 0; + + /** + * @brief Callback for the incoming Attitude+ThrottleCmd control reference. + * + * @param msg incoming ROS message + * + * @return success (true if processed). + */ virtual bool callbackAttitudeCmd(const mrs_msgs::HwApiAttitudeCmd::ConstPtr msg) = 0; + + /** + * @brief Callback for the incoming Acceleration+HdgRateCmd control reference. + * + * @param msg incoming ROS message + * + * @return success (true if processed). + */ virtual bool callbackAccelerationHdgRateCmd(const mrs_msgs::HwApiAccelerationHdgRateCmd::ConstPtr msg) = 0; + + /** + * @brief Callback for the incoming Acceleration+HdgCmd control reference. + * + * @param msg incoming ROS message + * + * @return success (true if processed). + */ virtual bool callbackAccelerationHdgCmd(const mrs_msgs::HwApiAccelerationHdgCmd::ConstPtr msg) = 0; + + /** + * @brief Callback for the incoming Velocity+HdgRateCmd control reference. + * + * @param msg incoming ROS message + * + * @return success (true if processed). + */ virtual bool callbackVelocityHdgRateCmd(const mrs_msgs::HwApiVelocityHdgRateCmd::ConstPtr msg) = 0; + + /** + * @brief Callback for the incoming Velocity+HdgCmd control reference. + * + * @param msg incoming ROS message + * + * @return success (true if processed). + */ virtual bool callbackVelocityHdgCmd(const mrs_msgs::HwApiVelocityHdgCmd::ConstPtr msg) = 0; + + /** + * @brief Callback for the incoming Velocity+PositionCmd control reference. + * + * @param msg incoming ROS message + * + * @return success (true if processed). + */ virtual bool callbackPositionCmd(const mrs_msgs::HwApiPositionCmd::ConstPtr msg) = 0; - virtual void callbackTrackerCmd(const mrs_msgs::TrackerCommand::ConstPtr msg) = 0; - // | -------------------- service callbacks ------------------- | + /** + * @brief Callback for the incoming TrackerCommand. The tracker command is provided by the reference generator of the MRS UAV System and it can be used as a + * source for feed forward control action. + * + * @param msg incoming ROS message + */ + virtual void callbackTrackerCmd(const mrs_msgs::TrackerCommand::ConstPtr msg) = 0; + // -------------------------------------------------------------- + // | service callbacks | + // -------------------------------------------------------------- + + /** + * @brief Callback for a service call for arming/disarming the flight controller. + * + * @param request Arm/Disarm. + * + * @return tuple(succes, message) + */ virtual std::tuple callbackArming(const bool& request) = 0; + /** + * @brief Callback for a service call for switching the flight controller to the "offboard" mode. When in offboard mode, the flight controller is using the + * provided reference commands for control. + * + * @return tuple(succes, message) + */ virtual std::tuple callbackOffboard(void) = 0; -}; -// A pure virtual destructor requires a function body. -MrsUavHwApi::~MrsUavHwApi(){}; + /** + * @brief Destructor + */ + virtual ~MrsUavHwApi() = default; +}; } // namespace mrs_uav_hw_api diff --git a/include/mrs_uav_hw_api/common_handlers.h b/include/mrs_uav_hw_api/common_handlers.h index c18333d..5afd33a 100644 --- a/include/mrs_uav_hw_api/common_handlers.h +++ b/include/mrs_uav_hw_api/common_handlers.h @@ -14,19 +14,34 @@ typedef std::function getBodyFrameName_t; typedef std::function getWorldFrameName_t; /** - * @brief A tructure with methods and variables provided to the HW API Plugin. + * @brief A structure with methods and variables provided to the HW API Plugin. */ struct CommonHandlers_t { /** - * @brief A + * @brief A structure with publishers that the plugin should use to provide data to the MRS UAV System. */ - Publishers_t publishers; + Publishers_t publishers; + /** + * @brief The mrs_lib's TF2 transformer wrapper. + */ std::shared_ptr transformer; - getUavName_t getUavName; - getBodyFrameName_t getBodyFrameName; + /** + * @brief Method for obtaining the current "UAV_NAME" (the one set from the $UAV_NAME env variable). + */ + getUavName_t getUavName; + + /** + * @brief Method for obtaining the body-fixed frame_id (as it is called by the MRS UAV System). This frame_id should be used when publishing data in the + * body-fixed frame, e.g., the IMU data. + */ + getBodyFrameName_t getBodyFrameName; + + /** + * @brief Method for obtaining the world-frame for marking the flight-controller's data, e.g., the position of the UAV in the flight-controllers frame. + */ getWorldFrameName_t getWorldFrameName; }; diff --git a/include/mrs_uav_hw_api/publishers.h b/include/mrs_uav_hw_api/publishers.h index 83e21f2..512fe24 100644 --- a/include/mrs_uav_hw_api/publishers.h +++ b/include/mrs_uav_hw_api/publishers.h @@ -53,35 +53,94 @@ typedef std::function publish */ struct Publishers_t { - publishIMU_t publishIMU; - publishGNSS_t publishGNSS; + /** + * @brief Publisher for the raw IMU data. + */ + publishIMU_t publishIMU; + + /** + * @brief Publisher for the main GNSS data. + */ + publishGNSS_t publishGNSS; + + /** + * @brief Publisher for the GNSS status message. + */ publishGNSSStatus_t publishGNSSStatus; - publishRTK_t publishRTK; /** - * @brief The distance sensor is expected to be pointing down towards the ground. + * @brief Publisher for the RTK GNSS data. + */ + publishRTK_t publishRTK; + + /** + * @brief Publisher for the distance sensor readings. The distance sensor is expected to be pointing down towards the ground. */ publishDistanceSensor_t publishDistanceSensor; /** - * @brief Altitude of AMSL (Above Mean Seal Level). + * @brief Altitude of the UAV in AMSL (Above Mean Seal Level). + */ + publishAltitude_t publishAltitude; + + /** + * @brief Publisher for the estimated magnetometer heading. */ - publishAltitude_t publishAltitude; publishMagnetometerHeading_t publishMagnetometerHeading; - publishMagneticField_t publishMagneticField; - publishStatus_t publishStatus; - publishRcChannels_t publishRcChannels; - publishBatteryState_t publishBatteryState; - publishPosition_t publishPosition; - publishOrientation_t publishOrientation; /** - * @brief The UAV Velocity expressed in the UAV Body frame. + * @brief Publisher for the RAW magnetometer readings. + */ + publishMagneticField_t publishMagneticField; + + /** + * @brief Publisher for the HW API Plugin status. + */ + publishStatus_t publishStatus; + + /** + * @brief Publisher for the RC Channels received by the flight controller. + */ + publishRcChannels_t publishRcChannels; + + /** + * @brief Publisher for the UAV battery state. + */ + publishBatteryState_t publishBatteryState; + + /** + * @brief Publisher for the UAV position as estimated by the flight controller. The position is supposed to be expressed in the "world frame" provided by the + * common handlers. + */ + publishPosition_t publishPosition; + + /** + * @brief Publisher for the UAV orientation. The orientation is supposed to be expressed in the "world frame" provided by the common handlers. + */ + publishOrientation_t publishOrientation; + + /** + * @brief Publisher for the UAV velocity expressed in the body-fixed frame, provided by the common handlers. + */ + publishVelocity_t publishVelocity; + + /** + * @brief Publisher for the intrinsic angular velocity. This can be already pre-filtered. The angular velocity is supposed to be expressed in the "body-fixed" + * frame provided in the common handlers. */ - publishVelocity_t publishVelocity; publishAngularVelocity_t publishAngularVelocity; - publishOdometry_t publishOdometry; - publishOdometry_t publishGroundTruth; + + /** + * @brief Publisher for a general "odometry" message that can be used for vizualizing the state of the UAV, as produced by the flight controller. The MRS UAV + * System will use this when running the "passthrough_ state estimator. + */ + publishOdometry_t publishOdometry; + + /** + * @brief Publisher for ground truth odometry which can be provided by, e.g., a simulator or a MoCap system. The MRS UAV System will use this when running the + * "ground_truth" state estimator. + */ + publishOdometry_t publishGroundTruth; }; } // namespace mrs_uav_hw_api