Skip to content

Commit

Permalink
changes from the robofly
Browse files Browse the repository at this point in the history
  • Loading branch information
klaxalk committed Oct 23, 2024
1 parent 46a728b commit cf57c6f
Show file tree
Hide file tree
Showing 3 changed files with 11 additions and 8 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@
<remap from="tag_detections" to="~/tag_detections" />
<remap from="tag_detections_image" to="~/tag_detections_image" />

<param name="publish_tag_detections_image" type="bool" value="true" /><!-- default: false -->
<param name="publish_tag_detections_image" type="bool" value="false" /><!-- default: false -->
<param name="queue_size" type="int" value="$(arg queue_size)" />

</node>
Expand Down
13 changes: 7 additions & 6 deletions ros_packages/mrs_precise_landing/config/precise_landing.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,8 @@ desired_heading:
stages:

aligning:
speed: 1.0 # [m/s]
height: 3.0 # [m]
speed: 0.5 # [m/s]
height: 1.5 # [m]
radius: 0.2 # [m]
timeout: 10.0 # [s]

Expand All @@ -42,16 +42,17 @@ stages:

landing:
speed: 0.5 # [m/s]
height: -1.0 # [m], this is relative to the tracker_cmdk
repeat_threshold: 2 # [-]
height: -1.0 # [m], this is relative to the tracker_cmdk
repeat_threshold: 2 # [-]
mass_factor: 0.75 # [-] mass factor for landing detection

repeating:
height: 3.0 # [m]
height: 1.5 # [m]
speed: 1.5 # [m/s]
timeout: 10.0 # [m]

aborting:
height: 3.0 # [m]
height: 1.5 # [m]

# rate of the feedback loop
rate: 10.0 # [Hz]
Expand Down
4 changes: 3 additions & 1 deletion ros_packages/mrs_precise_landing/src/precise_landing.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -161,6 +161,7 @@ class PreciseLanding : public nodelet::Nodelet {

double _landing_speed_;
double _landing_height_;
double _landing_mass_factor_;
int _landing_repeat_threshold_;

// repeating params
Expand Down Expand Up @@ -262,6 +263,7 @@ void PreciseLanding::onInit() {
// landing params
param_loader.loadParam("stages/landing/speed", _landing_speed_);
param_loader.loadParam("stages/landing/height", _landing_height_);
param_loader.loadParam("stages/landing/mass_factor", _landing_mass_factor_);
param_loader.loadParam("stages/landing/repeat_threshold", _landing_repeat_threshold_);

// repeating params
Expand Down Expand Up @@ -1436,7 +1438,7 @@ void PreciseLanding::stateMachineTimer([[maybe_unused]] const ros::TimerEvent &e

auto estimated_mass = sh_mass_estimate_.getMsg();

if (estimated_mass->data < (0.5 * inital_mass_estimate_)) {
if (estimated_mass->data < (_landing_mass_factor_ * inital_mass_estimate_)) {

ROS_INFO("[PreciseLanding]: landing finished");

Expand Down

0 comments on commit cf57c6f

Please sign in to comment.