diff --git a/src/hw_api_plugin.cpp b/src/hw_api_plugin.cpp index db5aeee..9f11f86 100644 --- a/src/hw_api_plugin.cpp +++ b/src/hw_api_plugin.cpp @@ -76,6 +76,9 @@ class Api : public mrs_uav_hw_api::MrsUavHwApi { std::shared_ptr common_handlers_; + ros::Time last_cmd_time_; + std::mutex mutex_last_cmd_time_; + // | ----------------------- subscribers ---------------------- | mrs_lib::SubscribeHandler sh_odom_; @@ -142,6 +145,8 @@ void Api::initialize(const ros::NodeHandle& parent_nh, std::shared_ptrgetBodyFrameName(); _world_frame_name_ = common_handlers->getWorldFrameName(); + last_cmd_time_ = ros::Time::UNINITIALIZED; + // | ------------------- loading parameters ------------------- | mrs_lib::ParamLoader param_loader(nh_, "MrsUavHwApi"); @@ -329,6 +334,14 @@ std::tuple Api::callbackOffboard(void) { return {false, ss.str()}; } + auto last_cmd_time = mrs_lib::get_mutexed(mutex_last_cmd_time_, last_cmd_time_); + + if (last_cmd_time != ros::Time::UNINITIALIZED && (ros::Time::now() - last_cmd_time).toSec() < 1.0) { + ss << "Cannot switch to offboard, missing control input."; + ROS_INFO_THROTTLE(1.0, "[MrsSimulatorHwApi]: %s", ss.str().c_str()); + return {false, ss.str()}; + } + offboard_ = true; ss << "Offboard set"; @@ -352,6 +365,12 @@ bool Api::callbackActuatorCmd(const mrs_msgs::HwApiActuatorCmd::ConstPtr msg) { ph_actuators_cmd_.publish(msg); + { + std::scoped_lock lock(mutex_last_cmd_time_); + + last_cmd_time_ = ros::Time::now(); + } + return true; } @@ -369,6 +388,12 @@ bool Api::callbackControlGroupCmd(const mrs_msgs::HwApiControlGroupCmd::ConstPtr ph_control_group_cmd_.publish(msg); + { + std::scoped_lock lock(mutex_last_cmd_time_); + + last_cmd_time_ = ros::Time::now(); + } + return true; } @@ -386,6 +411,12 @@ bool Api::callbackAttitudeRateCmd(const mrs_msgs::HwApiAttitudeRateCmd::ConstPtr ph_attitude_rate_cmd_.publish(msg); + { + std::scoped_lock lock(mutex_last_cmd_time_); + + last_cmd_time_ = ros::Time::now(); + } + return true; } @@ -403,6 +434,12 @@ bool Api::callbackAttitudeCmd(const mrs_msgs::HwApiAttitudeCmd::ConstPtr msg) { ph_attitude_cmd_.publish(msg); + { + std::scoped_lock lock(mutex_last_cmd_time_); + + last_cmd_time_ = ros::Time::now(); + } + return true; } @@ -420,6 +457,12 @@ bool Api::callbackAccelerationHdgRateCmd(const mrs_msgs::HwApiAccelerationHdgRat ph_acceleration_hdg_rate_cmd_.publish(msg); + { + std::scoped_lock lock(mutex_last_cmd_time_); + + last_cmd_time_ = ros::Time::now(); + } + return true; } @@ -438,6 +481,12 @@ bool Api::callbackAccelerationHdgCmd(const mrs_msgs::HwApiAccelerationHdgCmd::Co ph_acceleration_hdg_cmd_.publish(msg); + { + std::scoped_lock lock(mutex_last_cmd_time_); + + last_cmd_time_ = ros::Time::now(); + } + return true; } @@ -455,6 +504,12 @@ bool Api::callbackVelocityHdgRateCmd(const mrs_msgs::HwApiVelocityHdgRateCmd::Co ph_velocity_hdg_rate_cmd_.publish(msg); + { + std::scoped_lock lock(mutex_last_cmd_time_); + + last_cmd_time_ = ros::Time::now(); + } + return true; } @@ -472,6 +527,12 @@ bool Api::callbackVelocityHdgCmd(const mrs_msgs::HwApiVelocityHdgCmd::ConstPtr m ph_velocity_hdg_cmd_.publish(msg); + { + std::scoped_lock lock(mutex_last_cmd_time_); + + last_cmd_time_ = ros::Time::now(); + } + return true; } @@ -489,6 +550,12 @@ bool Api::callbackPositionCmd(const mrs_msgs::HwApiPositionCmd::ConstPtr msg) { ph_position_cmd_.publish(msg); + { + std::scoped_lock lock(mutex_last_cmd_time_); + + last_cmd_time_ = ros::Time::now(); + } + return true; }