Skip to content

Commit 0f86a2e

Browse files
ugol-1ugol-1
committed
Removed circular poiner dependencies created by plugins
1 parent 0a7661f commit 0f86a2e

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

54 files changed

+182
-42
lines changed

‎mavros/include/mavros/mission_protocol_base.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -634,6 +634,7 @@ class MissionBase : public plugin::Plugin
634634
std::is_same<MsgT, MISSION_ITEM>::value || std::is_same<MsgT,
635635
MISSION_ITEM_INT>::value, "wrong type");
636636

637+
auto uas = uas_.lock();
637638
uas->msg_set_target(wpi);
638639
uas->send_message(wpi);
639640
}

‎mavros/include/mavros/plugin.hpp

+7-6
Original file line numberDiff line numberDiff line change
@@ -74,10 +74,10 @@ class Plugin : public std::enable_shared_from_this<Plugin>
7474
//! Subscriptions vector
7575
using Subscriptions = std::vector<HandlerInfo>;
7676

77-
explicit Plugin(UASPtr uas_);
77+
explicit Plugin(UASPtr uas);
7878

7979
explicit Plugin(
80-
UASPtr uas_, const std::string & subnode,
80+
UASPtr uas, const std::string & subnode,
8181
const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
8282

8383
virtual ~Plugin() = default;
@@ -103,7 +103,7 @@ class Plugin : public std::enable_shared_from_this<Plugin>
103103
}
104104

105105
protected:
106-
UASPtr uas; // uas back link
106+
std::weak_ptr<UAS> uas_; // uas back link
107107
rclcpp::Node::SharedPtr node; // most of plugins uses sub-node
108108

109109
using SetParametersResult = rcl_interfaces::msg::SetParametersResult;
@@ -149,13 +149,14 @@ class Plugin : public std::enable_shared_from_this<Plugin>
149149
const auto id = _T::MSG_ID;
150150
const auto name = _T::NAME;
151151
const auto type_hash_ = typeid(_T).hash_code();
152-
auto uas_ = this->uas;
153152

154153
return HandlerInfo {
155154
id, name, type_hash_,
156-
[bfn, uas_](const mavlink::mavlink_message_t * msg, const mavconn::Framing framing) {
155+
[bfn, uas = this->uas_](const mavlink::mavlink_message_t * msg, const mavconn::Framing framing) {
157156
auto filter = _F();
158-
if (!filter(uas_, msg, framing)) {
157+
158+
auto uas_ptr = uas.lock();
159+
if (uas_ptr && !filter(uas_ptr, msg, framing)) {
159160
return;
160161
}
161162

‎mavros/include/mavros/setpoint_mixin.hpp

+15-14
Original file line numberDiff line numberDiff line change
@@ -54,10 +54,10 @@ class SetPositionTargetLocalNEDMixin
5454
std::is_base_of<plugin::Plugin, D>::value,
5555
"SetPositionTargetLocalNEDMixin should be used by mavros::plugin::Plugin child");
5656

57-
plugin::UASPtr uas_ = static_cast<D *>(this)->uas;
57+
auto uas = static_cast<D *>(this)->uas_.lock();
5858

5959
mavlink::common::msg::SET_POSITION_TARGET_LOCAL_NED sp = {};
60-
uas_->msg_set_target(sp);
60+
uas->msg_set_target(sp);
6161

6262
// [[[cog:
6363
// for f in ('time_boot_ms', 'coordinate_frame', 'type_mask', 'yaw', 'yaw_rate'):
@@ -82,7 +82,7 @@ class SetPositionTargetLocalNEDMixin
8282
sp.afz = af.z();
8383
// [[[end]]] (checksum: f8942bb13a7463a2cbadc9b745df25d0)
8484

85-
uas_->send_message(sp);
85+
uas->send_message(sp);
8686
}
8787
};
8888

@@ -106,10 +106,10 @@ class SetPositionTargetGlobalIntMixin
106106
std::is_base_of<plugin::Plugin, D>::value,
107107
"SetPositionTargetGlobalIntMixin should be used by mavros::plugin::Plugin child");
108108

109-
plugin::UASPtr uas_ = static_cast<D *>(this)->uas;
109+
plugin::UASPtr uas = static_cast<D *>(this)->uas_.lock();
110110

