[DO_NOT_MERGE_YET] rotors_hil_interface: take advantage of MAVROS hil plugin

rotors_gazebo/launch/fixed_wing_hil.launch

@@ -34,9 +34,11 @@
       <param name="gps_topic" value="gps" />
       <param name="imu_topic" value="imu" />
       <param name="odometry_topic" value="ground_truth/odometry" />
-      <param name="hil_controls_sub_topic" value="/mavros/hil_controls/hil_controls" />
+      <param name="hil_controls_sub_topic" value="/mavros/hil/controls" />
       <param name="actuators_pub_topic" value="gazebo/command/motor_speed" />
-      <param name="mavlink_pub_topic" value="/mavlink/to" />
+      <param name="hil_gps_pub_topic" value="/mavros/hil/gps" />
+      <param name="hil_sensor_pub_topic" value="/mavros/hil/sensor" />
+      <param name="hil_state_pub_topic" value="/mavros/hil/state" />
       <param name="sensor_level_hil" value="true" />
       <param name="hil_gps_frequency" value="5.0" />
       <param name="hil_frequency" value="100.0" />
@@ -48,4 +50,3 @@
 
   <node name="hil_plugin_gui" pkg="rqt_rotors" type="hil_plugin" clear_params="true" output="screen" />
 </launch>
-

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'}

rotors_hil_interface/include/rotors_hil_interface/hil_interface.h

@@ -20,7 +20,9 @@
 #include <mav_msgs/Actuators.h>
 #include <mav_msgs/default_topics.h>
 #include <mavros_msgs/HilControls.h>
-#include <mavros_msgs/mavlink_convert.h>
+#include <mavros_msgs/HilGPS.h>
+#include <mavros_msgs/HilSensor.h>
+#include <mavros_msgs/HilStateQuaternion.h>
 
 #include <rotors_hil_interface/hil_listeners.h>
 
@@ -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<uint64_t>(rostime.nsec * 1e-3) +
-          static_cast<uint64_t>(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<mavros_msgs::Mavlink> 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<mavros_msgs::Mavlink> 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<mavros_msgs::Mavlink> 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_

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<HilInterface> 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<HilInterface> hil_interface_;
 };
 }
 
-#endif // ROTORS_HIL_INTERFACE_NODE_H_
+#endif	// ROTORS_HIL_INTERFACE_NODE_H_