Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
7 changes: 7 additions & 0 deletions mavros/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -85,6 +85,13 @@ if(rclcpp_VERSION VERSION_LESS 17.0.0)
)
endif()

if(message_filters_VERSION VERSION_LESS 5.0.0)
# Jazzy and older
add_definitions(
-DUSE_OLD_RMW_QOS_MESSAGE_FILTERS
)
endif()

# [[[cog:
# import mavros_cog
# ]]]
Expand Down
25 changes: 18 additions & 7 deletions mavros/src/plugins/setpoint_attitude.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -16,10 +16,15 @@
* @{
*/

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#if __has_include(<message_filters/subscriber.hpp>)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hm, looks interesting, haven't seen that macro...

#include <message_filters/subscriber.hpp>
#include <message_filters/synchronizer.hpp>
#include <message_filters/sync_policies/approximate_time.hpp>
#else
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#endif
#include <memory>

#include "tf2_eigen/tf2_eigen.hpp"
Expand Down Expand Up @@ -64,6 +69,12 @@ class SetpointAttitudePlugin : public plugin::Plugin,

auto qos = rclcpp::QoS(10);

#ifdef USE_OLD_RMW_QOS_MESSAGE_FILTERS
auto subscriber_qos = qos.get_rmw_qos_profile();
#else
auto subscriber_qos = qos;
#endif

node_declare_and_watch_parameter(
"reverse_thrust", false, [&](const rclcpp::Parameter & p) {
reverse_thrust = p.as_bool();
Expand All @@ -82,13 +93,13 @@ class SetpointAttitudePlugin : public plugin::Plugin,
/**
* @brief Use message_filters to sync attitude and thrust msg coming from different topics
*/
pose_sub.subscribe(node, "~/attitude", qos.get_rmw_qos_profile());
pose_sub.subscribe(node, "~/attitude", subscriber_qos);

sync_pose = std::make_unique<SyncPoseThrust>(SyncPoseThrustPolicy(10), pose_sub, th_sub);
sync_pose->registerCallback(&SetpointAttitudePlugin::attitude_pose_cb, this);

} else {
twist_sub.subscribe(node, "~/cmd_vel", qos.get_rmw_qos_profile());
twist_sub.subscribe(node, "~/cmd_vel", subscriber_qos);

sync_twist =
std::make_unique<SyncTwistThrust>(SyncTwistThrustPolicy(10), twist_sub, th_sub);
Expand All @@ -97,7 +108,7 @@ class SetpointAttitudePlugin : public plugin::Plugin,
});

// thrust msg subscriber to sync
th_sub.subscribe(node, "~/thrust", qos.get_rmw_qos_profile());
th_sub.subscribe(node, "~/thrust", subscriber_qos);
}

Subscriptions get_subscriptions() override
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,11 @@
#define MAVROS_EXTRAS__SERVO_STATE_PUBLISHER_HPP_

#include <yaml-cpp/yaml.h>
#include <urdf/model.h>
#if __has_include(<urdf/model.hpp>)
#include <urdf/model.hpp>
#else
#include <urdf/model.h>
#endif

#include <algorithm>
#include <memory>
Expand Down
Loading