diff --git a/rotors_gazebo/launch/fixed_wing_hil.launch b/rotors_gazebo/launch/fixed_wing_hil.launch index 3d40d5c6a..ea30d1e16 100644 --- a/rotors_gazebo/launch/fixed_wing_hil.launch +++ b/rotors_gazebo/launch/fixed_wing_hil.launch @@ -34,9 +34,11 @@ - + - + + + @@ -48,4 +50,3 @@ - diff --git a/rotors_hil.rosinstall b/rotors_hil.rosinstall index 2871322d5..0560d41a5 100644 --- a/rotors_hil.rosinstall +++ b/rotors_hil.rosinstall @@ -1,4 +1,4 @@ - git: {local-name: mav_comm, uri: 'https://github.com/ethz-asl/mav_comm.git'} - git: {local-name: mavlink, uri: 'https://github.com/mavlink/mavlink-gbp-release.git', version: 'upstream'} -- git: {local-name: mavros, uri: 'https://github.com/ethz-asl/mavros.git'} +- git: {local-name: mavros, uri: 'https://github.com/mavlink/mavros.git'} - git: {local-name: rotors_simulator, uri: 'https://github.com/ethz-asl/rotors_simulator.git'} diff --git a/rotors_hil_interface/include/rotors_hil_interface/hil_interface.h b/rotors_hil_interface/include/rotors_hil_interface/hil_interface.h index f6fb6b23f..0f61a126a 100644 --- a/rotors_hil_interface/include/rotors_hil_interface/hil_interface.h +++ b/rotors_hil_interface/include/rotors_hil_interface/hil_interface.h @@ -20,7 +20,9 @@ #include #include #include -#include +#include +#include +#include #include @@ -32,103 +34,87 @@ static constexpr int kAllFieldsUpdated = 4095; static constexpr double kDefaultGpsFrequency = 5.0; static const std::string kDefaultPressureSubTopic = "air_pressure"; -/// \brief Convert ros::Time into single value in microseconds. -/// \param[in] rostime Time, in ROS format, to be converted. -/// \return Time in microseconds. -inline u_int64_t RosTimeToMicroseconds(const ros::Time& rostime) { - return (static_cast(rostime.nsec * 1e-3) + - static_cast(rostime.sec * 1e6)); -} class HilInterface { - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW - /// \brief Destructor - virtual ~HilInterface() {}; + /// \brief Destructor + virtual ~HilInterface() {}; - /// \brief Gather data collected from ROS messages into MAVLINK messages. - /// \return Vector of MAVLINK messages (in MAVROS format) to be publised. - std::vector virtual CollectData() = 0; + /// \brief Gather data collected from ROS messages into MAVROS HilGPS message. + /// \return HilSensor message to be publised. + virtual mavros_msgs::HilGPS CollectGPSData() {}; - protected: - /// ROS node handle. - ros::NodeHandle nh_; + /// \brief Gather data collected from ROS messages into MAVROS HilSensor message. + /// \return MAVROS HilSensor message to be publised. + virtual mavros_msgs::HilSensor CollectSensorData() {}; - /// ROS air speed subscriber. - ros::Subscriber air_speed_sub_; + /// \brief Gather data collected from ROS messages into MAVROS HilSensor message. + /// \return MAVROS HilStateQuaternion message to be publised. + virtual mavros_msgs::HilStateQuaternion CollectStateData() {}; - /// ROS GPS subscriber. - ros::Subscriber gps_sub_; +protected: + /// ROS node handle. + ros::NodeHandle nh_; - /// ROS ground speed subscriber. - ros::Subscriber ground_speed_sub_; + /// ROS air speed subscriber. + ros::Subscriber air_speed_sub_; - /// ROS IMU subscriber. - ros::Subscriber imu_sub_; + /// ROS GPS subscriber. + ros::Subscriber gps_sub_; - /// ROS magnetometer subscriber. - ros::Subscriber mag_sub_; + /// ROS ground speed subscriber. + ros::Subscriber ground_speed_sub_; - /// ROS air pressure subscriber. - ros::Subscriber pressure_sub_; + /// ROS IMU subscriber. + ros::Subscriber imu_sub_; - /// Rotation, in quaternion form, from body into sensor (NED) frame. - Eigen::Quaterniond q_S_B_; + /// ROS magnetometer subscriber. + ros::Subscriber mag_sub_; - /// Rotation, in matrix form, from body into sensor (NED) frame. - Eigen::Matrix3f R_S_B_; + /// ROS air pressure subscriber. + ros::Subscriber pressure_sub_; - /// Object for storing the latest data. - HilData hil_data_; + /// Object for storing the latest data. + HilData hil_data_; - /// Object with callbacks for receiving data. - HilListeners hil_listeners_; + /// Object with callbacks for receiving data. + HilListeners hil_listeners_; - /// Mutex lock for thread safety of reading hil data. - boost::mutex mtx_; + /// Mutex lock for thread safety of reading hil data. + boost::mutex mtx_; }; class HilSensorLevelInterface : public HilInterface { - public: - /// \brief Constructor - /// \param[in] q_S_B Quaternion rotation from body frame to NED frame. - HilSensorLevelInterface(const Eigen::Quaterniond& q_S_B); +public: + /// \brief Constructor + HilSensorLevelInterface(); - /// \brief Destructor - virtual ~HilSensorLevelInterface(); + /// \brief Destructor + virtual ~HilSensorLevelInterface(); - std::vector CollectData(); + mavros_msgs::HilGPS CollectGPSData(); + mavros_msgs::HilSensor CollectSensorData(); - private: - /// MAVLINK HIL_GPS message. - mavlink_hil_gps_t hil_gps_msg_; +private: + /// Interval between outgoing HIL_GPS messages. + uint64_t gps_interval_nsec_; - /// MAVLINK HIL_SENSOR message. - mavlink_hil_sensor_t hil_sensor_msg_; - - /// Interval between outgoing HIL_GPS messages. - uint64_t gps_interval_nsec_; - - /// Nanosecond portion of the last HIL_GPS message timestamp. - uint64_t last_gps_pub_time_nsec_; + /// Nanosecond portion of the last HIL_GPS message timestamp. + uint64_t last_gps_pub_time_nsec_; }; class HilStateLevelInterface : public HilInterface { - public: - /// \brief Constructor - /// \param[in] q_S_B Quaternion rotation from body frame to NED frame. - HilStateLevelInterface(const Eigen::Quaterniond &q_S_B); - - /// \brief Destructor - virtual ~HilStateLevelInterface(); +public: + /// \brief Constructor + HilStateLevelInterface(); - std::vector CollectData(); + /// \brief Destructor + virtual ~HilStateLevelInterface(); - private: - /// MAVLINK HIL_STATE_QUATERNION message. - mavlink_hil_state_quaternion_t hil_state_qtrn_msg_; + mavros_msgs::HilStateQuaternion CollectStateData(); }; -} +} // namespace rotors_hil -#endif // ROTORS_HIL_INTERFACE_H_ +#endif // ROTORS_HIL_INTERFACE_H_ diff --git a/rotors_hil_interface/include/rotors_hil_interface/hil_interface_node.h b/rotors_hil_interface/include/rotors_hil_interface/hil_interface_node.h index ec9a19361..a58ee4e5c 100644 --- a/rotors_hil_interface/include/rotors_hil_interface/hil_interface_node.h +++ b/rotors_hil_interface/include/rotors_hil_interface/hil_interface_node.h @@ -25,43 +25,51 @@ namespace rotors_hil { // Default values static constexpr bool kDefaultSensorLevelHil = true; static constexpr double kDefaultHilFrequency = 100.0; -static constexpr double kDefaultBodyToSensorsRoll = M_PI; -static constexpr double kDefaultBodyToSensorsPitch = 0.0; -static constexpr double kDefaultBodyToSensorsYaw = 0.0; -static const std::string kDefaultMavlinkPubTopic = "mavlink/to"; -static const std::string kDefaultHilControlsSubTopic = "mavros/hil_controls/hil_controls"; +static const std::string kDefaultHilGPSPubTopic = "mavros/hil/gps"; +static const std::string kDefaultHilSensorPubTopic = "mavros/hil/imu_ned"; +static const std::string kDefaultHilStatePubTopic = "mavros/hil/state"; +static const std::string kDefaultHilControlsSubTopic = "mavros/hil/controls"; class HilInterfaceNode { - public: - HilInterfaceNode(); - virtual ~HilInterfaceNode(); +public: + HilInterfaceNode(); + virtual ~HilInterfaceNode(); - /// \brief Main execution loop. - void MainTask(); + /// \brief Main execution loop. + void MainTask(); - /// \brief Callback for handling HilControls messages. - /// \param[in] hil_controls_msg A HilControls message. - void HilControlsCallback(const mavros_msgs::HilControlsConstPtr& hil_controls_msg); + /// \brief Callback for handling HilControls messages. + /// \param[in] hil_controls_msg A HilControls message. + void HilControlsCallback(const mavros_msgs::HilControlsConstPtr& hil_controls_msg); - private: - /// ROS node handle. - ros::NodeHandle nh_; +private: + /// ROS node handle. + ros::NodeHandle nh_; - /// ROS publisher for sending actuator commands. - ros::Publisher actuators_pub_; + /// \brief Choose the interface level + bool sensor_level_hil; - /// ROS publisher for sending MAVLINK messages. - ros::Publisher mavlink_pub_; + /// ROS publisher for sending actuator commands. + ros::Publisher actuators_pub_; - /// ROS subscriber for handling HilControls messages. - ros::Subscriber hil_controls_sub_; + /// ROS publisher for sending MAVROS HilGPS messages. + ros::Publisher hil_gps_pub_; - /// Object for spinning. - ros::Rate rate_; + /// ROS publisher for sending MAVROS HilSensor messages. + ros::Publisher hil_sensor_pub_; - /// Pointer to the HIL interface object. - std::unique_ptr hil_interface_; + /// ROS publisher for sending MAVROS HilStateQuaternion messages. + ros::Publisher hil_state_pub_; + + /// ROS subscriber for handling HilControls messages. + ros::Subscriber hil_controls_sub_; + + /// Object for spinning. + ros::Rate rate_; + + /// Pointer to the HIL interface object. + std::shared_ptr hil_interface_; }; } -#endif // ROTORS_HIL_INTERFACE_NODE_H_ +#endif // ROTORS_HIL_INTERFACE_NODE_H_ diff --git a/rotors_hil_interface/include/rotors_hil_interface/hil_listeners.h b/rotors_hil_interface/include/rotors_hil_interface/hil_listeners.h index 087a7dcaa..53fa4d18a 100644 --- a/rotors_hil_interface/include/rotors_hil_interface/hil_listeners.h +++ b/rotors_hil_interface/include/rotors_hil_interface/hil_listeners.h @@ -30,7 +30,7 @@ namespace rotors_hil { // Constants static constexpr float kAirDensity_kg_per_m3 = 1.18; static constexpr float kGravityMagnitude_m_per_s2 = 9.8068; -static constexpr float kStandardPressure_MBar = 1013.25; +static constexpr float kStandardPressure_Pascal = 10.1325; static constexpr float kTemperature_C = 15.0; static constexpr int kFixNone = 0; static constexpr int kFix3D = 3; @@ -40,186 +40,169 @@ static constexpr int kSatellitesVisible = 4; static constexpr int kUnknown = 65535; // Conversions -static constexpr float kDegreesToHil = 1e7; static constexpr float kFeetToMeters = 0.3048; -static constexpr float kMetersToCm = 100.0; -static constexpr float kMetersToMm = 1000.0; -static constexpr float kPascalToMillibar = 0.01; static constexpr float kPressureToAltExp = 0.190284; static constexpr float kPressureToAltMult = 145366.45; static constexpr float kSecToNsec = 1e9; -static constexpr float kTeslaToGauss = 10000.0; struct HilData { - HilData() : - temperature_degC(kTemperature_C), - eph_cm(kHDOP), - epv_cm(kVDOP), - cog_1e2deg(kUnknown), - ind_airspeed_1e2m_per_s(0), - satellites_visible(kSatellitesVisible) {} - - Eigen::Quaterniond att; // Attitude quaternion - Eigen::Vector3f acc_m_per_s2; // Linear acceleration (m/s^2) - Eigen::Vector3f gyro_rad_per_s; // Angular rate in body frame (rad / sec) - Eigen::Vector3f mag_G; // Magnetic field (Gauss) - Eigen::Vector3i gps_vel_cm_per_s; // GPS velocity in cm/s in earth-fixed NED frame - float pressure_abs_mBar; // Absolute pressure in millibar - float pressure_diff_mBar; // Differential pressure (airspeed) in millibar - float pressure_alt; // Altitude calculated from pressure - float temperature_degC; // Temperature in degrees celsius - uint32_t lat_1e7deg; // Latitude (WGS84), in degrees * 1E7 - uint32_t lon_1e7deg; // Longitude (WGS84), in degrees * 1E7 - uint32_t alt_mm; // Altitude (AMSL, not WGS84), in meters * 1000 (positive for up) - uint16_t eph_cm; // GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - uint16_t epv_cm; // GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: 65535 - uint16_t vel_1e2m_per_s; // GPS ground speed (m/s * 100). If unknown, set to: 65535 - uint16_t cog_1e2deg; // Course over ground (NOT heading, but direction of movement) in degrees * 100. If unknown, set to: 65535 - uint16_t ind_airspeed_1e2m_per_s; // Indicated airspeed, expressed as m/s * 100 - uint16_t true_airspeed_1e2m_per_s; // True airspeed, expressed as m/s * 100*/ - uint8_t fix_type; // < 0-1: no fix, 2: 2D fix, 3: 3D fix - uint8_t satellites_visible; // Number of satellites visible. If unknown, set to 255 + HilData() : + temperature(kTemperature_C), + eph(kHDOP), + epv(kVDOP), + cog(kUnknown), + ind_airspeed(0), + satellites_visible(kSatellitesVisible) {} + + Eigen::Quaterniond att; // Attitude quaternion + Eigen::Vector3f acc; // Linear acceleration (m/s^2) + Eigen::Vector3f gyro; // Angular rate in body frame (rad / sec) + Eigen::Vector3f mag; // Magnetic field (Gauss) + Eigen::Vector3i gps_vel; // GPS velocity in cm/s in earth-fixed NED frame + float pressure_abs; // Absolute pressure in millibar + float pressure_diff; // Differential pressure (airspeed) in millibar + float pressure_alt; // Altitude calculated from pressure + float temperature; // Temperature in degrees celsius + uint32_t lat; // Latitude (WGS84), in degrees * 1E7 + uint32_t lon; // Longitude (WGS84), in degrees * 1E7 + uint32_t alt; // Altitude (AMSL, not WGS84), in meters * 1000 (positive for up) + uint16_t eph; // GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + uint16_t epv; // GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: 65535 + uint16_t vel; // GPS ground speed (m/s * 100). If unknown, set to: 65535 + uint16_t cog; // Course over ground (NOT heading, but direction of movement) in degrees * 100. If unknown, set to: 65535 + uint16_t ind_airspeed; // Indicated airspeed, expressed as m/s * 100 + uint16_t true_airspeed; // True airspeed, expressed as m/s * 100*/ + uint8_t fix_type; // < 0-1: no fix, 2: 2D fix, 3: 3D fix + uint8_t satellites_visible; // Number of satellites visible. If unknown, set to 255 }; class HilListeners { - public: - HilListeners() {} - virtual ~HilListeners() {} - - /// \brief Callback for handling Air Speed messages. - /// \param[in] air_speed_msg An Air Speed message. - /// \param[out] hil_data Pointer to latest data collected for HIL publishing. - void AirSpeedCallback(const geometry_msgs::TwistStampedConstPtr& air_speed_msg, - HilData* hil_data) { - boost::mutex::scoped_lock lock(mtx_); - - ROS_ASSERT(hil_data); - - Eigen::Vector3d air_velocity(air_speed_msg->twist.linear.x, - air_speed_msg->twist.linear.y, - air_speed_msg->twist.linear.z); - - double air_speed = air_velocity.norm(); - - // TODO(pvechersky): Simulate indicated air speed. - - // MAVLINK HIL_STATE_QUATERNION message measured airspeed in cm/s. - hil_data->ind_airspeed_1e2m_per_s = air_speed * kMetersToCm; - hil_data->true_airspeed_1e2m_per_s = air_speed * kMetersToCm; - } - - /// \brief Callback for handling GPS messages. - /// \param[in] gps_msg A GPS message. - /// \param[out] hil_data Pointer to latest data collected for HIL publishing. - void GpsCallback(const sensor_msgs::NavSatFixConstPtr& gps_msg, - HilData* hil_data) { - boost::mutex::scoped_lock lock(mtx_); - - ROS_ASSERT(hil_data); - - // MAVLINK HIL_GPS message measures latitude and longitude in degrees * 1e7 - // while altitude is reported in mm. - hil_data->lat_1e7deg = gps_msg->latitude * kDegreesToHil; - hil_data->lon_1e7deg = gps_msg->longitude * kDegreesToHil; - hil_data->alt_mm = gps_msg->altitude * kMetersToMm; - - hil_data->fix_type = - (gps_msg->status.status > sensor_msgs::NavSatStatus::STATUS_NO_FIX) ? - kFix3D : kFixNone; - } - - /// \brief Callback for handling Ground Speed messages. - /// \param[in] ground_speed_msg A ground speed message. - /// \param[out] hil_data Pointer to latest data collected for HIL publishing. - void GroundSpeedCallback(const geometry_msgs::TwistStampedConstPtr &ground_speed_msg, - HilData* hil_data) { - boost::mutex::scoped_lock lock(mtx_); - - ROS_ASSERT(hil_data); - - // MAVLINK HIL_GPS message measures GPS velocity in cm/s - hil_data->gps_vel_cm_per_s = - (Eigen::Vector3f(ground_speed_msg->twist.linear.x, - ground_speed_msg->twist.linear.y, - ground_speed_msg->twist.linear.z) * - kMetersToCm) - .cast(); - - hil_data->vel_1e2m_per_s = hil_data->gps_vel_cm_per_s.norm(); - } - - /// \brief Callback for handling IMU messages. - /// \param[in] imu_msg An IMU message. - /// \param[out] hil_data Pointer to latest data collected for HIL publishing. - void ImuCallback(const sensor_msgs::ImuConstPtr& imu_msg, - HilData* hil_data) { - boost::mutex::scoped_lock lock(mtx_); - - ROS_ASSERT(hil_data); - - hil_data->acc_m_per_s2 = Eigen::Vector3f(imu_msg->linear_acceleration.x, - imu_msg->linear_acceleration.y, - imu_msg->linear_acceleration.z); - - hil_data->att = Eigen::Quaterniond(imu_msg->orientation.w, - imu_msg->orientation.x, - imu_msg->orientation.y, - imu_msg->orientation.z); - - hil_data->gyro_rad_per_s = Eigen::Vector3f(imu_msg->angular_velocity.x, - imu_msg->angular_velocity.y, - imu_msg->angular_velocity.z); - } - - /// \brief Callback for handling Magnetometer messages. - /// \param[in] mag_msg A Magnetometer message. - /// \param[out] hil_data Pointer to latest data collected for HIL publishing. - void MagCallback(const sensor_msgs::MagneticFieldConstPtr &mag_msg, - HilData* hil_data) { - boost::mutex::scoped_lock lock(mtx_); - - ROS_ASSERT(hil_data); - - // ROS magnetic field sensor message is in Tesla, while - // MAVLINK HIL_SENSOR message measures magnetic field in Gauss. - // 1 Tesla = 10000 Gauss - hil_data->mag_G = Eigen::Vector3f(mag_msg->magnetic_field.x, - mag_msg->magnetic_field.y, - mag_msg->magnetic_field.z) * kTeslaToGauss; - } - - /// \brief Callback for handling Air Pressure messages. - /// \param[in] pressure_msg An Air Pressure message. - /// \param[out] hil_data Pointer to latest data collected for HIL publishing. - void PressureCallback(const sensor_msgs::FluidPressureConstPtr &pressure_msg, - HilData* hil_data) { - boost::mutex::scoped_lock lock(mtx_); - - ROS_ASSERT(hil_data); - - // ROS fluid pressure sensor message is in Pascals, while - // MAVLINK HIL_SENSOR message measures fluid pressure in millibar. - // 1 Pascal = 0.01 millibar - float pressure_mbar = pressure_msg->fluid_pressure * kPascalToMillibar; - hil_data->pressure_abs_mBar = pressure_mbar; - - // From the following formula: p_stag - p_static = 0.5 * rho * v^2 - // HIL air speed is in cm/s and is converted to m/s for the purpose of - // computing pressure. - hil_data->pressure_diff_mBar = 0.5 * kAirDensity_kg_per_m3 * hil_data->ind_airspeed_1e2m_per_s * - hil_data->ind_airspeed_1e2m_per_s * kPascalToMillibar / - (kMetersToCm * kMetersToCm); - - hil_data->pressure_alt = - (1 - pow((pressure_mbar / kStandardPressure_MBar), kPressureToAltExp)) * - kPressureToAltMult * kFeetToMeters; - } - - private: - /// Mutex lock for thread safety of writing hil data. - boost::mutex mtx_; +public: + HilListeners() {} + virtual ~HilListeners() {} + + /// \brief Callback for handling Air Speed messages. + /// \param[in] air_speed_msg An Air Speed message. + /// \param[out] hil_data Pointer to latest data collected for HIL publishing. + void AirSpeedCallback(const geometry_msgs::TwistStampedConstPtr& air_speed_msg, + HilData* hil_data) { + boost::mutex::scoped_lock lock(mtx_); + + ROS_ASSERT(hil_data); + + Eigen::Vector3d air_velocity(air_speed_msg->twist.linear.x, + air_speed_msg->twist.linear.y, + air_speed_msg->twist.linear.z); + + double air_speed = air_velocity.norm(); + + // TODO(pvechersky): Simulate indicated air speed. + + // MAVROS HIL_STATE_QUATERNION message measured airspeed in m/s. + hil_data->ind_airspeed = air_speed; + hil_data->true_airspeed = air_speed; + } + + /// \brief Callback for handling GPS messages. + /// \param[in] gps_msg A GPS message. + /// \param[out] hil_data Pointer to latest data collected for HIL publishing. + void GpsCallback(const sensor_msgs::NavSatFixConstPtr& gps_msg, + HilData* hil_data) { + boost::mutex::scoped_lock lock(mtx_); + + ROS_ASSERT(hil_data); + + // MAVROS HIL_GPS message measures latitude and longitude in degrees + // while altitude is reported in m. + hil_data->lat = gps_msg->latitude; + hil_data->lon = gps_msg->longitude; + hil_data->alt = gps_msg->altitude; + + hil_data->fix_type = + (gps_msg->status.status > sensor_msgs::NavSatStatus::STATUS_NO_FIX) ? + kFix3D : kFixNone; + } + + /// \brief Callback for handling Ground Speed messages. + /// \param[in] ground_speed_msg A ground speed message. + /// \param[out] hil_data Pointer to latest data collected for HIL publishing. + void GroundSpeedCallback(const geometry_msgs::TwistStampedConstPtr &ground_speed_msg, + HilData* hil_data) { + boost::mutex::scoped_lock lock(mtx_); + + ROS_ASSERT(hil_data); + + // MAVROS HIL_GPS message measures GPS velocity in m/s + hil_data->gps_vel = + (Eigen::Vector3f(ground_speed_msg->twist.linear.x, + ground_speed_msg->twist.linear.y, + ground_speed_msg->twist.linear.z)).cast(); + + hil_data->vel = hil_data->gps_vel.norm(); + } + + /// \brief Callback for handling IMU messages. + /// \param[in] imu_msg An IMU message. + /// \param[out] hil_data Pointer to latest data collected for HIL publishing. + void ImuCallback(const sensor_msgs::ImuConstPtr& imu_msg, + HilData* hil_data) { + boost::mutex::scoped_lock lock(mtx_); + + ROS_ASSERT(hil_data); + + hil_data->acc = Eigen::Vector3f(imu_msg->linear_acceleration.x, + imu_msg->linear_acceleration.y, + imu_msg->linear_acceleration.z); + + hil_data->att = Eigen::Quaterniond(imu_msg->orientation.w, + imu_msg->orientation.x, + imu_msg->orientation.y, + imu_msg->orientation.z); + + hil_data->gyro = Eigen::Vector3f(imu_msg->angular_velocity.x, + imu_msg->angular_velocity.y, + imu_msg->angular_velocity.z); + } + + /// \brief Callback for handling Magnetometer messages. + /// \param[in] mag_msg A Magnetometer message. + /// \param[out] hil_data Pointer to latest data collected for HIL publishing. + void MagCallback(const sensor_msgs::MagneticFieldConstPtr &mag_msg, + HilData* hil_data) { + boost::mutex::scoped_lock lock(mtx_); + + ROS_ASSERT(hil_data); + + // MAVROS HIL_SENSOR message measures magnetic field in Tesla. + hil_data->mag = Eigen::Vector3f(mag_msg->magnetic_field.x, + mag_msg->magnetic_field.y, + mag_msg->magnetic_field.z); + } + + /// \brief Callback for handling Air Pressure messages. + /// \param[in] pressure_msg An Air Pressure message. + /// \param[out] hil_data Pointer to latest data collected for HIL publishing. + void PressureCallback(const sensor_msgs::FluidPressureConstPtr &pressure_msg, + HilData* hil_data) { + boost::mutex::scoped_lock lock(mtx_); + + ROS_ASSERT(hil_data); + + // MAVROS HIL_SENSOR message measures fluid pressure in Pascal. + hil_data->pressure_abs = pressure_msg->fluid_pressure; + + // From the following formula: p_stag - p_static = 0.5 * rho * v^2 + hil_data->pressure_diff = 0.5 * kAirDensity_kg_per_m3 * hil_data->ind_airspeed * hil_data->ind_airspeed; + + hil_data->pressure_alt = + (1 - pow((pressure_msg->fluid_pressure / kStandardPressure_Pascal), kPressureToAltExp)) * + kPressureToAltMult * kFeetToMeters; + } + +private: + /// Mutex lock for thread safety of writing hil data. + boost::mutex mtx_; }; - } -#endif // ROTORS_HIL_LISTENERS_H_ +#endif // ROTORS_HIL_LISTENERS_H_ diff --git a/rotors_hil_interface/src/hil_interface_node.cpp b/rotors_hil_interface/src/hil_interface_node.cpp index 7c0d5de38..cd0f9a7ee 100644 --- a/rotors_hil_interface/src/hil_interface_node.cpp +++ b/rotors_hil_interface/src/hil_interface_node.cpp @@ -17,91 +17,89 @@ #include "rotors_hil_interface/hil_interface_node.h" namespace rotors_hil { - HilInterfaceNode::HilInterfaceNode() : - rate_(kDefaultHilFrequency) { - ros::NodeHandle pnh("~"); - - bool sensor_level_hil; - double hil_frequency; - double S_B_roll; - double S_B_pitch; - double S_B_yaw; - std::string actuators_pub_topic; - std::string mavlink_pub_topic; - std::string hil_controls_sub_topic; - - pnh.param("sensor_level_hil", sensor_level_hil, kDefaultSensorLevelHil); - pnh.param("hil_frequency", hil_frequency, kDefaultHilFrequency); - pnh.param("body_to_sensor_roll", S_B_roll, kDefaultBodyToSensorsRoll); - pnh.param("body_to_sensor_pitch", S_B_pitch, kDefaultBodyToSensorsPitch); - pnh.param("body_to_sensor_yaw", S_B_yaw, kDefaultBodyToSensorsYaw); - pnh.param("actuators_pub_topic", actuators_pub_topic, std::string(mav_msgs::default_topics::COMMAND_ACTUATORS)); - pnh.param("mavlink_pub_topic", mavlink_pub_topic, kDefaultMavlinkPubTopic); - pnh.param("hil_controls_sub_topic", hil_controls_sub_topic, kDefaultHilControlsSubTopic); - - // Create the quaternion and rotation matrix to rotate data into NED frame. - Eigen::AngleAxisd roll_angle(S_B_roll, Eigen::Vector3d::UnitX()); - Eigen::AngleAxisd pitch_angle(S_B_pitch, Eigen::Vector3d::UnitY()); - Eigen::AngleAxisd yaw_angle(S_B_yaw, Eigen::Vector3d::UnitZ()); - - const Eigen::Quaterniond q_S_B = roll_angle * pitch_angle * yaw_angle; - - if (sensor_level_hil) - hil_interface_ = std::auto_ptr(new HilSensorLevelInterface(q_S_B)); - else - hil_interface_ = std::auto_ptr(new HilStateLevelInterface(q_S_B)); - - rate_ = ros::Rate(hil_frequency); - - actuators_pub_ = nh_.advertise(actuators_pub_topic, 1); - mavlink_pub_ = nh_.advertise(mavlink_pub_topic, 5); - hil_controls_sub_ = nh_.subscribe(hil_controls_sub_topic, 1, - &HilInterfaceNode::HilControlsCallback, this); + rate_(kDefaultHilFrequency) { + ros::NodeHandle pnh("~"); + + double hil_frequency; + std::string actuators_pub_topic; + std::string hil_gps_pub_topic; + std::string hil_sensor_pub_topic; + std::string hil_state_pub_topic; + std::string hil_controls_sub_topic; + + pnh.param("sensor_level_hil", sensor_level_hil, kDefaultSensorLevelHil); + pnh.param("hil_frequency", hil_frequency, kDefaultHilFrequency); + pnh.param("actuators_pub_topic", actuators_pub_topic, std::string(mav_msgs::default_topics::COMMAND_ACTUATORS)); + pnh.param("hil_gps_pub_topic", hil_gps_pub_topic, kDefaultHilGPSPubTopic); + pnh.param("hil_sensor_pub_topic", hil_sensor_pub_topic, kDefaultHilSensorPubTopic); + pnh.param("hil_state_pub_topic", hil_state_pub_topic, kDefaultHilStatePubTopic); + pnh.param("hil_controls_sub_topic", hil_controls_sub_topic, kDefaultHilControlsSubTopic); + + if (sensor_level_hil) + hil_interface_ = std::shared_ptr(new HilSensorLevelInterface()); + else + hil_interface_ = std::shared_ptr(new HilStateLevelInterface()); + + rate_ = ros::Rate(hil_frequency); + + // Publishers + actuators_pub_ = nh_.advertise(actuators_pub_topic, 1); + hil_gps_pub_ = nh_.advertise(hil_gps_pub_topic, 10); + hil_sensor_pub_ = nh_.advertise(hil_sensor_pub_topic, 10); + hil_state_pub_ = nh_.advertise(hil_state_pub_topic, 10); + + // Subscribers + hil_controls_sub_ = nh_.subscribe(hil_controls_sub_topic, 1, + &HilInterfaceNode::HilControlsCallback, this); } -HilInterfaceNode::~HilInterfaceNode() { -} +HilInterfaceNode::~HilInterfaceNode() {} void HilInterfaceNode::MainTask() { - while (ros::ok()) { - std::vector hil_msgs = hil_interface_->CollectData(); - - while (!hil_msgs.empty()) { - mavlink_pub_.publish(hil_msgs.back()); - hil_msgs.pop_back(); - } - - ros::spinOnce(); - rate_.sleep(); - } + while (ros::ok()) { + if (sensor_level_hil) { + mavros_msgs::HilGPS hil_gps_msg = hil_interface_->CollectGPSData(); + mavros_msgs::HilSensor hil_sensor_msg = hil_interface_->CollectSensorData(); + + hil_gps_pub_.publish(hil_gps_msg); + hil_sensor_pub_.publish(hil_sensor_msg); + } + else { + mavros_msgs::HilStateQuaternion hil_state_msg = hil_interface_->CollectStateData(); + + hil_state_pub_.publish(hil_state_msg); + } + + ros::spinOnce(); + rate_.sleep(); + } } void HilInterfaceNode::HilControlsCallback(const mavros_msgs::HilControlsConstPtr& hil_controls_msg) { - mav_msgs::Actuators act_msg; - - ros::Time current_time = ros::Time::now(); + mav_msgs::Actuators act_msg; - act_msg.normalized.push_back(hil_controls_msg->roll_ailerons); - act_msg.normalized.push_back(hil_controls_msg->pitch_elevator); - act_msg.normalized.push_back(hil_controls_msg->yaw_rudder); - act_msg.normalized.push_back(hil_controls_msg->aux1); - act_msg.normalized.push_back(hil_controls_msg->aux2); - act_msg.normalized.push_back(hil_controls_msg->throttle); + ros::Time current_time = ros::Time::now(); - act_msg.header.stamp.sec = current_time.sec; - act_msg.header.stamp.nsec = current_time.nsec; + act_msg.normalized.push_back(hil_controls_msg->roll_ailerons); + act_msg.normalized.push_back(hil_controls_msg->pitch_elevator); + act_msg.normalized.push_back(hil_controls_msg->yaw_rudder); + act_msg.normalized.push_back(hil_controls_msg->aux1); + act_msg.normalized.push_back(hil_controls_msg->aux2); + act_msg.normalized.push_back(hil_controls_msg->throttle); - actuators_pub_.publish(act_msg); -} + act_msg.header.stamp.sec = current_time.sec; + act_msg.header.stamp.nsec = current_time.nsec; + actuators_pub_.publish(act_msg); } +} // namespace rotors_hil int main(int argc, char** argv) { - ros::init(argc, argv, "rotors_hil_interface_node"); - rotors_hil::HilInterfaceNode hil_interface_node; + ros::init(argc, argv, "rotors_hil_interface_node"); + rotors_hil::HilInterfaceNode hil_interface_node; - hil_interface_node.MainTask(); + hil_interface_node.MainTask(); - return 0; + return 0; } diff --git a/rotors_hil_interface/src/hil_sensor_level_interface.cpp b/rotors_hil_interface/src/hil_sensor_level_interface.cpp index 1a182c4d3..b0ad55576 100644 --- a/rotors_hil_interface/src/hil_sensor_level_interface.cpp +++ b/rotors_hil_interface/src/hil_sensor_level_interface.cpp @@ -17,144 +17,113 @@ #include "rotors_hil_interface/hil_interface.h" namespace rotors_hil { - -HilSensorLevelInterface::HilSensorLevelInterface(const Eigen::Quaterniond& q_S_B) { - ros::NodeHandle pnh("~"); - - // Retrieve the necessary parameters. - double gps_freq; - std::string air_speed_sub_topic; - std::string gps_sub_topic; - std::string ground_speed_sub_topic; - std::string imu_sub_topic; - std::string mag_sub_topic; - std::string pressure_sub_topic; - - pnh.param("gps_frequency", gps_freq, kDefaultGpsFrequency); - pnh.param("air_speed_topic", air_speed_sub_topic, std::string(mav_msgs::default_topics::AIR_SPEED)); - pnh.param("gps_topic", gps_sub_topic, std::string(mav_msgs::default_topics::GPS)); - pnh.param("ground_speed_topic", ground_speed_sub_topic, std::string(mav_msgs::default_topics::GROUND_SPEED)); - pnh.param("imu_topic", imu_sub_topic, std::string(mav_msgs::default_topics::IMU)); - pnh.param("mag_topic", mag_sub_topic, std::string(mav_msgs::default_topics::MAGNETIC_FIELD)); - pnh.param("pressure_topic", pressure_sub_topic, kDefaultPressureSubTopic); - - // Compute the desired interval between published GPS messages. - gps_interval_nsec_ = static_cast(kSecToNsec / gps_freq); - - // Compute the rotation matrix to rotate data into NED frame - q_S_B_ = q_S_B; - R_S_B_ = q_S_B_.matrix().cast(); - - // Initialize the subscribers. - air_speed_sub_ = - nh_.subscribe( - air_speed_sub_topic, 1, boost::bind( - &HilListeners::AirSpeedCallback, &hil_listeners_, _1, &hil_data_)); - - gps_sub_ = - nh_.subscribe( - gps_sub_topic, 1, boost::bind( - &HilListeners::GpsCallback, &hil_listeners_, _1, &hil_data_)); - - ground_speed_sub_ = - nh_.subscribe( - ground_speed_sub_topic, 1, boost::bind( - &HilListeners::GroundSpeedCallback, &hil_listeners_, _1, &hil_data_)); - - imu_sub_ = - nh_.subscribe( - imu_sub_topic, 1, boost::bind( - &HilListeners::ImuCallback, &hil_listeners_, _1, &hil_data_)); - - mag_sub_ = - nh_.subscribe( - mag_sub_topic, 1, boost::bind( - &HilListeners::MagCallback, &hil_listeners_, _1, &hil_data_)); - - pressure_sub_ = - nh_.subscribe( - pressure_sub_topic, 1, boost::bind( - &HilListeners::PressureCallback, &hil_listeners_, _1, &hil_data_)); -} - -HilSensorLevelInterface::~HilSensorLevelInterface() { +HilSensorLevelInterface::HilSensorLevelInterface() { + ros::NodeHandle pnh("~"); + + // Retrieve the necessary parameters. + double gps_freq; + std::string air_speed_sub_topic; + std::string gps_sub_topic; + std::string ground_speed_sub_topic; + std::string imu_sub_topic; + std::string mag_sub_topic; + std::string pressure_sub_topic; + + pnh.param("gps_frequency", gps_freq, kDefaultGpsFrequency); + pnh.param("air_speed_topic", air_speed_sub_topic, std::string(mav_msgs::default_topics::AIR_SPEED)); + pnh.param("gps_topic", gps_sub_topic, std::string(mav_msgs::default_topics::GPS)); + pnh.param("ground_speed_topic", ground_speed_sub_topic, std::string(mav_msgs::default_topics::GROUND_SPEED)); + pnh.param("imu_topic", imu_sub_topic, std::string(mav_msgs::default_topics::IMU)); + pnh.param("mag_topic", mag_sub_topic, std::string(mav_msgs::default_topics::MAGNETIC_FIELD)); + pnh.param("pressure_topic", pressure_sub_topic, kDefaultPressureSubTopic); + + // Compute the desired interval between published GPS messages. + gps_interval_nsec_ = static_cast(kSecToNsec / gps_freq); + + // Initialize the subscribers. + air_speed_sub_ = + nh_.subscribe( + air_speed_sub_topic, 1, boost::bind( + &HilListeners::AirSpeedCallback, &hil_listeners_, _1, &hil_data_)); + + gps_sub_ = + nh_.subscribe( + gps_sub_topic, 1, boost::bind( + &HilListeners::GpsCallback, &hil_listeners_, _1, &hil_data_)); + + ground_speed_sub_ = + nh_.subscribe( + ground_speed_sub_topic, 1, boost::bind( + &HilListeners::GroundSpeedCallback, &hil_listeners_, _1, &hil_data_)); + + imu_sub_ = + nh_.subscribe( + imu_sub_topic, 1, boost::bind( + &HilListeners::ImuCallback, &hil_listeners_, _1, &hil_data_)); + + mag_sub_ = + nh_.subscribe( + mag_sub_topic, 1, boost::bind( + &HilListeners::MagCallback, &hil_listeners_, _1, &hil_data_)); + + pressure_sub_ = + nh_.subscribe( + pressure_sub_topic, 1, boost::bind( + &HilListeners::PressureCallback, &hil_listeners_, _1, &hil_data_)); } -std::vector HilSensorLevelInterface::CollectData() { - boost::mutex::scoped_lock lock(mtx_); - - ros::Time current_time = ros::Time::now(); - uint64_t time_usec = RosTimeToMicroseconds(current_time); - - mavlink_message_t mmsg; - std::vector hil_msgs; - - // Rotate gyroscope, accelerometer, and magnetometer data into NED frame - Eigen::Vector3f gyro = R_S_B_ * hil_data_.gyro_rad_per_s; - Eigen::Vector3f acc = R_S_B_ * hil_data_.acc_m_per_s2; - Eigen::Vector3f mag = R_S_B_ * hil_data_.mag_G; - - // Check if we need to publish a HIL_GPS message. - if ((current_time.nsec - last_gps_pub_time_nsec_) >= gps_interval_nsec_) { - last_gps_pub_time_nsec_ = current_time.nsec; - - // Rotate ground speed data into NED frame - Eigen::Vector3i gps_vel = (R_S_B_ * hil_data_.gps_vel_cm_per_s.cast()).cast(); - - // Fill in a MAVLINK HIL_GPS message and convert it to MAVROS format. - hil_gps_msg_.time_usec = time_usec; - hil_gps_msg_.fix_type = hil_data_.fix_type; - hil_gps_msg_.lat = hil_data_.lat_1e7deg; - hil_gps_msg_.lon = hil_data_.lon_1e7deg; - hil_gps_msg_.alt = hil_data_.alt_mm; - hil_gps_msg_.eph = hil_data_.eph_cm; - hil_gps_msg_.epv = hil_data_.epv_cm; - hil_gps_msg_.vel = hil_data_.vel_1e2m_per_s; - hil_gps_msg_.vn = gps_vel.x(); - hil_gps_msg_.ve = gps_vel.y(); - hil_gps_msg_.vd = gps_vel.z(); - hil_gps_msg_.cog = hil_data_.cog_1e2deg; - hil_gps_msg_.satellites_visible = hil_data_.satellites_visible; - - mavlink_hil_gps_t* hil_gps_msg_ptr = &hil_gps_msg_; - mavlink_msg_hil_gps_encode(1, 0, &mmsg, hil_gps_msg_ptr); - - mavros_msgs::MavlinkPtr rmsg_hil_gps = boost::make_shared(); - rmsg_hil_gps->header.stamp.sec = current_time.sec; - rmsg_hil_gps->header.stamp.nsec = current_time.nsec; - mavros_msgs::mavlink::convert(mmsg, *rmsg_hil_gps); - - hil_msgs.push_back(*rmsg_hil_gps); - } - - // Fill in a MAVLINK HIL_SENSOR message and convert it to MAVROS format. - hil_sensor_msg_.time_usec = time_usec; - hil_sensor_msg_.xacc = acc.x(); - hil_sensor_msg_.yacc = acc.y(); - hil_sensor_msg_.zacc = acc.z(); - hil_sensor_msg_.xgyro = gyro.x(); - hil_sensor_msg_.ygyro = gyro.y(); - hil_sensor_msg_.zgyro = gyro.z(); - hil_sensor_msg_.xmag = mag.x(); - hil_sensor_msg_.ymag = mag.y(); - hil_sensor_msg_.zmag = mag.z(); - hil_sensor_msg_.abs_pressure = hil_data_.pressure_abs_mBar; - hil_sensor_msg_.diff_pressure = hil_data_.pressure_diff_mBar; - hil_sensor_msg_.pressure_alt = hil_data_.pressure_alt; - hil_sensor_msg_.temperature = hil_data_.temperature_degC; - hil_sensor_msg_.fields_updated = kAllFieldsUpdated; - - mavlink_hil_sensor_t* hil_sensor_msg_ptr = &hil_sensor_msg_; - mavlink_msg_hil_sensor_encode(1, 0, &mmsg, hil_sensor_msg_ptr); - - mavros_msgs::MavlinkPtr rmsg_hil_sensor = boost::make_shared(); - rmsg_hil_sensor->header.stamp.sec = current_time.sec; - rmsg_hil_sensor->header.stamp.nsec = current_time.nsec; - mavros_msgs::mavlink::convert(mmsg, *rmsg_hil_sensor); - - hil_msgs.push_back(*rmsg_hil_sensor); - - return hil_msgs; +HilSensorLevelInterface::~HilSensorLevelInterface() {} + +mavros_msgs::HilGPS HilSensorLevelInterface::CollectGPSData() { + boost::mutex::scoped_lock lock(mtx_); + + mavros_msgs::HilGPSPtr hil_gps_msg = boost::make_shared(); + + // Check if we need to publish a HIL_GPS message. + if ((ros::Time::now().toNSec() - last_gps_pub_time_nsec_) >= gps_interval_nsec_) { + last_gps_pub_time_nsec_ = ros::Time::now().toNSec(); + + // Fill in a MAVROS HIL_GPS message. + hil_gps_msg->header.stamp = ros::Time::now(); + hil_gps_msg->fix_type = hil_data_.fix_type; + hil_gps_msg->geo.latitude = hil_data_.lat; + hil_gps_msg->geo.longitude = hil_data_.lon; + hil_gps_msg->geo.altitude = hil_data_.alt; + hil_gps_msg->eph = hil_data_.eph; + hil_gps_msg->epv = hil_data_.epv; + hil_gps_msg->vel = hil_data_.vel; + hil_gps_msg->vn = hil_data_.gps_vel.x(); + hil_gps_msg->ve = hil_data_.gps_vel.y(); + hil_gps_msg->vd = hil_data_.gps_vel.z(); + hil_gps_msg->cog = hil_data_.cog; + hil_gps_msg->satellites_visible = hil_data_.satellites_visible; + + return *hil_gps_msg; + } } +mavros_msgs::HilSensor HilSensorLevelInterface::CollectSensorData() { + boost::mutex::scoped_lock lock(mtx_); + + mavros_msgs::HilSensorPtr hil_sensor_msg = boost::make_shared(); + + // Fill in a ROS HIL_SENSOR message. + hil_sensor_msg->header.stamp = ros::Time::now(); + hil_sensor_msg->acc.x = hil_data_.acc.x(); + hil_sensor_msg->acc.y = hil_data_.acc.y(); + hil_sensor_msg->acc.z = hil_data_.acc.z(); + hil_sensor_msg->gyro.x = hil_data_.gyro.x(); + hil_sensor_msg->gyro.y = hil_data_.gyro.y(); + hil_sensor_msg->gyro.z = hil_data_.gyro.z(); + hil_sensor_msg->mag.x = hil_data_.mag.x(); + hil_sensor_msg->mag.y = hil_data_.mag.y(); + hil_sensor_msg->mag.z = hil_data_.mag.z(); + hil_sensor_msg->abs_pressure = hil_data_.pressure_abs; + hil_sensor_msg->diff_pressure = hil_data_.pressure_diff; + hil_sensor_msg->pressure_alt = hil_data_.pressure_alt; + hil_sensor_msg->temperature = hil_data_.temperature; + hil_sensor_msg->fields_updated = kAllFieldsUpdated; + + return *hil_sensor_msg; } +} // namespace rotors_hil diff --git a/rotors_hil_interface/src/hil_state_level_interface.cpp b/rotors_hil_interface/src/hil_state_level_interface.cpp index f687dc027..951857e14 100644 --- a/rotors_hil_interface/src/hil_state_level_interface.cpp +++ b/rotors_hil_interface/src/hil_state_level_interface.cpp @@ -17,99 +17,70 @@ #include "rotors_hil_interface/hil_interface.h" namespace rotors_hil { - -HilStateLevelInterface::HilStateLevelInterface(const Eigen::Quaterniond& q_S_B) { - ros::NodeHandle pnh("~"); - - // Retrieve the necessary parameters. - std::string air_speed_sub_topic; - std::string gps_sub_topic; - std::string ground_speed_sub_topic; - std::string imu_sub_topic; - - pnh.param("air_speed_topic", air_speed_sub_topic, std::string(mav_msgs::default_topics::AIR_SPEED)); - pnh.param("gps_topic", gps_sub_topic, std::string(mav_msgs::default_topics::GPS)); - pnh.param("ground_speed_topic", ground_speed_sub_topic, std::string(mav_msgs::default_topics::GROUND_SPEED)); - pnh.param("imu_topic", imu_sub_topic, std::string(mav_msgs::default_topics::IMU)); - - // Compute the rotation matrix to rotate data into NED frame - q_S_B_ = q_S_B; - R_S_B_ = q_S_B_.matrix().cast(); - - // Initialize the subscribers. - air_speed_sub_ = - nh_.subscribe( - air_speed_sub_topic, 1, boost::bind( - &HilListeners::AirSpeedCallback, &hil_listeners_, _1, &hil_data_)); - - gps_sub_ = - nh_.subscribe( - gps_sub_topic, 1, boost::bind( - &HilListeners::GpsCallback, &hil_listeners_, _1, &hil_data_)); - - ground_speed_sub_ = - nh_.subscribe( - ground_speed_sub_topic, 1, boost::bind( - &HilListeners::GroundSpeedCallback, &hil_listeners_, _1, &hil_data_)); - - imu_sub_ = - nh_.subscribe( - imu_sub_topic, 1, boost::bind( - &HilListeners::ImuCallback, &hil_listeners_, _1, &hil_data_)); -} - -HilStateLevelInterface::~HilStateLevelInterface() { -} - -std::vector HilStateLevelInterface::CollectData() { - boost::mutex::scoped_lock lock(mtx_); - - ros::Time current_time = ros::Time::now(); - uint64_t time_usec = RosTimeToMicroseconds(current_time); - - mavlink_message_t mmsg; - std::vector hil_msgs; - - // Rotate the attitude into NED frame - Eigen::Quaterniond att = q_S_B_ * hil_data_.att; - - // Rotate gyroscope, accelerometer, and ground speed data into NED frame - Eigen::Vector3f gyro = R_S_B_ * hil_data_.gyro_rad_per_s; - Eigen::Vector3f acc = R_S_B_ * hil_data_.acc_m_per_s2; - Eigen::Vector3i gps_vel = (R_S_B_ * hil_data_.gps_vel_cm_per_s.cast()).cast(); - - // Fill in a MAVLINK HIL_STATE_QUATERNION message and convert it to MAVROS format. - hil_state_qtrn_msg_.time_usec = time_usec; - hil_state_qtrn_msg_.attitude_quaternion[0] = att.w(); - hil_state_qtrn_msg_.attitude_quaternion[1] = att.x(); - hil_state_qtrn_msg_.attitude_quaternion[2] = att.y(); - hil_state_qtrn_msg_.attitude_quaternion[3] = att.z(); - hil_state_qtrn_msg_.rollspeed = gyro.x(); - hil_state_qtrn_msg_.pitchspeed = gyro.y(); - hil_state_qtrn_msg_.yawspeed = gyro.z(); - hil_state_qtrn_msg_.lat = hil_data_.lat_1e7deg; - hil_state_qtrn_msg_.lon = hil_data_.lon_1e7deg; - hil_state_qtrn_msg_.alt = hil_data_.alt_mm; - hil_state_qtrn_msg_.vx = gps_vel.x(); - hil_state_qtrn_msg_.vy = gps_vel.y(); - hil_state_qtrn_msg_.vz = gps_vel.z(); - hil_state_qtrn_msg_.ind_airspeed = hil_data_.ind_airspeed_1e2m_per_s; - hil_state_qtrn_msg_.true_airspeed = hil_data_.true_airspeed_1e2m_per_s; - hil_state_qtrn_msg_.xacc = acc.x() * kMetersToMm / kGravityMagnitude_m_per_s2; - hil_state_qtrn_msg_.yacc = acc.y() * kMetersToMm / kGravityMagnitude_m_per_s2; - hil_state_qtrn_msg_.zacc = acc.z() * kMetersToMm / kGravityMagnitude_m_per_s2; - - mavlink_hil_state_quaternion_t* hil_state_qtrn_msg_ptr = &hil_state_qtrn_msg_; - mavlink_msg_hil_state_quaternion_encode(1, 0, &mmsg, hil_state_qtrn_msg_ptr); - - mavros_msgs::MavlinkPtr rmsg_hil_state_qtrn = boost::make_shared(); - rmsg_hil_state_qtrn->header.stamp.sec = current_time.sec; - rmsg_hil_state_qtrn->header.stamp.nsec = current_time.nsec; - mavros_msgs::mavlink::convert(mmsg, *rmsg_hil_state_qtrn); - - hil_msgs.push_back(*rmsg_hil_state_qtrn); - - return hil_msgs; +HilStateLevelInterface::HilStateLevelInterface() { + ros::NodeHandle pnh("~"); + + // Retrieve the necessary parameters. + std::string air_speed_sub_topic; + std::string gps_sub_topic; + std::string ground_speed_sub_topic; + std::string imu_sub_topic; + + pnh.param("air_speed_topic", air_speed_sub_topic, std::string(mav_msgs::default_topics::AIR_SPEED)); + pnh.param("gps_topic", gps_sub_topic, std::string(mav_msgs::default_topics::GPS)); + pnh.param("ground_speed_topic", ground_speed_sub_topic, std::string(mav_msgs::default_topics::GROUND_SPEED)); + pnh.param("imu_topic", imu_sub_topic, std::string(mav_msgs::default_topics::IMU)); + + // Initialize the subscribers. + air_speed_sub_ = + nh_.subscribe( + air_speed_sub_topic, 1, boost::bind( + &HilListeners::AirSpeedCallback, &hil_listeners_, _1, &hil_data_)); + + gps_sub_ = + nh_.subscribe( + gps_sub_topic, 1, boost::bind( + &HilListeners::GpsCallback, &hil_listeners_, _1, &hil_data_)); + + ground_speed_sub_ = + nh_.subscribe( + ground_speed_sub_topic, 1, boost::bind( + &HilListeners::GroundSpeedCallback, &hil_listeners_, _1, &hil_data_)); + + imu_sub_ = + nh_.subscribe( + imu_sub_topic, 1, boost::bind( + &HilListeners::ImuCallback, &hil_listeners_, _1, &hil_data_)); } +HilStateLevelInterface::~HilStateLevelInterface() {} + +mavros_msgs::HilStateQuaternion HilStateLevelInterface::CollectStateData() { + boost::mutex::scoped_lock lock(mtx_); + + mavros_msgs::HilStateQuaternionPtr hil_state_msg = boost::make_shared(); + + // Fill in a MAVROS HIL_STATE_QUATERNION message. + hil_state_msg->header.stamp = ros::Time::now(); + hil_state_msg->orientation.w = hil_data_.att.w(); + hil_state_msg->orientation.x = hil_data_.att.x(); + hil_state_msg->orientation.y = hil_data_.att.y(); + hil_state_msg->orientation.z = hil_data_.att.z(); + hil_state_msg->angular_velocity.x = hil_data_.gyro.x(); + hil_state_msg->angular_velocity.y = hil_data_.gyro.y(); + hil_state_msg->angular_velocity.z = hil_data_.gyro.z(); + hil_state_msg->geo.latitude = hil_data_.lat; + hil_state_msg->geo.longitude = hil_data_.lon; + hil_state_msg->geo.altitude = hil_data_.alt; + hil_state_msg->linear_velocity.x = hil_data_.gps_vel.x(); + hil_state_msg->linear_velocity.y = hil_data_.gps_vel.y(); + hil_state_msg->linear_velocity.z = hil_data_.gps_vel.z(); + hil_state_msg->ind_airspeed = hil_data_.ind_airspeed; + hil_state_msg->true_airspeed = hil_data_.true_airspeed; + hil_state_msg->linear_acceleration.x = hil_data_.acc.x() * kGravityMagnitude_m_per_s2; + hil_state_msg->linear_acceleration.y = hil_data_.acc.y() * kGravityMagnitude_m_per_s2; + hil_state_msg->linear_acceleration.z = hil_data_.acc.z() * kGravityMagnitude_m_per_s2; + + return *hil_state_msg; } +} // namespace rotors_hil