From a805ef2d83eccaa08699cde80a59becfdc87d613 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Tue, 18 Jan 2022 20:32:38 +0000 Subject: [PATCH] Ensure the model base link is set to the correct model - Set the link for the IMU contained in this model - the ECM foreach iterates over all model components in the world. Signed-off-by: Rhys Mainwaring --- src/ArduPilotPlugin.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ArduPilotPlugin.cc b/src/ArduPilotPlugin.cc index 3940378..f525504 100755 --- a/src/ArduPilotPlugin.cc +++ b/src/ArduPilotPlugin.cc @@ -764,7 +764,6 @@ void ignition::gazebo::systems::ArduPilotPlugin::PreUpdate( { // The parent of the imu is imu_link ignition::gazebo::Entity parent = _ecm.ParentEntity(_imu_entity); - this->dataPtr->modelLink = parent; if (parent != ignition::gazebo::kNullEntity) { // The grandparent of the imu is the quad itself, which is where this plugin is attached @@ -774,6 +773,7 @@ void ignition::gazebo::systems::ArduPilotPlugin::PreUpdate( ignition::gazebo::Model gparent_model(gparent); if (gparent_model.Name(_ecm) == this->dataPtr->modelName) { + this->dataPtr->modelLink = parent; imuTopicName = ignition::gazebo::scopedName(_imu_entity, _ecm) + "/imu"; igndbg << "Computed IMU topic to be: " << imuTopicName << std::endl; }