From a0234b8809f91befaf630200391f066e733fe133 Mon Sep 17 00:00:00 2001 From: Tomas Baca Date: Sun, 24 Mar 2024 17:33:55 +0100 Subject: [PATCH] added minz setter instead of safety area disabler --- .../launch/precise_landing.launch | 2 +- .../src/precise_landing.cpp | 70 +++++++++++++------ .../tmux/config/world_config.yaml | 10 +-- 3 files changed, 54 insertions(+), 28 deletions(-) diff --git a/ros_packages/mrs_precise_landing/launch/precise_landing.launch b/ros_packages/mrs_precise_landing/launch/precise_landing.launch index 31517a4..cf32941 100644 --- a/ros_packages/mrs_precise_landing/launch/precise_landing.launch +++ b/ros_packages/mrs_precise_landing/launch/precise_landing.launch @@ -54,7 +54,7 @@ - + diff --git a/ros_packages/mrs_precise_landing/src/precise_landing.cpp b/ros_packages/mrs_precise_landing/src/precise_landing.cpp index 0e06dc0..3f9b01f 100644 --- a/ros_packages/mrs_precise_landing/src/precise_landing.cpp +++ b/ros_packages/mrs_precise_landing/src/precise_landing.cpp @@ -26,6 +26,7 @@ #include #include #include +#include //} @@ -109,12 +110,12 @@ class PreciseLanding : public nodelet::Nodelet { ros::ServiceServer service_server_land_; ros::ServiceServer service_servcer_stop_; - mrs_lib::ServiceClientHandler sch_switch_controller_; - mrs_lib::ServiceClientHandler sch_switch_tracker_; - mrs_lib::ServiceClientHandler sch_arming_; - mrs_lib::ServiceClientHandler sch_enable_safety_area_; - mrs_lib::ServiceClientHandler sch_enable_min_height_check_; - mrs_lib::ServiceClientHandler sch_path_; + mrs_lib::ServiceClientHandler sch_switch_controller_; + mrs_lib::ServiceClientHandler sch_switch_tracker_; + mrs_lib::ServiceClientHandler sch_arming_; + mrs_lib::ServiceClientHandler sch_set_min_z; + mrs_lib::ServiceClientHandler sch_enable_min_height_check_; + mrs_lib::ServiceClientHandler sch_path_; // params loaded from config file double _trajectory_dt_; @@ -179,7 +180,7 @@ class PreciseLanding : public nodelet::Nodelet { void disarm(void); - bool enableSafetyArea(const bool state); + bool setMinZ(const double state); bool enableMinHeightCheck(const bool state); @@ -289,7 +290,7 @@ void PreciseLanding::onInit() { sch_switch_controller_ = mrs_lib::ServiceClientHandler(nh_, "switch_controller_out"); sch_switch_tracker_ = mrs_lib::ServiceClientHandler(nh_, "switch_tracker_out"); sch_arming_ = mrs_lib::ServiceClientHandler(nh_, "arming_out"); - sch_enable_safety_area_ = mrs_lib::ServiceClientHandler(nh_, "enable_safety_area_out"); + sch_set_min_z = mrs_lib::ServiceClientHandler(nh_, "set_min_z_out"); sch_enable_min_height_check_ = mrs_lib::ServiceClientHandler(nh_, "enable_min_height_check_out"); sch_path_ = mrs_lib::ServiceClientHandler(nh_, "path_out"); @@ -512,12 +513,6 @@ void PreciseLanding::changeState(int newState) { return; } - if (!enableSafetyArea(false)) { - ROS_ERROR("[PreciseLanding]: failed to disable the safety area"); - changeState(IDLE_STATE); - return; - } - if (!enableMinHeightCheck(false)) { ROS_ERROR("[PreciseLanding]: failed to disable the min height check"); changeState(IDLE_STATE); @@ -912,24 +907,25 @@ void PreciseLanding::disarm(void) { //} -/* enableSafetyArea() //{ */ +/* setMinZ() //{ */ -bool PreciseLanding::enableSafetyArea(const bool state) { +bool PreciseLanding::setMinZ(const double z) { - std_srvs::SetBool srv; - srv.request.data = state; + mrs_msgs::Float64StampedSrv srv; + srv.request.header.frame_id = _frame_id_; + srv.request.value = z; - ROS_INFO("[PreciseLanding]: %s safety area", state ? "enabling" : "disabling"); + ROS_INFO("[PreciseLanding]: setting safety area's min Z"); - bool res = sch_enable_safety_area_.call(srv); + bool res = sch_set_min_z.call(srv); if (res) { if (!srv.response.success) { - ROS_WARN_THROTTLE(1.0, "[PreciseLanding]: service call for enableSafetyArea() returned false: %s", srv.response.message.c_str()); + ROS_WARN_THROTTLE(1.0, "[PreciseLanding]: service call for setMinZ() returned false: %s", srv.response.message.c_str()); return false; } } else { - ROS_ERROR("[PreciseLanding]: service call for enableSafetyArea() failed!"); + ROS_ERROR("[PreciseLanding]: service call for setMinZ() failed!"); return false; } @@ -1236,6 +1232,12 @@ void PreciseLanding::stateMachineTimer([[maybe_unused]] const ros::TimerEvent &e double des_z = landing_pad->reference.position.z + _aligning_height_; double des_heading; + if (!setMinZ(landing_pad->reference.position.z + _landing_height_)) { + ROS_ERROR("[PreciseLanding]: failed to set safety area's min Z"); + changeState(ABORT_STATE); + return; + } + if (_heading_relative_to_pad_enabled_) { des_heading = landing_pad->reference.heading + _heading_relative_to_pad_; } else { @@ -1280,6 +1282,14 @@ void PreciseLanding::stateMachineTimer([[maybe_unused]] const ros::TimerEvent &e return; } + auto landing_pad = getTransformedLandingPad(_frame_id_); + + if (!setMinZ(landing_pad->reference.position.z + _landing_height_)) { + ROS_ERROR("[PreciseLanding]: failed to set safety area's min Z"); + changeState(ABORT_STATE); + return; + } + auto trajectory = createTrajectory(DESCEND_TRAJECTORY); // publish the trajectory @@ -1327,6 +1337,14 @@ void PreciseLanding::stateMachineTimer([[maybe_unused]] const ros::TimerEvent &e return; } + auto landing_pad = getTransformedLandingPad(_frame_id_); + + if (!setMinZ(landing_pad->reference.position.z + _landing_height_)) { + ROS_ERROR("[PreciseLanding]: failed to set safety area's min Z"); + changeState(ABORT_STATE); + return; + } + auto trajectory = createTrajectory(DESCEND_TRAJECTORY); if (trajectory) { @@ -1392,6 +1410,14 @@ void PreciseLanding::stateMachineTimer([[maybe_unused]] const ros::TimerEvent &e case LANDING_STATE: { + auto landing_pad = getTransformedLandingPad(_frame_id_); + + if (!setMinZ(landing_pad->reference.position.z + _landing_height_)) { + ROS_ERROR("[PreciseLanding]: failed to set safety area's min Z"); + changeState(ABORT_STATE); + return; + } + auto trajectory = createTrajectory(LANDING_TRAJECTORY); if (trajectory) { diff --git a/ros_packages/mrs_precise_landing_gazebo/tmux/config/world_config.yaml b/ros_packages/mrs_precise_landing_gazebo/tmux/config/world_config.yaml index 9b55067..9e3e913 100644 --- a/ros_packages/mrs_precise_landing_gazebo/tmux/config/world_config.yaml +++ b/ros_packages/mrs_precise_landing_gazebo/tmux/config/world_config.yaml @@ -19,10 +19,10 @@ safety_area: # x, y [m] for any frame_name except latlon_origin # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" points: [ - -50, -50, - 50, -50, - 50, 50, - -50, 50, + -10, -10, + 10, -10, + 10, 10, + -10, 10, ] vertical: @@ -31,4 +31,4 @@ safety_area: frame_name: "world_origin" max_z: 15.0 - min_z: 0.5 + min_z: 1.0