Skip to content

Commit

Permalink
fix usage of ROS namespaces
Browse files Browse the repository at this point in the history
  • Loading branch information
damien-robotsix authored and bkueng committed Jun 6, 2024
1 parent b11608e commit aced150
Show file tree
Hide file tree
Showing 28 changed files with 69 additions and 66 deletions.
3 changes: 3 additions & 0 deletions .vscode/settings.json
Original file line number Diff line number Diff line change
@@ -0,0 +1,3 @@
{
"editor.formatOnSave": false
}
2 changes: 1 addition & 1 deletion README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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");
}
```
Expand Down
2 changes: 1 addition & 1 deletion examples/cpp/modes/rtl_replacement/include/mode.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ class FlightModeTest : public px4_ros2::ModeBase
: ModeBase(node, Settings{kName, true, ModeBase::kModeIDRtl})
{
_vehicle_land_detected_sub = node.create_subscription<px4_msgs::msg::VehicleLandDetected>(
"/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;
});
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -10,31 +10,31 @@ using namespace std::chrono_literals; // NOLINT

// Set of all messages used by the library (<topic_name>[, <topic_type>])
#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
Expand All @@ -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
};

Expand Down
4 changes: 2 additions & 2 deletions px4_ros2_cpp/src/components/health_and_arming_checks.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -21,10 +21,10 @@ HealthAndArmingChecks::HealthAndArmingChecks(
_check_callback(std::move(check_callback))
{
_arming_check_reply_pub = _node.create_publisher<px4_msgs::msg::ArmingCheckReply>(
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<px4_msgs::msg::ArmingCheckRequest>(
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) {

Expand Down
2 changes: 1 addition & 1 deletion px4_ros2_cpp/src/components/manual_control_input.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@ ManualControlInput::ManualControlInput(Context & context, bool is_optional)

_manual_control_setpoint_sub =
context.node().create_subscription<px4_msgs::msg::ManualControlSetpoint>(
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;
Expand Down
4 changes: 2 additions & 2 deletions px4_ros2_cpp/src/components/message_compatibility_check.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -229,13 +229,13 @@ bool messageCompatibilityCheck(
message_format_response_sub
=
node.create_subscription<px4_msgs::msg::MessageFormatResponse>(
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<px4_msgs::msg::MessageFormatRequest>::SharedPtr message_format_request_pub
=
node.create_publisher<px4_msgs::msg::MessageFormatRequest>(
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()) {
Expand Down
6 changes: 3 additions & 3 deletions px4_ros2_cpp/src/components/mode.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -27,16 +27,16 @@ ModeBase::ModeBase(
topic_namespace_prefix), _config_overrides(node, topic_namespace_prefix)
{
_vehicle_status_sub = node.create_subscription<px4_msgs::msg::VehicleStatus>(
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<px4_msgs::msg::ModeCompleted>(
topic_namespace_prefix + "/fmu/in/mode_completed", 1);
topic_namespace_prefix + "fmu/in/mode_completed", 1);
_config_control_setpoints_pub = node.create_publisher<px4_msgs::msg::VehicleControlMode>(
topic_namespace_prefix + "/fmu/in/config_control_setpoints", 1);
topic_namespace_prefix + "fmu/in/config_control_setpoints", 1);
}

ModeBase::ModeID ModeBase::id() const
Expand Down
8 changes: 4 additions & 4 deletions px4_ros2_cpp/src/components/mode_executor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -25,18 +25,18 @@ ModeExecutorBase::ModeExecutorBase(
_config_overrides(node, topic_namespace_prefix)
{
_vehicle_status_sub = _node.create_subscription<px4_msgs::msg::VehicleStatus>(
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);
}
});

_vehicle_command_pub = _node.create_publisher<px4_msgs::msg::VehicleCommand>(
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<px4_msgs::msg::VehicleCommandAck>(
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) {});
}

Expand Down Expand Up @@ -397,7 +397,7 @@ ModeExecutorBase::ScheduledMode::ScheduledMode(
const std::string & topic_namespace_prefix)
{
_mode_completed_sub = node.create_subscription<px4_msgs::msg::ModeCompleted>(
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<uint8_t>(_mode_id)) {
RCLCPP_DEBUG(
Expand Down
2 changes: 1 addition & 1 deletion px4_ros2_cpp/src/components/overrides.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@ ConfigOverrides::ConfigOverrides(rclcpp::Node & node, const std::string & topic_
: _node(node)
{
_config_overrides_pub = _node.create_publisher<px4_msgs::msg::ConfigOverrides>(
topic_namespace_prefix + "/fmu/in/config_overrides_request", 1);
topic_namespace_prefix + "fmu/in/config_overrides_request", 1);
}

void ConfigOverrides::controlAutoDisarm(bool enabled)
Expand Down
6 changes: 3 additions & 3 deletions px4_ros2_cpp/src/components/registration.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -18,17 +18,17 @@ Registration::Registration(rclcpp::Node & node, const std::string & topic_namesp
{
_register_ext_component_reply_sub =
node.create_subscription<px4_msgs::msg::RegisterExtComponentReply>(
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<px4_msgs::msg::RegisterExtComponentRequest>(
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<px4_msgs::msg::UnregisterExtComponent>(
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;
}
Expand Down
2 changes: 1 addition & 1 deletion px4_ros2_cpp/src/components/wait_for_fmu.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@ bool waitForFMU(
RCLCPP_INFO(node.get_logger(), "Waiting for FMU...");
const rclcpp::Subscription<px4_msgs::msg::VehicleStatus>::SharedPtr vehicle_status_sub =
node.create_subscription<px4_msgs::msg::VehicleStatus>(
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;
Expand Down
2 changes: 1 addition & 1 deletion px4_ros2_cpp/src/control/peripheral_actuators.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@ PeripheralActuatorControls::PeripheralActuatorControls(Context & context)
: _node(context.node())
{
_vehicle_command_pub = _node.create_publisher<px4_msgs::msg::VehicleCommand>(
context.topicNamespacePrefix() + "/fmu/in/vehicle_command", 1);
context.topicNamespacePrefix() + "fmu/in/vehicle_command", 1);
_last_update = _node.get_clock()->now();
}

Expand Down
4 changes: 2 additions & 2 deletions px4_ros2_cpp/src/control/setpoint_types/direct_actuators.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -13,9 +13,9 @@ DirectActuatorsSetpointType::DirectActuatorsSetpointType(Context & context)
: SetpointBase(context), _node(context.node())
{
_actuator_motors_pub = context.node().create_publisher<px4_msgs::msg::ActuatorMotors>(
context.topicNamespacePrefix() + "/fmu/in/actuator_motors", 1);
context.topicNamespacePrefix() + "fmu/in/actuator_motors", 1);
_actuator_servos_pub = context.node().create_publisher<px4_msgs::msg::ActuatorServos>(
context.topicNamespacePrefix() + "/fmu/in/actuator_servos", 1);
context.topicNamespacePrefix() + "fmu/in/actuator_servos", 1);
}

void DirectActuatorsSetpointType::updateMotors(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ AttitudeSetpointType::AttitudeSetpointType(Context & context)
{
_vehicle_attitude_setpoint_pub =
context.node().create_publisher<px4_msgs::msg::VehicleAttitudeSetpoint>(
context.topicNamespacePrefix() + "/fmu/in/vehicle_attitude_setpoint", 1);
context.topicNamespacePrefix() + "fmu/in/vehicle_attitude_setpoint", 1);
}

void AttitudeSetpointType::update(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@ RatesSetpointType::RatesSetpointType(Context & context)
{
_vehicle_rates_setpoint_pub =
context.node().create_publisher<px4_msgs::msg::VehicleRatesSetpoint>(
context.topicNamespacePrefix() + "/fmu/in/vehicle_rates_setpoint", 1);
context.topicNamespacePrefix() + "fmu/in/vehicle_rates_setpoint", 1);
}

void RatesSetpointType::update(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@ TrajectorySetpointType::TrajectorySetpointType(Context & context)
: SetpointBase(context), _node(context.node())
{
_trajectory_setpoint_pub = context.node().create_publisher<px4_msgs::msg::TrajectorySetpoint>(
context.topicNamespacePrefix() + "/fmu/in/trajectory_setpoint", 1);
context.topicNamespacePrefix() + "fmu/in/trajectory_setpoint", 1);
}

void TrajectorySetpointType::update(
Expand Down
4 changes: 2 additions & 2 deletions px4_ros2_cpp/src/control/setpoint_types/goto.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@ GotoSetpointType::GotoSetpointType(Context & context)
{
_goto_setpoint_pub =
context.node().create_publisher<px4_msgs::msg::GotoSetpoint>(
context.topicNamespacePrefix() + "/fmu/in/goto_setpoint", 1);
context.topicNamespacePrefix() + "fmu/in/goto_setpoint", 1);
}

void GotoSetpointType::update(
Expand Down Expand Up @@ -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;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@ GlobalPositionMeasurementInterface::GlobalPositionMeasurementInterface(rclcpp::N
{
_aux_global_position_pub =
node.create_publisher<VehicleGlobalPosition>(
topicNamespacePrefix() + "/fmu/in/aux_global_position", 10);
topicNamespacePrefix() + "fmu/in/aux_global_position", 10);
}

void GlobalPositionMeasurementInterface::update(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ LocalPositionMeasurementInterface::LocalPositionMeasurementInterface(
_velocity_frame(velocityFrameToMessageFrame(velocity_frame))
{
_aux_local_position_pub = node.create_publisher<AuxLocalPosition>(
topicNamespacePrefix() + "/fmu/in/vehicle_visual_odometry", 10);
topicNamespacePrefix() + "fmu/in/vehicle_visual_odometry", 10);
}

void LocalPositionMeasurementInterface::update(
Expand Down
2 changes: 1 addition & 1 deletion px4_ros2_cpp/src/odometry/attitude.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@ namespace px4_ros2
{

OdometryAttitude::OdometryAttitude(Context & context)
: Subscription<px4_msgs::msg::VehicleAttitude>(context, "/fmu/out/vehicle_attitude")
: Subscription<px4_msgs::msg::VehicleAttitude>(context, "fmu/out/vehicle_attitude")
{
RequirementFlags requirements{};
requirements.attitude = true;
Expand Down
2 changes: 1 addition & 1 deletion px4_ros2_cpp/src/odometry/global_position.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@ namespace px4_ros2
{

OdometryGlobalPosition::OdometryGlobalPosition(Context & context)
: Subscription<px4_msgs::msg::VehicleGlobalPosition>(context, "/fmu/out/vehicle_global_position")
: Subscription<px4_msgs::msg::VehicleGlobalPosition>(context, "fmu/out/vehicle_global_position")
{
RequirementFlags requirements{};
requirements.global_position = true;
Expand Down
2 changes: 1 addition & 1 deletion px4_ros2_cpp/src/odometry/local_position.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@ namespace px4_ros2
{

OdometryLocalPosition::OdometryLocalPosition(Context & context)
: Subscription<px4_msgs::msg::VehicleLocalPosition>(context, "/fmu/out/vehicle_local_position")
: Subscription<px4_msgs::msg::VehicleLocalPosition>(context, "fmu/out/vehicle_local_position")
{
RequirementFlags requirements{};
requirements.local_position = true;
Expand Down
2 changes: 1 addition & 1 deletion px4_ros2_cpp/src/utils/geodesic.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ MapProjection::MapProjection(Context & context)
{
_map_projection_math = std::make_unique<MapProjectionImpl>();
_vehicle_local_position_sub = _node.create_subscription<px4_msgs::msg::VehicleLocalPosition>(
"/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));
});
Expand Down
4 changes: 2 additions & 2 deletions px4_ros2_cpp/test/integration/global_navigation.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ class GlobalPositionInterfaceTest : public BaseTest

// Subscribe to PX4 EKF estimator status flags
_subscriber = _node->create_subscription<EstimatorStatusFlags>(
"/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);
});
Expand Down Expand Up @@ -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;
Expand Down
4 changes: 2 additions & 2 deletions px4_ros2_cpp/test/integration/local_navigation.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@ class LocalPositionInterfaceTest : public BaseTest

// Subscribe to PX4 EKF estimator status flags
_subscriber = _node->create_subscription<EstimatorStatusFlags>(
"/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);
});
Expand Down Expand Up @@ -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;
Expand Down
4 changes: 2 additions & 2 deletions px4_ros2_cpp/test/integration/util.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@ VehicleState::VehicleState(rclcpp::Node & node, const std::string & topic_namesp
: _node(node)
{
_vehicle_status_sub = _node.create_subscription<px4_msgs::msg::VehicleStatus>(
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);
Expand All @@ -43,7 +43,7 @@ VehicleState::VehicleState(rclcpp::Node & node, const std::string & topic_namesp
});

_vehicle_command_pub = _node.create_publisher<px4_msgs::msg::VehicleCommand>(
topic_namespace_prefix + "/fmu/in/vehicle_command", 1);
topic_namespace_prefix + "fmu/in/vehicle_command", 1);

}

Expand Down
2 changes: 1 addition & 1 deletion scripts/check-used-topics.py
Original file line number Diff line number Diff line change
Expand Up @@ -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))

Expand Down

0 comments on commit aced150

Please sign in to comment.