Added updates to fix frame issue

traclabs · Nov 15, 2024 · e0dff68 · e0dff68
1 parent 06520d7
commit e0dff68
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 16 deletions.
diff --git a/simulation/include/astrobee_gazebo/astrobee_gazebo.h b/simulation/include/astrobee_gazebo/astrobee_gazebo.h
@@ -167,9 +167,12 @@ class FreeFlyerModelPlugin : public FreeFlyerPlugin,
  protected:
   sdf::ElementPtr sdf_;
   gz::sim::Entity world_entity_;
+  gz::sim::Entity model_entity_;
+  gz::sim::Entity link_entity_;  
+
   std::shared_ptr<gz::sim::Link> link_;
   std::shared_ptr<gz::sim::Model> model_;
-  gz::sim::Entity model_entity_;
+
   bool update_extrinsics_;
   gz::math::Pose3d extrinsics_pose_;
 };
@@ -214,7 +217,7 @@ class FreeFlyerSensorPlugin : public FreeFlyerPlugin,
   gz::sim::Entity GetWorld();
 
   // Get the sensor model
-  std::shared_ptr<gz::sim::Model> GetModel();
+  gz::sim::Entity GetModel();
 
   // Get sensor entity
   gz::sim::Entity GetSensor();
@@ -234,7 +237,9 @@ class FreeFlyerSensorPlugin : public FreeFlyerPlugin,
 
  private:
   gz::sim::Entity sensor_entity_;
+  gz::sim::Entity model_entity_;
   gz::sim::Entity world_entity_;
+
   std::shared_ptr<gz::sim::Model> model_;
   sdf::ElementPtr sdf_;
 

diff --git a/simulation/src/astrobee_gazebo.cc b/simulation/src/astrobee_gazebo.cc
@@ -155,21 +155,21 @@ FreeFlyerModelPlugin::FreeFlyerModelPlugin(std::string const& plugin_name,
 FreeFlyerModelPlugin::~FreeFlyerModelPlugin() {}
 
 // Auto-called when Gazebo loads the plugin
-void FreeFlyerModelPlugin::Configure(const gz::sim::Entity &_model_entity,
+void FreeFlyerModelPlugin::Configure(const gz::sim::Entity &_entity,
                          const std::shared_ptr<const sdf::Element> &_sdf,
                          gz::sim::EntityComponentManager &_ecm,
                          gz::sim::EventManager &_eventMgr) {
 
   sdf_   = _sdf->Clone();
-  model_.reset( new gz::sim::Model(_model_entity) );
-
-  auto link_entity = model_->CanonicalLink(_ecm);
-  link_.reset( new gz::sim::Link(link_entity) );
+
+  model_entity_ = _entity;
+  world_entity_ = gz::sim::kNullEntity;
+  world_entity_ = gz::sim::worldEntity(_ecm);
+
+  model_.reset( new gz::sim::Model(model_entity_) );
+  link_entity_ = model_->CanonicalLink(_ecm);  
+  link_.reset( new gz::sim::Link(link_entity_) );
 
-  world_entity_ = gz::sim::worldEntity(_ecm); // NOTE: For some reason, calling worldEntity(_model, _ecm) didn't work
-  if(world_entity_ == gz::sim::kNullEntity)
-    FF_ERROR("World entity wasn't obtained correctly");
-
   // Read namespace
   std::string ns = model_->Name(_ecm);
   if (ns == "bsharp")
@@ -265,12 +265,12 @@ void FreeFlyerSensorPlugin::Configure(const gz::sim::Entity &_entity,
                          gz::sim::EventManager &_eventMgr) {
 
   sdf_ = _sdf->Clone();
+
   sensor_entity_ = _entity; 
-
   world_entity_ = gz::sim::worldEntity(sensor_entity_, _ecm);
+  model_entity_ = gz::sim::topLevelModel(sensor_entity_, _ecm);
 
-  // Store pointer to model
-  model_.reset( new gz::sim::Model(_entity));
+  model_.reset( new gz::sim::Model(model_entity_) );
 
   // Read namespace
   std::string ns = model_->Name(_ecm);
@@ -298,8 +298,8 @@ gz::sim::Entity FreeFlyerSensorPlugin::GetWorld() {
 }
 
 // Get the sensor model
-std::shared_ptr<gz::sim::Model> FreeFlyerSensorPlugin::GetModel() {
-  return model_;
+gz::sim::Entity FreeFlyerSensorPlugin::GetModel() {
+  return model_entity_;
 }
 
 // Get sensor entity