200hz publishing works as expected

Author: mpkuse

src/Composer.cpp

@@ -227,10 +227,23 @@ void Composer::pose_assember_thread( int looprate )
 }
 
 
+int Composer::get_last_known_camerapose( Matrix4d& w_T_lastcam, ros::Time& stamp_of_it )
+{
+    std::lock_guard<std::mutex> lk(mx);
+    const int sz = global_lmb.size();
+    if( sz == 0 )
+        return -1;
+
+    auto it = global_lmb.rbegin();
+    w_T_lastcam = *(it);
+
+    stamp_of_it = manager->getNodeTimestamp( sz-1 );
+    return sz-1;
+}
 
 // #define __Composer_bf_traj_publish_thread( msg ) msg;
 #define __Composer_bf_traj_publish_thread( msg ) ;
-void Composer::bf_traj_publish_thread( int looprate )
+void Composer::bf_traj_publish_thread( int looprate ) const
 {
     //--- Options
     const int options_line_color_style = 10; // should be either 10 or 12. 10:color by WorldID; 12:  //color by world setID
@@ -242,6 +255,7 @@ void Composer::bf_traj_publish_thread( int looprate )
     cout << TermColor::GREEN() << "start `Composer::bf_traj_publish_thread` @ " << looprate << " hz" << TermColor::RESET() << endl;
     assert( looprate > 0 && looprate < 50 );
     ros::Rate rate(looprate);
+    bool published_axis = false;
 
     static thread_local std::mt19937 generator;
     std::uniform_int_distribution<int> distribution(0,100);
@@ -327,14 +341,20 @@ void Composer::bf_traj_publish_thread( int looprate )
 
         }
 
+        if( published_axis==false || rand() % 100 == 0 ) {
+        // Publish Axis - publish infrequently
+        Matrix4d _axis_pose = Matrix4d::Identity();
+        viz->publishXYZAxis( _axis_pose, "opt_traj_axis", 0  );
+        published_axis = true;
+        }
     }
 
     cout << TermColor::RED() << "Finished `Composer::bf_traj_publish_thread`\n" << TermColor::RESET() << endl;
 }
 
 
 
