397 lines
12 KiB
C++
397 lines
12 KiB
C++
|
/*
|
||
|
|
* Copyright (C) 2012 Open Source Robotics Foundation
|
||
|
|
*
|
||
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
||
|
|
* you may not use this file except in compliance with the License.
|
||
|
|
* You may obtain a copy of the License at
|
||
|
|
*
|
||
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||
|
|
*
|
||
|
|
* Unless required by applicable law or agreed to in writing, software
|
||
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
||
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||
|
|
* See the License for the specific language governing permissions and
|
||
|
|
* limitations under the License.
|
||
|
|
*
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include <sstream>
|
||
|
|
#include <gazebo/math/Rand.hh>
|
||
|
|
#include <gazebo/physics/World.hh>
|
||
|
|
#include "RubblePlugin.hh"
|
||
|
|
|
||
|
|
using namespace gazebo;
|
||
|
|
|
||
|
|
GZ_REGISTER_WORLD_PLUGIN(RubblePlugin)
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
RubblePlugin::RubblePlugin()
|
||
|
|
{
|
||
|
|
}
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
void RubblePlugin::Load(physics::WorldPtr _world, sdf::ElementPtr _sdf)
|
||
|
|
{
|
||
|
|
this->world = _world;
|
||
|
|
|
||
|
|
math::Vector3 bottomRight = _sdf->Get<math::Vector3>("bottom_right");
|
||
|
|
math::Vector3 topLeft = _sdf->Get<math::Vector3>("top_left");
|
||
|
|
math::Vector3 minSize = _sdf->Get<math::Vector3>("min_size");
|
||
|
|
math::Vector3 maxSize = _sdf->Get<math::Vector3>("max_size");
|
||
|
|
double minMass = _sdf->Get<double>("min_mass");
|
||
|
|
double maxMass = _sdf->Get<double>("max_mass");
|
||
|
|
unsigned int count = _sdf->Get<unsigned int>("count");
|
||
|
|
|
||
|
|
for (unsigned int i = 0; i < count; ++i)
|
||
|
|
{
|
||
|
|
int rubbleType = math::Rand::GetIntUniform(0, 1);
|
||
|
|
double mass = math::Rand::GetDblUniform(minMass, maxMass);
|
||
|
|
|
||
|
|
Obj obj;
|
||
|
|
|
||
|
|
obj.pose.pos.x = math::Rand::GetDblUniform(bottomRight.x, topLeft.x);
|
||
|
|
obj.pose.pos.y = math::Rand::GetDblUniform(bottomRight.y, topLeft.y);
|
||
|
|
obj.pose.pos.z = math::Rand::GetDblUniform(bottomRight.z, topLeft.z);
|
||
|
|
|
||
|
|
obj.pose.rot.SetFromEuler(math::Vector3(
|
||
|
|
math::Rand::GetDblUniform(0.0, 3.1415),
|
||
|
|
math::Rand::GetDblUniform(-0.1, 0.1),
|
||
|
|
math::Rand::GetDblUniform(0.0, 3.1415)));
|
||
|
|
|
||
|
|
|
||
|
|
obj.type = rubbleType;
|
||
|
|
|
||
|
|
// Make a 2x4
|
||
|
|
if (rubbleType == 0)
|
||
|
|
{
|
||
|
|
// between 2 and 8 feet.
|
||
|
|
obj.size.x = math::Rand::GetDblUniform(0.6096, 2.4384);
|
||
|
|
|
||
|
|
// 4 inches
|
||
|
|
obj.size.y = 0.1016;
|
||
|
|
|
||
|
|
// 2 inches
|
||
|
|
obj.size.z = 0.0508;
|
||
|
|
}
|
||
|
|
// Make a cinder block
|
||
|
|
else if (rubbleType == 1)
|
||
|
|
{
|
||
|
|
// Standard US cinder block size
|
||
|
|
obj.size.x = 0.2032;
|
||
|
|
obj.size.y = 0.2032;
|
||
|
|
obj.size.z = 0.4064;
|
||
|
|
}
|
||
|
|
// Make a cylinder
|
||
|
|
else
|
||
|
|
{
|
||
|
|
obj.size.x = math::Rand::GetDblUniform(minSize.x, maxSize.x);
|
||
|
|
obj.size.z = math::Rand::GetDblUniform(minSize.z, maxSize.z);
|
||
|
|
obj.size.y = obj.size.x;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Make sure the bottom of the rubble piece is above the bottomRight.z
|
||
|
|
// This will prevent ground penetration.
|
||
|
|
if (obj.pose.pos.z - obj.size.z * 0.5 < bottomRight.z)
|
||
|
|
obj.pose.pos.z = bottomRight.z + obj.size.z * 0.5;
|
||
|
|
|
||
|
|
std::ostringstream name;
|
||
|
|
name << "rubble_" << i;
|
||
|
|
|
||
|
|
if (rubbleType == 0)
|
||
|
|
this->MakeBox(name.str(), obj.pose, obj.size, mass);
|
||
|
|
else if (rubbleType == 1)
|
||
|
|
this->MakeCinderBlock(name.str(), obj.pose, obj.size, mass);
|
||
|
|
/*else
|
||
|
|
this->MakeCylinder(name.str(), obj.pos, obj.size, mass);
|
||
|
|
*/
|
||
|
|
|
||
|
|
// Disable compound objects for now.
|
||
|
|
// bool merged = false;
|
||
|
|
/* std::vector<CompoundObj> objects;
|
||
|
|
std::vector<CompoundObj>::iterator iter;
|
||
|
|
for (iter = objects.begin(); iter != objects.end(); ++iter)
|
||
|
|
{
|
||
|
|
bool x = fabs(obj.pos.x - (*iter).pos.x) <=
|
||
|
|
(*iter).size.x * 0.5 + obj.size.x * 0.5;
|
||
|
|
bool y = fabs(obj.pos.y - (*iter).pos.y) <=
|
||
|
|
(*iter).size.y * 0.5 + obj.size.y * 0.5;
|
||
|
|
bool z = fabs(obj.pos.z - (*iter).pos.z) <=
|
||
|
|
(*iter).size.z * 0.5 + obj.size.z * 0.5;
|
||
|
|
|
||
|
|
if (x && y && z)
|
||
|
|
{
|
||
|
|
(*iter).objects.push_back(obj);
|
||
|
|
std::vector<Obj>::iterator objIter;
|
||
|
|
|
||
|
|
(*iter).pos.Set(0, 0, 0);
|
||
|
|
math::Vector3 min, max;
|
||
|
|
min.x = min.y = min.z = std::numeric_limits<double>::max();
|
||
|
|
max.x = max.y = max.z = std::numeric_limits<double>::min();
|
||
|
|
|
||
|
|
for (objIter = (*iter).objects.begin();
|
||
|
|
objIter != (*iter).objects.end(); ++objIter)
|
||
|
|
{
|
||
|
|
(*iter).pos += (*objIter).pos;
|
||
|
|
min.x = std::min(min.x, (*objIter).pos.x - (*objIter).size.x * 0.5);
|
||
|
|
min.y = std::min(min.y, (*objIter).pos.y - (*objIter).size.y * 0.5);
|
||
|
|
min.z = std::min(min.z, (*objIter).pos.z - (*objIter).size.z * 0.5);
|
||
|
|
|
||
|
|
max.x = std::max(max.x, (*objIter).pos.x + (*objIter).size.x * 0.5);
|
||
|
|
max.y = std::max(max.y, (*objIter).pos.y + (*objIter).size.y * 0.5);
|
||
|
|
max.z = std::max(max.z, (*objIter).pos.z + (*objIter).size.z * 0.5);
|
||
|
|
}
|
||
|
|
|
||
|
|
// Recalculate the middle position of the compound object
|
||
|
|
(*iter).pos /= (*iter).objects.size();
|
||
|
|
|
||
|
|
// Recalculate the total size of the compound object
|
||
|
|
(*iter).size.Set(max.x - min.x, max.y - min.y, max.z - min.z);
|
||
|
|
|
||
|
|
merged = true;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
if (!merged)
|
||
|
|
{
|
||
|
|
RubblePlugin::CompoundObj co;
|
||
|
|
co.pos = obj.pos;
|
||
|
|
co.size = obj.size;
|
||
|
|
co.objects.push_back(obj);
|
||
|
|
objects.push_back(co);
|
||
|
|
}*/
|
||
|
|
}
|
||
|
|
|
||
|
|
// Disable compound objects for now.
|
||
|
|
/*
|
||
|
|
int i =0;
|
||
|
|
for (iter = objects.begin(); iter != objects.end(); ++iter, ++i)
|
||
|
|
{
|
||
|
|
std::ostringstream name;
|
||
|
|
name << "rubble_" << i;
|
||
|
|
this->MakeCompound(name.str(), *iter);
|
||
|
|
}*/
|
||
|
|
}
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
void RubblePlugin::Init()
|
||
|
|
{
|
||
|
|
}
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
void RubblePlugin::MakeCinderBlock(const std::string &_name, math::Pose &_pose,
|
||
|
|
math::Vector3 &_size, double _mass)
|
||
|
|
{
|
||
|
|
std::ostringstream newModelStr;
|
||
|
|
|
||
|
|
float sx = _size.x;
|
||
|
|
float sy = _size.y;
|
||
|
|
float sz = _size.z;
|
||
|
|
|
||
|
|
newModelStr << "<sdf version='" << SDF_VERSION << "'>"
|
||
|
|
"<model name='" << _name << "'>"
|
||
|
|
"<pose>" << _pose << "</pose>"
|
||
|
|
"<link name='link'>"
|
||
|
|
"<velocity_decay>"
|
||
|
|
"<linear>0.01</linear>"
|
||
|
|
"<angular>0.01</angular>"
|
||
|
|
"</velocity_decay>"
|
||
|
|
"<inertial><mass>" << _mass << "</mass>"
|
||
|
|
"<inertia>"
|
||
|
|
"<ixx>" << (1.0/12.0) * _mass * (sy*sy + sz*sz) << "</ixx>"
|
||
|
|
"<iyy>" << (1.0/12.0) * _mass * (sz*sz + sx*sx) << "</iyy>"
|
||
|
|
"<izz>" << (1.0/12.0) * _mass * (sx*sx + sy*sy) << "</izz>"
|
||
|
|
"<ixy>" << 0.0 << "</ixy>"
|
||
|
|
"<ixz>" << 0.0 << "</ixz>"
|
||
|
|
"<iyz>" << 0.0 << "</iyz>"
|
||
|
|
"</inertia>"
|
||
|
|
"</inertial>"
|
||
|
|
"<collision name='collision'>"
|
||
|
|
"<geometry>"
|
||
|
|
"<box><size>" << _size << "</size></box>"
|
||
|
|
"</geometry>"
|
||
|
|
"</collision>"
|
||
|
|
"<visual name='visual'>"
|
||
|
|
"<geometry>"
|
||
|
|
"<box><size>" << _size << "</size></box>"
|
||
|
|
"</geometry>"
|
||
|
|
"</visual>"
|
||
|
|
"</link>"
|
||
|
|
"</model>"
|
||
|
|
"</sdf>";
|
||
|
|
|
||
|
|
this->world->InsertModelString(newModelStr.str());
|
||
|
|
}
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
void RubblePlugin::MakeBox(const std::string &_name, math::Pose &_pose,
|
||
|
|
math::Vector3 &_size, double _mass)
|
||
|
|
{
|
||
|
|
std::ostringstream newModelStr;
|
||
|
|
|
||
|
|
float sx = _size.x;
|
||
|
|
float sy = _size.y;
|
||
|
|
float sz = _size.z;
|
||
|
|
|
||
|
|
newModelStr << "<sdf version='" << SDF_VERSION << "'>"
|
||
|
|
"<model name='" << _name << "'>"
|
||
|
|
"<allow_auto_disable>true</allow_auto_disable>"
|
||
|
|
"<pose>" << _pose << "</pose>"
|
||
|
|
"<link name='link'>"
|
||
|
|
"<velocity_decay>"
|
||
|
|
"<linear>0.01</linear>"
|
||
|
|
"<angular>0.01</angular>"
|
||
|
|
"</velocity_decay>"
|
||
|
|
"<inertial><mass>" << _mass << "</mass>"
|
||
|
|
"<inertia>"
|
||
|
|
"<ixx>" << (1.0/12.0) * _mass * (sy*sy + sz*sz) << "</ixx>"
|
||
|
|
"<iyy>" << (1.0/12.0) * _mass * (sz*sz + sx*sx) << "</iyy>"
|
||
|
|
"<izz>" << (1.0/12.0) * _mass * (sx*sx + sy*sy) << "</izz>"
|
||
|
|
"<ixy>" << 0.0 << "</ixy>"
|
||
|
|
"<ixz>" << 0.0 << "</ixz>"
|
||
|
|
"<iyz>" << 0.0 << "</iyz>"
|
||
|
|
"</inertia>"
|
||
|
|
"</inertial>"
|
||
|
|
"<collision name='collision'>"
|
||
|
|
"<geometry>"
|
||
|
|
"<box><size>" << _size << "</size></box>"
|
||
|
|
"</geometry>"
|
||
|
|
"</collision>"
|
||
|
|
"<visual name='visual'>"
|
||
|
|
"<geometry>"
|
||
|
|
"<box><size>" << _size << "</size></box>"
|
||
|
|
"</geometry>"
|
||
|
|
"</visual>"
|
||
|
|
"</link>"
|
||
|
|
"</model>"
|
||
|
|
"</sdf>";
|
||
|
|
|
||
|
|
this->world->InsertModelString(newModelStr.str());
|
||
|
|
}
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
/*void RubblePlugin::MakeCylinder(const std::string &_name, math::Vector3 &_pos,
|
||
|
|
math::Vector3 &_size, double _mass)
|
||
|
|
{
|
||
|
|
std::ostringstream newModelStr;
|
||
|
|
|
||
|
|
float r = _size.x * 0.5;
|
||
|
|
float h = _size.z;
|
||
|
|
|
||
|
|
newModelStr << "<sdf version='" << SDF_VERSION << "'>"
|
||
|
|
"<model name='" << _name << "'>"
|
||
|
|
"<pose>" << _pos << " 0 0 0</pose>"
|
||
|
|
"<link name='link'>"
|
||
|
|
"<velocity_decay>"
|
||
|
|
"<linear>0.01</linear>"
|
||
|
|
"<angular>0.01</angular>"
|
||
|
|
"</velocity_decay>"
|
||
|
|
"<inertial><mass>" << _mass << "</mass>"
|
||
|
|
"<inertia>"
|
||
|
|
"<ixx>" << (1.0/12.0) * _mass * (3*r*r + h*h) << "</ixx>"
|
||
|
|
"<iyy>" << (1.0/12.0) * _mass * (3*r*r + h*h) << "</iyy>"
|
||
|
|
"<izz>" << (1.0/2.0) * _mass * r * r << "</izz>"
|
||
|
|
"<ixy>" << 0.0 << "</ixy>"
|
||
|
|
"<ixz>" << 0.0 << "</ixz>"
|
||
|
|
"<iyz>" << 0.0 << "</iyz>"
|
||
|
|
"</inertia>"
|
||
|
|
"</inertial>"
|
||
|
|
"<collision name ='collision'>"
|
||
|
|
"<geometry>"
|
||
|
|
"<cylinder><radius>" << r << "</radius>"
|
||
|
|
"<length>" << h << "</length></cylinder>"
|
||
|
|
"</geometry>"
|
||
|
|
"</collision>"
|
||
|
|
"<visual name='visual'>"
|
||
|
|
"<geometry>"
|
||
|
|
"<cylinder><radius>" << r << "</radius>"
|
||
|
|
"<length>" << h << "</length></cylinder>"
|
||
|
|
"</geometry>"
|
||
|
|
"</visual>"
|
||
|
|
"</link>"
|
||
|
|
"</model>"
|
||
|
|
"</sdf>";
|
||
|
|
|
||
|
|
this->world->InsertModelString(newModelStr.str());
|
||
|
|
}*/
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
/*void RubblePlugin::MakeCompound(const std::string &_name, CompoundObj &_obj)
|
||
|
|
{
|
||
|
|
std::ostringstream newModelStr, geomStr, inertiaStr;
|
||
|
|
|
||
|
|
newModelStr << "<sdf version ='1.3'>"
|
||
|
|
<< "<model name='" << _name << "'>"
|
||
|
|
<< " <pose>" << _obj.pos << " 0 0 0</pose>"
|
||
|
|
<< " <link name='link'>";
|
||
|
|
|
||
|
|
float mass = 0.1;
|
||
|
|
|
||
|
|
inertiaStr << "<inertial><mass>" << mass << "</mass><inertial>";
|
||
|
|
|
||
|
|
int i = 0;
|
||
|
|
for (std::vector<Obj>::iterator iter = _obj.objects.begin();
|
||
|
|
iter != _obj.objects.end(); ++iter, ++i)
|
||
|
|
{
|
||
|
|
if ((*iter).type == 0)
|
||
|
|
{
|
||
|
|
float h = (*iter).size.z;
|
||
|
|
float w = (*iter).size.y;
|
||
|
|
float d = (*iter).size.x;
|
||
|
|
|
||
|
|
inertiaStr << "<ixx>" << (1.0/12.0) * mass * (h*h + d*d) << "</ixx>"
|
||
|
|
<< "<iyy>" << (1.0/12.0) * mass * (w*w + d*d) << "</iyy>"
|
||
|
|
<< "<izz>" << (1.0/12.0) * mass * (w*w + h*h) << "</izz>";
|
||
|
|
|
||
|
|
geomStr << "<box><size>" << (*iter).size << "</size></box>";
|
||
|
|
}
|
||
|
|
else if ((*iter).type == 1)
|
||
|
|
{
|
||
|
|
float radius = (*iter).size.x * 0.5;
|
||
|
|
|
||
|
|
inertiaStr << "<ixx>" << (2.0/5.0)* mass * radius << "</ixx>"
|
||
|
|
<< "<iyy>" << (2.0/5.0)* mass * radius << "</iyy>"
|
||
|
|
<< "<izz>" << (2.0/5.0)* mass * radius << "</izz>";
|
||
|
|
|
||
|
|
geomStr << "<sphere><radius>" << radius << "</radius></sphere>";
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
float r = (*iter).size.x * 0.5;
|
||
|
|
float h = (*iter).size.z;
|
||
|
|
inertiaStr << "<ixx>" << (1.0/12.0) * mass * (3*r*r + h*h) << "</ixx>"
|
||
|
|
<< "<iyy>" << (1.0/12.0) * mass * (3*r*r + h*h) << "</iyy>"
|
||
|
|
<< "<izz>" << (1.0/12.0) * mass * r * r << "</izz>";
|
||
|
|
|
||
|
|
geomStr << "<cylinder><radius>" << r << "</radius>"
|
||
|
|
<< "<length>" << h << "</length></cylinder>";
|
||
|
|
}
|
||
|
|
|
||
|
|
inertiaStr << "<ixy>" << 0.0 << "</ixy>"
|
||
|
|
<< "<ixz>" << 0.0 << "</ixz>"
|
||
|
|
<< "<iyz>" << 0.0 << "</iyz>"
|
||
|
|
<< "</inertial>";
|
||
|
|
|
||
|
|
newModelStr << inertiaStr.str();
|
||
|
|
|
||
|
|
newModelStr << " <collision name ='collision_" << i << "'>"
|
||
|
|
<< " <pose>" << (*iter).pose << "</pose>"
|
||
|
|
<< " <geometry>"
|
||
|
|
<< " " << geomStr.str()
|
||
|
|
<< " </geometry>"
|
||
|
|
<< " </collision>"
|
||
|
|
<< " <visual name ='visual_" << i << "'>"
|
||
|
|
<< " <pose>" << (*iter).pose << "</pose>"
|
||
|
|
<< " <geometry>"
|
||
|
|
<< " " << geomStr.str()
|
||
|
|
<< " </geometry>"
|
||
|
|
<< " </visual>";
|
||
|
|
}
|
||
|
|
|
||
|
|
newModelStr << " </link>"
|
||
|
|
<< "</model>"
|
||
|
|
<< "</sdf>";
|
||
|
|
|
||
|
|
this->world->InsertModelString(newModelStr.str());
|
||
|
|
}*/
|