diff --git a/config/hw_api.yaml b/config/hw_api.yaml index f7ea56d..1b37067 100644 --- a/config/hw_api.yaml +++ b/config/hw_api.yaml @@ -6,6 +6,9 @@ gnss: utm_y: 5249465.43086 amsl: 340.0 +# determines if offboard can be switched on +input_timeout: 1.0 # [s] + input_mode: actuators: false control_group: false diff --git a/src/hw_api_plugin.cpp b/src/hw_api_plugin.cpp index 9f11f86..cb42b9f 100644 --- a/src/hw_api_plugin.cpp +++ b/src/hw_api_plugin.cpp @@ -47,6 +47,8 @@ class Api : public mrs_uav_hw_api::MrsUavHwApi { std::string _world_frame_name_; std::string _body_frame_name_; + double _input_timeout_; + // | --------------------- status methods --------------------- | mrs_msgs::HwApiStatus getStatus(); @@ -151,6 +153,8 @@ void Api::initialize(const ros::NodeHandle& parent_nh, std::shared_ptr Api::callbackOffboard(void) { 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) { + if (last_cmd_time != ros::Time::UNINITIALIZED && (ros::Time::now() - last_cmd_time).toSec() > _input_timeout_) { ss << "Cannot switch to offboard, missing control input."; ROS_INFO_THROTTLE(1.0, "[MrsSimulatorHwApi]: %s", ss.str().c_str()); return {false, ss.str()}; @@ -363,7 +367,9 @@ bool Api::callbackActuatorCmd(const mrs_msgs::HwApiActuatorCmd::ConstPtr msg) { ROS_INFO_ONCE("[Api]: getting actuators cmd"); - ph_actuators_cmd_.publish(msg); + if (offboard_) { + ph_actuators_cmd_.publish(msg); + } { std::scoped_lock lock(mutex_last_cmd_time_); @@ -386,7 +392,9 @@ bool Api::callbackControlGroupCmd(const mrs_msgs::HwApiControlGroupCmd::ConstPtr ROS_INFO_ONCE("[Api]: getting control group cmd"); - ph_control_group_cmd_.publish(msg); + if (offboard_) { + ph_control_group_cmd_.publish(msg); + } { std::scoped_lock lock(mutex_last_cmd_time_); @@ -409,7 +417,9 @@ bool Api::callbackAttitudeRateCmd(const mrs_msgs::HwApiAttitudeRateCmd::ConstPtr ROS_INFO_ONCE("[Api]: getting attitude rate cmd"); - ph_attitude_rate_cmd_.publish(msg); + if (offboard_) { + ph_attitude_rate_cmd_.publish(msg); + } { std::scoped_lock lock(mutex_last_cmd_time_); @@ -432,7 +442,9 @@ bool Api::callbackAttitudeCmd(const mrs_msgs::HwApiAttitudeCmd::ConstPtr msg) { ROS_INFO_ONCE("[Api]: getting attitude cmd"); - ph_attitude_cmd_.publish(msg); + if (offboard_) { + ph_attitude_cmd_.publish(msg); + } { std::scoped_lock lock(mutex_last_cmd_time_); @@ -455,7 +467,9 @@ bool Api::callbackAccelerationHdgRateCmd(const mrs_msgs::HwApiAccelerationHdgRat ROS_INFO_ONCE("[Api]: getting acceleration+hdg rate cmd"); - ph_acceleration_hdg_rate_cmd_.publish(msg); + if (offboard_) { + ph_acceleration_hdg_rate_cmd_.publish(msg); + } { std::scoped_lock lock(mutex_last_cmd_time_); @@ -473,13 +487,14 @@ bool Api::callbackAccelerationHdgRateCmd(const mrs_msgs::HwApiAccelerationHdgRat bool Api::callbackAccelerationHdgCmd(const mrs_msgs::HwApiAccelerationHdgCmd::ConstPtr msg) { if (!_capabilities_.accepts_acceleration_hdg_cmd) { - return false; } ROS_INFO_ONCE("[Api]: getting acceleration+hdg cmd"); - ph_acceleration_hdg_cmd_.publish(msg); + if (offboard_) { + ph_acceleration_hdg_cmd_.publish(msg); + } { std::scoped_lock lock(mutex_last_cmd_time_); @@ -502,7 +517,9 @@ bool Api::callbackVelocityHdgRateCmd(const mrs_msgs::HwApiVelocityHdgRateCmd::Co ROS_INFO_ONCE("[Api]: getting velocity+hdg rate cmd"); - ph_velocity_hdg_rate_cmd_.publish(msg); + if (offboard_) { + ph_velocity_hdg_rate_cmd_.publish(msg); + } { std::scoped_lock lock(mutex_last_cmd_time_); @@ -525,7 +542,9 @@ bool Api::callbackVelocityHdgCmd(const mrs_msgs::HwApiVelocityHdgCmd::ConstPtr m ROS_INFO_ONCE("[Api]: getting velocity+hdg cmd"); - ph_velocity_hdg_cmd_.publish(msg); + if (offboard_) { + ph_velocity_hdg_cmd_.publish(msg); + } { std::scoped_lock lock(mutex_last_cmd_time_); @@ -548,7 +567,9 @@ bool Api::callbackPositionCmd(const mrs_msgs::HwApiPositionCmd::ConstPtr msg) { ROS_INFO_ONCE("[Api]: getting position cmd"); - ph_position_cmd_.publish(msg); + if (offboard_) { + ph_position_cmd_.publish(msg); + } { std::scoped_lock lock(mutex_last_cmd_time_); @@ -567,7 +588,9 @@ void Api::callbackTrackerCmd(const mrs_msgs::TrackerCommand::ConstPtr msg) { ROS_INFO_ONCE("[Api]: getting tracker cmd"); - ph_tracker_cmd_.publish(msg); + if (offboard_) { + ph_tracker_cmd_.publish(msg); + } } //} @@ -796,6 +819,8 @@ void Api::timerMain([[maybe_unused]] const ros::TimerEvent& event) { publishBatteryState(); publishRC(); + + timeoutInputs(); } //} @@ -849,6 +874,12 @@ void Api::publishRC(void) { /* MrsUavHwApi() //{ */ void Api::timeoutInputs(void) { + + 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() > _input_timeout_) { + offboard_ = false; + } } //}