bool WindowsManager::addEdgeToRoadmap(const char* nameRoadmapCorba, const value_type* posFromCorba, const value_type* posToCorba){
const std::string nameRoadmap (nameRoadmapCorba);
if (roadmapNodes_.find (nameRoadmap) == roadmapNodes_.end ()) {
//no node named nodeName
std::cout << "No roadmap named \"" << nameRoadmap << "\"" << std::endl;
return false;
}
else {
RoadmapViewerPtr_t rm_ptr = roadmapNodes_[nameRoadmap];
osgVector3 posFrom = osgVector3(posFromCorba[0], posFromCorba[1],posFromCorba[2]);
osgVector3 posTo = osgVector3(posToCorba[0], posToCorba[1],posToCorba[2]);
// mtx_.lock(); mtx is now locked only when required in addEdge
rm_ptr->addEdge(posFrom,posTo,mtx_);
// mtx_.unlock();
return true;
}
}
void getStaticTransform (const boost::shared_ptr < urdf::Link >& link,
osgVector3 &static_pos, osgQuat &static_quat,
bool visual, long unsigned int visual_index)
{
if (visual || (link->visual_array.size()>1))
{
if (link->visual_array.size()>1)
{
// Set staticTransform = transform from link to visual
static_pos = osgVector3((float)link->visual_array[visual_index]->origin.position.x,
(float)link->visual_array[visual_index]->origin.position.y,
(float)link->visual_array[visual_index]->origin.position.z);
static_quat=osgQuat( (float)link->visual_array[visual_index]->origin.rotation.x,
(float)link->visual_array[visual_index]->origin.rotation.y,
(float)link->visual_array[visual_index]->origin.rotation.z,
(float)link->visual_array[visual_index]->origin.rotation.w);
}
else
{
// Set staticTransform = transform from link to visual
static_pos = osgVector3((float)link->visual->origin.position.x,
(float)link->visual->origin.position.y,
(float)link->visual->origin.position.z);
static_quat=osgQuat( (float)link->visual->origin.rotation.x,
(float)link->visual->origin.rotation.y,
(float)link->visual->origin.rotation.z,
(float)link->visual->origin.rotation.w);
}
} else {
// Set staticTransform = transform from link to visual
static_pos = osgVector3((float)link->collision->origin.position.x,
(float)link->collision->origin.position.y,
(float)link->collision->origin.position.z);
static_quat=osgQuat( (float)link->collision->origin.rotation.x,
(float)link->collision->origin.rotation.y,
(float)link->collision->origin.rotation.z,
(float)link->collision->origin.rotation.w);
}
}
void addMesh (const std::string &robotName,
const std::string &meshDataRootDir,
boost::shared_ptr < urdf::Link >& urdfLink,
std::size_t j,
GroupNodePtr_t &linkNode, bool visual, bool linkFrame)
{
std::string link_name;
std::string mesh_path;
::boost::shared_ptr< ::urdf::Mesh > mesh_shared_ptr;
if (visual) {
mesh_shared_ptr = boost::static_pointer_cast< ::urdf::Mesh >
( urdfLink->visual_array [j]->geometry );
} else {
mesh_shared_ptr = boost::static_pointer_cast< ::urdf::Mesh >
( urdfLink->collision_array [j]->geometry );
}
link_name = urdfLink->name;
if ( mesh_shared_ptr != 0 )
{
mesh_path = getFilename (mesh_shared_ptr->filename, meshDataRootDir);
std::ostringstream oss;
oss << robotName << "/" << link_name << "_" << j;
LeafNodeColladaPtr_t meshNode = LeafNodeCollada::create
(oss.str (), mesh_path);
osgVector3 static_pos; osgQuat static_quat;
if (linkFrame) {
getStaticTransform (urdfLink, static_pos, static_quat, visual,j);
}
meshNode->setStaticTransform(static_pos,static_quat);
meshNode->setScale(osgVector3((float)mesh_shared_ptr->scale.x,
(float)mesh_shared_ptr->scale.y,
(float)mesh_shared_ptr->scale.z));
linkNode->addChild (meshNode);
// Set Color if specified
if (visual && urdfLink->visual_array [j]->material != NULL) {
osgVector4 color(urdfLink->visual_array [j]->material->color.r,
urdfLink->visual_array [j]->material->color.g,
urdfLink->visual_array [j]->material->color.b,
urdfLink->visual_array [j]->material->color.a);
meshNode->setColor(color);
if (urdfLink->visual_array [j]->material->texture_filename != "") {
std::string textureFilename = getFilename
(urdfLink->visual_array [j]->material->texture_filename, meshDataRootDir);
meshNode->setTexture(textureFilename);
}
}
}
}
void addBox (const std::string &robotName,
const std::string& meshDataRootDir,
boost::shared_ptr < urdf::Link >& urdfLink,
std::size_t j,
GroupNodePtr_t &linkNode, bool visual, bool linkFrame)
{
std::string link_name;
::boost::shared_ptr< ::urdf::Box > box_shared_ptr;
if (visual) {
box_shared_ptr = boost::static_pointer_cast< ::urdf::Box >
( urdfLink->visual_array [j]->geometry);
} else {
box_shared_ptr = boost::static_pointer_cast< ::urdf::Box >
( urdfLink->collision_array [j]->geometry);
}
link_name = urdfLink->name;
std::cout << "Box" << std::endl;
if ( box_shared_ptr != 0 ) {
std::ostringstream oss;
oss << robotName << "/" << link_name << "_" << j;
LeafNodeBoxPtr_t boxNode = LeafNodeBox::create
(oss.str (),
osgVector3((float)(.5*box_shared_ptr->dim.x),
(float)(.5*box_shared_ptr->dim.y),
(float)(.5*box_shared_ptr->dim.z)));
osgVector3 static_pos; osgQuat static_quat;
if (linkFrame) {
getStaticTransform (urdfLink, static_pos, static_quat, visual,j);
}
boxNode->setStaticTransform(static_pos,static_quat);
// Set Color if specified
if (visual && urdfLink->visual_array [j]->material != NULL) {
osgVector4 color(urdfLink->visual_array [j]->material->color.r, urdfLink->visual_array [j]->material->color.g, urdfLink->visual_array [j]->material->color.b, urdfLink->visual_array [j]->material->color.a);
boxNode->setColor(color);
if (urdfLink->visual_array [j]->material->texture_filename != "") {
std::string textureFilename = getFilename
(urdfLink->visual_array [j]->material->texture_filename, meshDataRootDir);
boxNode->setTexture(textureFilename);
}
}
// add object to link node
linkNode->addChild(boxNode);
}
}
// reimplmented from Node : use the mathematical representation instead of OSG representation :
//( origin in the extremity and not in the center, length on the X+ and not Z+)
// if size <0, display it on the opposite extremity
void LeafNodeCapsule::addLandmark(const float& size)
{
::osg::GeometryRefPtr geom_ptr = new ::osg::Geometry();
/* Define points of the beam */
::osg::Vec3ArrayRefPtr points_ptr = new ::osg::Vec3Array(6);
float offset;
float absSize = (float) fabs(size);
if(size<0){
offset = getHeight()/2;
}else
offset = - getHeight()/2;
points_ptr->at(0) = osgVector3(0.,0.,offset);
points_ptr->at(1) = osgVector3(0.,0.,absSize+offset);
points_ptr->at(2) = osgVector3(0.,0.,offset);
points_ptr->at(3) = osgVector3(0.,absSize,offset);
points_ptr->at(4) = osgVector3(0.,0.,offset);
points_ptr->at(5) = osgVector3(-absSize,0.,offset);
/* Define the color */
::osg::Vec4ArrayRefPtr color_ptr = new ::osg::Vec4Array(3);
color_ptr->at(0) = osgVector4(1.,0.,0.,1.);
color_ptr->at(1) = osgVector4(0.,1.,0.,1.);
color_ptr->at(2) = osgVector4(0.,0.,1.,1.);
geom_ptr->setVertexArray(points_ptr.get());
geom_ptr->setColorArray(color_ptr.get());
geom_ptr->setColorBinding(::osg::Geometry::BIND_PER_PRIMITIVE_SET);
geom_ptr->addPrimitiveSet(new osg::DrawArrays(GL_LINES,0,2));
geom_ptr->addPrimitiveSet(new osg::DrawArrays(GL_LINES,2,2));
geom_ptr->addPrimitiveSet(new osg::DrawArrays(GL_LINES,4,2));
landmark_geode_ptr_ = new osg::Geode();
landmark_geode_ptr_->addDrawable(geom_ptr);
//set Landmark as ALWAYS ON TOP
landmark_geode_ptr_->setNodeMask(0xffffffff);
landmark_geode_ptr_->getOrCreateStateSet()->setRenderBinDetails(INT_MAX, "DepthSortedBin");
landmark_geode_ptr_->getOrCreateStateSet()->setMode(GL_DEPTH_TEST, ::osg::StateAttribute::OFF | ::osg::StateAttribute::PROTECTED);
landmark_geode_ptr_->getOrCreateStateSet()->setMode(GL_CULL_FACE, ::osg::StateAttribute::ON | ::osg::StateAttribute::PROTECTED );
landmark_geode_ptr_->getOrCreateStateSet()->setMode(GL_LIGHTING, ::osg::StateAttribute::OFF | ::osg::StateAttribute::PROTECTED);
this->asQueue()->addChild(landmark_geode_ptr_);
}
bool WindowsManager::addBox (const char* boxNameCorba,
const float boxSize1,
const float boxSize2,
const float boxSize3,
const value_type* colorCorba)
{
std::string boxName (boxNameCorba);
if (nodes_.find (boxName) != nodes_.end ()) {
std::cout << "You need to chose an other name, \"" << boxName
<< "\" already exist." << std::endl;
return false;
}
else {
mtx_.lock();
LeafNodeBoxPtr_t box = LeafNodeBox::create
(boxName, osgVector3 (boxSize1, boxSize2, boxSize3),
getColor (colorCorba));
WindowsManager::initParent (boxName, box);
addNode (boxName, box);
mtx_.unlock();
return true;
}
}
osgVector3 WindowsManager::corbaConfToOsgVec3 (const value_type* configCorba)
{
return osgVector3 (configCorba[0], configCorba[1], configCorba[2]);
}
