i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

obstacles_detection: fixed transformation back of segmented clouds to topic's frame

Browse Source
This commit is contained in:
matlabbe
2017-07-27 16:29:43 -04:00
parent 936e5ad093
commit dcfbab67a9
1 changed files with 21 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/obstacles_detection.cpp
+21 -8
View File
@@ -50,7 +50,8 @@ class ObstaclesDetection : public nodelet::Nodelet
public: public:
ObstaclesDetection() : ObstaclesDetection() :
frameId_("base_link"), frameId_("base_link"),
waitForTransform_(false) waitForTransform_(false),
mapFrameProjection_(rtabmap::Parameters::defaultGridMapFrameProjection())
{} {}
virtual ~ObstaclesDetection() virtual ~ObstaclesDetection()
@@ -194,7 +195,8 @@ private:
parameterMoved(pnh, "normal_k", rtabmap::Parameters::kGridNormalK(), parameters); parameterMoved(pnh, "normal_k", rtabmap::Parameters::kGridNormalK(), parameters);
UASSERT(uContains(parameters, rtabmap::Parameters::kGridMapFrameProjection())); UASSERT(uContains(parameters, rtabmap::Parameters::kGridMapFrameProjection()));
if(uStr2Bool(parameters.at(rtabmap::Parameters::kGridMapFrameProjection())) && mapFrameId_.empty()) mapFrameProjection_ = uStr2Bool(parameters.at(rtabmap::Parameters::kGridMapFrameProjection()));
if(mapFrameProjection_ && mapFrameId_.empty())
{ {
NODELET_ERROR("obstacles_detection: Parameter \"%s\" is true but map_frame_id is not set!", rtabmap::Parameters::kGridMapFrameProjection().c_str()); NODELET_ERROR("obstacles_detection: Parameter \"%s\" is true but map_frame_id is not set!", rtabmap::Parameters::kGridMapFrameProjection().c_str());
} }
@@ -220,7 +222,7 @@ private:
return; return;
} }
rtabmap::Transform localTransform; rtabmap::Transform localTransform = rtabmap::Transform::getIdentity();
try try
{ {
if(waitForTransform_) if(waitForTransform_)
@@ -326,17 +328,27 @@ private:
obstaclesCloudWithoutFlatSurfaces->resize(oi); obstaclesCloudWithoutFlatSurfaces->resize(oi);
} }
if(!localTransform.isIdentity()) if(!localTransform.isIdentity() || !pose.isIdentity())
{ {
//transform back in topic frame //transform back in topic frame for 3d clouds and base frame for 2d clouds
rtabmap::Transform localTransformInv = localTransform.inverse();
float roll, pitch, yaw;
pose.getEulerAngles(roll, pitch, yaw);
rtabmap::Transform t = rtabmap::Transform(0,0, mapFrameProjection_?pose.z():0, roll, pitch, 0);
if(obstaclesCloudWithoutFlatSurfaces->size() && !pose.isIdentity())
{
obstaclesCloudWithoutFlatSurfaces = rtabmap::util3d::transformPointCloud(obstaclesCloudWithoutFlatSurfaces, t.inverse());
}
t = (t*localTransform).inverse();
if(groundCloud->size()) if(groundCloud->size())
{ {
groundCloud = rtabmap::util3d::transformPointCloud(groundCloud, localTransformInv); groundCloud = rtabmap::util3d::transformPointCloud(groundCloud, t);
} }
if(obstaclesCloud->size()) if(obstaclesCloud->size())
{ {
obstaclesCloud = rtabmap::util3d::transformPointCloud(obstaclesCloud, localTransformInv); obstaclesCloud = rtabmap::util3d::transformPointCloud(obstaclesCloud, t);
} }
} }
} }
@@ -382,6 +394,7 @@ private:
bool waitForTransform_; bool waitForTransform_;
rtabmap::OccupancyGrid grid_; rtabmap::OccupancyGrid grid_;
bool mapFrameProjection_;
tf::TransformListener tfListener_; tf::TransformListener tfListener_;