Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

plugin: setpoint_raw: move getParam to initializer #1669

Merged
merged 1 commit into from
Dec 8, 2021
Merged

plugin: setpoint_raw: move getParam to initializer #1669

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 11 additions & 4 deletions mavros/src/plugins/setpoint_raw.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,13 @@ class SetpointRawPlugin : public plugin::PluginBase,
target_local_pub = sp_nh.advertise<mavros_msgs::PositionTarget>("target_local", 10);
target_global_pub = sp_nh.advertise<mavros_msgs::GlobalPositionTarget>("target_global", 10);
target_attitude_pub = sp_nh.advertise<mavros_msgs::AttitudeTarget>("target_attitude", 10);

// Set Thrust scaling in px4_config.yaml, setpoint_raw block.
if (!sp_nh.getParam("thrust_scaling", thrust_scaling))
{
ROS_WARN_THROTTLE_NAMED(5, "setpoint_raw", "thrust_scaling parameter is unset. Attitude (and angular rate/thrust) setpoints will be ignored.");
thrust_scaling = -1.0;
}
}

Subscriptions get_subscriptions() override
Expand All @@ -71,6 +78,8 @@ class SetpointRawPlugin : public plugin::PluginBase,
ros::Subscriber local_sub, global_sub, attitude_sub;
ros::Publisher target_local_pub, target_global_pub, target_attitude_pub;

double thrust_scaling;

/* -*- message handlers -*- */
void handle_position_target_local_ned(const mavlink::mavlink_message_t *msg, mavlink::common::msg::POSITION_TARGET_LOCAL_NED &tgt)
{
Expand Down Expand Up @@ -226,15 +235,13 @@ class SetpointRawPlugin : public plugin::PluginBase,

void attitude_cb(const mavros_msgs::AttitudeTarget::ConstPtr &req)
{
double thrust_scaling;
Eigen::Quaterniond desired_orientation;
Eigen::Vector3d baselink_angular_rate;
Eigen::Vector3d body_rate;
double thrust;

// Set Thrust scaling in px4_config.yaml, setpoint_raw block.
// ignore thrust is false by default, unless no thrust scalling is set or thrust is zero
auto ignore_thrust = req->thrust != 0.0 && !sp_nh.getParam("thrust_scaling", thrust_scaling);
// ignore thrust is false by default, unless no thrust scaling is set or thrust is zero
auto ignore_thrust = req->thrust != 0.0 && thrust_scaling < 0.0;

if (ignore_thrust) {
// I believe it's safer without sending zero thrust, but actually ignoring the actuation.
Expand Down