GetBoundingBox() values not changing after scale in gazebo (using : visual_plugin)
Gazebo ver: 7.0, ROS : Kinetic [Posted in ros.answers, closed and told to post here]
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 > box_points_, remain the same but if I check in Gazebo into the properties of the box, the points are updated (If I use GetWorldPose(), and printout the pose and move the box, it works, the values 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 ...