111111
mavlink::common::msg::SET_POSITION_TARGET_GLOBAL_INT sp = {};
112-
uas_->msg_set_target(sp);
112+
uas->msg_set_target(sp);
113113

114114
// [[[cog:
115115
// for f in ('time_boot_ms', 'coordinate_frame', 'type_mask',
@@ -135,7 +135,7 @@ class SetPositionTargetGlobalIntMixin
135135
sp.afz = af.z();
136136
// [[[end]]] (checksum: 82301ecd7936657d65e006cf7525e82a)
137137

138-
uas_->send_message(sp);
138+
uas->send_message(sp);
139139
}
140140
};
141141

@@ -158,11 +158,11 @@ class SetAttitudeTargetMixin
158158
std::is_base_of<plugin::Plugin, D>::value,
159159
"SetAttitudeTargetMixin should be used by mavros::plugin::Plugin child");
160160

161-
plugin::UASPtr uas_ = static_cast<D *>(this)->uas;
161+
plugin::UASPtr uas = static_cast<D *>(this)->uas_.lock();
162162

163163
mavlink::common::msg::SET_ATTITUDE_TARGET sp = {};
164164

165-
uas_->msg_set_target(sp);
165+
uas->msg_set_target(sp);
166166
mavros::ftf::quaternion_to_mavlink(orientation, sp.q);
167167

168168
// [[[cog:
@@ -179,7 +179,7 @@ class SetAttitudeTargetMixin
179179
sp.body_yaw_rate = body_rate.z();
180180
// [[[end]]] (checksum: d0910b0f92d233024163ebf957a3d642)
181181

182-
uas_->send_message(sp);
182+
uas->send_message(sp);
183183
}
184184
};
185185

@@ -211,28 +211,29 @@ class TF2ListenerMixin
211211
auto tf_transform_cb = std::bind(cbp, base, std::placeholders::_1);
212212