-void Composer::cam_visual_publish_thread( int looprate )
+void Composer::cam_visual_publish_thread( int looprate ) const
 {
     //--- Options
     const int options_linewidth_multiplier = 3; //default 3
@@ -381,7 +401,7 @@ void Composer::cam_visual_publish_thread( int looprate )
 
 // #define __Composer__loopedge_publish_thread(msg) msg;
 #define __Composer__loopedge_publish_thread(msg) ;
-void Composer::loopedge_publish_thread( int looprate )
+void Composer::loopedge_publish_thread( int looprate ) const
 {
     cout << TermColor::GREEN() << "start `Composer::loopedge_publish_thread` @" << looprate << " hz" << TermColor::RESET() << endl;
     assert( looprate > 0 && looprate < 50 );
@@ -468,7 +488,7 @@ void Composer::loopedge_publish_thread( int looprate )
 
 // #define __Composer__disjointset_statusimage_publish_thread(msg) msg;
 #define __Composer__disjointset_statusimage_publish_thread(msg) ;
-void Composer::disjointset_statusimage_publish_thread( int looprate )
+void Composer::disjointset_statusimage_publish_thread( int looprate ) const
 {
     cout << TermColor::GREEN() << "start `Composer::disjointset_statusimage_publish_thread` @" << looprate << " hz" << TermColor::RESET() << endl;
     assert( looprate > 0 && looprate < 50 );
@@ -509,3 +529,101 @@ void Composer::disjointset_statusimage_publish_thread( int looprate )
 
     cout << TermColor::RED() << "Finished `Composer::disjointset_statusimage_publish_thread`\n" << TermColor::RESET() << endl;
 }
+
+
+
+
+//------ Publish body pose @200Hz ------//
+
+void Composer::setup_200hz_publishers()
+{
+    // pubx =
+    // cmpr->set_200hz_marker_pub( pubx )
+    string marker_topic = string( "hz200/visualization_marker");
+    ROS_INFO( "[Composer::setup_200hz_publishers] Publish to %s", marker_topic.c_str() );
+    pub_hz200_marker = nh.advertise<visualization_msgs::Marker>( marker_topic , 1000 );
+
+
+    // puby =
+    // cmpr->set_200hz_odom_pub( puby )
+}
+
+
+// #define _Composer__imu_propagate_callback_( msg ) msg;
+#define _Composer__imu_propagate_callback_( msg ) ;
+void Composer::imu_propagate_callback( const nav_msgs::Odometry::ConstPtr& msg )
+{
+    _Composer__imu_propagate_callback_(
+    cout << TermColor::iBLUE() << "[Composer::imu_propagate_callback] t=" << msg->header.stamp << TermColor::RESET() << endl;
+    )
+
+    Matrix4d w_T_imucurr;  // = pose from msg
+    PoseManipUtils::geometry_msgs_Pose_to_eigenmat( msg->pose.pose, w_T_imucurr );
+
+
+
+
+    //
+    // Last Known camera-pose? - after pose graph solver
+    Matrix4d wf_T_camlast;
+    ros::Time cam_t;
+    int posegraph_nodeidx = get_last_known_camerapose( wf_T_camlast, cam_t );
+    if( posegraph_nodeidx < 0  ) {
+        _Composer__imu_propagate_callback_(
+        cout << TermColor::iYELLOW() << "posegraph_nodeidx was neg, meaning non existant last known cam pose\n" << TermColor::RESET();
+        )
+        return ;
+    }
+
+    _Composer__imu_propagate_callback_(
+    cout << "\tLast known camera-pose is at posegraphnode postion=" << posegraph_nodeidx << " is at t=" << cam_t << " which is "<< msg->header.stamp - cam_t << " behind current imu_prop msg\n";
+    )
+
+
+    //
+    // extrinsic-calibration value:  imu_T_cam
+    if( manager->is_imu_T_cam_available() == false ) {
+        _Composer__imu_propagate_callback_(
+        cout << TermColor::iYELLOW() << "imu-cam extrinsic is not yet available\n" << TermColor::RESET() << endl;
+        )
+        return;
+    }
+    Matrix4d imu_T_cam = manager->get_imu_T_cam();
+
+    //
+    // relative pose between when imu at t=now_t and when imu was at t=last_known_t
+    Matrix4d w_T_imulast = manager->getNodePose(posegraph_nodeidx) * imu_T_cam.inverse();
+    Matrix4d imulast_T_imucurr = w_T_imulast.inverse() * w_T_imucurr;
+
+
+    //
+    // Add this relative pose to `wf_T_camlast`
+    Matrix4d wf_T_imucurr = ( wf_T_camlast * imu_T_cam.inverse() ) * imulast_T_imucurr;
+
+
+
+    //
+    // Make viz marker and publish
+    visualization_msgs::Marker imu_m;
+    RosMarkerUtils::init_XYZ_axis_marker( imu_m, 1.0 );
+    imu_m.ns = "hz100_imu";
+    imu_m.id = 0;
+    RosMarkerUtils::setpose_to_marker( wf_T_imucurr, imu_m  );
+    pub_hz200_marker.publish( imu_m );
+
+    visualization_msgs::Marker imu_txt;
+    RosMarkerUtils::init_text_marker( imu_txt );
+    imu_txt.ns = "hz100_imu_txt";
+    imu_txt.text = "IMU@200hz";
+    imu_txt.scale.z = 0.5;
+    RosMarkerUtils::setcolor_to_marker( 1.0, 1.0, 1.0, imu_txt );
+    RosMarkerUtils::setpose_to_marker( wf_T_imucurr, imu_txt  );
+    pub_hz200_marker.publish( imu_txt );
+
+
+
+
+}
+
+
+//------ END Publish body pose @200Hz ------//

src/Composer.h

@@ -14,6 +14,9 @@
 #include <mutex>
 #include <atomic>
 
+// ROS
+
+
 // Eigen
 #include <Eigen/Dense>
 #include <Eigen/Geometry>
@@ -29,12 +32,18 @@ using namespace std;
 class Composer
 {
 public:
-    Composer( const NodeDataManager * manager, const PoseGraphSLAM * slam, const VizPoseGraph * viz )
-    : manager(manager), slam(slam), viz(viz)
+    Composer( const NodeDataManager * manager,
+              const PoseGraphSLAM * slam,
+              const VizPoseGraph * viz,
+              ros::NodeHandle& _nh
+             )
+    : manager(manager), slam(slam), viz(viz), nh( _nh )
     {
         b_pose_assember = false;
         b_bf_traj_publish = false;
         b_cam_visual_publish = false;
+        b_loopedge_publish = false;
+        b_disjointset_statusimage_publish = false;
     }
 
 private:
@@ -45,6 +54,7 @@ class Composer
     const NodeDataManager * manager;
     const PoseGraphSLAM * slam;
     const VizPoseGraph * viz ;
+    ros::NodeHandle nh;
 
 
 
@@ -58,6 +68,10 @@ class Composer
     void pose_assember_enable() { b_pose_assember = true;}
     void pose_assember_disable() {b_pose_assember = false; }
 
+    // Returns the last element in `global_lmb` along with the timestamp
+    // Returns -1 when len(global_lmb) is zero, else return the posegraph node index of it. 
+    int get_last_known_camerapose( Matrix4d& w_T_lastcam, ros::Time& stamp_of_it );
+
 private:
     atomic<bool> b_pose_assember;
     map<int, vector<Matrix4d> > global_jmb; // key: worldID, value: vector of poses
@@ -71,7 +85,7 @@ class Composer
     //-------------------------------------------//
     //  makes use of global_jmb and publish all
 public:
-    void bf_traj_publish_thread( int looprate=15 );
+    void bf_traj_publish_thread( int looprate=15 ) const;
     void bf_traj_publish_enable() { b_bf_traj_publish = true;}
     void bf_traj_publish_disable() {b_bf_traj_publish = false; }
 
@@ -86,7 +100,7 @@ class Composer
     //---------------------//
     //  makes use of global_jmb and publish last cam
 public:
-    void cam_visual_publish_thread( int looprate = 30 );
+    void cam_visual_publish_thread( int looprate = 30 ) const;
     void cam_visual_publish_enable() { b_cam_visual_publish = true;}
     void cam_visual_publish_disable() {b_cam_visual_publish = false; }
 
@@ -100,7 +114,7 @@ class Composer
     //-------------------------//
     //  makes use of  manager->getEdgeLen() and global_lmb and publish edges.
 public:
-    void loopedge_publish_thread( int looprate = 10 );
+    void loopedge_publish_thread( int looprate = 10 ) const;
     void loopedge_publish_enable() { b_loopedge_publish = true;}
     void loopedge_publish_disable() {b_loopedge_publish = false; }
 
@@ -115,13 +129,23 @@ class Composer
     //--------------------------------//
     //  makes use of  manager->getEdgeLen() and global_lmb and publish edges.
 public:
-    void disjointset_statusimage_publish_thread( int looprate = 10 );
+    void disjointset_statusimage_publish_thread( int looprate = 10 ) const;
     void disjointset_statusimage_publish_enable() { b_disjointset_statusimage_publish = true;}
     void disjointset_statusimage_publish_disable() {b_disjointset_statusimage_publish = false; }
 
 private:
         atomic<bool> b_disjointset_statusimage_publish;
 
 
+    //-----------------------------------//
+    //--- 200 Hz imu pose publication ---//
+    //-----------------------------------//
+    // Note: The publisher and subscribers are private to this thread itself.
+public:
+    void setup_200hz_publishers();
+    void imu_propagate_callback( const nav_msgs::Odometry::ConstPtr& msg );
+
+private:
+    ros::Publisher pub_hz200_marker;
 
 };

src/NodeDataManager.cpp

@@ -18,8 +18,8 @@ worlds_handle_raw_ptr = new Worlds();
 
 }
 
-#define __NODEDATAMANAGER_CALLBACKS( msg ) msg;
-// #define __NODEDATAMANAGER_CALLBACKS(msg) ;
+// #define __NODEDATAMANAGER_CALLBACKS( msg ) msg;
+#define __NODEDATAMANAGER_CALLBACKS(msg) ;
 void NodeDataManager::camera_pose_callback( const nav_msgs::Odometry::ConstPtr& msg )
 {
     // ROS_INFO( "NodeDataManager::camera_pose_callback");

src/keyframe_pose_graph_slam_node.cpp

@@ -276,9 +276,8 @@ int main( int argc, char ** argv)
     ros::Subscriber sub_cam_imu_extrinsic = nh.subscribe( extrinsic_cam_imu_topic, 1000, &NodeDataManager::extrinsic_cam_imu_callback, manager );
 
 
-    // TODO
-    // subscribes to w_T_imu @100hz and publish the pose in my world frame at 100hz
-    // implement this in composer class.
+
+
 
     // Setup publishers
     //--- Marker ---//
@@ -312,7 +311,7 @@ int main( int argc, char ** argv)
     // std::thread th_slam( &PoseGraphSLAM::new_optimize6DOF, slam );
 
     slam->reinit_ceres_problem_onnewloopedge_optimize6DOF_enable();
-    slam->reinit_ceres_problem_onnewloopedge_optimize6DOF_disable();
+    // slam->reinit_ceres_problem_onnewloopedge_optimize6DOF_disable();
     std::thread th_slam( &PoseGraphSLAM::reinit_ceres_problem_onnewloopedge_optimize6DOF, slam );
 
 
@@ -324,38 +323,58 @@ int main( int argc, char ** argv)
     viz->setOdometryPublisher( pub_odometry_opt );
 
     //----Pose Composer---//
-    Composer * cmpr = new Composer( manager, slam, viz );
+    Composer * cmpr = new Composer( manager, slam, viz , nh);
 
     // ++ start the pose assember thread - This is needed for the following publish threads
     // It queries data from manager, slam and assembles the upto date data. This is threadsafe.
     cmpr->pose_assember_enable();
-    cmpr->pose_assember_disable();
+    // cmpr->pose_assember_disable();
     std::thread assember_th( &Composer::pose_assember_thread, cmpr, 30 );
 
     // ++ start bf_traj_publish_thread
     cmpr->bf_traj_publish_enable();
-    cmpr->bf_traj_publish_disable();
+    // cmpr->bf_traj_publish_disable();
     std::thread bf_pub_th( &Composer::bf_traj_publish_thread, cmpr, 15 );
 
     // ++ start camera visual publisher thread
     cmpr->cam_visual_publish_enable();
-    cmpr->cam_visual_publish_disable();
+    // cmpr->cam_visual_publish_disable();
     std::thread cam_visual_pub_th( &Composer::cam_visual_publish_thread, cmpr, 30 );
 
     // ++ loop edge publish thread
     cmpr->loopedge_publish_enable();
-    cmpr->loopedge_publish_disable();
+    // cmpr->loopedge_publish_disable();
     std::thread loopedge_pub_th( &Composer::loopedge_publish_thread, cmpr, 10 );
 
     // ++ disjointset status image
     cmpr->disjointset_statusimage_publish_enable();
-    cmpr->disjointset_statusimage_publish_disable();
+    // cmpr->disjointset_statusimage_publish_disable();
     std::thread disjointset_monitor_pub_th( &Composer::disjointset_statusimage_publish_thread, cmpr, 1 );
 
 
+    // TODO
+    // subscribes to w_T_imu @100hz and publish the pose in my world frame at 100hz
+    // implement this in composer class.
+    // /vins_estimator/imu_propagate @ 200hz
+
+    #if 1
+        //++(1) Setup publisher for imu poses 200hz
+        cmpr->setup_200hz_publishers();
+
+
+        //++(2) Setup subscriber / callback for /vins_estimator/imu_propagate topic
+        string imu_worldpose_topic = string("/vins_estimator/imu_propagate");
+        ROS_INFO( "Subscribe to imu_worldpose_topic: %s", imu_worldpose_topic.c_str() );
+        // ros::Subscriber sub_imu_worldpose_topic = nh.subscribe( imu_worldpose_topic, 1000, &Composer::imu_propagate_callback, this );
+        ros::Subscriber sub_imu_worldpose_topic = nh.subscribe( imu_worldpose_topic, 1000, &Composer::imu_propagate_callback, cmpr );
+    #endif
+
+
+
     //----END Pose Composer---//
+
     //--- setup manager publishers threads - adhoc ---//
-    // std::thread th4( periodic_publish_odoms, manager, viz, 30.0, 0.0, 0.0 ); //if you enable this, dont forget to join(), after spin() returns.
+    std::thread th4( periodic_publish_odoms, manager, viz, 30.0, 0.0, 0.0 ); //if you enable this, dont forget to join(), after spin() returns.
 
 
 
@@ -382,7 +401,7 @@ int main( int argc, char ** argv)
     cmpr->disjointset_statusimage_publish_disable();
 
 
-    // th4.join();
+    th4.join();
 
     th_slam.join();
     assember_th.join();