diff --git a/CMakeLists.txt b/CMakeLists.txt index d4becb0e07..d25898a962 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,6 +21,7 @@ list(APPEND CMAKE_MODULE_PATH /usr/local/share/cmake/Modules) list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake") option(BUILD_GSTREAMER_PLUGIN "enable gstreamer plugin" "OFF") +option(BUILD_HIL_INTERFACE "build the HIL interface" "OFF") ## System dependencies are found with CMake's conventions find_package(PkgConfig REQUIRED) @@ -31,6 +32,14 @@ find_package(Boost REQUIRED COMPONENTS system thread timer) if (BUILD_GSTREAMER_PLUGIN) find_package(GStreamer) endif() +if (BUILD_HIL_INTERFACE) + find_package(roscpp REQUIRED) + find_package(mavros REQUIRED) + find_package(mavros_msgs REQUIRED) + find_package(geometry_msgs REQUIRED) + find_package(sensor_msgs REQUIRED) + find_package(mav_msgs REQUIRED) +endif() add_subdirectory( external/OpticalFlow OpticalFlow ) set( OpticalFlow_LIBS "OpticalFlow" ) @@ -231,6 +240,22 @@ if (NOT roscpp_FOUND) list(APPEND plugins gazebo_geotagged_images_plugin) endif() +# If BUILD_HIL_INTERFACE set to ON, build gazebo_hil_interface +if (BUILD_HIL_INTERFACE) + include_directories( + include + ${mavros_msgs_INCLUDE_DIRS} + ${geometry_msgs_INCLUDE_DIRS} + ${sensor_msgs_INCLUDE_DIRS} + ${mav_msgs_INCLUDE_DIRS} + ) + add_library(gazebo_hil_interface SHARED src/gazebo_hil_interface.cpp) + list(APPEND plugins gazebo_hil_interface) + target_link_libraries(gazebo_hil_interface ${catkin_LIBRARIES} ${mavros_LIBRARIES}) + add_dependencies(gazebo_hil_interface ${catkin_EXPORTED_TARGETS} ${mavros_EXPORTED_TARGETS} ${mavros_msgs_EXPORTED_TARGETS}) + message(STATUS "adding gazebo_hil_interface to build") +endif() + if (GSTREAMER_FOUND) add_library(gazebo_gst_camera_plugin SHARED src/gazebo_gst_camera_plugin.cpp) set(plugins diff --git a/include/gazebo_hil_interface.h b/include/gazebo_hil_interface.h new file mode 100644 index 0000000000..ddda50df2c --- /dev/null +++ b/include/gazebo_hil_interface.h @@ -0,0 +1,217 @@ +/* + * Copyright 2016 Pavel Vechersky, ASL, ETH Zurich, Switzerland + * Copyright 2017 Nuno Marques, PX4 Pro Dev Team, Lisbon + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef HIL_INTERFACE_NODE_H_ +#define HIL_INTERFACE_NODE_H_ + +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace hil_interface { +// message_filters ApproximateTime policies +using SyncGPSPolicy = message_filters::sync_policies::ApproximateTime + < sensor_msgs::NavSatFix, // GPS + geometry_msgs::TwistStamped >; // Ground Speed +using SyncSensorPolicy = message_filters::sync_policies::ApproximateTime + < geometry_msgs::TwistStamped, // Air Speed + sensor_msgs::Imu, // IMU + sensor_msgs::MagneticField, // Magnetometer + sensor_msgs::FluidPressure >; // Pressure Sensor +using SyncStatePolicy = message_filters::sync_policies::ApproximateTime + < geometry_msgs::TwistStamped, // Air Speed + sensor_msgs::NavSatFix, // GPS + geometry_msgs::TwistStamped, // Ground Speed + sensor_msgs::Imu, // IMU + sensor_msgs::MagneticField >; // Magnetometer + +// message_filters Synchronizers +using SyncGPS = message_filters::Synchronizer; +using SyncSensor = message_filters::Synchronizer; +using SyncState = message_filters::Synchronizer; + +// Default values +static constexpr bool kDefaultSensorLevelHil = true; +static constexpr double kDefaultHilFrequency = 100.0; +static constexpr double kDefaultGpsFrequency = 5.0; +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"; +static const std::string kDefaultPressureSubTopic = "air_pressure"; + +// Constants +static constexpr float kAirDensity_kg_per_m3 = 1.18; +static constexpr float kGravityMagnitude_m_per_s2 = 9.8068; +static constexpr float kStandardPressure_Pascal = 10.1325; +static constexpr float kTemperature_C = 15.0; +static constexpr int kFixNone = 0; +static constexpr int kFix3D = 3; +static constexpr int kHDOP = 100; +static constexpr int kVDOP = 100; +static constexpr int kSatellitesVisible = 4; +static constexpr int kUnknown = 65535; +static constexpr int kAllFieldsUpdated = 4095; + +// Conversions +static constexpr float kFeetToMeters = 0.3048; +static constexpr float kPressureToAltExp = 0.190284; +static constexpr float kPressureToAltMult = 145366.45; +static constexpr float kSecToNsec = 1e9; + +class HilInterfaceNode { +public: + HilInterfaceNode(); + virtual ~HilInterfaceNode(); + + //! @brief Main execution loop. + void MainTask(); + + /** + * brief Sync Callback for handling HIL GPS data and publish MAVROS HilGPS messages. + * param[in] gps_msg A GPS message. + * param[in] ground_speed_msg A ground speed message. + */ + void HilGPSPubCallback(const sensor_msgs::NavSatFixConstPtr& gps_msg, + const geometry_msgs::TwistStampedConstPtr &ground_speed_msg); + + /** + * brief Sync Callback for handling HIL Sensor data and publish MAVROS HilSensor messages. + * param[in] gps_msg A GPS message. + * param[in] ground_speed_msg A ground speed message. + */ + void HilSensorPubCallback(const geometry_msgs::TwistStampedConstPtr& air_speed_msg, + const sensor_msgs::ImuConstPtr& imu_msg, + const sensor_msgs::MagneticFieldConstPtr &mag_msg, + const sensor_msgs::FluidPressureConstPtr &pressure_msg); + + /** + * brief Sync Callback for handling HIL State data and publish MAVROS HilStateQuaternion messages. + * param[in] air_speed_msg An Air Speed message. + * param[in] gps_msg A GPS message. + * param[in] ground_speed_msg A ground speed message. + * param[in] imu_msg An IMU message. + * param[in] mag_msg A Magnetometer message. + */ + void HilStateQuaternionPubCallback(const geometry_msgs::TwistStampedConstPtr& air_speed_msg, + const sensor_msgs::NavSatFixConstPtr& gps_msg, + const geometry_msgs::TwistStampedConstPtr &ground_speed_msg, + const sensor_msgs::ImuConstPtr& imu_msg, + const sensor_msgs::MagneticFieldConstPtr &mag_msg); + + /** + * @brief Callback for handling MAVROS 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_; + + //! Choose the interface level. + bool sensor_level_hil; + + //! Pointer for GPS level data messages syncronous subscribers. + std::unique_ptr sync_gps; + + //! Pointer for Sensor level data messages syncronous subscribers. + std::unique_ptr sync_sensor; + + //! Pointer for State level data messages syncronous subscribers. + std::unique_ptr sync_state; + + //! ROS subscriber for handling TwistStamped Air Speed messages. + message_filters::Subscriber air_speed_sub_; + + //! ROS subscriber for handling NavSatFix GPS messages. + message_filters::Subscriber gps_sub_; + + //! ROS subscriber for handling TwistStamped Ground Speed messages. + message_filters::Subscriber ground_speed_sub_; + + //! ROS subscriber for handling Imu messages. + message_filters::Subscriber imu_sub_; + + //! ROS subscriber for handling MagneticField messages. + message_filters::Subscriber mag_sub_; + + //! ROS subscriber for handling FluidPressure sensor messages. + message_filters::Subscriber pressure_sub_; + + //! ROS subscriber for handling HilControls messages. + ros::Subscriber hil_controls_sub_; + + //! ROS publisher for sending actuator commands. + ros::Publisher actuators_pub_; + + //! ROS publisher for sending MAVROS HilGPS messages. + ros::Publisher hil_gps_pub_; + + //! ROS publisher for sending MAVROS HilSensor messages. + ros::Publisher hil_sensor_pub_; + + //! ROS publisher for sending MAVROS HilStateQuaternion messages. + ros::Publisher hil_state_pub_; + + //! ROS parameters that define the Publisher topics. + std::string actuators_pub_topic; + std::string hil_gps_pub_topic; + std::string hil_sensor_pub_topic; + std::string hil_state_pub_topic; + + //! ROS parameters that define the Subscriber topics. + std::string hil_controls_sub_topic; + 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; + + //! Object for spinning. + ros::Rate rate_; + + //! HilGPS message publish rate. + double gps_freq; + + //! Interval between outgoing HilGPS messages. + uint64_t gps_interval_nsec_; + + //! Nanosecond portion of the last HilGPS message timestamp. + uint64_t last_gps_pub_time_nsec_; +}; +} // namespace hil_interface + +#endif // HIL_INTERFACE_NODE_H_ diff --git a/package.xml b/package.xml index d154a0d6ac..2946b0383b 100644 --- a/package.xml +++ b/package.xml @@ -4,7 +4,7 @@ 1.0.0 The mavlink_sitl_gazebo package - + Lorenz Meier @@ -27,7 +27,7 @@ Lorenz Meier - James Goppert + James Goppert @@ -45,8 +45,20 @@ gazebo_ros gazebo_ros - gazebo_ros + geometry_msgs + mav_msgs + mavros + mavros_msgs + roscpp + sensor_msgs + gazebo_ros + geometry_msgs + mav_msgs + mavros + mavros_msgs + roscpp + sensor_msgs diff --git a/src/gazebo_hil_interface.cpp b/src/gazebo_hil_interface.cpp new file mode 100644 index 0000000000..3e0ca4967d --- /dev/null +++ b/src/gazebo_hil_interface.cpp @@ -0,0 +1,275 @@ +/* + * Copyright 2016 Pavel Vechersky, ASL, ETH Zurich, Switzerland + * Copyright 2017 Nuno Marques, PX4 Pro Dev Team, Lisbon + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "gazebo_hil_interface.h" + +namespace hil_interface { +HilInterfaceNode::HilInterfaceNode() : + sensor_level_hil(kDefaultSensorLevelHil), + rate_(kDefaultHilFrequency), + gps_freq(kDefaultGpsFrequency), + actuators_pub_topic(std::string(mav_msgs::default_topics::COMMAND_ACTUATORS)), + hil_gps_pub_topic(kDefaultHilGPSPubTopic), + hil_sensor_pub_topic(kDefaultHilSensorPubTopic), + hil_state_pub_topic(kDefaultHilStatePubTopic), + hil_controls_sub_topic(kDefaultHilControlsSubTopic), + air_speed_sub_topic(std::string(mav_msgs::default_topics::AIR_SPEED)), + gps_sub_topic(std::string(mav_msgs::default_topics::GPS)), + ground_speed_sub_topic(std::string(mav_msgs::default_topics::GROUND_SPEED)), + imu_sub_topic(std::string(mav_msgs::default_topics::IMU)), + mag_sub_topic(std::string(mav_msgs::default_topics::MAGNETIC_FIELD)), + pressure_sub_topic(kDefaultPressureSubTopic) { + double hil_freq; + + nh_.param("sensor_level_hil", sensor_level_hil, kDefaultSensorLevelHil); + nh_.param("hil_frequency", hil_freq, kDefaultHilFrequency); + nh_.param("gps_frequency", gps_freq, kDefaultGpsFrequency); + nh_.param("actuators_pub_topic", actuators_pub_topic, std::string(mav_msgs::default_topics::COMMAND_ACTUATORS)); + nh_.param("hil_gps_pub_topic", hil_gps_pub_topic, kDefaultHilGPSPubTopic); + nh_.param("hil_sensor_pub_topic", hil_sensor_pub_topic, kDefaultHilSensorPubTopic); + nh_.param("hil_state_pub_topic", hil_state_pub_topic, kDefaultHilStatePubTopic); + nh_.param("hil_controls_sub_topic", hil_controls_sub_topic, kDefaultHilControlsSubTopic); + nh_.param("air_speed_sub_topic", air_speed_sub_topic, std::string(mav_msgs::default_topics::AIR_SPEED)); + nh_.param("gps_sub_topic", gps_sub_topic, std::string(mav_msgs::default_topics::GPS)); + nh_.param("ground_speed_sub_topic", ground_speed_sub_topic, std::string(mav_msgs::default_topics::GROUND_SPEED)); + nh_.param("imu_sub_topic", imu_sub_topic, std::string(mav_msgs::default_topics::IMU)); + nh_.param("mag_sub_topic", mag_sub_topic, std::string(mav_msgs::default_topics::MAGNETIC_FIELD)); + nh_.param("pressure_sub_topic", pressure_sub_topic, kDefaultPressureSubTopic); + + rate_ = ros::Rate(hil_freq); + + // Compute the desired interval between published GPS messages. + gps_interval_nsec_ = static_cast(kSecToNsec / gps_freq); + + // Publishers + actuators_pub_ = nh_.advertise(actuators_pub_topic, 10); + 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); + + // Synced subscribers: + air_speed_sub_.subscribe(nh_, air_speed_sub_topic, 1); + gps_sub_.subscribe(nh_, gps_sub_topic, 1); + ground_speed_sub_.subscribe(nh_, ground_speed_sub_topic, 1); + imu_sub_.subscribe(nh_, imu_sub_topic, 1); + mag_sub_.subscribe(nh_, mag_sub_topic, 1); + pressure_sub_.subscribe(nh_, pressure_sub_topic, 1); +} + +HilInterfaceNode::~HilInterfaceNode() {} + +void HilInterfaceNode::MainTask() { + while (ros::ok()) { + if (sensor_level_hil) { + sync_gps.reset(new SyncGPS(SyncGPSPolicy(10), gps_sub_, ground_speed_sub_)); + sync_gps->registerCallback(boost::bind(&HilInterfaceNode::HilGPSPubCallback, this, _1, _2)); + + sync_sensor.reset(new SyncSensor(SyncSensorPolicy(10), air_speed_sub_, imu_sub_, mag_sub_, pressure_sub_)); + sync_sensor->registerCallback(boost::bind(&HilInterfaceNode::HilSensorPubCallback, this, _1, _2, _3, _4)); + } + else { + sync_state.reset(new SyncState(SyncStatePolicy(10), air_speed_sub_, gps_sub_, ground_speed_sub_, imu_sub_, mag_sub_)); + sync_state->registerCallback(boost::bind(&HilInterfaceNode::HilStateQuaternionPubCallback, this, _1, _2, _3, _4, _5)); + } + + ros::spinOnce(); + rate_.sleep(); + } +} + +void HilInterfaceNode::HilGPSPubCallback(const sensor_msgs::NavSatFixConstPtr& gps_msg, + const geometry_msgs::TwistStampedConstPtr &ground_speed_msg) { + // Handle Ground Speed data. + Eigen::Vector3f gps_vel(Eigen::Vector3f(ground_speed_msg->twist.linear.x, + ground_speed_msg->twist.linear.y, + ground_speed_msg->twist.linear.z)); + + auto 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 HilGPS message. + hil_gps_msg->header.stamp = ros::Time::now(); + hil_gps_msg->fix_type = (gps_msg->status.status > sensor_msgs::NavSatStatus::STATUS_NO_FIX) ? kFix3D : kFixNone; + hil_gps_msg->geo.latitude = gps_msg->latitude; + hil_gps_msg->geo.longitude = gps_msg->longitude; + hil_gps_msg->geo.altitude = gps_msg->altitude; + hil_gps_msg->eph = kHDOP; + hil_gps_msg->epv = kVDOP; + hil_gps_msg->vel = gps_vel.norm(); + 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 = kUnknown; + hil_gps_msg->satellites_visible = kSatellitesVisible; + + // Publish MAVROS HilGPS message. + hil_gps_pub_.publish(hil_gps_msg); + } +} + +void HilInterfaceNode::HilSensorPubCallback(const geometry_msgs::TwistStampedConstPtr& air_speed_msg, + const sensor_msgs::ImuConstPtr& imu_msg, + const sensor_msgs::MagneticFieldConstPtr &mag_msg, + const sensor_msgs::FluidPressureConstPtr &pressure_msg) { + // Handle Air speed data. + Eigen::Vector3d air_velocity(air_speed_msg->twist.linear.x, + air_speed_msg->twist.linear.y, + air_speed_msg->twist.linear.z); + + // Handle IMU data. + Eigen::Vector3f acc(imu_msg->linear_acceleration.x, + imu_msg->linear_acceleration.y, + imu_msg->linear_acceleration.z); + + Eigen::Quaterniond att(imu_msg->orientation.w, + imu_msg->orientation.x, + imu_msg->orientation.y, + imu_msg->orientation.z); + + Eigen::Vector3f gyro(imu_msg->angular_velocity.x, + imu_msg->angular_velocity.y, + imu_msg->angular_velocity.z); + + // Handle Mag data + Eigen::Vector3f mag(mag_msg->magnetic_field.x, + mag_msg->magnetic_field.y, + mag_msg->magnetic_field.z); + + // From the following formula: p_stag - p_static = 0.5 * rho * v^2. + float pressure_diff = 0.5 * kAirDensity_kg_per_m3 * air_velocity.norm() * air_velocity.norm(); + + float pressure_alt = + (1 - pow((pressure_msg->fluid_pressure / kStandardPressure_Pascal), kPressureToAltExp)) * + kPressureToAltMult * kFeetToMeters; + + auto hil_sensor_msg = boost::make_shared(); + + // Fill in a MAVROS HilSensor message. + hil_sensor_msg->header.stamp = ros::Time::now(); + hil_sensor_msg->acc.x = acc.x(); + hil_sensor_msg->acc.y = acc.y(); + hil_sensor_msg->acc.z = acc.z(); + hil_sensor_msg->gyro.x = gyro.x(); + hil_sensor_msg->gyro.y = gyro.y(); + hil_sensor_msg->gyro.z = gyro.z(); + hil_sensor_msg->mag.x = mag.x(); + hil_sensor_msg->mag.y = mag.y(); + hil_sensor_msg->mag.z = mag.z(); + hil_sensor_msg->abs_pressure = pressure_msg->fluid_pressure; + hil_sensor_msg->diff_pressure = pressure_diff; + hil_sensor_msg->pressure_alt = pressure_alt; + hil_sensor_msg->temperature = kTemperature_C; + hil_sensor_msg->fields_updated = kAllFieldsUpdated; + + // Publish MAVROS HilSensor message. + hil_sensor_pub_.publish(hil_sensor_msg); +} + +void HilInterfaceNode::HilStateQuaternionPubCallback(const geometry_msgs::TwistStampedConstPtr& air_speed_msg, + const sensor_msgs::NavSatFixConstPtr& gps_msg, + const geometry_msgs::TwistStampedConstPtr &ground_speed_msg, + const sensor_msgs::ImuConstPtr& imu_msg, + const sensor_msgs::MagneticFieldConstPtr &mag_msg) { + // Handle Air speed data. + Eigen::Vector3d air_velocity(air_speed_msg->twist.linear.x, + air_speed_msg->twist.linear.y, + air_speed_msg->twist.linear.z); + + // Handle Ground Speed data. + Eigen::Vector3f gps_vel(Eigen::Vector3f(ground_speed_msg->twist.linear.x, + ground_speed_msg->twist.linear.y, + ground_speed_msg->twist.linear.z)); + + // Handle IMU data. + Eigen::Vector3f acc(imu_msg->linear_acceleration.x, + imu_msg->linear_acceleration.y, + imu_msg->linear_acceleration.z); + + Eigen::Quaterniond att(imu_msg->orientation.w, + imu_msg->orientation.x, + imu_msg->orientation.y, + imu_msg->orientation.z); + + Eigen::Vector3f gyro(imu_msg->angular_velocity.x, + imu_msg->angular_velocity.y, + imu_msg->angular_velocity.z); + + // Handle Mag data + Eigen::Vector3f mag(mag_msg->magnetic_field.x, + mag_msg->magnetic_field.y, + mag_msg->magnetic_field.z); + + auto hil_state_msg = boost::make_shared(); + + // Fill in a MAVROS HilStateQuaternion message. + hil_state_msg->header.stamp = ros::Time::now(); + hil_state_msg->orientation.w = att.w(); + hil_state_msg->orientation.x = att.x(); + hil_state_msg->orientation.y = att.y(); + hil_state_msg->orientation.z = att.z(); + hil_state_msg->angular_velocity.x = gyro.x(); + hil_state_msg->angular_velocity.y = gyro.y(); + hil_state_msg->angular_velocity.z = gyro.z(); + hil_state_msg->geo.latitude = gps_msg->latitude; + hil_state_msg->geo.longitude = gps_msg->longitude; + hil_state_msg->geo.altitude = gps_msg->altitude; + hil_state_msg->linear_velocity.x = gps_vel.x(); + hil_state_msg->linear_velocity.y = gps_vel.y(); + hil_state_msg->linear_velocity.z = gps_vel.z(); + hil_state_msg->ind_airspeed = air_velocity.norm(); + hil_state_msg->true_airspeed = air_velocity.norm(); + hil_state_msg->linear_acceleration.x = acc.x() * kGravityMagnitude_m_per_s2; + hil_state_msg->linear_acceleration.y = acc.y() * kGravityMagnitude_m_per_s2; + hil_state_msg->linear_acceleration.z = acc.z() * kGravityMagnitude_m_per_s2; + + // Publish MAVROS HilStateQuaternion message. + hil_state_pub_.publish(hil_state_msg); +} + +void HilInterfaceNode::HilControlsCallback(const mavros_msgs::HilControlsConstPtr& hil_controls_msg) { + mav_msgs::Actuators act_msg; + + ros::Time current_time = ros::Time::now(); + + 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); + + act_msg.header.stamp.sec = current_time.sec; + act_msg.header.stamp.nsec = current_time.nsec; + + actuators_pub_.publish(act_msg); +} +} // namespace hil_interface + +int main(int argc, char** argv) { + ros::init(argc, argv, "hil_interface_node"); + hil_interface::HilInterfaceNode hil_interface_node; + + hil_interface_node.MainTask(); + + return 0; +}