From c035b8ff1ffb042db235648371f5d7d2e531fae4 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Wed, 16 Feb 2022 20:13:39 +0300 Subject: [PATCH] plugins: Fix misprint Fix #1709 Signed-off-by: Vladimir Ermakov --- mavros/include/mavros/plugin.hpp | 4 +- mavros/src/plugins/altitude.cpp | 2 +- mavros/src/plugins/command.cpp | 4 +- mavros/src/plugins/geofence.cpp | 4 +- mavros/src/plugins/global_position.cpp | 18 ++++---- mavros/src/plugins/imu.cpp | 10 ++--- mavros/src/plugins/local_position.cpp | 8 ++-- mavros/src/plugins/rallypoint.cpp | 4 +- mavros/src/plugins/setpoint_attitude.cpp | 4 +- mavros/src/plugins/setpoint_position.cpp | 2 +- mavros/src/plugins/setpoint_raw.cpp | 2 +- mavros/src/plugins/setpoint_trajectory.cpp | 4 +- mavros/src/plugins/setpoint_velocity.cpp | 2 +- mavros/src/plugins/sys_status.cpp | 10 ++--- mavros/src/plugins/sys_time.cpp | 26 ++++++------ mavros/src/plugins/waypoint.cpp | 6 +-- mavros_extras/src/plugins/3dr_radio.cpp | 2 +- mavros_extras/src/plugins/distance_sensor.cpp | 4 +- mavros_extras/src/plugins/fake_gps.cpp | 42 +++++++++---------- mavros_extras/src/plugins/gps_input.cpp | 2 +- mavros_extras/src/plugins/landing_target.cpp | 32 +++++++------- .../src/plugins/obstacle_distance.cpp | 2 +- mavros_extras/src/plugins/odom.cpp | 4 +- mavros_extras/src/plugins/px4flow.cpp | 8 ++-- mavros_extras/src/plugins/vibration.cpp | 2 +- .../src/plugins/vision_pose_estimate.cpp | 8 ++-- mavros_extras/src/plugins/wheel_odometry.cpp | 26 ++++++------ 27 files changed, 121 insertions(+), 121 deletions(-) diff --git a/mavros/include/mavros/plugin.hpp b/mavros/include/mavros/plugin.hpp index 770814ad1..ee4c48bba 100644 --- a/mavros/include/mavros/plugin.hpp +++ b/mavros/include/mavros/plugin.hpp @@ -218,7 +218,7 @@ class Plugin : public std::enable_shared_from_this * Adds parameter to watch and declares it */ template - auto node_declate_and_watch_parameter( + auto node_declare_and_watch_parameter( const std::string & name, ParameterFunctorT cb, const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = rcl_interfaces::msg::ParameterDescriptor(), @@ -239,7 +239,7 @@ class Plugin : public std::enable_shared_from_this } template - auto node_declate_and_watch_parameter( + auto node_declare_and_watch_parameter( const std::string & name, const ParameterT & default_value, ParameterFunctorT cb, const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = diff --git a/mavros/src/plugins/altitude.cpp b/mavros/src/plugins/altitude.cpp index f28c0c499..4ec63c169 100644 --- a/mavros/src/plugins/altitude.cpp +++ b/mavros/src/plugins/altitude.cpp @@ -41,7 +41,7 @@ class AltitudePlugin : public plugin::Plugin { enable_node_watch_parameters(); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "frame_id", "map", [&](const rclcpp::Parameter & p) { frame_id = p.as_string(); }); diff --git a/mavros/src/plugins/command.cpp b/mavros/src/plugins/command.cpp index ac8df93d6..b50db296b 100644 --- a/mavros/src/plugins/command.cpp +++ b/mavros/src/plugins/command.cpp @@ -73,12 +73,12 @@ class CommandPlugin : public plugin::Plugin { enable_node_watch_parameters(); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "command_ack_timeout", command_ack_timeout_dt.seconds(), [&](const rclcpp::Parameter & p) { command_ack_timeout_dt = rclcpp::Duration::from_seconds(p.as_double()); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "use_comp_id_system_control", false, [&](const rclcpp::Parameter & p) { use_comp_id_system_control = p.as_bool(); }); diff --git a/mavros/src/plugins/geofence.cpp b/mavros/src/plugins/geofence.cpp index e7e16aad1..a26257121 100644 --- a/mavros/src/plugins/geofence.cpp +++ b/mavros/src/plugins/geofence.cpp @@ -37,12 +37,12 @@ class GeofencePlugin : public plugin::MissionBase enable_node_watch_parameters(); // NOTE(vooon): I'm not quite sure that this option would work with mavros router - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "pull_after_gcs", true, [&](const rclcpp::Parameter & p) { do_pull_after_gcs = p.as_bool(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "use_mission_item_int", true, [&](const rclcpp::Parameter & p) { use_mission_item_int = p.as_bool(); }); diff --git a/mavros/src/plugins/global_position.cpp b/mavros/src/plugins/global_position.cpp index 160b94d50..b06865c84 100644 --- a/mavros/src/plugins/global_position.cpp +++ b/mavros/src/plugins/global_position.cpp @@ -66,42 +66,42 @@ class GlobalPositionPlugin : public plugin::Plugin enable_node_watch_parameters(); // general params - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "frame_id", "map", [&](const rclcpp::Parameter & p) { frame_id = p.as_string(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "child_frame_id", "base_link", [&](const rclcpp::Parameter & p) { child_frame_id = p.as_string(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "rot_covariance", 99999.0, [&](const rclcpp::Parameter & p) { rot_cov = p.as_double(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "gps_uere", 1.0, [&](const rclcpp::Parameter & p) { gps_uere = p.as_double(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "use_relative_alt", true, [&](const rclcpp::Parameter & p) { use_relative_alt = p.as_bool(); }); // tf subsection - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "tf.send", false, [&](const rclcpp::Parameter & p) { tf_send = p.as_bool(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "tf.frame_id", "map", [&](const rclcpp::Parameter & p) { tf_frame_id = p.as_string(); }); // The global_origin should be represented as "earth" coordinate frame (ECEF) (REP 105) - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "tf.global_frame_id", "earth", [&](const rclcpp::Parameter & p) { tf_global_frame_id = p.as_string(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "tf.child_frame_id", "base_link", [&](const rclcpp::Parameter & p) { tf_child_frame_id = p.as_string(); }); diff --git a/mavros/src/plugins/imu.cpp b/mavros/src/plugins/imu.cpp index e0818b34c..d71057683 100644 --- a/mavros/src/plugins/imu.cpp +++ b/mavros/src/plugins/imu.cpp @@ -79,27 +79,27 @@ class IMUPlugin : public plugin::Plugin * transformation from the ENU frame to the base_link frame (ENU <-> base_link). * THIS ORIENTATION IS NOT THE SAME AS THAT REPORTED BY THE FCU (NED <-> aircraft). */ - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "frame_id", "base_link", [&](const rclcpp::Parameter & p) { frame_id = p.as_string(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "linear_acceleration_stdev", 0.0003, [&](const rclcpp::Parameter & p) { auto linear_stdev = p.as_double(); setup_covariance(linear_acceleration_cov, linear_stdev); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "angular_velocity_stdev", 0.02 * (M_PI / 180.0), [&](const rclcpp::Parameter & p) { auto angular_stdev = p.as_double(); setup_covariance(angular_velocity_cov, angular_stdev); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "orientation_stdev", 1.0, [&](const rclcpp::Parameter & p) { auto orientation_stdev = p.as_double(); setup_covariance(orientation_cov, orientation_stdev); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "magnetic_stdev", 0.0, [&](const rclcpp::Parameter & p) { auto mag_stdev = p.as_double(); setup_covariance(magnetic_cov, mag_stdev); diff --git a/mavros/src/plugins/local_position.cpp b/mavros/src/plugins/local_position.cpp index caffc1428..03d58fa5e 100644 --- a/mavros/src/plugins/local_position.cpp +++ b/mavros/src/plugins/local_position.cpp @@ -59,22 +59,22 @@ class LocalPositionPlugin : public plugin::Plugin // header frame_id. // default to map (world-fixed, ENU as per REP-105). - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "frame_id", "map", [&](const rclcpp::Parameter & p) { frame_id = p.as_string(); }); // Important tf subsection // Report the transform from world to base_link here. - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "tf.send", false, [&](const rclcpp::Parameter & p) { tf_send = p.as_bool(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "tf.frame_id", "map", [&](const rclcpp::Parameter & p) { tf_frame_id = p.as_string(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "tf.child_frame_id", "base_link", [&](const rclcpp::Parameter & p) { tf_child_frame_id = p.as_string(); }); diff --git a/mavros/src/plugins/rallypoint.cpp b/mavros/src/plugins/rallypoint.cpp index 2387c848e..08c85a963 100644 --- a/mavros/src/plugins/rallypoint.cpp +++ b/mavros/src/plugins/rallypoint.cpp @@ -36,12 +36,12 @@ class RallypointPlugin : public plugin::MissionBase enable_node_watch_parameters(); // NOTE(vooon): I'm not quite sure that this option would work with mavros router - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "pull_after_gcs", true, [&](const rclcpp::Parameter & p) { do_pull_after_gcs = p.as_bool(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "use_mission_item_int", true, [&](const rclcpp::Parameter & p) { use_mission_item_int = p.as_bool(); }); diff --git a/mavros/src/plugins/setpoint_attitude.cpp b/mavros/src/plugins/setpoint_attitude.cpp index a34c2cd12..62b9cde9c 100644 --- a/mavros/src/plugins/setpoint_attitude.cpp +++ b/mavros/src/plugins/setpoint_attitude.cpp @@ -64,12 +64,12 @@ class SetpointAttitudePlugin : public plugin::Plugin, auto qos = rclcpp::QoS(10); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "reverse_thrust", false, [&](const rclcpp::Parameter & p) { reverse_thrust = p.as_bool(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "use_quaternion", false, [&](const rclcpp::Parameter & p) { auto use_quaternion = p.as_bool(); diff --git a/mavros/src/plugins/setpoint_position.cpp b/mavros/src/plugins/setpoint_position.cpp index 315bdde83..00f9b0ee0 100644 --- a/mavros/src/plugins/setpoint_position.cpp +++ b/mavros/src/plugins/setpoint_position.cpp @@ -52,7 +52,7 @@ class SetpointPositionPlugin : public plugin::Plugin, { enable_node_watch_parameters(); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "mav_frame", "LOCAL_NED", [&](const rclcpp::Parameter & p) { auto mav_frame_str = p.as_string(); auto new_mav_frame = utils::mav_frame_from_str(mav_frame_str); diff --git a/mavros/src/plugins/setpoint_raw.cpp b/mavros/src/plugins/setpoint_raw.cpp index 36f2be229..653c180c7 100644 --- a/mavros/src/plugins/setpoint_raw.cpp +++ b/mavros/src/plugins/setpoint_raw.cpp @@ -53,7 +53,7 @@ class SetpointRawPlugin : public plugin::Plugin, { enable_node_watch_parameters(); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "thrust_scaling", NAN, [&](const rclcpp::Parameter & p) { thrust_scaling = p.as_double(); }); diff --git a/mavros/src/plugins/setpoint_trajectory.cpp b/mavros/src/plugins/setpoint_trajectory.cpp index b680042da..95c5386e2 100644 --- a/mavros/src/plugins/setpoint_trajectory.cpp +++ b/mavros/src/plugins/setpoint_trajectory.cpp @@ -55,12 +55,12 @@ class SetpointTrajectoryPlugin : public plugin::Plugin, { enable_node_watch_parameters(); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "frame_id", "map", [&](const rclcpp::Parameter & p) { frame_id = p.as_string(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "mav_frame", "LOCAL_NED", [&](const rclcpp::Parameter & p) { auto mav_frame_str = p.as_string(); auto new_mav_frame = utils::mav_frame_from_str(mav_frame_str); diff --git a/mavros/src/plugins/setpoint_velocity.cpp b/mavros/src/plugins/setpoint_velocity.cpp index efb59c792..850b43ecb 100644 --- a/mavros/src/plugins/setpoint_velocity.cpp +++ b/mavros/src/plugins/setpoint_velocity.cpp @@ -49,7 +49,7 @@ class SetpointVelocityPlugin : public plugin::Plugin, { enable_node_watch_parameters(); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "mav_frame", "LOCAL_NED", [&](const rclcpp::Parameter & p) { auto mav_frame_str = p.as_string(); auto new_mav_frame = utils::mav_frame_from_str(mav_frame_str); diff --git a/mavros/src/plugins/sys_status.cpp b/mavros/src/plugins/sys_status.cpp index 77011a879..2f6c570b0 100644 --- a/mavros/src/plugins/sys_status.cpp +++ b/mavros/src/plugins/sys_status.cpp @@ -428,7 +428,7 @@ class SystemStatusPlugin : public plugin::Plugin { enable_node_watch_parameters(); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "conn_timeout", 10.0, [&](const rclcpp::Parameter & p) { auto conn_timeout = rclcpp::Duration::from_seconds(p.as_double()); @@ -438,14 +438,14 @@ class SystemStatusPlugin : public plugin::Plugin std::bind(&SystemStatusPlugin::timeout_cb, this)); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "min_voltage", 10.0, [&](const rclcpp::Parameter & p) { auto min_voltage = p.as_double(); batt_diag.set_min_voltage(min_voltage); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "disable_diag", false, [&](const rclcpp::Parameter & p) { disable_diag = p.as_bool(); @@ -460,13 +460,13 @@ class SystemStatusPlugin : public plugin::Plugin } }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "heartbeat_mav_type", utils::enum_to_name( conn_heartbeat_mav_type), [&](const rclcpp::Parameter & p) { conn_heartbeat_mav_type = utils::mav_type_from_str(p.as_string()); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "heartbeat_rate", 1.0, [&](const rclcpp::Parameter & p) { auto rate_d = p.as_double(); diff --git a/mavros/src/plugins/sys_time.cpp b/mavros/src/plugins/sys_time.cpp index 9d24f3563..b76bc9690 100644 --- a/mavros/src/plugins/sys_time.cpp +++ b/mavros/src/plugins/sys_time.cpp @@ -160,19 +160,19 @@ class SystemTimePlugin : public plugin::Plugin { enable_node_watch_parameters(); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "time_ref_source", "fcu", [&](const rclcpp::Parameter & p) { time_ref_source = p.as_string(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "timesync_mode", "MAVLINK", [&](const rclcpp::Parameter & p) { auto ts_mode = utils::timesync_mode_from_str(p.as_string()); uas->set_timesync_mode(ts_mode); RCLCPP_INFO_STREAM(get_logger(), "TM: Timesync mode: " << utils::to_string(ts_mode)); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "system_time_rate", 0.0, [&](const rclcpp::Parameter & p) { auto rate_d = p.as_double(); @@ -191,7 +191,7 @@ class SystemTimePlugin : public plugin::Plugin } }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "timesync_rate", 0.0, [&](const rclcpp::Parameter & p) { auto rate_d = p.as_double(); @@ -223,22 +223,22 @@ class SystemTimePlugin : public plugin::Plugin // tighter estimation of the skew (derivative), but will negatively affect how fast the // filter reacts to clock skewing (e.g cause by temperature changes to the oscillator). // Larger values will cause large-amplitude oscillations. - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "timesync_alpha_initial", 0.05, [&](const rclcpp::Parameter & p) { filter_alpha_initial = p.as_double(); reset_filter(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "timesync_beta_initial", 0.05, [&](const rclcpp::Parameter & p) { filter_beta_initial = p.as_double(); reset_filter(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "timesync_alpha_final", 0.003, [&](const rclcpp::Parameter & p) { filter_alpha_final = p.as_double(); reset_filter(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "timesync_beta_final", 0.003, [&](const rclcpp::Parameter & p) { filter_beta_final = p.as_double(); reset_filter(); @@ -250,7 +250,7 @@ class SystemTimePlugin : public plugin::Plugin // exhanged timesync packets is less than convergence_window. A lower value will // allow the timesync to converge faster, but with potentially less accurate initial // offset and skew estimates. - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "convergence_window", 500, [&](const rclcpp::Parameter & p) { convergence_window = p.as_int(); }); @@ -264,19 +264,19 @@ class SystemTimePlugin : public plugin::Plugin // estimate are not used to update the filter. More than max_consecutive_high_deviation number // of such events in a row will reset the filter. This usually happens only due to a time jump // on the remote system. - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "max_rtt_sample", 10, [&](const rclcpp::Parameter & p) { max_rtt_sample = p.as_int(); // in ms }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "max_deviation_sample", 10, [&](const rclcpp::Parameter & p) { max_deviation_sample = p.as_int(); // in ms }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "max_consecutive_high_rtt", 10, [&](const rclcpp::Parameter & p) { max_cons_high_rtt = p.as_int(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "max_consecutive_high_deviation", 10, [&](const rclcpp::Parameter & p) { max_cons_high_deviation = p.as_int(); }); diff --git a/mavros/src/plugins/waypoint.cpp b/mavros/src/plugins/waypoint.cpp index 738c1272c..f117c6a11 100644 --- a/mavros/src/plugins/waypoint.cpp +++ b/mavros/src/plugins/waypoint.cpp @@ -45,17 +45,17 @@ class WaypointPlugin : public plugin::MissionBase enable_node_watch_parameters(); // NOTE(vooon): I'm not quite sure that this option would work with mavros router - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "pull_after_gcs", true, [&](const rclcpp::Parameter & p) { do_pull_after_gcs = p.as_bool(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "use_mission_item_int", true, [&](const rclcpp::Parameter & p) { use_mission_item_int = p.as_bool(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "enable_partial_push", 2, [&](const rclcpp::Parameter & p) { RCLCPP_DEBUG_STREAM(get_logger(), log_prefix << ": enable_partial_push = " << p); diff --git a/mavros_extras/src/plugins/3dr_radio.cpp b/mavros_extras/src/plugins/3dr_radio.cpp index a59056229..3b8b2613d 100644 --- a/mavros_extras/src/plugins/3dr_radio.cpp +++ b/mavros_extras/src/plugins/3dr_radio.cpp @@ -60,7 +60,7 @@ class TDRRadioPlugin : public plugin::Plugin { enable_node_watch_parameters(); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "low_rssi", 40, [&](const rclcpp::Parameter & p) { low_rssi = p.as_int(); }); diff --git a/mavros_extras/src/plugins/distance_sensor.cpp b/mavros_extras/src/plugins/distance_sensor.cpp index 9c2ec0b16..63bc201c2 100644 --- a/mavros_extras/src/plugins/distance_sensor.cpp +++ b/mavros_extras/src/plugins/distance_sensor.cpp @@ -144,11 +144,11 @@ class DistanceSensorPlugin : public plugin::Plugin { enable_node_watch_parameters(); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "base_frame_id", "base_link", [&](const rclcpp::Parameter & p) { base_frame_id = p.as_string(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "config", "", [&](const rclcpp::Parameter & p) { std::unique_lock lock(mutex); diff --git a/mavros_extras/src/plugins/fake_gps.cpp b/mavros_extras/src/plugins/fake_gps.cpp index 9c9bc1bdc..cd86bf6e4 100644 --- a/mavros_extras/src/plugins/fake_gps.cpp +++ b/mavros_extras/src/plugins/fake_gps.cpp @@ -90,55 +90,55 @@ class FakeGPSPlugin : public plugin::Plugin, last_pos_time = rclcpp::Time(0.0); // general params - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "gps_id", 0, [&](const rclcpp::Parameter & p) { gps_id = p.as_int(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "fix_type", utils::enum_value(GPS_FIX_TYPE::NO_GPS), [&](const rclcpp::Parameter & p) { fix_type = static_cast( p.as_int()); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "gps_rate", 5.0, [&](const rclcpp::Parameter & p) { rclcpp::Rate rate(p.as_double()); gps_rate_period = rate.period(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "eph", 2.0, [&](const rclcpp::Parameter & p) { eph = p.as_double(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "epv", 2.0, [&](const rclcpp::Parameter & p) { epv = p.as_double(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "horiz_accuracy", 0.0, [&](const rclcpp::Parameter & p) { horiz_accuracy = p.as_double(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "vert_accuracy", 0.0, [&](const rclcpp::Parameter & p) { vert_accuracy = p.as_double(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "speed_accuracy", 0.0, [&](const rclcpp::Parameter & p) { speed_accuracy = p.as_double(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "satellites_visible", 5, [&](const rclcpp::Parameter & p) { satellites_visible = p.as_int(); }); // default origin/starting point: Zürich geodetic coordinates - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "geo_origin.lat", 47.3667, [&](const rclcpp::Parameter & p) { map_origin.x() = p.as_double(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "geo_origin.lon", 8.5500, [&](const rclcpp::Parameter & p) { map_origin.y() = p.as_double(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "geo_origin.alt", 408.0, [&](const rclcpp::Parameter & p) { map_origin.z() = p.as_double(); }); @@ -156,28 +156,28 @@ class FakeGPSPlugin : public plugin::Plugin, } // source set params - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( // listen to MoCap source "use_mocap", true, [&](const rclcpp::Parameter & p) { use_mocap = p.as_bool(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( // listen to MoCap source (TransformStamped if true; PoseStamped if false) "mocap_transform", true, [&](const rclcpp::Parameter & p) { mocap_transform = p.as_bool(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( // ~mocap/pose uses PoseWithCovarianceStamped Message "mocap_withcovariance", false, [&](const rclcpp::Parameter & p) { mocap_withcovariance = p.as_bool(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( // listen to Vision source "use_vision", false, [&](const rclcpp::Parameter & p) { use_vision = p.as_bool(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "use_hil_gps", false, [&](const rclcpp::Parameter & p) { // send HIL_GPS MAVLink messages if true, // send GPS_INPUT mavlink messages if false @@ -185,19 +185,19 @@ class FakeGPSPlugin : public plugin::Plugin, }); // tf params - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "tf.frame_id", "map", [&](const rclcpp::Parameter & p) { tf_frame_id = p.as_string(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "tf.child_frame_id", "base_link", [&](const rclcpp::Parameter & p) { tf_child_frame_id = p.as_string(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "tf.rate_limit", 10.0, [&](const rclcpp::Parameter & p) { tf_rate = p.as_double(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "tf.listen", false, [&](const rclcpp::Parameter & p) { tf_listen = p.as_bool(); }); diff --git a/mavros_extras/src/plugins/gps_input.cpp b/mavros_extras/src/plugins/gps_input.cpp index 3b15f7cae..06a971b97 100644 --- a/mavros_extras/src/plugins/gps_input.cpp +++ b/mavros_extras/src/plugins/gps_input.cpp @@ -43,7 +43,7 @@ class GpsInputPlugin : public plugin::Plugin { enable_node_watch_parameters(); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "gps_rate", 5.0, [&](const rclcpp::Parameter & p) { rclcpp::Rate rate(p.as_double()); diff --git a/mavros_extras/src/plugins/landing_target.cpp b/mavros_extras/src/plugins/landing_target.cpp index 07a6da1d6..fbb6311da 100644 --- a/mavros_extras/src/plugins/landing_target.cpp +++ b/mavros_extras/src/plugins/landing_target.cpp @@ -69,12 +69,12 @@ class LandingTargetPlugin : public plugin::Plugin, enable_node_watch_parameters(); // general params - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "frame_id", "landing_target_1", [&](const rclcpp::Parameter & p) { frame_id = p.as_string(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "listen_lt", false, [&](const rclcpp::Parameter & p) { auto listen_lt = p.as_bool(); @@ -88,14 +88,14 @@ class LandingTargetPlugin : public plugin::Plugin, } }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "mav_frame", "LOCAL_NED", [&](const rclcpp::Parameter & p) { mav_frame = p.as_string(); frame = utils::mav_frame_from_str(mav_frame); // MAV_FRAME index based on given frame name (If unknown, defaults to GENERIC) }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "land_target_type", "VISION_FIDUCIAL", [&](const rclcpp::Parameter & p) { land_target_type = p.as_string(); type = utils::landing_target_type_from_str(land_target_type); @@ -103,68 +103,68 @@ class LandingTargetPlugin : public plugin::Plugin, }); // target size - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "target_size.x", 1.0, [&](const rclcpp::Parameter & p) { target_size_x = p.as_double(); // [meters] }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "target_size.y", 1.0, [&](const rclcpp::Parameter & p) { target_size_y = p.as_double(); }); // image size - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "image.width", 640, [&](const rclcpp::Parameter & p) { image_width = p.as_int(); // [pixels] }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "image.height", 480, [&](const rclcpp::Parameter & p) { image_height = p.as_int(); }); // camera field-of-view -> should be precised using the calibrated camera intrinsics - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "camera.fov_x", 2.0071286398, [&](const rclcpp::Parameter & p) { fov_x = p.as_double(); // default: 115 degrees in [radians] }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "camera.fov_y", 2.0071286398, [&](const rclcpp::Parameter & p) { fov_y = p.as_double(); // default: 115 degrees in [radians] }); // camera focal length - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "camera.focal_length", 2.8, [&](const rclcpp::Parameter & p) { focal_length = p.as_double(); // ex: OpenMV Cam M7: 2.8 [mm] }); // tf subsection - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "tf.rate_limit", 50.0, [&](const rclcpp::Parameter & p) { // no dynamic update here yet. need to modify the thread in // setpoint_mixin to handle new rates tf_rate = p.as_double(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "tf.send", true, [&](const rclcpp::Parameter & p) { tf_send = p.as_bool(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "tf.frame_id", frame_id, [&](const rclcpp::Parameter & p) { tf_frame_id = p.as_string(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "tf.child_frame_id", "camera_center", [&](const rclcpp::Parameter & p) { tf_child_frame_id = p.as_string(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "tf.listen", false, [&](const rclcpp::Parameter & p) { tf_listen = p.as_bool(); if (!tf_listen) { diff --git a/mavros_extras/src/plugins/obstacle_distance.cpp b/mavros_extras/src/plugins/obstacle_distance.cpp index 8bfc7a816..b204eb7b4 100644 --- a/mavros_extras/src/plugins/obstacle_distance.cpp +++ b/mavros_extras/src/plugins/obstacle_distance.cpp @@ -50,7 +50,7 @@ class ObstacleDistancePlugin : public plugin::Plugin { enable_node_watch_parameters(); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "mav_frame", "GLOBAL", [&](const rclcpp::Parameter & p) { auto mav_frame = p.as_string(); frame = utils::mav_frame_from_str(mav_frame); diff --git a/mavros_extras/src/plugins/odom.cpp b/mavros_extras/src/plugins/odom.cpp index fd319dcdc..00ddf8f04 100644 --- a/mavros_extras/src/plugins/odom.cpp +++ b/mavros_extras/src/plugins/odom.cpp @@ -64,11 +64,11 @@ class OdometryPlugin : public plugin::Plugin enable_node_watch_parameters(); // frame params: - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "fcu.odom_parent_id_des", "map", [&](const rclcpp::Parameter & p) { fcu_odom_parent_id_des = p.as_string(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "fcu.odom_child_id_des", "map", [&](const rclcpp::Parameter & p) { fcu_odom_child_id_des = p.as_string(); }); diff --git a/mavros_extras/src/plugins/px4flow.cpp b/mavros_extras/src/plugins/px4flow.cpp index 4cd929c4e..c0607ac60 100644 --- a/mavros_extras/src/plugins/px4flow.cpp +++ b/mavros_extras/src/plugins/px4flow.cpp @@ -50,7 +50,7 @@ class PX4FlowPlugin : public plugin::Plugin { enable_node_watch_parameters(); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "frame_id", "px4flow", [&](const rclcpp::Parameter & p) { frame_id = p.as_string(); }); @@ -61,17 +61,17 @@ class PX4FlowPlugin : public plugin::Plugin * but also at 1 meter). 6.8 degrees at 5 meters, 31 degrees * at 1 meter */ - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "ranger_fov", 0.119428926, [&](const rclcpp::Parameter & p) { ranger_fov = p.as_double(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "ranger_min_range", 0.3, [&](const rclcpp::Parameter & p) { ranger_min_range = p.as_double(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "ranger_max_range", 5.0, [&](const rclcpp::Parameter & p) { ranger_max_range = p.as_double(); }); diff --git a/mavros_extras/src/plugins/vibration.cpp b/mavros_extras/src/plugins/vibration.cpp index b69f8bc98..c345a4978 100644 --- a/mavros_extras/src/plugins/vibration.cpp +++ b/mavros_extras/src/plugins/vibration.cpp @@ -45,7 +45,7 @@ class VibrationPlugin : public plugin::Plugin { enable_node_watch_parameters(); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "frame_id", "base_link", [&](const rclcpp::Parameter & p) { frame_id = p.as_string(); }); diff --git a/mavros_extras/src/plugins/vision_pose_estimate.cpp b/mavros_extras/src/plugins/vision_pose_estimate.cpp index b46e757e1..f93c7a2d7 100644 --- a/mavros_extras/src/plugins/vision_pose_estimate.cpp +++ b/mavros_extras/src/plugins/vision_pose_estimate.cpp @@ -56,7 +56,7 @@ class VisionPoseEstimatePlugin : public plugin::Plugin, enable_node_watch_parameters(); // tf params - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "tf/listen", false, [&](const rclcpp::Parameter & p) { auto tf_listen = p.as_bool(); if (tf_listen) { @@ -75,17 +75,17 @@ class VisionPoseEstimatePlugin : public plugin::Plugin, } }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "tf/frame_id", "map", [&](const rclcpp::Parameter & p) { tf_frame_id = p.as_string(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "tf/child_frame_id", "vision_estimate", [&](const rclcpp::Parameter & p) { tf_child_frame_id = p.as_string(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "tf/rate_limit", 10.0, [&](const rclcpp::Parameter & p) { tf_rate = p.as_double(); }); diff --git a/mavros_extras/src/plugins/wheel_odometry.cpp b/mavros_extras/src/plugins/wheel_odometry.cpp index 78766d5d5..b85eb2a37 100644 --- a/mavros_extras/src/plugins/wheel_odometry.cpp +++ b/mavros_extras/src/plugins/wheel_odometry.cpp @@ -63,7 +63,7 @@ class WheelOdometryPlugin : public plugin::Plugin enable_node_watch_parameters(); // General params - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "send_raw", false, [&](const rclcpp::Parameter & p) { raw_send = p.as_bool(); @@ -76,13 +76,13 @@ class WheelOdometryPlugin : public plugin::Plugin } }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "count", 2, [&](const rclcpp::Parameter & p) { int count_ = p.as_int(); count = std::max(2, count_); // bound check }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "use_rpm", false, [&](const rclcpp::Parameter & p) { bool use_rpm = p.as_bool(); if (use_rpm) { @@ -93,39 +93,39 @@ class WheelOdometryPlugin : public plugin::Plugin }); // Odometry params - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "send_twist", false, [&](const rclcpp::Parameter & p) { twist_send = p.as_bool(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "frame_id", "odom", [&](const rclcpp::Parameter & p) { frame_id = p.as_string(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "child_frame_id", "base_link", [&](const rclcpp::Parameter & p) { frame_id = p.as_string(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "vel_error", 0.1, [&](const rclcpp::Parameter & p) { double vel_error = p.as_double(); vel_cov = vel_error * vel_error; // std -> cov }); // TF subsection - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "tf.frame_id", "odom", [&](const rclcpp::Parameter & p) { tf_frame_id = p.as_string(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "tf.child_frame_id", "base_link", [&](const rclcpp::Parameter & p) { tf_child_frame_id = p.as_string(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( "tf.send", false, [&](const rclcpp::Parameter & p) { tf_send = p.as_bool(); }); @@ -139,15 +139,15 @@ class WheelOdometryPlugin : public plugin::Plugin // Check if we have "wheelX" parameter. // Indices starts from 0 and should increase without gaps. - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( utils::format("wheel%i.x", wi), 0.0, [wi, this](const rclcpp::Parameter & p) { wheel_offset[wi][0] = p.as_double(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( utils::format("wheel%i.y", wi), 0.0, [wi, this](const rclcpp::Parameter & p) { wheel_offset[wi][1] = p.as_double(); }); - node_declate_and_watch_parameter( + node_declare_and_watch_parameter( utils::format("wheel%i.radius", wi), 0.05, [wi, this](const rclcpp::Parameter & p) { wheel_radius[wi] = p.as_double(); });