i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +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

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());
}
}
}