rnav supports different TF names

(partly address #486 )
MRPT · Jun 2, 2017 · dc0a1b8 · dc0a1b8
1 parent 95a07f0
commit dc0a1b8
Show file tree

Hide file tree

Showing 3 changed files with 36 additions and 7 deletions.
diff --git a/libs/base/include/mrpt/poses/FrameTransformer.h b/libs/base/include/mrpt/poses/FrameTransformer.h
@@ -49,6 +49,7 @@ class BASE_IMPEXP FrameTransformerInterface
 {
 public:
 	typedef typename SE_traits<DIM>::pose_t pose_t;  //!< This will be mapped to CPose2D (DIM=2) or CPose3D (DIM=3)
+	typedef typename SE_traits<DIM>::lightweight_pose_t lightweight_pose_t;  //!< This will be mapped to mrpt::math::TPose2D (DIM=2) or mrpt::math::TPose3D (DIM=3)
 
 	FrameTransformerInterface();
 	virtual ~FrameTransformerInterface();
@@ -68,11 +69,29 @@ class BASE_IMPEXP FrameTransformerInterface
 	virtual FrameLookUpStatus lookupTransform(
 		const std::string & target_frame,
 		const std::string & source_frame,
-		pose_t & child_wrt_parent,
+		lightweight_pose_t & child_wrt_parent,
 		const mrpt::system::TTimeStamp query_time = INVALID_TIMESTAMP,
 		const double timeout_secs = .0   //!< Timeout
 	) = 0;
 
+	/** \overload */
+	FrameLookUpStatus lookupTransform(
+		const std::string & target_frame,
+		const std::string & source_frame,
+		pose_t & child_wrt_parent,
+		const mrpt::system::TTimeStamp query_time = INVALID_TIMESTAMP,
+		const double timeout_secs = .0   //!< Timeout
+	)
+	{
+		lightweight_pose_t p;
+		FrameLookUpStatus ret = lookupTransform(target_frame, source_frame,p, query_time, timeout_secs);
+		child_wrt_parent = pose_t(p);
+		return ret;
+	}
+
+
+
+
 }; // End of class def.
 
 /** See docs in FrameTransformerInterface.
@@ -92,7 +111,7 @@ class BASE_IMPEXP FrameTransformer : public FrameTransformerInterface<DIM>
 	// See base docs
 	virtual void sendTransform(const std::string & parent_frame,const std::string & child_frame,const typename base_t::pose_t & child_wrt_parent, const mrpt::system::TTimeStamp & timestamp = mrpt::system::now() )  MRPT_OVERRIDE;
 	// See base docs
-	virtual FrameLookUpStatus lookupTransform(const std::string & target_frame, const std::string & source_frame, typename base_t::pose_t & child_wrt_parent, const mrpt::system::TTimeStamp query_time = INVALID_TIMESTAMP, const double timeout_secs = .0) MRPT_OVERRIDE;
+	virtual FrameLookUpStatus lookupTransform(const std::string & target_frame, const std::string & source_frame, typename base_t::lightweight_pose_t & child_wrt_parent, const mrpt::system::TTimeStamp query_time = INVALID_TIMESTAMP, const double timeout_secs = .0) MRPT_OVERRIDE;
 
 protected:
 	//double m_max_extrapolation_time;  //!< for extrapolation in the past or in the future [s]

diff --git a/libs/base/src/poses/FrameTransformer.cpp b/libs/base/src/poses/FrameTransformer.cpp
@@ -65,7 +65,7 @@ template <int DIM>
 FrameLookUpStatus FrameTransformer<DIM>::lookupTransform(
 	const std::string & target_frame,
 	const std::string & source_frame,
-	typename base_t::pose_t & child_wrt_parent,
+	typename base_t::lightweight_pose_t & child_wrt_parent,
 	const mrpt::system::TTimeStamp query_time,
 	const double timeout_secs)
 {

diff --git a/libs/nav/src/reactive/CAbstractPTGBasedReactive.cpp b/libs/nav/src/reactive/CAbstractPTGBasedReactive.cpp
@@ -394,11 +394,21 @@ void CAbstractPTGBasedReactive::performNavigationStep()
 		}
 
 
-		if (m_navigationParams->target.target_frame_id!=m_curPoseVel.pose_frame_id)
+		for (auto &t : targets)
 		{
-			MRPT_TODO("Support submapping navigation");
-			// m_frame_tf->lookupTransform(...);
-			THROW_EXCEPTION_FMT("Different frame_id's are not supported yet!: target_frame_id=`%s` != pose_frame_id=`%s`", m_navigationParams->target.target_frame_id.c_str(), m_curPoseVel.pose_frame_id.c_str());
+			if ( t.target_frame_id != m_curPoseVel.pose_frame_id)
+			{
+				if (m_frame_tf == nullptr) {
+					THROW_EXCEPTION_FMT("Different frame_id's but no frame_tf object attached!: target_frame_id=`%s` != pose_frame_id=`%s`", t.target_frame_id.c_str(), m_curPoseVel.pose_frame_id.c_str());
+				}
+				mrpt::math::TPose2D robot_frame2map_frame;
+				m_frame_tf->lookupTransform(t.target_frame_id, m_curPoseVel.pose_frame_id, robot_frame2map_frame);
+
+				MRPT_LOG_DEBUG_FMT("frame_tf: target_frame_id=`%s` as seen from pose_frame_id=`%s` = %s", t.target_frame_id.c_str(), m_curPoseVel.pose_frame_id.c_str(), robot_frame2map_frame.asString().c_str());
+
+				t.target_coords = robot_frame2map_frame + t.target_coords;
+				t.target_frame_id = m_curPoseVel.pose_frame_id; // Now the coordinates are in the same frame than robot pose
+			}
 		}
 
 		std::vector<TPose2D> relTargets;