Gazebo ver: 7.0, ROS : Kinetic

Asked in Gazebo forum : http://answers.gazebosim.org/question/18864/getboundingbox-values-not-changing-after-scale-in-gazebo-using-visual_plugin/

Hello,

I am using a visual_plugin to get the box corner points, so later while scaling the object to change the color. I run this plugin inside the box model from the sdf file.

Everything loads good, the box points in the beginning are ok, same as defined in sdf, but if I scale the box (Inside Gazebo), the points when I printout > boxpoints, remain the same but if I check in Gazebo into the properties of the box, the points are updated. How can I get the boundingbox to update the points in the plugin as I scale.

TheBox.sdf

<?xml version="1.0"?>
<sdf version="1.6">
  <world name='default'>   
    <model name="TheBox">  
    <pose frame=''>30 13.9 3.05 0 0 0.012944</pose>

    <link name="TheBox">
        <gravity>false</gravity>
        <visual name="visual">
        <transparency>0.5</transparency>
        <geometry>
            <box>
            <size>18 2.9 6</size>
            </box>
        </geometry>
        <material>
            <ambient>1 0 0 1</ambient>
            <diffuse>1 0 0 1</diffuse>
            <specular>0.1 0.1 0.1 1</specular>
            <emissive>0 0 0 0</emissive>
        </material>

        <plugin name="BoxManipulator" filename="libBoxManipulator.so"/>

        </visual>
    </link>
    </model>
  </world>
</sdf>

BoxManipulator.cpp

#include "BoxManipulator.h"

void BoxManipulator::Load(gazebo::rendering::VisualPtr _parent, sdf::ElementPtr _sdf) {

    // Store the pointer to the model
    this->world_ = _parent;

    this->scene_ =  gazebo::rendering::get_scene();
    this->box_ = this->scene_->GetVisual("TheBox");

    // Listen to the update event. This event is broadcast every simulation iteration.
    this->update_connection_ = gazebo::event::Events::ConnectPreRender(std::bind(&BoxManipulator::OnUpdate, this));
  }

  // Called by the world update start event
  void BoxManipulator::OnUpdate() {

    // Box model
    gazebo::math::Vector3 box_origin_ = this->box_->GetWorldPose().pos;
    gazebo::math::Quaternion box_orientation_ = this->box_->GetWorldPose().rot;

    //Update the points
    getBoxPoints();

    //Printout the points
    for (int i = 0; i < box_points_.size(); i++) {
      std::cout << "Box corner points  : [" << i << "] - " << box_points_[i] << std::endl;
    }

    std::cout << std::endl;

    box_points_.clear();

    // //Later So I can change the color of the box, in different scales

    //     ROS_INFO("Box smaller than 1^3");
    //     red_ = 1; green_ = 0;
    //     gazebo::common::Color _color(red_, green_, blue_, alpha_);

    //     this->world_->SetAmbient(_color);
    //     this->world_->SetDiffuse(_color);


  }

  bool BoxManipulator::getBoxPoints(void){
    gazebo::math::Vector3 _buff; box_points_.reserve(8);

    _buff.x = this->box_->GetBoundingBox().min.x; _buff.y = this->box_->GetBoundingBox().min.y; _buff.z = this->box_->GetBoundingBox().min.z;
    box_points_.push_back(_buff);
    _buff.x = this->box_->GetBoundingBox().min.x; _buff.y = this->box_->GetBoundingBox().max.y; _buff.z = this->box_->GetBoundingBox().min.z;
    box_points_.push_back(_buff);
    _buff.x = this->box_->GetBoundingBox().min.x; _buff.y = this->box_->GetBoundingBox().max.y; _buff.z = this->box_->GetBoundingBox().max.z;
    box_points_.push_back(_buff);
    _buff.x = this->box_->GetBoundingBox().min.x; _buff.y = this->box_->GetBoundingBox().min.y; _buff.z = this->box_->GetBoundingBox().max.z;
    box_points_.push_back(_buff);

    _buff.x = this->box_->GetBoundingBox().max.x; _buff.y = this->box_->GetBoundingBox().min.y; _buff.z = this->box_->GetBoundingBox().min.z;
    box_points_.push_back(_buff);
    _buff.x = this->box_->GetBoundingBox().max.x; _buff.y = this->box_->GetBoundingBox().max.y; _buff.z = this->box_->GetBoundingBox().min.z;
    box_points_.push_back(_buff);
    _buff.x = this->box_->GetBoundingBox().max.x; _buff.y = this->box_->GetBoundingBox().max.y; _buff.z = this->box_->GetBoundingBox().max.z;
    box_points_.push_back(_buff);
    _buff.x = this->box_->GetBoundingBox().max.x; _buff.y = this->box_->GetBoundingBox().min.y; _buff.z = this->box_->GetBoundingBox().max.z;
    box_points_.push_back(_buff);

    return true;
  }

  // Register this plugin with the simulator
  GZ_REGISTER_VISUAL_PLUGIN(BoxManipulator)
}

BoxManipulator.h

#ifndef ___BOX_MANIPULATOR___
#define ___BOX_MANIPULATOR___

#include <ros/ros.h>
#include <ros/callback_queue.h>

#include <sdf/sdf.hh>

#include <gazebo/math/gzmath.hh>

#include <gazebo/rendering/rendering.hh>


class BoxManipulator : public gazebo::VisualPlugin {
public: 

/// \brief Load the model from the sdf
void Load(gazebo::rendering::VisualPtr _parent, sdf::ElementPtr _sdf);

/// \brief Called by the world update start event
virtual void OnUpdate();

private:

/// \brief Pointer to the model
gazebo::rendering::VisualPtr world_;

/// \brief Variable to store the box model
gazebo::rendering::VisualPtr box_;

/// \brief Pointer to the update event connection
gazebo::event::ConnectionPtr update_connection_;

/// \brief get box points
bool getBoxPoints(void);

/// \brief check
bool check(void);

gazebo::rendering::ScenePtr scene_;

double  red_ = 0,
        green_ = 0,
        blue_ = 0,
        alpha_ = 0.5;

std::vector<gazebo::math::Vector3> box_points_;

};
#endif