Ignition Fortress: Parsing top-level models with bounding box from within a global world sensor plugin

edit

Hi,

we are currently implementing a global world sensor plugin in Ignition Fortress. This plugin collects different information from the current scene (wrt. models) and converts them to an ignition::Model_V message. Later on, we are using the rosbridge to convert the ignition::Model_V messages to a own ROS message definition.

The model shall parse the scene graph wrt.

• identifier
• name
• pose
• bounding box of each model.

Therefore our implementation mainly takes place in the PoseUpdate step of the simulation:

/////////////////////////////////////////////////
void ObstacleWorldSensorPlugin::PostUpdate(const ignition::gazebo::UpdateInfo& _info,
                                       const ignition::gazebo::EntityComponentManager& _ecm)
{
   if (_info.paused) return; 

  // prepare variable to be filled with the objects/models inside simulation.
  ignition::msgs::Model_V model_vector_msg;

  // fill header, especially time stamp by sim_time
  ignition::msgs::Time* timestamp = model_vector_msg.mutable_header()->mutable_stamp();
  timestamp->CopyFrom(ignition::gazebo::convert<ignition::msgs::Time>(_info.simTime));

 _ecm.Each<ignition::gazebo::components::Model,  ignition::gazebo::components::Name,  ignition::gazebo::components::Pose>( [&](const ignition::gazebo::Entity& _entity, const ignition::gazebo::components::Model* _model,  const ignition::gazebo::components::Name* _nameComp, const ignition::gazebo::components::Pose* _poseComp) -> bool 
 {
  ignmsg << "Handling current entity [" << _entity << "]: " << _nameComp->Data() << " of scene graph." << std::endl;
  //
  // issue #1: Filter or manipulate iteration, so that only top-level models are iterated...
  //

  // prepare variable for the model
  // fill header and pose with position and orientation of model
  ignition::msgs::Model* model_msg       = model_vector_msg.add_models();
  ignition::msgs::Header* header         = model_msg->mutable_header();
  ignition::msgs::Pose* pose_msg         = model_msg->mutable_pose();
  ignition::msgs::Vector3d* position_msg = pose_msg->mutable_position();

  // fill message content
  model_msg->set_name(_nameComp->Data());
  model_msg->set_id(_entity);
  ignition::msgs::Set(pose_msg, _poseComp->Data());

  //
  // issue #2: get bounding box of model
  //
 return true;
 });
 this->dataPtr->model_pub.publish(model_vector_msg);

Currently, we are facing the following issues:

• Firstly, we only want to detect the top-level model (in the case of nested models).
• Secondly, we want to parse the bounding box (geometric extensions) of the model inside the scene graph

Any help is appreciated.

BR.