Commit d833afe2 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Use curve to display waypoints of a path.

parent 0bc1e7b3
...@@ -93,7 +93,7 @@ namespace hpp { ...@@ -93,7 +93,7 @@ namespace hpp {
char* tmps[1]; char* tmps[1];
hpp::Names_t names (1, 1, tmps, false); hpp::Names_t names (1, 1, tmps, false);
graphics::Configuration pos; graphics::Configuration pos;
osgVector3 pos1, pos2; ::osg::Vec3ArrayRefPtr posSeq = new ::osg::Vec3Array;
for (unsigned int i = 0; i < waypoints->length(); ++i) { for (unsigned int i = 0; i < waypoints->length(); ++i) {
// Make name // Make name
ss.clear(); ss.str(std::string()); ss << pn << "/node" << i; ss.clear(); ss.str(std::string()); ss << pn << "/node" << i;
...@@ -102,18 +102,13 @@ namespace hpp { ...@@ -102,18 +102,13 @@ namespace hpp {
names[0] = jointName.c_str(); names[0] = jointName.c_str();
hpp::TransformSeq_var Ts = hpp->robot()->getJointsPosition(waypoints[i], names); hpp::TransformSeq_var Ts = hpp->robot()->getJointsPosition(waypoints[i], names);
fromHPP(Ts[0], pos); fromHPP(Ts[0], pos);
pos1 = pos2; pos2 = pos.position; posSeq->push_back(pos.position);
// Create the nodes // Create the nodes
if (wsm->nodeExists(xyzName)) wsm->deleteNode(xyzName, false); if (wsm->nodeExists(xyzName)) wsm->deleteNode(xyzName, false);
wsm->addXYZaxis(xyzName, colorN, 0.01f, 0.05f); wsm->addXYZaxis(xyzName, colorN, 0.01f, 0.05f);
wsm->applyConfiguration(xyzName, pos); wsm->applyConfiguration(xyzName, pos);
if (i > 0) {
xyzName.replace(pn.length() + 1, 4, "edge");
qDebug () << xyzName.c_str();
if (wsm->nodeExists(xyzName)) wsm->deleteNode(xyzName, false);
wsm->addLine(xyzName, pos1, pos2, colorE);
}
} }
wsm->addCurve(pn + "/curve", posSeq, colorE);
wsm->refresh(); wsm->refresh();
} }
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment