Commit dbd4b677 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Update to changes in gepetto-viewer-corba

parent 23cb82f6
......@@ -146,31 +146,30 @@ namespace hpp {
hpp::intSeq_var& indexes,
hpp::floatSeqSeq_var& points,
CORBA::ULong j,
double epsilon)
float epsilon)
{
gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance ();
const float color[] = {0, 1, 0, 1};
QVector3D norm(0, 0, 0);
osgVector4 color (0, 1, 0, 1);
osgVector3 norm(0, 0, 0);
CORBA::Long iPts = (j == 0) ? 0 : indexes[j - 1];
gepetto::corbaserver::PositionSeq ps; ps.length (indexes[j] - iPts);
if (ps.length() > 3) {
QVector3D a(points[iPts][0] - points[iPts + 1][0],
points[iPts][1] - points[iPts + 1][1],
points[iPts][2] - points[iPts + 1][2]);
QVector3D b(points[iPts + 1][0] - points[iPts + 2][0],
points[iPts + 1][1] - points[iPts + 2][1],
points[iPts + 1][2] - points[iPts + 2][2]);
QVector3D c = QVector3D::crossProduct(a, b);
if (c.length() > 0.00001 || c.length() < -0.00001)
graphics::WindowsManager::Vec3ArrayPtr_t ps;
ps->resize (indexes[j] - iPts);
if (ps->size() > 3) {
osgVector3 a((float)(points[iPts][0] - points[iPts + 1][0]),
(float)(points[iPts][1] - points[iPts + 1][1]),
(float)(points[iPts][2] - points[iPts + 1][2]));
osgVector3 b((float)(points[iPts + 1][0] - points[iPts + 2][0]),
(float)(points[iPts + 1][1] - points[iPts + 2][1]),
(float)(points[iPts + 1][2] - points[iPts + 2][2]));
osgVector3 c = a ^ b;
if (c.length() > 0.00001)
norm = c / c.length();
}
for (CORBA::Long k = iPts; k < indexes[j]; ++k) {
ps[k - iPts][0] = (float)points[k][0] + norm.x() * epsilon;
ps[k - iPts][1] = (float)points[k][1] + norm.y() * epsilon;
ps[k - iPts][2] = (float)points[k][2] + norm.z() * epsilon;
(*ps)[k - iPts] = osgVector3((float)points[k][0],(float)points[k][1],(float)points[k][2]) + norm * epsilon;
}
main->osg()->addCurve (name.c_str(), ps, color);
main->osg()->setCurveMode (name.c_str(), GL_POLYGON);
main->osg()->addCurve (name, ps, color);
main->osg()->setCurveMode (name, GL_POLYGON);
}
void HppManipulationWidgetsPlugin::drawRobotContacts()
......@@ -221,17 +220,17 @@ namespace hpp {
gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance ();
hpp::Names_t_var rcs = hpp_->problem()->getAvailable("handle");
hpp::Transform__var t (new Transform_);
graphics::WindowsManager::value_type t_gv[7];
const float color[] = {0, 1, 0, 1};
graphics::Configuration config;
const graphics::WindowsManager::Color_t color (0, 1, 0, 1);
for (CORBA::ULong i = 0; i < rcs->length(); ++i) {
const std::string jn =
hpp_->robot()->getHandlePositionInJoint (rcs[i],t.out());
std::string groupName = createJointGroup (jn.c_str());
std::string groupName = createJointGroup (jn);
std::string hn = "handle_" + escapeJointName (std::string(rcs[i]));
fromHPP(t, t_gv);
main->osg()->addXYZaxis (hn.c_str(), color, 0.005f, 1.f);
main->osg()->applyConfiguration (hn.c_str(), t_gv);
main->osg()->addToGroup (hn.c_str(), groupName.c_str());
fromHPP(t, config);
main->osg()->addXYZaxis (hn, color, 0.005f, 1.f);
main->osg()->applyConfiguration (hn, config);
main->osg()->addToGroup (hn, groupName);
}
main->osg()->refresh();
gepetto::gui::MainWindow::instance()->requestRefresh();
......@@ -242,17 +241,17 @@ namespace hpp {
gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance ();
hpp::Names_t_var rcs = hpp_->problem()->getAvailable("gripper");
hpp::Transform__var t (new Transform_);
graphics::WindowsManager::value_type t_gv[7];
const float color[] = {0, 1, 0, 1};
graphics::Configuration config;
const graphics::WindowsManager::Color_t color (0, 1, 0, 1);
for (CORBA::ULong i = 0; i < rcs->length(); ++i) {
const std::string jn =
hpp_->robot()->getGripperPositionInJoint (rcs[i],t.out());
std::string groupName = createJointGroup (jn.c_str());
std::string groupName = createJointGroup (jn);
std::string hn = "gripper_" + escapeJointName (std::string(rcs[i]));
fromHPP(t, t_gv);
main->osg()->addXYZaxis (hn.c_str(), color, 0.005f, 1.f);
main->osg()->applyConfiguration (hn.c_str(), t_gv);
main->osg()->addToGroup (hn.c_str(), groupName.c_str());
fromHPP(t, config);
main->osg()->addXYZaxis (hn, color, 0.005f, 1.f);
main->osg()->applyConfiguration (hn, config);
main->osg()->addToGroup (hn, groupName);
}
main->osg()->refresh();
gepetto::gui::MainWindow::instance()->requestRefresh();
......
......@@ -119,7 +119,7 @@ signals:
/// \param j index in table indexes
/// \param epsilon offset
void drawContactSurface(const std::string& name, hpp::intSeq_var& indexes,
hpp::floatSeqSeq_var& points, CORBA::ULong j, double epsilon = 0.0001);
hpp::floatSeqSeq_var& points, CORBA::ULong j, float epsilon = 0.0001f);
HppManipClient* hpp_;
......
#ifndef HPP_GUI_CONVERSIONS_HH
#define HPP_GUI_CONVERSIONS_HH
#include <gepetto/gui/windows-manager.hh>
namespace hpp {
namespace gui {
template <typename Out>
void fromHPP(const hpp::Transform__var& in, Out out[7])
inline void fromHPP(const hpp::Transform__var& in, osgVector3 v)
{
typedef graphics::WindowsManager::value_type type;
const hpp::Transform__slice* t (in.in());
v.set((type)t[0], (type)t[1], (type)t[2]);
}
inline void fromHPP(const hpp::Transform__var& in, osgQuat q)
{
for (int i = 0; i < 3; ++i) {
out[i] = (Out)in.in()[i];
out[4+i] = (Out)in.in()[3+i];
}
out[3] = (Out)in.in()[6];
typedef graphics::WindowsManager::value_type type;
const hpp::Transform__slice* t (in.in());
q.set((type)t[3], (type)t[4], (type)t[5], (type)t[6]);
}
inline void fromHPP(const hpp::Transform__var& in, graphics::Configuration c)
{
fromHPP(in, c.position);
fromHPP(in, c.quat);
}
} // namespace gui
} // namespace hpp
......
......@@ -29,6 +29,8 @@ using CORBA::ULong;
namespace hpp {
namespace gui {
using gepetto::gui::MainWindow;
typedef graphics::WindowsManager::Color_t OsgColor_t;
typedef graphics::Configuration OsgConfiguration_t;
HppWidgetsPlugin::JointElement::JointElement (
const std::string& n, const std::string& prefix,
......@@ -37,7 +39,7 @@ namespace hpp {
bodyNames (bns.length()), item (i), updateViewer (bns.length(), updateV)
{
for (std::size_t i = 0; i < bns.length(); ++i)
bodyNames[i] = std::string(bns[i]);
bodyNames[i] = std::string(bns[(CORBA::ULong)i]);
}
HppWidgetsPlugin::HppWidgetsPlugin() :
......@@ -224,7 +226,9 @@ namespace hpp {
"interface) and you did not refresh this GUI. "
"Use the refresh button \"Tools\" menu.");
}
float T[7];
// Something smarter could be done here.
// For instance, the joint tree item could know the NodePtr_t of their bodies.
OsgConfiguration_t T;
for (JointMap::iterator ite = jointMap_.begin ();
ite != jointMap_.end (); ite++) {
for (std::size_t i = 0; i < ite->bodyNames.size(); ++i)
......@@ -233,7 +237,7 @@ namespace hpp {
fromHPP(t, T);
if (ite->updateViewer[i]) {
std::string n = ite->prefix + ite->bodyNames[i];
ite->updateViewer[i] = main->osg()->applyConfiguration(n.c_str(), T);
ite->updateViewer[i] = main->osg()->applyConfiguration(n, T);
}
}
if (!ite->item) continue;
......@@ -245,7 +249,7 @@ namespace hpp {
std::string n = escapeJointName(*it);
hpp::Transform__var t = client()->robot()->getJointPosition(it->c_str());
fromHPP(t, T);
main->osg()->applyConfiguration (n.c_str (), T);
main->osg()->applyConfiguration (n, T);
}
main->osg()->refresh();
}
......@@ -304,7 +308,7 @@ namespace hpp {
std::string group; group.assign(what[1].first, what[1].second);
std::string joint; joint.assign(what[2].first, what[2].second);
std::string type; type .assign(what[3].first, what[3].second);
std::size_t n = std::atoi (what[4].first);
CORBA::ULong n = (CORBA::ULong)std::atoi (what[4].first);
qDebug () << "Detected the" << group.c_str() << type.c_str() << n << "of joint" << joint.c_str();
if (group == "roadmap") {
if (type == "node") {
......@@ -439,14 +443,14 @@ namespace hpp {
gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance ();
std::string target = createJointGroup(jointName);
const std::string n = target + "/XYZ";
const float color[4] = {1,0,0,1};
const OsgColor_t color(1,0,0,1);
/// This returns false if the frame already exists
if (main->osg()->addXYZaxis (n.c_str(), color, 0.005f, 1.f)) {
main->osg()->setVisibility (n.c_str(), "ALWAYS_ON_TOP");
if (main->osg()->addXYZaxis (n, color, 0.005f, 1.f)) {
main->osg()->setVisibility (n, "ALWAYS_ON_TOP");
return;
} else {
main->osg()->setVisibility (n.c_str(), "ALWAYS_ON_TOP");
main->osg()->setVisibility (n, "ALWAYS_ON_TOP");
}
}
......@@ -455,11 +459,11 @@ namespace hpp {
gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance ();
hpp::Names_t_var obs = client()->obstacle()->getObstacleNames (true, false);
hpp::Transform__var cfg = hpp::Transform__alloc () ;
float d[7];
for (size_t i = 0; i < obs->length(); ++i) {
client()->obstacle()->getObstaclePosition (obs[(ULong) i], cfg.out());
OsgConfiguration_t d;
for (ULong i = 0; i < obs->length(); ++i) {
client()->obstacle()->getObstaclePosition (obs[i], cfg.out());
fromHPP(cfg, d);
main->osg ()->applyConfiguration(obs[(ULong) i], d);
main->osg ()->applyConfiguration(std::string(obs[i]), d);
}
main->osg()->refresh();
}
......@@ -494,12 +498,11 @@ namespace hpp {
if (main->osg()->createGroup(target.c_str())) {
main->osg()->addToGroup(target.c_str(), "joints");
hpp::Transform__var t = client()->robot()->getJointPosition
(jn.c_str());
float p[7];
hpp::Transform__var t = client()->robot()->getJointPosition (jn.c_str());
OsgConfiguration_t p;
fromHPP(t, p);
jointFrames_.push_back(jn);
main->osg()->applyConfiguration (target.c_str(), p);
main->osg()->applyConfiguration (target, p);
main->osg()->refresh();
}
return target;
......
......@@ -58,31 +58,35 @@ namespace hpp {
int pid = pathIndex()->value();
std::stringstream ss; ss << "path" << pid << "_" << jointName;
std::string pn = ss.str();
float colorN[] = {0.f, 0.f, 1.f, 1.f};
float colorE[] = {1.f, 0.f, 0.f, 1.f};
const osgVector4 colorN (0.f, 0.f, 1.f, 1.f),
colorE (1.f, 0.f, 0.f, 1.f);
gepetto::gui::WindowsManagerPtr_t wsm = main->osg();
HppWidgetsPlugin::HppClient* hpp = plugin_->client();
hpp::floatSeqSeq_var waypoints = hpp->problem()->getWaypoints((CORBA::UShort)pid);
wsm->createScene (pn.c_str());
wsm->createScene (pn);
hpp::floatSeq_var curCfg = hpp->robot()->getCurrentConfig();
graphics::Configuration pos;
osgVector3 pos1, pos2;
for (unsigned int i = 0; i < waypoints->length(); ++i) {
float pos[7];
float pos1[3], pos2[3];
// Make name
ss.clear(); ss.str(std::string()); ss << pn << "/node" << i;
std::string xyzName = ss.str();
// Get positions
hpp->robot()->setCurrentConfig(waypoints[i]);
hpp::Transform__var t = hpp->robot()->getLinkPosition(jointName.c_str());
fromHPP(t, pos);
for (int j = 0; j < 3; ++j) { pos1[j] = pos2[j]; }
for (int j = 0; j < 3; ++j) { pos2[j] = pos[j]; }
QString xyzName = QString::fromStdString(pn).append("/node%1").arg (i);
wsm->addXYZaxis(xyzName.toLocal8Bit().data(), colorN, 0.01f, 1.f);
wsm->applyConfiguration(xyzName.toLocal8Bit().data(), pos);
pos1 = pos2; pos2 = pos.position;
// Create the nodes
wsm->addXYZaxis(xyzName, colorN, 0.01f, 1.f);
wsm->applyConfiguration(xyzName, pos);
if (i > 0) {
QString lineName = QString::fromStdString(pn).append("/edge%1").arg (i);
wsm->addLine(lineName.toLocal8Bit().data(), pos1, pos2, colorE);
xyzName.replace(pn.length() + 1, 4, "edge");
qDebug () << xyzName.c_str();
wsm->addLine(xyzName, pos1, pos2, colorE);
}
}
hpp->robot()->setCurrentConfig(curCfg.in());
wsm->addToGroup(pn.c_str(), "hpp-gui");
wsm->addToGroup(pn, "hpp-gui");
wsm->refresh();
}
......@@ -110,26 +114,26 @@ namespace hpp {
CORBA::UShort pid = (CORBA::UShort) pathIndex()->value();
std::stringstream ss; ss << "curvedpath_" << pid << "_" << jointName;
std::string pn = ss.str();
float colorE[] = {1.f, 0.f, 0.f, 1.f};
graphics::WindowsManager::Color_t colorE (1.f, 0.f, 0.f, 1.f);
gepetto::gui::WindowsManagerPtr_t wsm = main->osg();
HppWidgetsPlugin::HppClient* hpp = plugin_->client();
hpp::floatSeq_var curCfg = hpp->robot()->getCurrentConfig();
CORBA::Double length = hpp->problem()->pathLength(pid);
double dt = lengthBetweenRefresh();
std::size_t nbPos = (std::size_t)(length / dt) + 1;
gepetto::corbaserver::PositionSeq_var posSeq = new gepetto::corbaserver::PositionSeq (nbPos);
posSeq->length(nbPos);
CORBA::ULong nbPos = (CORBA::ULong)(length / dt) + 1;
::osg::Vec3ArrayRefPtr posSeq = new ::osg::Vec3Array;
float statusStep = 100.f / (float) nbPos;
emit displayPath_status(0);
for (std::size_t i = 0; i < nbPos; ++i) {
for (CORBA::ULong i = 0; i < nbPos; ++i) {
double t = std::min (i * dt, length);
hpp::floatSeq_var q = hpp->problem()->configAtParam(pid, t);
hpp->robot()->setCurrentConfig(q);
hpp::Transform__var transform = hpp->robot()->getLinkPosition(jointName.c_str());
for (int j = 0; j < 3; ++j) { posSeq[i][j] = (float)transform.in()[j]; }
const hpp::Transform__slice* tt = transform.in();
posSeq->push_back(::osg::Vec3 ((float)tt[0],(float)tt[1],(float)tt[2]));
emit displayPath_status(qFloor ((float)i * statusStep));
}
wsm->addCurve(pn.c_str(), posSeq.in(), colorE);
wsm->addCurve(pn, posSeq, colorE);
hpp->robot()->setCurrentConfig(curCfg.in());
wsm->addToGroup(pn.c_str(), "hpp-gui");
wsm->refresh();
......
......@@ -58,7 +58,7 @@ namespace hpp {
if (currentNodeId_ >= nbNodes && currentEdgeId_ >= nbEdges) return;
std::string rn = roadmapName ();
float color[4];
Color color;
gepetto::gui::WindowsManagerPtr_t wsm = gepetto::gui::MainWindow::instance()->osg();
if (nbNodes == 0) {
gepetto::gui::MainWindow::instance()->logError("There is no node in the roadmap.");
......@@ -70,17 +70,17 @@ namespace hpp {
nodePosition (currentNodeId_, pos);
nodeColor (currentNodeId_, color);
std::string name = nodeName (currentNodeId_);
wsm->addXYZaxis(name.c_str(), color, radius, axisSize);
wsm->applyConfiguration(name.c_str(), pos);
wsm->addXYZaxis(name, color, radius, axisSize);
wsm->applyConfiguration(name, pos);
}
for (; currentEdgeId_ < nbEdges; ++currentEdgeId_) {
Position pos1, pos2;
edgePositions (currentEdgeId_, pos1, pos2);
edgeColor (currentEdgeId_, color);
std::string name = edgeName (currentEdgeId_);
wsm->addLine(name.c_str(), pos1, pos2, color);
wsm->addLine(name, pos1, pos2, color);
}
wsm->addToGroup(rn.c_str(), "hpp-gui");
wsm->addToGroup(rn, "hpp-gui");
afterDisplay ();
wsm->refresh();
}
......
......@@ -3,6 +3,7 @@
#include <hpp/corbaserver/common.hh>
#include <gepetto/gui/color-map.hh>
#include <gepetto/gui/windows-manager.hh>
#include <hppwidgetsplugin/hppwidgetsplugin.hh>
namespace hpp {
......@@ -11,9 +12,9 @@ namespace hpp {
public:
typedef unsigned int NodeID;
typedef unsigned int EdgeID;
typedef float Frame[7];
typedef float Position[3];
typedef float Color[4];
typedef graphics::Configuration Frame;
typedef osgVector3 Position;
typedef graphics::WindowsManager::Color_t Color;
float radius, axisSize;
......
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