diff --git a/vins_estimator/launch/fisheye.launch b/vins_estimator/launch/fisheye.launch index 5572ceb..affe8c2 100644 --- a/vins_estimator/launch/fisheye.launch +++ b/vins_estimator/launch/fisheye.launch @@ -1,9 +1,9 @@ - + + args="load vins_nodelet_pkg/VinsNodeletClass /$(arg manager) --no-bond" output="screen" > diff --git a/vins_estimator/launch/fisheye_node.launch b/vins_estimator/launch/fisheye_node.launch index e20f433..87e8739 100644 --- a/vins_estimator/launch/fisheye_node.launch +++ b/vins_estimator/launch/fisheye_node.launch @@ -1,7 +1,7 @@ - + diff --git a/vins_estimator/src/estimator/estimator.cpp b/vins_estimator/src/estimator/estimator.cpp index 97d4d3d..6691222 100644 --- a/vins_estimator/src/estimator/estimator.cpp +++ b/vins_estimator/src/estimator/estimator.cpp @@ -148,7 +148,7 @@ void Estimator::inputIMU(double t, const Vector3d &linearAcceleration, const Vec if (fast_prop_inited) { //if (fast_prop_inited) { if (solver_flag != NON_LINEAR) { - ROS_ERROR("Is not non linear!!!!"); + std::cerr<< "Is not non linear!!!!" << std::endl; exit(-1); } pubLatestOdometry(latest_P, latest_Q, latest_V, t); diff --git a/vins_estimator/src/fisheyeNode.cpp b/vins_estimator/src/fisheyeNode.cpp index dbaf282..812e9e7 100644 --- a/vins_estimator/src/fisheyeNode.cpp +++ b/vins_estimator/src/fisheyeNode.cpp @@ -293,7 +293,6 @@ void VinsNodeBaseClass::pack_and_send_thread(const ros::TimerEvent & e) { void VinsNodeBaseClass::processFlattened(const ros::TimerEvent & e) { TicToc t0; if (fisheye_handler->has_image_in_buffer()) { - auto ret = fisheye_handler->pop_from_buffer(); cur_frame_gray = ret.first; cur_frame = ret.second; @@ -362,8 +361,10 @@ void VinsNodeBaseClass::imu_callback(const sensor_msgs::ImuConstPtr &imu_msg) else { double now_time = ros::Time::now().toSec(); - if (now_time - last_time > 3) + if (now_time - last_time > 3) { + std::cerr << "IMU DT too big, shuting ros down" << std::endl; ros::shutdown(); + } last_time = now_time; } // test end diff --git a/vins_estimator/src/rosNodelet.cpp b/vins_estimator/src/rosNodelet.cpp index 47df2f4..369dba5 100644 --- a/vins_estimator/src/rosNodelet.cpp +++ b/vins_estimator/src/rosNodelet.cpp @@ -19,9 +19,6 @@ namespace vins_nodelet_pkg public: VinsNodeletClass() {} private: - message_filters::Subscriber * image_sub_l; - message_filters::Subscriber * image_sub_r; - virtual void onInit() override { ros::NodeHandle & n = getMTPrivateNodeHandle(); @@ -29,5 +26,4 @@ namespace vins_nodelet_pkg } }; PLUGINLIB_EXPORT_CLASS(vins_nodelet_pkg::VinsNodeletClass, nodelet::Nodelet); - }