Add a Bounding Box to an Actor on Gazebo

edit

Hi,

I'm working with AutonomousActorPlugin to create a natural human simulation. I'm trying to modify the HandleObstacles function in order to use the bounding box to detect possible collitions in place of the distance. This is my function now:

    void AutoActorPlugin::HandleObstacles(ignition::math::Vector3d &_pos)
{
  for (unsigned int i = 0; i < this->world->ModelCount(); ++i)
  {
    physics::ModelPtr model = this->world->ModelByIndex(i);
    if (std::find(this->ignoreModels.begin(), this->ignoreModels.end(),
                  model->GetName()) == this->ignoreModels.end())
    {

      ignition::math::Vector3d offset = model->WorldPose().Pos() -
                                        this->actor->WorldPose().Pos();
      double modelDist = offset.Length();

#ifdef DEBUG_
      // For debug
      gzdbg << "....................OBSTACLE HANDLING................." << std::endl;
      gzdbg << "OBSTACLE: " << model->BoundingBox() << std::endl;
          gzdbg << "HUMAN BOX: " << this->actor->BoundingBox() << std::endl;
          gzdbg << "......................................................" << std::endl;
    #endif 

          if (model->BoundingBox().Intersects(this->actor->BoundingBox()))
      {
#ifdef DEBUG_

        // For debug
        gzdbg << "!!!!! POSSIBLE COLLISION DETECTED  !!!!!!!" << std::endl;
#endif
        double invModelDist = this->obstacleWeight / modelDist;
        offset.Normalize();
        offset *= invModelDist;
        _pos -= offset;
      }
    }
  }
}

The problem is that the bounding box attached to the actor is no sense:

[Dbg] [AutonomousActorPlugin.cc:180] HUMAN BOX: Min[3.40282e+38 3.40282e+38 3.40282e+38] Max[0 0 0]

In Gazebo GUI it seems very different (if I interpret correctly the with box aroud the man)

Is it possible to change the bounding box? How?

I tried also to use a collision bounding box, by attaching it to the actor using this. It works because I can see the Collision Box in orange on the Gazebo GUI, but when I try to call the function CollisionBoundingBox() in place of CollisionBoundingBox(), I get a segmentation fault.

Can someone help me with some advices? Thanks a lot.