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

Add the ability to display paths.

parent 19a182a7
......@@ -187,13 +187,14 @@ void HppWidgetsPlugin::configurationValidation()
void HppWidgetsPlugin::selectJointFromBodyName(const std::string &bodyName)
{
boost::regex roadmap ("^roadmap_(.*)/((node)|(edge))([0-9]+)$");
boost::regex roadmap ("^(roadmap|path[0-9]+)_(.*)/(node|edge)([0-9]+)$");
boost::cmatch what;
if (boost::regex_match (bodyName.c_str(), what, roadmap)) {
int n = std::atoi (what[what.size() - 1].first);
std::string type; type.assign(what[2].first, what[2].second);
std::string joint; joint.assign(what[1].first, what[1].second);
qDebug () << "Detected the roadmap" << type.c_str() << n << "of joint" << joint.c_str();
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);
int n = std::atoi (what[4].first);
qDebug () << "Detected the" << group.c_str() << type.c_str() << n << "of joint" << joint.c_str();
if (type == "node") {
try {
hpp::floatSeq_var q = hpp_->problem()->node(n);
......@@ -226,6 +227,9 @@ QList<QAction *> HppWidgetsPlugin::getJointActions(const std::string &jointName)
a = new JointAction (tr("Display roadmap"), jointName, 0);
connect (a, SIGNAL (triggered(std::string)), solverWidget_, SLOT (displayRoadmap(std::string)));
l.append(a);
a = new JointAction (tr("Display selected path"), jointName, 0);
connect (a, SIGNAL (triggered(std::string)), pathPlayer_, SLOT (displayPath(std::string)));
l.append(a);
return l;
}
......
......@@ -45,6 +45,42 @@ PathPlayer::~PathPlayer()
delete ui_;
}
void PathPlayer::displayPath(const std::string jointName)
{
MainWindow* main = MainWindow::instance();
if (!pathIndex()->isEnabled())
main->logError("There is no path. Did you solve a problem ?");
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};
WindowsManagerPtr_t wsm = main->osg();
HppWidgetsPlugin::HppClient* hpp = plugin_->client();
hpp::floatSeqSeq_var waypoints = hpp->problem()->getWaypoints(pid);
wsm->createScene (pn.c_str());
hpp::floatSeq_var curCfg = hpp->robot()->getCurrentConfig();
for (unsigned int i = 0; i < waypoints->length(); ++i) {
float pos[7];
float pos1[3], pos2[3];
hpp->robot()->setCurrentConfig(waypoints[i]);
hpp::Transform__var t = hpp->robot()->getLinkPosition(jointName.c_str());
for (int j = 0; j < 7; ++j) { pos[j] = (float)t.in()[j]; }
for (int j = 0; j < 3; ++j) { pos1[j] = pos2[j]; }
for (int j = 0; j < 3; ++j) { pos2[j] = (float)t.in()[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);
if (i > 0) {
QString lineName = QString::fromStdString(pn).append("/edge%1").arg (i);
wsm->addLine(lineName.toLocal8Bit().data(), pos1, pos2, colorE);
}
}
hpp->robot()->setCurrentConfig(curCfg.in());
wsm->addToGroup(pn.c_str(), "hpp-gui");
wsm->refresh();
}
void PathPlayer::update ()
{
CORBA::Short nbPath = plugin_->client()->problem ()->numberPaths ();
......
......@@ -22,6 +22,9 @@ public:
PathPlayer (HppWidgetsPlugin* plugin, QWidget* parent = 0);
~PathPlayer();
public slots:
void displayPath (const std::string jointName);
private slots:
void update ();
......
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