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

changed dynamic cast into type function

Browse Source
This commit is contained in:
Di Zeng
2016-07-28 12:09:18 -07:00
parent 9be1a1b3d3
commit 0f134d7ba4
1 changed files with 8 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/OdometryROS.cpp
+8 -5
View File
@@ -426,7 +426,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
} }
// local map / reference frame // local map / reference frame
if(odomLocalMap_.getNumSubscribers() && dynamic_cast<OdometryF2M*>(odometry_)) if(odomLocalMap_.getNumSubscribers() && odometry_->isF2M())
{ {
pcl::PointCloud<pcl::PointXYZ> cloud; pcl::PointCloud<pcl::PointXYZ> cloud;
const std::multimap<int, cv::Point3f> & map = ((OdometryF2M*)odometry_)->getMap().getWords3(); const std::multimap<int, cv::Point3f> & map = ((OdometryF2M*)odometry_)->getMap().getWords3();
@@ -443,7 +443,8 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
if(odomLastFrame_.getNumSubscribers()) if(odomLastFrame_.getNumSubscribers())
{ {
if(dynamic_cast<OdometryF2M*>(odometry_)) // check which type of Odometry is using
if(odometry_->isF2M()) // If it's Frame to Map Odometry
{ {
const std::multimap<int, cv::Point3f> & words3 = ((OdometryF2M*)odometry_)->getLastFrame().getWords3(); const std::multimap<int, cv::Point3f> & words3 = ((OdometryF2M*)odometry_)->getLastFrame().getWords3();
if(words3.size()) if(words3.size())
@@ -463,10 +464,10 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
odomLastFrame_.publish(cloudMsg); odomLastFrame_.publish(cloudMsg);
} }
} }
else else if(odometry_->isF2F()) // if Using Frame to Frame Odometry
{ {
//Frame to Frame
const Signature & refFrame = ((OdometryF2F*)odometry_)->getRefFrame(); const Signature & refFrame = ((OdometryF2F*)odometry_)->getRefFrame();
if(refFrame.getWords3().size()) if(refFrame.getWords3().size())
{ {
pcl::PointCloud<pcl::PointXYZ> cloud; pcl::PointCloud<pcl::PointXYZ> cloud;
@@ -482,6 +483,8 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
cloudMsg.header.frame_id = odomFrameId_; cloudMsg.header.frame_id = odomFrameId_;
odomLastFrame_.publish(cloudMsg); odomLastFrame_.publish(cloudMsg);
} }
}else{
NODELET_ERROR("ERROR, Wrong Type of Odometry, Shouldn't happen");
} }
} }
} }
@@ -549,7 +552,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
bool OdometryROS::isOdometryF2M() const bool OdometryROS::isOdometryF2M() const
{ {
return dynamic_cast<OdometryF2M*>(odometry_) != 0; return odometry_->isF2M();
} }
bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&) bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)