i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +08:00
Code Issues Packages Projects Releases Wiki Activity

GUI: The global path is transformed in the current referential as the robot moves. Moved Goal actions in Detection menu.

Browse Source
This commit is contained in:
matlabbe
2015-09-21 18:12:55 -04:00
parent b6d2c6163e
commit c60a808c75
5 changed files with 43 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/include/rtabmap/core/Rtabmap.h
View File

@@ -137,6 +137,8 @@ public:
std::vector<std::pair<int, Transform> > getPathNextPoses() const;
std::vector<int> getPathNextNodes() const;
int getPathCurrentGoalId() const;
unsigned int getPathCurrentIndex() const {return _pathCurrentIndex;}
unsigned int getPathCurrentGoalIndex() const {return _pathGoalIndex;}
const Transform & getPathTransformToGoal() const {return _pathTransformToGoal;}
std::map<int, Transform> getForwardWMPoses(int fromId, int maxNearestNeighbors, float radius, int maxDiffID) const;

2
guilib/include/rtabmap/gui/GraphViewer.h
View File

@@ -62,7 +62,7 @@ public:
void updatePosterior(const std::map<int, float> & posterior);
void updateLocalPath(const std::vector<int> & localPath);
void setGlobalPath(const std::vector<std::pair<int, Transform> > & globalPath);
void setCurrentGoalID(int id);
void setCurrentGoalID(int id, const Transform & pose = Transform());
void setLocalRadius(float radius);
void clearGraph();
void clearMap();

24
guilib/src/GraphViewer.cpp
View File

@@ -129,6 +129,15 @@ public:
_poseB = poseB;
}
const Transform & getPoseA() const
{
return _poseA;
}
const Transform & getPoseB() const
{
return _poseB;
}
Link::Type linkType() const {return _type;}
bool isInterSession() const {return _interSession;}
int from() const {return _from;}
@@ -515,7 +524,7 @@ void GraphViewer::setGlobalPath(const std::vector<std::pair<int, Transform> > &
}
}
void GraphViewer::setCurrentGoalID(int id)
void GraphViewer::setCurrentGoalID(int id, const Transform & pose)
{
NodeItem * node = _nodeItems.value(id, 0);
if(node)
@@ -524,7 +533,18 @@ void GraphViewer::setCurrentGoalID(int id)
}
else
{
UWARN("Curent goal %d not found in the graph", id);
UWARN("Current goal %d not found in the graph", id);
}
if(!pose.isNull() && _globalPathLinkItems.size() && _globalPathLinkItems.contains(id))
{
// transform the global path in the goal referential
const LinkItem * oldPose = _globalPathLinkItems.value(id);
Transform t = pose * oldPose->getPoseA().inverse();
for(QMultiMap<int, LinkItem*>::iterator iter=_globalPathLinkItems.begin(); iter!=_globalPathLinkItems.end(); ++iter)
{
iter.value()->setPoses(t*iter.value()->getPoseA(), t*iter.value()->getPoseB());
}
}
}

17
guilib/src/MainWindow.cpp
View File

@@ -1311,7 +1311,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
// update current goal id
if(stat.currentGoalId() > 0)
{
_ui->graphicsView_graphView->setCurrentGoalID(stat.currentGoalId());
_ui->graphicsView_graphView->setCurrentGoalID(stat.currentGoalId(), uValue(stat.poses(), stat.currentGoalId(), Transform()));
}
UDEBUG("");
@@ -5799,6 +5799,9 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionDownload_all_clouds->setEnabled(false);
_ui->actionDownload_graph->setEnabled(false);
_ui->menuSelect_source->setEnabled(false);
_ui->actionLabel_current_location->setEnabled(false);
_ui->actionSend_goal->setEnabled(false);
_ui->actionCancel_goal->setEnabled(false);
_ui->toolBar->findChild<QAction*>("toolbar_source")->setEnabled(false);
_ui->actionTrigger_a_new_map->setEnabled(false);
_ui->doubleSpinBox_stats_imgRate->setEnabled(true);
@@ -5845,6 +5848,9 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionDownload_all_clouds->setEnabled(true);
_ui->actionDownload_graph->setEnabled(true);
_ui->menuSelect_source->setEnabled(true);
_ui->actionLabel_current_location->setEnabled(true);
_ui->actionSend_goal->setEnabled(true);
_ui->actionCancel_goal->setEnabled(true);
_ui->toolBar->findChild<QAction*>("toolbar_source")->setEnabled(true);
_ui->actionTrigger_a_new_map->setEnabled(true);
_ui->doubleSpinBox_stats_imgRate->setEnabled(true);
@@ -5880,6 +5886,9 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionDownload_all_clouds->setEnabled(false);
_ui->actionDownload_graph->setEnabled(false);
_ui->menuSelect_source->setEnabled(false);
_ui->actionLabel_current_location->setEnabled(true);
_ui->actionSend_goal->setEnabled(true);
_ui->actionCancel_goal->setEnabled(true);
_ui->toolBar->findChild<QAction*>("toolbar_source")->setEnabled(false);
_ui->actionTrigger_a_new_map->setEnabled(true);
_ui->doubleSpinBox_stats_imgRate->setEnabled(true);
@@ -5971,6 +5980,9 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionDownload_all_clouds->setEnabled(true);
_ui->actionDownload_graph->setEnabled(true);
_ui->actionTrigger_a_new_map->setEnabled(true);
_ui->actionLabel_current_location->setEnabled(true);
_ui->actionSend_goal->setEnabled(true);
_ui->actionCancel_goal->setEnabled(true);
_ui->statusbar->showMessage(tr("Monitoring..."));
_state = newState;
_elapsedTime->start();
@@ -5990,6 +6002,9 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionDownload_all_clouds->setEnabled(true);
_ui->actionDownload_graph->setEnabled(true);
_ui->actionTrigger_a_new_map->setEnabled(true);
_ui->actionLabel_current_location->setEnabled(true);
_ui->actionSend_goal->setEnabled(true);
_ui->actionCancel_goal->setEnabled(true);
_ui->statusbar->showMessage(tr("Monitoring paused..."));
_state = newState;
_oneSecondTimer->stop();

5
guilib/src/ui/mainWindow.ui
View File

@@ -72,9 +72,6 @@
<addaction name="actionGenerate_map"/>
<addaction name="menuExport_poses"/>
<addaction name="actionPrint_loop_closure_IDs_to_console"/>
<addaction name="separator"/>
<addaction name="actionSend_goal"/>
<addaction name="actionCancel_goal"/>
</widget>
<addaction name="actionDownload_all_clouds"/>
<addaction name="actionDownload_graph"/>
@@ -214,6 +211,8 @@
<addaction name="actionTrigger_a_new_map"/>
<addaction name="separator"/>
<addaction name="actionLabel_current_location"/>
<addaction name="actionSend_goal"/>
<addaction name="actionCancel_goal"/>
</widget>
<widget class="QMenu" name="menuWindow">
<property name="title">