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

added test_d435i_vio.launch example (VINS-Fusion integration)

Browse Source
This commit is contained in:
matlabbe
2019-08-15 16:45:35 -04:00
parent f859eca619
commit 34c75139f2
5 changed files with 75 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/OdometryROS.cpp
+5 -2
View File
@@ -81,7 +81,8 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
previousStamp_(0.0),
expectedUpdateRate_(0.0),
odomStrategy_(Parameters::defaultOdomStrategy()),
waitIMUToinit_(false)
waitIMUToinit_(false),
imuProcessed_(false)
{
}
@@ -487,13 +488,14 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg)
{
SensorData data(imu, 0, stamp);
this->processData(data, msg->header.stamp);
imuProcessed_ = true;
}
}
}
void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
{
if(waitIMUToinit_ && odometry_->framesProcessed() == 0 && odometry_->getPose().isIdentity() && data.imu().empty())
if((waitIMUToinit_ && !imuProcessed_) && odometry_->framesProcessed() == 0 && odometry_->getPose().isIdentity() && data.imu().empty())
{
NODELET_WARN("odometry: waiting imu to initialize orientation (wait_imu_to_init=true)");
return;
@@ -876,6 +878,7 @@ void OdometryROS::reset(const Transform & pose)
guessPreviousPose_.setNull();
previousStamp_ = 0.0;
resetCurrentCount_ = resetCountdown_;
imuProcessed_ = false;
this->flushCallbacks();
}
src/nodelets/stereo_odometry.cpp
-1
View File
@@ -365,7 +365,6 @@ private:
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncPolicy;
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
ros::Subscriber rgbdSub_;
ros::Subscriber imuSub_;
int queueSize_;
};