Fix nodelet issue

xuhao1 · Sep 10, 2020 · 4bb53d2 · 4bb53d2
1 parent ac56202
commit 4bb53d2
Show file tree

Hide file tree

Showing 5 changed files with 7 additions and 10 deletions.
diff --git a/vins_estimator/launch/fisheye.launch b/vins_estimator/launch/fisheye.launch
@@ -1,9 +1,9 @@
 <launch>
     <arg name="manager"     default="swarm_manager"/>
     <arg name="config_file" default="$(find vins)/../config/fisheye_ptgrey_n3/fisheye_cuda.yaml" />
-    <node pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager" output="screen" />
+    <node pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager --no-bond" output="screen"  />
     <node pkg="nodelet" type="nodelet" name="vins_estimator" 
-        args="load vins_nodelet_pkg/VinsNodeletClass /$(arg manager)" output="screen" >
+        args="load vins_nodelet_pkg/VinsNodeletClass /$(arg manager) --no-bond" output="screen" >
         <param name="config_file" type="string" value="$(arg config_file)"/>
         <param name="fisheye_external_flatten" type="bool" value="false"/>
     </node>

diff --git a/vins_estimator/launch/fisheye_node.launch b/vins_estimator/launch/fisheye_node.launch
@@ -1,7 +1,7 @@
 <launch>
     <arg name="manager"     default="swarm_manager"/>
     <arg name="config_file" default="$(find vins)/../config/fisheye_ptgrey_n3/fisheye_cuda.yaml" />
-    <node pkg="vins" type="vins_node_fisheye" name="vins_estimator"  output="screen" launch-prefix="gdb -ex run --args" >
+    <node pkg="vins" type="vins_node_fisheye" name="vins_estimator"  output="screen" >
         <param name="config_file" type="string" value="$(arg config_file)"/>
         <param name="fisheye_external_flatten" type="bool" value="false"/>
     </node>

diff --git 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
@@ -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
@@ -19,15 +19,11 @@ namespace vins_nodelet_pkg
         public:
             VinsNodeletClass() {}
         private:
-            message_filters::Subscriber<sensor_msgs::Image> * image_sub_l;
-            message_filters::Subscriber<sensor_msgs::Image> * image_sub_r;
-
             virtual void onInit() override
             {
                 ros::NodeHandle & n = getMTPrivateNodeHandle();
                 Init(n);
             }
     };
     PLUGINLIB_EXPORT_CLASS(vins_nodelet_pkg::VinsNodeletClass, nodelet::Nodelet);
-
 }