Skip to content

Commit

Permalink
added minz setter instead of safety area disabler
Browse files Browse the repository at this point in the history
  • Loading branch information
@klaxalk
klaxalk committed Mar 24, 2024
1 parent d6bb7d2 commit a0234b8
Show file tree
Hide file tree
Showing 3 changed files with 54 additions and 28 deletions.
2 changes: 1 addition & 1 deletion ros_packages/mrs_precise_landing/launch/precise_landing.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@
<remap from="~switch_controller_out" to="control_manager/switch_controller" />
<remap from="~switch_tracker_out" to="control_manager/switch_tracker" />
<remap from="~arming_out" to="control_manager/arm" />
<remap from="~enable_safety_area_out" to="control_manager/use_safety_area" />
<remap from="~set_min_z_out" to="control_manager/set_min_z" />
<remap from="~enable_min_height_check_out" to="uav_manager/enable_min_height_check" />
<remap from="~path_out" to="trajectory_generation/path" />

Expand Down
70 changes: 48 additions & 22 deletions ros_packages/mrs_precise_landing/src/precise_landing.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@
#include <mrs_msgs/String.h>
#include <mrs_msgs/PathSrv.h>
#include <mrs_msgs/ControlManagerDiagnostics.h>
#include <mrs_msgs/Float64StampedSrv.h>

//}

Expand Down Expand Up @@ -109,12 +110,12 @@ class PreciseLanding : public nodelet::Nodelet {
ros::ServiceServer service_server_land_;
ros::ServiceServer service_servcer_stop_;

mrs_lib::ServiceClientHandler<mrs_msgs::String> sch_switch_controller_;
mrs_lib::ServiceClientHandler<mrs_msgs::String> sch_switch_tracker_;
mrs_lib::ServiceClientHandler<std_srvs::SetBool> sch_arming_;
mrs_lib::ServiceClientHandler<std_srvs::SetBool> sch_enable_safety_area_;
mrs_lib::ServiceClientHandler<std_srvs::SetBool> sch_enable_min_height_check_;
mrs_lib::ServiceClientHandler<mrs_msgs::PathSrv> sch_path_;
mrs_lib::ServiceClientHandler<mrs_msgs::String> sch_switch_controller_;
mrs_lib::ServiceClientHandler<mrs_msgs::String> sch_switch_tracker_;
mrs_lib::ServiceClientHandler<std_srvs::SetBool> sch_arming_;
mrs_lib::ServiceClientHandler<mrs_msgs::Float64StampedSrv> sch_set_min_z;
mrs_lib::ServiceClientHandler<std_srvs::SetBool> sch_enable_min_height_check_;
mrs_lib::ServiceClientHandler<mrs_msgs::PathSrv> sch_path_;

// params loaded from config file
double _trajectory_dt_;
Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -289,7 +290,7 @@ void PreciseLanding::onInit() {
sch_switch_controller_ = mrs_lib::ServiceClientHandler<mrs_msgs::String>(nh_, "switch_controller_out");
sch_switch_tracker_ = mrs_lib::ServiceClientHandler<mrs_msgs::String>(nh_, "switch_tracker_out");
sch_arming_ = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "arming_out");
sch_enable_safety_area_ = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "enable_safety_area_out");
sch_set_min_z = mrs_lib::ServiceClientHandler<mrs_msgs::Float64StampedSrv>(nh_, "set_min_z_out");
sch_enable_min_height_check_ = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "enable_min_height_check_out");
sch_path_ = mrs_lib::ServiceClientHandler<mrs_msgs::PathSrv>(nh_, "path_out");

Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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;
}

Expand Down Expand Up @@ -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 {
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -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) {
Expand Down
10 changes: 5 additions & 5 deletions ros_packages/mrs_precise_landing_gazebo/tmux/config/world_config.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand All @@ -31,4 +31,4 @@ safety_area:
frame_name: "world_origin"

max_z: 15.0
min_z: 0.5
min_z: 1.0

0 comments on commit a0234b8

Please sign in to comment.