i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +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
if(odomLocalMap_.getNumSubscribers() && dynamic_cast<OdometryF2M*>(odometry_))
if(odomLocalMap_.getNumSubscribers() && odometry_->isF2M())
{
pcl::PointCloud<pcl::PointXYZ> cloud;
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(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();
if(words3.size())
@@ -463,10 +464,10 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
odomLastFrame_.publish(cloudMsg);
}
}
else
else if(odometry_->isF2F()) // if Using Frame to Frame Odometry
{
//Frame to Frame
const Signature & refFrame = ((OdometryF2F*)odometry_)->getRefFrame();
if(refFrame.getWords3().size())
{
pcl::PointCloud<pcl::PointXYZ> cloud;
@@ -482,6 +483,8 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
cloudMsg.header.frame_id = odomFrameId_;
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
{
return dynamic_cast<OdometryF2M*>(odometry_) != 0;
return odometry_->isF2M();
}
bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)