message: obstacles_detection: fixed transformation back of segmented clouds to topic's frame
diff: @@ -50,7 +50,8 @@ class ObstaclesDetection : public nodelet::Nodelet
  public:
  ObstaclesDetection() :
  frameId_("base_link"),
  - waitForTransform_(false)
  + waitForTransform_(false),
  + mapFrameProjection_(rtabmap::Parameters::defaultGridMapFrameProjection())
  {}
  virtual ~ObstaclesDetection()
  @@ -194,7 +195,8 @@ private:
  parameterMoved(pnh, "normal_k", rtabmap::Parameters::kGridNormalK(), parameters);
  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());
  }
  @@ -220,7 +222,7 @@ private:
  return;
  }
  - rtabmap::Transform localTransform;
  + rtabmap::Transform localTransform = rtabmap::Transform::getIdentity();
  try
  {
  if(waitForTransform_)
  @@ -326,17 +328,27 @@ private:
  obstaclesCloudWithoutFlatSurfaces->resize(oi);
  }
  - if(!localTransform.isIdentity())
  + if(!localTransform.isIdentity() || !pose.isIdentity())
  {
  - //transform back in topic frame
  - rtabmap::Transform localTransformInv = localTransform.inverse();
  + //transform back in topic frame for 3d clouds and base frame for 2d clouds
  +
  + 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())
  {
  - groundCloud = rtabmap::util3d::transformPointCloud(groundCloud, localTransformInv);
  + groundCloud = rtabmap::util3d::transformPointCloud(groundCloud, t);
  }
  if(obstaclesCloud->size())
  {
  - obstaclesCloud = rtabmap::util3d::transformPointCloud(obstaclesCloud, localTransformInv);
  + obstaclesCloud = rtabmap::util3d::transformPointCloud(obstaclesCloud, t);
  }
  }
  }
  @@ -382,6 +394,7 @@ private:
  bool waitForTransform_;
  rtabmap::OccupancyGrid grid_;
  + bool mapFrameProjection_;
  tf::TransformListener tfListener_;
  