213213
auto timer_callback = [this, base, tf_transform_cb]() -> void {
214-
plugin::UASPtr _uas = base->uas;
214+
plugin::UASPtr uas = base->uas_.lock();
215215
std::string & _frame_id = base->tf_frame_id;
216216
std::string & _child_frame_id = base->tf_child_frame_id;
217-
if (_uas->tf2_buffer.canTransform(
217+
if (uas->tf2_buffer.canTransform(
218218
_frame_id, _child_frame_id, tf2::TimePoint(),
219219
tf2::durationFromSec(3.0)))
220220
{
221221
try {
222-
auto transform = _uas->tf2_buffer.lookupTransform(
222+
auto transform = uas->tf2_buffer.lookupTransform(
223223
_frame_id, _child_frame_id, tf2::TimePoint(), tf2::durationFromSec(3.0));
224224
tf_transform_cb(transform);
225225
} catch (tf2::LookupException & ex) {
226226
RCLCPP_ERROR(
227-
_uas->get_logger(), "%s: %s", tf_thd_name.c_str(),
227+
uas->get_logger(), "%s: %s", tf_thd_name.c_str(),
228228
ex.what());
229229
}
230230
}
231231
};
232232

233+
auto uas = base->uas_.lock();
233234
timer_ =
234235
create_timer(
235-
base->node, base->uas->get_clock(),
236+
base->node, uas->get_clock(),
236237
rclcpp::Duration::from_seconds(1.0 / base->tf_rate), timer_callback);
237238
}
238239

‎mavros/src/lib/plugin.cpp

+9-7
Original file line numberDiff line numberDiff line change
@@ -18,24 +18,25 @@
1818

1919
using mavros::plugin::Plugin;
2020

21-
Plugin::Plugin(UASPtr uas_)
22-
: uas(uas_), node(uas_)
21+
Plugin::Plugin(UASPtr uas)
22+
: uas_(uas), node(uas_)
2323
{}
2424

2525
Plugin::Plugin(
26-
UASPtr uas_, const std::string & subnode_name,
26+
UASPtr uas, const std::string & subnode_name,
2727
const rclcpp::NodeOptions & options)
28-
: uas(uas_)
28+
: uas_(uas)
2929
// node(std::dynamic_pointer_cast<rclcpp::Node>(uas_)->create_sub_node(subnode_name)) // https://github.com/ros2/rclcpp/issues/731
3030
{
31-
RCLCPP_INFO_STREAM(uas_->get_logger(), "Create subnode " << subnode_name
32-
<< ", uas_->get_fully_qualified_name() == " << uas_->get_fully_qualified_name());
31+
RCLCPP_INFO_STREAM(uas->get_logger(), "Create subnode " << subnode_name
32+
<< ", uas_->get_fully_qualified_name() == " << uas->get_fully_qualified_name());
3333
// node = uas_->create_sub_node(subnode_name);
34-
node = rclcpp::Node::make_shared(subnode_name, uas_->get_fully_qualified_name(), options);
34+
node = rclcpp::Node::make_shared(subnode_name, uas->get_fully_qualified_name(), options);
3535
}
3636

3737
void Plugin::enable_connection_cb()
3838
{
39+
auto uas = uas_.lock();
3940
uas->add_connection_change_handler(
4041
std::bind(
4142
&Plugin::connection_cb, this,
@@ -44,6 +45,7 @@ void Plugin::enable_connection_cb()
4445

4546
void Plugin::enable_capabilities_cb()
4647
{
48+
auto uas = uas_.lock();
4749
uas->add_capabilities_change_handler(
4850
std::bind(
4951
&Plugin::capabilities_cb, this,

‎mavros/src/plugins/actuator_control.cpp

+2
Original file line numberDiff line numberDiff line change
@@ -67,6 +67,7 @@ class ActuatorControlPlugin : public plugin::Plugin
6767
mavlink::common::msg::ACTUATOR_CONTROL_TARGET & act,
6868
plugin::filter::ComponentAndOk filter [[maybe_unused]])
6969
{
70+
auto uas = uas_.lock();
7071
auto ract = mavros_msgs::msg::ActuatorControl();
7172
ract.header.stamp = uas->synchronise_stamp(act.time_usec);
7273
ract.group_mix = act.group_mlx;
@@ -82,6 +83,7 @@ class ActuatorControlPlugin : public plugin::Plugin
8283
//! about groups, mixing and channels: @p https://pixhawk.org/dev/mixing
8384
//! message definiton here: @p https://mavlink.io/en/messages/common.html#SET_ACTUATOR_CONTROL_TARGET
8485
mavlink::common::msg::SET_ACTUATOR_CONTROL_TARGET act{};
86+
auto uas = uas_.lock();
8587

8688
act.time_usec = get_time_usec(req->header.stamp);
8789
act.group_mlx = req->group_mix;

‎mavros/src/plugins/altitude.cpp

+1
Original file line numberDiff line numberDiff line change
@@ -67,6 +67,7 @@ class AltitudePlugin : public plugin::Plugin
6767
const mavlink::mavlink_message_t * msg [[maybe_unused]],
6868
mavlink::common::msg::ALTITUDE & altitude, plugin::filter::SystemAndOk filter [[maybe_unused]])
6969
{
70+
auto uas = uas_.lock();
7071
auto ros_msg = mavros_msgs::msg::Altitude();
7172
ros_msg.header = uas->synchronized_header(frame_id, altitude.time_usec);
7273
ros_msg.monotonic = altitude.altitude_monotonic;

‎mavros/src/plugins/command.cpp

+4
Original file line numberDiff line numberDiff line change
@@ -248,6 +248,7 @@ class CommandPlugin : public plugin::Plugin
248248
* @note APM & PX4 master always send COMMAND_ACK. Old PX4 never.
249249
* Don't expect any ACK in broadcast mode.
250250
*/
251+
auto uas = uas_.lock();
251252
bool is_ack_required = (confirmation != 0 || uas->is_ardupilotmega() || uas->is_px4()) &&
252253
!broadcast;
253254
if (is_ack_required) {
@@ -308,6 +309,7 @@ class CommandPlugin : public plugin::Plugin
308309
inline void set_target(MsgT & cmd, bool broadcast)
309310
{
310311
using mavlink::minimal::MAV_COMPONENT;
312+
auto uas = uas_.lock();
311313

312314
const uint8_t tgt_sys_id = (broadcast) ? 0 : uas->get_tgt_system();
313315
const uint8_t tgt_comp_id = (broadcast) ? 0 :
@@ -341,6 +343,7 @@ class CommandPlugin : public plugin::Plugin
341343
cmd.param6 = param6;
342344
cmd.param7 = param7;
343345

346+
auto uas = uas_.lock();
344347
uas->send_message(cmd);
345348
}
346349

@@ -368,6 +371,7 @@ class CommandPlugin : public plugin::Plugin
368371
cmd.y = y;
369372
cmd.z = z;
370373

374+
auto uas = uas_.lock();
371375
uas->send_message(cmd);
372376
}
373377

‎mavros/src/plugins/ftp.cpp

+6
Original file line numberDiff line numberDiff line change
@@ -408,6 +408,7 @@ class FTPPlugin : public plugin::Plugin
408408
FTPRequest & req,
409409
plugin::filter::SystemAndOk filter [[maybe_unused]])
410410
{
411+
auto uas = uas_.lock();
411412
if (!req.decode_valid(uas)) {
412413
// RCLCPP_DEBUG(get_logger(), "FTP: Wrong System Id, MY %u, TGT %u",
413414
// uas->get_system_id(), req.get_target_system_id());
@@ -688,6 +689,7 @@ class FTPPlugin : public plugin::Plugin
688689

689690
op_state = OP::ACK;
690691
FTPRequest req(FTPRequest::kCmdResetSessions);
692+
auto uas = uas_.lock();
691693
req.send(uas, last_send_seqnr);
692694
}
693695

@@ -700,6 +702,7 @@ class FTPPlugin : public plugin::Plugin
700702
FTPRequest req(op);
701703
req.header()->offset = offset;
702704
req.set_data_string(path);
705+
auto uas = uas_.lock();
703706
req.send(uas, last_send_seqnr);
704707
}
705708

@@ -731,6 +734,7 @@ class FTPPlugin : public plugin::Plugin
731734
FTPRequest req(FTPRequest::kCmdTerminateSession, session);
732735
req.header()->offset = 0;
733736
req.header()->size = 0;
737+
auto uas = uas_.lock();
734738
req.send(uas, last_send_seqnr);
735739
}
736740

@@ -742,6 +746,7 @@ class FTPPlugin : public plugin::Plugin
742746
FTPRequest req(FTPRequest::kCmdReadFile, active_session);
743747
req.header()->offset = read_offset;
744748
req.header()->size = 0 /* FTPRequest::DATA_MAXSZ */;
749+
auto uas = uas_.lock();
745750
req.send(uas, last_send_seqnr);
746751
}
747752

@@ -755,6 +760,7 @@ class FTPPlugin : public plugin::Plugin
755760
req.header()->offset = write_offset;
756761
req.header()->size = bytes_to_copy;
757762
std::copy(write_it, write_it + bytes_to_copy, req.data());
763+
auto uas = uas_.lock();
758764
req.send(uas, last_send_seqnr);
759765
}
760766

‎mavros/src/plugins/geofence.cpp

+1
Original file line numberDiff line numberDiff line change
@@ -81,6 +81,7 @@ class GeofencePlugin : public plugin::MissionBase
8181
void capabilities_cb(uas::MAV_CAP capabilities [[maybe_unused]]) override
8282
{
8383
lock_guard lock(mutex);
84+
auto uas = uas_.lock();
8485

8586
if (uas->has_capability(uas::MAV_CAP::MISSION_INT)) {
8687
use_mission_item_int = true;

‎mavros/src/plugins/global_position.cpp

+10-2
Original file line numberDiff line numberDiff line change
@@ -56,8 +56,8 @@ class GlobalPositionPlugin : public plugin::Plugin
5656
public:
5757
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
5858

59-
explicit GlobalPositionPlugin(plugin::UASPtr uas_)
60-
: Plugin(uas_, "global_position"),
59+
explicit GlobalPositionPlugin(plugin::UASPtr uas)
60+
: Plugin(uas, "global_position"),
6161
tf_send(false),
6262
use_relative_alt(true),
6363
is_map_init(false),
@@ -189,6 +189,7 @@ class GlobalPositionPlugin : public plugin::Plugin
189189
template<typename MsgT>
190190
inline void fill_lla(const MsgT & msg, sensor_msgs::msg::NavSatFix & fix)
191191
{
192+
auto uas = uas_.lock();
192193
fix.latitude = msg.lat / 1E7; // deg
193194
fix.longitude = msg.lon / 1E7; // deg
194195
fix.altitude = msg.alt / 1E3 + uas->data.geoid_to_ellipsoid_height(fix); // in meters
@@ -213,6 +214,7 @@ class GlobalPositionPlugin : public plugin::Plugin
213214

214215
auto fix = NavSatFix();
215216

217+
auto uas = uas_.lock();
216218
fix.header = uas->synchronized_header(child_frame_id, raw_gps.time_usec);
217219

218220
fix.status.service = NavSatStatus::SERVICE_GPS;
@@ -282,6 +284,7 @@ class GlobalPositionPlugin : public plugin::Plugin
282284
plugin::filter::SystemAndOk filter [[maybe_unused]])
283285
{
284286
auto g_origin = geographic_msgs::msg::GeoPointStamped();
287+
auto uas = uas_.lock();
285288

286289
g_origin.header = uas->synchronized_header(tf_global_frame_id, glob_orig.time_usec);
287290
g_origin.position.latitude = glob_orig.latitude / 1E7;
@@ -319,6 +322,7 @@ class GlobalPositionPlugin : public plugin::Plugin
319322
auto relative_alt = std_msgs::msg::Float64();
320323
auto compass_heading = std_msgs::msg::Float64();
321324

325+
auto uas = uas_.lock();
322326
auto header = uas->synchronized_header(child_frame_id, gpos.time_boot_ms);
323327

324328
// Global position fix
@@ -462,6 +466,7 @@ class GlobalPositionPlugin : public plugin::Plugin
462466
plugin::filter::SystemAndOk filter [[maybe_unused]])
463467
{
464468
auto global_offset = geometry_msgs::msg::PoseStamped();
469+
auto uas = uas_.lock();
465470
global_offset.header = uas->synchronized_header(tf_global_frame_id, offset.time_boot_ms);
466471

467472
auto enu_position = ftf::transform_frame_ned_enu(Eigen::Vector3d(offset.x, offset.y, offset.z));
@@ -490,6 +495,7 @@ class GlobalPositionPlugin : public plugin::Plugin
490495
transform.transform.translation.y = global_offset.pose.position.y;
491496
transform.transform.translation.z = global_offset.pose.position.z;
492497

498+
auto uas = uas_.lock();
493499
uas->tf2_broadcaster.sendTransform(transform);
494500
}
495501
}
@@ -500,6 +506,7 @@ class GlobalPositionPlugin : public plugin::Plugin
500506
int fix_type, satellites_visible;
501507
float eph, epv;
502508

509+
auto uas = uas_.lock();
503510
uas->data.get_gps_epts(eph, epv, fix_type, satellites_visible);
504511

505512
if (satellites_visible <= 0) {
@@ -557,6 +564,7 @@ class GlobalPositionPlugin : public plugin::Plugin
557564
void set_gp_origin_cb(const geographic_msgs::msg::GeoPointStamped::SharedPtr req)
558565
{
559566
mavlink::common::msg::SET_GPS_GLOBAL_ORIGIN gpo = {};
567+
auto uas = uas_.lock();
560568

561569
gpo.time_usec = get_time_usec(req->header.stamp);
562570
gpo.target_system = uas->get_tgt_system();

‎mavros/src/plugins/home_position.cpp

+2
Original file line numberDiff line numberDiff line change
@@ -127,6 +127,7 @@ class HomePositionPlugin : public plugin::Plugin
127127
home_position.approach_x,
128128
home_position.approach_y, home_position.approach_z));
129129

130+
auto uas = uas_.lock();
130131
hp.header.stamp = uas->synchronise_stamp(home_position.time_usec);
131132
hp.geo.latitude = home_position.latitude / 1E7; // deg
132133
hp.geo.longitude = home_position.longitude / 1E7; // deg
@@ -159,6 +160,7 @@ class HomePositionPlugin : public plugin::Plugin
159160
tf2::fromMsg(req->approach, approach);
160161
approach = ftf::transform_frame_enu_ned(approach);
161162

163+
auto uas = uas_.lock();
162164
hp.target_system = uas->get_tgt_system();
163165
ftf::quaternion_to_mavlink(q, hp.q);
164166

0 commit comments

Comments
 (0)