Gazebo ver: 7.0,
ROS : Kinetic
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. How can I get the boundingbox to update the points in the plugin as I scale.
**TheBox.sdf**
30 13.9 3.05 0 0 0.012944 false 0.5 18 2.9 6 1 0 0 1 1 0 0 1 0.1 0.1 0.1 1 0 0 0 0
**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
#include
#include
#include
#include
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 box_points_;
};
#endif
↧