From aced150c31449eadba74e0e4e432c70ccbf8aac7 Mon Sep 17 00:00:00 2001 From: Damien SIX Date: Tue, 4 Jun 2024 09:38:22 +0200 Subject: [PATCH] fix usage of ROS namespaces --- .vscode/settings.json | 3 ++ README.md | 2 +- .../modes/rtl_replacement/include/mode.hpp | 2 +- .../message_compatibility_check.hpp | 52 +++++++++---------- .../components/health_and_arming_checks.cpp | 4 +- .../src/components/manual_control_input.cpp | 2 +- .../message_compatibility_check.cpp | 4 +- px4_ros2_cpp/src/components/mode.cpp | 6 +-- px4_ros2_cpp/src/components/mode_executor.cpp | 8 +-- px4_ros2_cpp/src/components/overrides.cpp | 2 +- px4_ros2_cpp/src/components/registration.cpp | 6 +-- px4_ros2_cpp/src/components/wait_for_fmu.cpp | 2 +- .../src/control/peripheral_actuators.cpp | 2 +- .../setpoint_types/direct_actuators.cpp | 4 +- .../setpoint_types/experimental/attitude.cpp | 2 +- .../setpoint_types/experimental/rates.cpp | 2 +- .../experimental/trajectory.cpp | 2 +- .../src/control/setpoint_types/goto.cpp | 4 +- .../global_position_measurement_interface.cpp | 2 +- .../local_position_measurement_interface.cpp | 2 +- px4_ros2_cpp/src/odometry/attitude.cpp | 2 +- px4_ros2_cpp/src/odometry/global_position.cpp | 2 +- px4_ros2_cpp/src/odometry/local_position.cpp | 2 +- px4_ros2_cpp/src/utils/geodesic.cpp | 2 +- .../test/integration/global_navigation.cpp | 4 +- .../test/integration/local_navigation.cpp | 4 +- px4_ros2_cpp/test/integration/util.cpp | 4 +- scripts/check-used-topics.py | 2 +- 28 files changed, 69 insertions(+), 66 deletions(-) create mode 100644 .vscode/settings.json diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 00000000..97924983 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "editor.formatOnSave": false +} \ No newline at end of file diff --git a/README.md b/README.md index 944db6f8..bd251653 100644 --- a/README.md +++ b/README.md @@ -15,7 +15,7 @@ Compatibility is only guaranteed if using latest `main` on the PX4 and px4_ros2/ The library checks for message compatibility on startup when registering a mode. `ALL_PX4_ROS2_MESSAGES` defines the set of checked messages. If you use other messages, you can check them using: ```cpp -if (!px4_ros2::messageCompatibilityCheck(node, {{"/fmu/in/vehicle_rates_setpoint"}})) { +if (!px4_ros2::messageCompatibilityCheck(node, {{"fmu/in/vehicle_rates_setpoint"}})) { throw std::runtime_error("Messages incompatible"); } ``` diff --git a/examples/cpp/modes/rtl_replacement/include/mode.hpp b/examples/cpp/modes/rtl_replacement/include/mode.hpp index f0c78654..ab36a2d1 100644 --- a/examples/cpp/modes/rtl_replacement/include/mode.hpp +++ b/examples/cpp/modes/rtl_replacement/include/mode.hpp @@ -25,7 +25,7 @@ class FlightModeTest : public px4_ros2::ModeBase : ModeBase(node, Settings{kName, true, ModeBase::kModeIDRtl}) { _vehicle_land_detected_sub = node.create_subscription( - "/fmu/out/vehicle_land_detected", rclcpp::QoS(1).best_effort(), + "fmu/out/vehicle_land_detected", rclcpp::QoS(1).best_effort(), [this](px4_msgs::msg::VehicleLandDetected::UniquePtr msg) { _landed = msg->landed; }); diff --git a/px4_ros2_cpp/include/px4_ros2/components/message_compatibility_check.hpp b/px4_ros2_cpp/include/px4_ros2/components/message_compatibility_check.hpp index d309240f..3299bba9 100644 --- a/px4_ros2_cpp/include/px4_ros2/components/message_compatibility_check.hpp +++ b/px4_ros2_cpp/include/px4_ros2/components/message_compatibility_check.hpp @@ -10,31 +10,31 @@ using namespace std::chrono_literals; // NOLINT // Set of all messages used by the library ([, ]) #define ALL_PX4_ROS2_MESSAGES \ - {"/fmu/in/actuator_motors"}, \ - {"/fmu/in/actuator_servos"}, \ - {"/fmu/in/arming_check_reply"}, \ - {"/fmu/in/aux_global_position", "VehicleGlobalPosition"}, \ - {"/fmu/in/config_control_setpoints", "VehicleControlMode"}, \ - {"/fmu/in/config_overrides_request", "ConfigOverrides"}, \ - {"/fmu/in/goto_setpoint"}, \ - {"/fmu/in/mode_completed"}, \ - {"/fmu/in/register_ext_component_request"}, \ - {"/fmu/in/trajectory_setpoint"}, \ - {"/fmu/in/unregister_ext_component"}, \ - {"/fmu/in/vehicle_attitude_setpoint"}, \ - {"/fmu/in/vehicle_command"}, \ - {"/fmu/in/vehicle_command_mode_executor", "VehicleCommand"}, \ - {"/fmu/in/vehicle_rates_setpoint"}, \ - {"/fmu/in/vehicle_visual_odometry", "VehicleOdometry"}, \ - {"/fmu/out/arming_check_request"}, \ - {"/fmu/out/manual_control_setpoint"}, \ - {"/fmu/out/mode_completed"}, \ - {"/fmu/out/register_ext_component_reply"}, \ - {"/fmu/out/vehicle_attitude"}, \ - {"/fmu/out/vehicle_command_ack"}, \ - {"/fmu/out/vehicle_global_position"}, \ - {"/fmu/out/vehicle_local_position"}, \ - {"/fmu/out/vehicle_status"} + {"fmu/in/actuator_motors"}, \ + {"fmu/in/actuator_servos"}, \ + {"fmu/in/arming_check_reply"}, \ + {"fmu/in/aux_global_position", "VehicleGlobalPosition"}, \ + {"fmu/in/config_control_setpoints", "VehicleControlMode"}, \ + {"fmu/in/config_overrides_request", "ConfigOverrides"}, \ + {"fmu/in/goto_setpoint"}, \ + {"fmu/in/mode_completed"}, \ + {"fmu/in/register_ext_component_request"}, \ + {"fmu/in/trajectory_setpoint"}, \ + {"fmu/in/unregister_ext_component"}, \ + {"fmu/in/vehicle_attitude_setpoint"}, \ + {"fmu/in/vehicle_command"}, \ + {"fmu/in/vehicle_command_mode_executor", "VehicleCommand"}, \ + {"fmu/in/vehicle_rates_setpoint"}, \ + {"fmu/in/vehicle_visual_odometry", "VehicleOdometry"}, \ + {"fmu/out/arming_check_request"}, \ + {"fmu/out/manual_control_setpoint"}, \ + {"fmu/out/mode_completed"}, \ + {"fmu/out/register_ext_component_reply"}, \ + {"fmu/out/vehicle_attitude"}, \ + {"fmu/out/vehicle_command_ack"}, \ + {"fmu/out/vehicle_global_position"}, \ + {"fmu/out/vehicle_local_position"}, \ + {"fmu/out/vehicle_status"} namespace px4_ros2 @@ -45,7 +45,7 @@ namespace px4_ros2 struct MessageCompatibilityTopic { - std::string topic_name; ///< e.g. "/fmu/out/vehicle_status" + std::string topic_name; ///< e.g. "fmu/out/vehicle_status" std::string topic_type{""}; ///< e.g. VehicleStatus. If empty, it's inferred from the topic_name // NOLINT }; diff --git a/px4_ros2_cpp/src/components/health_and_arming_checks.cpp b/px4_ros2_cpp/src/components/health_and_arming_checks.cpp index 4a6f9595..2dc68c3b 100644 --- a/px4_ros2_cpp/src/components/health_and_arming_checks.cpp +++ b/px4_ros2_cpp/src/components/health_and_arming_checks.cpp @@ -21,10 +21,10 @@ HealthAndArmingChecks::HealthAndArmingChecks( _check_callback(std::move(check_callback)) { _arming_check_reply_pub = _node.create_publisher( - topic_namespace_prefix + "/fmu/in/arming_check_reply", 1); + topic_namespace_prefix + "fmu/in/arming_check_reply", 1); _arming_check_request_sub = _node.create_subscription( - topic_namespace_prefix + "/fmu/out/arming_check_request", + topic_namespace_prefix + "fmu/out/arming_check_request", rclcpp::QoS(1).best_effort(), [this](px4_msgs::msg::ArmingCheckRequest::UniquePtr msg) { diff --git a/px4_ros2_cpp/src/components/manual_control_input.cpp b/px4_ros2_cpp/src/components/manual_control_input.cpp index 3e94133b..8af4a075 100644 --- a/px4_ros2_cpp/src/components/manual_control_input.cpp +++ b/px4_ros2_cpp/src/components/manual_control_input.cpp @@ -16,7 +16,7 @@ ManualControlInput::ManualControlInput(Context & context, bool is_optional) _manual_control_setpoint_sub = context.node().create_subscription( - context.topicNamespacePrefix() + "/fmu/out/manual_control_setpoint", rclcpp::QoS( + context.topicNamespacePrefix() + "fmu/out/manual_control_setpoint", rclcpp::QoS( 1).best_effort(), [this](px4_msgs::msg::ManualControlSetpoint::UniquePtr msg) { _manual_control_setpoint = *msg; diff --git a/px4_ros2_cpp/src/components/message_compatibility_check.cpp b/px4_ros2_cpp/src/components/message_compatibility_check.cpp index 1b9eb80d..d7f7168e 100644 --- a/px4_ros2_cpp/src/components/message_compatibility_check.cpp +++ b/px4_ros2_cpp/src/components/message_compatibility_check.cpp @@ -229,13 +229,13 @@ bool messageCompatibilityCheck( message_format_response_sub = node.create_subscription( - topic_namespace_prefix + "/fmu/out/message_format_response", rclcpp::QoS(1).best_effort(), + topic_namespace_prefix + "fmu/out/message_format_response", rclcpp::QoS(1).best_effort(), [](px4_msgs::msg::MessageFormatResponse::UniquePtr msg) {}); const rclcpp::Publisher::SharedPtr message_format_request_pub = node.create_publisher( - topic_namespace_prefix + "/fmu/in/message_format_request", 1); + topic_namespace_prefix + "fmu/in/message_format_request", 1); const std::string msgs_dir = ament_index_cpp::get_package_share_directory("px4_msgs"); if (msgs_dir.empty()) { diff --git a/px4_ros2_cpp/src/components/mode.cpp b/px4_ros2_cpp/src/components/mode.cpp index 6e2a258e..1ab8a070 100644 --- a/px4_ros2_cpp/src/components/mode.cpp +++ b/px4_ros2_cpp/src/components/mode.cpp @@ -27,16 +27,16 @@ ModeBase::ModeBase( topic_namespace_prefix), _config_overrides(node, topic_namespace_prefix) { _vehicle_status_sub = node.create_subscription( - topic_namespace_prefix + "/fmu/out/vehicle_status", rclcpp::QoS(1).best_effort(), + topic_namespace_prefix + "fmu/out/vehicle_status", rclcpp::QoS(1).best_effort(), [this](px4_msgs::msg::VehicleStatus::UniquePtr msg) { if (_registration->registered()) { vehicleStatusUpdated(msg); } }); _mode_completed_pub = node.create_publisher( - topic_namespace_prefix + "/fmu/in/mode_completed", 1); + topic_namespace_prefix + "fmu/in/mode_completed", 1); _config_control_setpoints_pub = node.create_publisher( - topic_namespace_prefix + "/fmu/in/config_control_setpoints", 1); + topic_namespace_prefix + "fmu/in/config_control_setpoints", 1); } ModeBase::ModeID ModeBase::id() const diff --git a/px4_ros2_cpp/src/components/mode_executor.cpp b/px4_ros2_cpp/src/components/mode_executor.cpp index 7f00f91e..be1ba50b 100644 --- a/px4_ros2_cpp/src/components/mode_executor.cpp +++ b/px4_ros2_cpp/src/components/mode_executor.cpp @@ -25,7 +25,7 @@ ModeExecutorBase::ModeExecutorBase( _config_overrides(node, topic_namespace_prefix) { _vehicle_status_sub = _node.create_subscription( - topic_namespace_prefix + "/fmu/out/vehicle_status", rclcpp::QoS(1).best_effort(), + topic_namespace_prefix + "fmu/out/vehicle_status", rclcpp::QoS(1).best_effort(), [this](px4_msgs::msg::VehicleStatus::UniquePtr msg) { if (_registration->registered()) { vehicleStatusUpdated(msg); @@ -33,10 +33,10 @@ ModeExecutorBase::ModeExecutorBase( }); _vehicle_command_pub = _node.create_publisher( - topic_namespace_prefix + "/fmu/in/vehicle_command_mode_executor", 1); + topic_namespace_prefix + "fmu/in/vehicle_command_mode_executor", 1); _vehicle_command_ack_sub = _node.create_subscription( - topic_namespace_prefix + "/fmu/out/vehicle_command_ack", rclcpp::QoS(1).best_effort(), + topic_namespace_prefix + "fmu/out/vehicle_command_ack", rclcpp::QoS(1).best_effort(), [](px4_msgs::msg::VehicleCommandAck::UniquePtr msg) {}); } @@ -397,7 +397,7 @@ ModeExecutorBase::ScheduledMode::ScheduledMode( const std::string & topic_namespace_prefix) { _mode_completed_sub = node.create_subscription( - topic_namespace_prefix + "/fmu/out/mode_completed", rclcpp::QoS(1).best_effort(), + topic_namespace_prefix + "fmu/out/mode_completed", rclcpp::QoS(1).best_effort(), [this, &node](px4_msgs::msg::ModeCompleted::UniquePtr msg) { if (active() && msg->nav_state == static_cast(_mode_id)) { RCLCPP_DEBUG( diff --git a/px4_ros2_cpp/src/components/overrides.cpp b/px4_ros2_cpp/src/components/overrides.cpp index 1c9a5071..6bf65e11 100644 --- a/px4_ros2_cpp/src/components/overrides.cpp +++ b/px4_ros2_cpp/src/components/overrides.cpp @@ -14,7 +14,7 @@ ConfigOverrides::ConfigOverrides(rclcpp::Node & node, const std::string & topic_ : _node(node) { _config_overrides_pub = _node.create_publisher( - topic_namespace_prefix + "/fmu/in/config_overrides_request", 1); + topic_namespace_prefix + "fmu/in/config_overrides_request", 1); } void ConfigOverrides::controlAutoDisarm(bool enabled) diff --git a/px4_ros2_cpp/src/components/registration.cpp b/px4_ros2_cpp/src/components/registration.cpp index d9ce1433..d7fd4ba2 100644 --- a/px4_ros2_cpp/src/components/registration.cpp +++ b/px4_ros2_cpp/src/components/registration.cpp @@ -18,17 +18,17 @@ Registration::Registration(rclcpp::Node & node, const std::string & topic_namesp { _register_ext_component_reply_sub = node.create_subscription( - topic_namespace_prefix + "/fmu/out/register_ext_component_reply", + topic_namespace_prefix + "fmu/out/register_ext_component_reply", rclcpp::QoS(1).best_effort(), [](px4_msgs::msg::RegisterExtComponentReply::UniquePtr msg) { }); _register_ext_component_request_pub = node.create_publisher( - topic_namespace_prefix + "/fmu/in/register_ext_component_request", 1); + topic_namespace_prefix + "fmu/in/register_ext_component_request", 1); _unregister_ext_component_pub = node.create_publisher( - topic_namespace_prefix + "/fmu/in/unregister_ext_component", 1); + topic_namespace_prefix + "fmu/in/unregister_ext_component", 1); _unregister_ext_component.mode_id = px4_ros2::ModeBase::kModeIDInvalid; } diff --git a/px4_ros2_cpp/src/components/wait_for_fmu.cpp b/px4_ros2_cpp/src/components/wait_for_fmu.cpp index 5a73ed1c..5994199f 100644 --- a/px4_ros2_cpp/src/components/wait_for_fmu.cpp +++ b/px4_ros2_cpp/src/components/wait_for_fmu.cpp @@ -16,7 +16,7 @@ bool waitForFMU( RCLCPP_INFO(node.get_logger(), "Waiting for FMU..."); const rclcpp::Subscription::SharedPtr vehicle_status_sub = node.create_subscription( - topic_namespace_prefix + "/fmu/out/vehicle_status", rclcpp::QoS(1).best_effort(), + topic_namespace_prefix + "fmu/out/vehicle_status", rclcpp::QoS(1).best_effort(), [](px4_msgs::msg::VehicleStatus::UniquePtr msg) {}); rclcpp::WaitSet wait_set; diff --git a/px4_ros2_cpp/src/control/peripheral_actuators.cpp b/px4_ros2_cpp/src/control/peripheral_actuators.cpp index fcb02f5e..fe6b7e70 100644 --- a/px4_ros2_cpp/src/control/peripheral_actuators.cpp +++ b/px4_ros2_cpp/src/control/peripheral_actuators.cpp @@ -14,7 +14,7 @@ PeripheralActuatorControls::PeripheralActuatorControls(Context & context) : _node(context.node()) { _vehicle_command_pub = _node.create_publisher( - context.topicNamespacePrefix() + "/fmu/in/vehicle_command", 1); + context.topicNamespacePrefix() + "fmu/in/vehicle_command", 1); _last_update = _node.get_clock()->now(); } diff --git a/px4_ros2_cpp/src/control/setpoint_types/direct_actuators.cpp b/px4_ros2_cpp/src/control/setpoint_types/direct_actuators.cpp index abcdbfed..8f42892f 100644 --- a/px4_ros2_cpp/src/control/setpoint_types/direct_actuators.cpp +++ b/px4_ros2_cpp/src/control/setpoint_types/direct_actuators.cpp @@ -13,9 +13,9 @@ DirectActuatorsSetpointType::DirectActuatorsSetpointType(Context & context) : SetpointBase(context), _node(context.node()) { _actuator_motors_pub = context.node().create_publisher( - context.topicNamespacePrefix() + "/fmu/in/actuator_motors", 1); + context.topicNamespacePrefix() + "fmu/in/actuator_motors", 1); _actuator_servos_pub = context.node().create_publisher( - context.topicNamespacePrefix() + "/fmu/in/actuator_servos", 1); + context.topicNamespacePrefix() + "fmu/in/actuator_servos", 1); } void DirectActuatorsSetpointType::updateMotors( diff --git a/px4_ros2_cpp/src/control/setpoint_types/experimental/attitude.cpp b/px4_ros2_cpp/src/control/setpoint_types/experimental/attitude.cpp index 85a137f3..82ca2531 100644 --- a/px4_ros2_cpp/src/control/setpoint_types/experimental/attitude.cpp +++ b/px4_ros2_cpp/src/control/setpoint_types/experimental/attitude.cpp @@ -15,7 +15,7 @@ AttitudeSetpointType::AttitudeSetpointType(Context & context) { _vehicle_attitude_setpoint_pub = context.node().create_publisher( - context.topicNamespacePrefix() + "/fmu/in/vehicle_attitude_setpoint", 1); + context.topicNamespacePrefix() + "fmu/in/vehicle_attitude_setpoint", 1); } void AttitudeSetpointType::update( diff --git a/px4_ros2_cpp/src/control/setpoint_types/experimental/rates.cpp b/px4_ros2_cpp/src/control/setpoint_types/experimental/rates.cpp index 312b368c..96d9bd08 100644 --- a/px4_ros2_cpp/src/control/setpoint_types/experimental/rates.cpp +++ b/px4_ros2_cpp/src/control/setpoint_types/experimental/rates.cpp @@ -14,7 +14,7 @@ RatesSetpointType::RatesSetpointType(Context & context) { _vehicle_rates_setpoint_pub = context.node().create_publisher( - context.topicNamespacePrefix() + "/fmu/in/vehicle_rates_setpoint", 1); + context.topicNamespacePrefix() + "fmu/in/vehicle_rates_setpoint", 1); } void RatesSetpointType::update( diff --git a/px4_ros2_cpp/src/control/setpoint_types/experimental/trajectory.cpp b/px4_ros2_cpp/src/control/setpoint_types/experimental/trajectory.cpp index 38811a5b..86c3fbb7 100644 --- a/px4_ros2_cpp/src/control/setpoint_types/experimental/trajectory.cpp +++ b/px4_ros2_cpp/src/control/setpoint_types/experimental/trajectory.cpp @@ -13,7 +13,7 @@ TrajectorySetpointType::TrajectorySetpointType(Context & context) : SetpointBase(context), _node(context.node()) { _trajectory_setpoint_pub = context.node().create_publisher( - context.topicNamespacePrefix() + "/fmu/in/trajectory_setpoint", 1); + context.topicNamespacePrefix() + "fmu/in/trajectory_setpoint", 1); } void TrajectorySetpointType::update( diff --git a/px4_ros2_cpp/src/control/setpoint_types/goto.cpp b/px4_ros2_cpp/src/control/setpoint_types/goto.cpp index 9d45f577..5af32578 100644 --- a/px4_ros2_cpp/src/control/setpoint_types/goto.cpp +++ b/px4_ros2_cpp/src/control/setpoint_types/goto.cpp @@ -14,7 +14,7 @@ GotoSetpointType::GotoSetpointType(Context & context) { _goto_setpoint_pub = context.node().create_publisher( - context.topicNamespacePrefix() + "/fmu/in/goto_setpoint", 1); + context.topicNamespacePrefix() + "fmu/in/goto_setpoint", 1); } void GotoSetpointType::update( @@ -84,7 +84,7 @@ void GotoGlobalSetpointType::update( if (!_map_projection->isInitialized()) { RCLCPP_ERROR_ONCE( _node.get_logger(), - "Goto global setpoint update failed: map projection is uninitialized. Is /fmu/out/vehicle_local_position published?"); + "Goto global setpoint update failed: map projection is uninitialized. Is fmu/out/vehicle_local_position published?"); return; } diff --git a/px4_ros2_cpp/src/navigation/experimental/global_position_measurement_interface.cpp b/px4_ros2_cpp/src/navigation/experimental/global_position_measurement_interface.cpp index 8c175d74..b440bb2d 100644 --- a/px4_ros2_cpp/src/navigation/experimental/global_position_measurement_interface.cpp +++ b/px4_ros2_cpp/src/navigation/experimental/global_position_measurement_interface.cpp @@ -16,7 +16,7 @@ GlobalPositionMeasurementInterface::GlobalPositionMeasurementInterface(rclcpp::N { _aux_global_position_pub = node.create_publisher( - topicNamespacePrefix() + "/fmu/in/aux_global_position", 10); + topicNamespacePrefix() + "fmu/in/aux_global_position", 10); } void GlobalPositionMeasurementInterface::update( diff --git a/px4_ros2_cpp/src/navigation/experimental/local_position_measurement_interface.cpp b/px4_ros2_cpp/src/navigation/experimental/local_position_measurement_interface.cpp index c63011d8..87266f75 100644 --- a/px4_ros2_cpp/src/navigation/experimental/local_position_measurement_interface.cpp +++ b/px4_ros2_cpp/src/navigation/experimental/local_position_measurement_interface.cpp @@ -19,7 +19,7 @@ LocalPositionMeasurementInterface::LocalPositionMeasurementInterface( _velocity_frame(velocityFrameToMessageFrame(velocity_frame)) { _aux_local_position_pub = node.create_publisher( - topicNamespacePrefix() + "/fmu/in/vehicle_visual_odometry", 10); + topicNamespacePrefix() + "fmu/in/vehicle_visual_odometry", 10); } void LocalPositionMeasurementInterface::update( diff --git a/px4_ros2_cpp/src/odometry/attitude.cpp b/px4_ros2_cpp/src/odometry/attitude.cpp index 4700d595..b5ccd188 100644 --- a/px4_ros2_cpp/src/odometry/attitude.cpp +++ b/px4_ros2_cpp/src/odometry/attitude.cpp @@ -9,7 +9,7 @@ namespace px4_ros2 { OdometryAttitude::OdometryAttitude(Context & context) -: Subscription(context, "/fmu/out/vehicle_attitude") +: Subscription(context, "fmu/out/vehicle_attitude") { RequirementFlags requirements{}; requirements.attitude = true; diff --git a/px4_ros2_cpp/src/odometry/global_position.cpp b/px4_ros2_cpp/src/odometry/global_position.cpp index aadba58c..118dd030 100644 --- a/px4_ros2_cpp/src/odometry/global_position.cpp +++ b/px4_ros2_cpp/src/odometry/global_position.cpp @@ -9,7 +9,7 @@ namespace px4_ros2 { OdometryGlobalPosition::OdometryGlobalPosition(Context & context) -: Subscription(context, "/fmu/out/vehicle_global_position") +: Subscription(context, "fmu/out/vehicle_global_position") { RequirementFlags requirements{}; requirements.global_position = true; diff --git a/px4_ros2_cpp/src/odometry/local_position.cpp b/px4_ros2_cpp/src/odometry/local_position.cpp index 5eb70b90..6f64c18d 100644 --- a/px4_ros2_cpp/src/odometry/local_position.cpp +++ b/px4_ros2_cpp/src/odometry/local_position.cpp @@ -9,7 +9,7 @@ namespace px4_ros2 { OdometryLocalPosition::OdometryLocalPosition(Context & context) -: Subscription(context, "/fmu/out/vehicle_local_position") +: Subscription(context, "fmu/out/vehicle_local_position") { RequirementFlags requirements{}; requirements.local_position = true; diff --git a/px4_ros2_cpp/src/utils/geodesic.cpp b/px4_ros2_cpp/src/utils/geodesic.cpp index 89bebad3..6389da79 100644 --- a/px4_ros2_cpp/src/utils/geodesic.cpp +++ b/px4_ros2_cpp/src/utils/geodesic.cpp @@ -15,7 +15,7 @@ MapProjection::MapProjection(Context & context) { _map_projection_math = std::make_unique(); _vehicle_local_position_sub = _node.create_subscription( - "/fmu/out/vehicle_local_position", rclcpp::QoS(1).best_effort(), + "fmu/out/vehicle_local_position", rclcpp::QoS(1).best_effort(), [this](px4_msgs::msg::VehicleLocalPosition::UniquePtr msg) { vehicleLocalPositionCallback(std::move(msg)); }); diff --git a/px4_ros2_cpp/test/integration/global_navigation.cpp b/px4_ros2_cpp/test/integration/global_navigation.cpp index d7de47ee..0153f8cc 100644 --- a/px4_ros2_cpp/test/integration/global_navigation.cpp +++ b/px4_ros2_cpp/test/integration/global_navigation.cpp @@ -35,7 +35,7 @@ class GlobalPositionInterfaceTest : public BaseTest // Subscribe to PX4 EKF estimator status flags _subscriber = _node->create_subscription( - "/fmu/out/estimator_status_flags", rclcpp::QoS(10).best_effort(), + "fmu/out/estimator_status_flags", rclcpp::QoS(10).best_effort(), [this](EstimatorStatusFlags::UniquePtr msg) { _estimator_status_flags = std::move(msg); }); @@ -84,7 +84,7 @@ class GlobalPositionInterfaceTest : public BaseTest if (elapsed_time >= kTimeoutDuration) { ASSERT_NE(_estimator_status_flags, nullptr) << - "Missing feedback from PX4: no estimator status flags published over /fmu/out/estimator_status_flags."; + "Missing feedback from PX4: no estimator status flags published over fmu/out/estimator_status_flags."; EXPECT_TRUE(is_fused_getter(_estimator_status_flags)) << "Position measurement update was not fused into the PX4 EKF."; break; diff --git a/px4_ros2_cpp/test/integration/local_navigation.cpp b/px4_ros2_cpp/test/integration/local_navigation.cpp index 2f291bda..f6fefc10 100644 --- a/px4_ros2_cpp/test/integration/local_navigation.cpp +++ b/px4_ros2_cpp/test/integration/local_navigation.cpp @@ -40,7 +40,7 @@ class LocalPositionInterfaceTest : public BaseTest // Subscribe to PX4 EKF estimator status flags _subscriber = _node->create_subscription( - "/fmu/out/estimator_status_flags", rclcpp::QoS(10).best_effort(), + "fmu/out/estimator_status_flags", rclcpp::QoS(10).best_effort(), [this](EstimatorStatusFlags::UniquePtr msg) { _estimator_status_flags = std::move(msg); }); @@ -91,7 +91,7 @@ class LocalPositionInterfaceTest : public BaseTest if (elapsed_time >= kTimeoutDuration) { ASSERT_NE(_estimator_status_flags, nullptr) << - "Missing feedback from PX4: no estimator status flags published over /fmu/out/estimator_status_flags."; + "Missing feedback from PX4: no estimator status flags published over fmu/out/estimator_status_flags."; EXPECT_TRUE(is_fused_getter(_estimator_status_flags)) << "Position measurement update was not fused into the PX4 EKF."; break; diff --git a/px4_ros2_cpp/test/integration/util.cpp b/px4_ros2_cpp/test/integration/util.cpp index 17c3ff9f..1505ef64 100644 --- a/px4_ros2_cpp/test/integration/util.cpp +++ b/px4_ros2_cpp/test/integration/util.cpp @@ -26,7 +26,7 @@ VehicleState::VehicleState(rclcpp::Node & node, const std::string & topic_namesp : _node(node) { _vehicle_status_sub = _node.create_subscription( - topic_namespace_prefix + "/fmu/out/vehicle_status", rclcpp::QoS(1).best_effort(), + topic_namespace_prefix + "fmu/out/vehicle_status", rclcpp::QoS(1).best_effort(), [this](px4_msgs::msg::VehicleStatus::UniquePtr msg) { if (_on_vehicle_status_update) { _on_vehicle_status_update(msg); @@ -43,7 +43,7 @@ VehicleState::VehicleState(rclcpp::Node & node, const std::string & topic_namesp }); _vehicle_command_pub = _node.create_publisher( - topic_namespace_prefix + "/fmu/in/vehicle_command", 1); + topic_namespace_prefix + "fmu/in/vehicle_command", 1); } diff --git a/scripts/check-used-topics.py b/scripts/check-used-topics.py index d837b21c..e3632aea 100755 --- a/scripts/check-used-topics.py +++ b/scripts/check-used-topics.py @@ -27,7 +27,7 @@ def extract_topics_from_file(filename: str, extract_start_after: str = None, ret = [] for line in file: - m = re.search(r'"/fmu/(in|out)/([^"]+)', line) + m = re.search(r'"fmu/(in|out)/([^"]+)', line) if m: ret.append(m.group(2))