Skip to content

Commit cc09c35

Browse files
mpkusempkuse
committed
Fixed the loading-saveing interop issue
I was facing issue when loading state from disk. So, the way I fixed it is as you saveStateToDisk, you mark the state as kidnap_and_signal_end_of_the_world. So state is always written as kidnapped status As you load the state, be mindup of the kidnapped status. Also as soon as you see the first timestamp (new node after stateload) mark_as_unkidnapped_and_signal_start_of_world. With this everything works correct as expected no segfaults. Done: - save pose graph - Now there is no need to update any variables in posegraphsolver since it will automatically lookup the length of datamanager which is threadsafe. TODO: - More testing on longer and more complex sequences. Test both this and cerebro works correctly together - test: run-save-terminate-load-run-save-terminate-load-run. Basically building maps in multiple runs and then finally loclize. - instead of recompile each time, make nh.params for loading the saving state.
1 parent 6f8a3ae commit cc09c35

6 files changed

Lines changed: 217 additions & 25 deletions

File tree

‎src/Composer.cpp‎

Lines changed: 84 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -279,6 +279,7 @@ void Composer::bf_traj_publish_thread( int looprate ) const
279279

280280
bool publish_all = (distribution(generator) < 5)?true:false;
281281

282+
282283
for( auto it=global_jmb.begin() ; it!=global_jmb.end() ; it++ )
283284
{
284285
// 10% of the times publish all worlds, 90% of the time publish only the newest
@@ -634,6 +635,8 @@ void Composer::imu_propagate_callback( const nav_msgs::Odometry::ConstPtr& msg )
634635

635636
bool Composer::saveStateToDisk( string save_dir_path )
636637
{
638+
639+
637640
//---
638641
// rm -rf && mkdir
639642
cout << TermColor::GREEN();
@@ -643,7 +646,22 @@ bool Composer::saveStateToDisk( string save_dir_path )
643646
cout << "\n^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^\n";
644647
cout << TermColor::RESET();
645648

646-
// TODO: rm -rf save_folder_name ; mkdir save_folder_name
649+
650+
//---
651+
// mark as kidnapped and signal end-of-world
652+
cout << TermColor::iYELLOW() << "manager->curr_kidnap_status="<< manager->curr_kidnap_status() << TermColor::RESET() << ". 0:=unkidnapped 1:=kidnapped" << endl;
653+
if( manager->curr_kidnap_status() == false ) { //mark as kidnapped when I am not kidnapped
654+
cout << TermColor::iYELLOW() << "manager->mark_as_kidnapped_and_signal_end_of_world()\n" << TermColor::RESET() ;
655+
manager->mark_as_kidnapped_and_signal_end_of_world();
656+
} else {
657+
cout << "I was already kidnapped when you pressed CTRL+C, so no further action taken\n";
658+
}
659+
cout << TermColor::iYELLOW() << "(after marking kidnapped) manager->curr_kidnap_status="<< manager->curr_kidnap_status() << TermColor::RESET() << ". 0:=unkidnapped 1:=kidnapped" << endl;
660+
661+
662+
663+
//---
664+
// rm -rf save_folder_name ; mkdir save_folder_name
647665
string system_cmd0 = string( "rm -rf ") + save_dir_path + " && mkdir "+ save_dir_path;
648666
const int rm_dir_err0 = RawFileIO::exec_cmd( system_cmd0 );
649667
if ( rm_dir_err0 == -1 )
@@ -697,8 +715,72 @@ bool Composer::saveStateToDisk( string save_dir_path )
697715
obj["WorldsData"] = manager->getWorldsConstPtr()->saveStateToDisk();
698716

699717

718+
//---
719+
// Don't keep worlds unended. look at
720+
// a) WorldsData.vec_world_starts.size
721+
// b) WorldsData.vec_world_ends.size
722+
// c) KidnapTimestamps.kidnap_starts.size
723+
// d) KidnapTimestamps.kidnap_ends.size
724+
// Every world that has started need to end, every kidnap that has started has to end.
725+
// This also implies that `WorldsData.vec_world_starts.size` need to be equal to `WorldsData.vec_world_ends.size`
726+
// also `KidnapTimestamps.kidnap_ends.size` need to be equal to `KidnapTimestamps.kidnap_starts.size`
727+
// If they are not equal make them equal. by ending the world with last known timestamp of node or in case of kidnap as well.
728+
// if( obj["KidnapTimestamps"]["kidnap_ends"] )
729+
int __a__ = obj.at("WorldsData").at("vec_world_starts").size();
730+
int __b__ = obj.at("WorldsData").at("vec_world_ends").size();
731+
int __c__ = obj.at("KidnapTimestamps").at("kidnap_starts").size() ;
732+
int __d__ = obj.at("KidnapTimestamps").at("kidnap_ends").size() ;
733+
cout << TermColor::BLUE() << "a) WorldsData.vec_world_starts.size" << __a__ << TermColor::RESET() << endl;
734+
cout << TermColor::BLUE() << "b) WorldsData.vec_world_ends.size" << __b__ << TermColor::RESET() << endl;
735+
cout << TermColor::BLUE() << "c) KidnapTimestamps.kidnap_starts.size" << __c__ << TermColor::RESET() << endl;
736+
cout << TermColor::BLUE() << "d) KidnapTimestamps.kidnap_ends.size" << __d__ << TermColor::RESET() << endl;
737+
auto last_node_timestamp = obj.at("SolvedPoseGraph").rbegin()->at( "stampNSec"); //long int
738+
739+
740+
//assuming CTRL+C was pressed when status was not KIDNAPED
741+
// do-1: end the world
742+
// do-2: start kidnap
743+
// also do when you begin new bag: end kidnap and start new world from 1st item of bag
744+
745+
746+
// assuming CTRL+C was pressed when status was KIDNAPPED
747+
// dont do anything as you save data
748+
// when loading data end status kidnap and start new world
749+
750+
751+
#if 0
752+
if( __a__ == __b__ ){
753+
; // nothing to do all good
754+
}
755+
else if( __a__-1 == __b__ ) {
756+
// TODO
757+
cout << "obj[\"WorldsData\"][\"vec_world_ends\"].push_back( __ );\n";
758+
json ___y;
759+
___y["stampNSec"] = last_node_timestamp;
760+
obj["WorldsData"]["vec_world_ends"].push_back( ___y );
761+
762+
} else {
763+
cout << "Either __a__ has to be equal to __b__ or __a__ has to be 1 more than b. Currently this is not the case, this looks like something is wrong\n";
764+
exit(1);
765+
}
700766

701767

768+
if( __c__ == __d__ ){
769+
; // nothing to do all good
770+
}
771+
else if( __c__ - 1 == __d__ ) {
772+
// TODO
773+
cout << "obj[\"KidnapTimestamps\"][\"kidnap_ends\"].push_back( __ );\n";
774+
json ___y;
775+
___y["stampNSec"] = last_node_timestamp;
776+
obj["KidnapTimestamps"]["kidnap_ends"].push_back( ___y );
777+
778+
} else {
779+
cout << "Either __c__ has to be equal to __d__ or __c__ has to be 1 more than b. Currently this is not the case, this looks like something is wrong\n";
780+
exit(1);
781+
}
782+
#endif
783+
702784
//---
703785
// Save JSON
704786
cout << "obj[\"SolvedPoseGraph\"].size()=" << obj["SolvedPoseGraph"].size() << endl;
@@ -766,7 +848,7 @@ bool Composer::loadStateFromDisk( string save_dir_path )
766848

767849
//---
768850
// Adjust variables in object slam (in PoseGraphSLAM), especially the solved_until
769-
851+
770852

771853

772854

‎src/NodeDataManager.cpp‎

Lines changed: 97 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@ worlds_handle_raw_ptr = new Worlds();
2222
#define __NODEDATAMANAGER_CALLBACKS(msg) ;
2323
void NodeDataManager::camera_pose_callback( const nav_msgs::Odometry::ConstPtr& msg )
2424
{
25+
static bool static_camera_pose_callback = false;
2526
// ROS_INFO( "NodeDataManager::camera_pose_callback");
2627

2728

@@ -65,10 +66,37 @@ void NodeDataManager::camera_pose_callback( const nav_msgs::Odometry::ConstPtr&
6566
node_pose.push_back( w_T_cam );
6667
node_pose_covariance.push_back( Cov );
6768

68-
// signal world0 start
69+
#if 0
70+
// signal world0 start - OLD CODE REMOVAL TODO
6971
if( node_pose.size() == 1 ) {
7072
worlds_handle_raw_ptr->world_starts( msg->header.stamp );
7173
}
74+
#else
75+
76+
cout << TermColor::iYELLOW();
77+
if( static_camera_pose_callback == false ) {
78+
cout << "[NodeDataManager::camera_pose_callback] 1st pose creating at t=" << msg->header.stamp << "\n" ;
79+
80+
if( node_pose.size() == 1 ) // fresh start, so signal start of world0
81+
{
82+
cout << "fresh start, so signal start of world0 \n";
83+
worlds_handle_raw_ptr->world_starts( msg->header.stamp );
84+
} else if( node_pose.size() > 1 ) // looks like you loaded from file
85+
{
86+
cout << "looks like you loaded from file\n";
87+
88+
// mark end of kidnap and start new world
89+
// mark_as_unkidnapped( msg->header.stamp ); // TODO remove
90+
// worlds_handle_raw_ptr->world_starts( msg->header.stamp ); // TODO remove
91+
mark_as_unkidnapped_and_signal_start_of_world( msg->header.stamp );
92+
93+
94+
}
95+
96+
}
97+
cout << TermColor::RESET();
98+
static_camera_pose_callback = true;
99+
#endif
72100

73101
}
74102

@@ -807,25 +835,36 @@ const ros::Time NodeDataManager::last_kidnap_started() const
807835
json NodeDataManager::kidnap_data_to_json() const
808836
{
809837
std::lock_guard<std::mutex> lk(mutex_kidnap);
810-
json A;
838+
json obj;
839+
cout << "^^^^^ NodeDataManager::kidnap_data_to_json ^^^^^\n";
840+
841+
// json A;
842+
cout << "kidnap_starts: ";
811843
for( int i=0 ; i<kidnap_starts.size() ; i++ )
812844
{
813845
json _a;
814846
_a["stampNSec"] = kidnap_starts[i].toNSec();
815-
A.push_back( _a );
847+
// A.push_back( _a );
848+
obj["kidnap_starts"].push_back( _a );
849+
cout << "\t" << kidnap_starts[i];
816850
}
851+
cout << endl;
817852

818-
json B;
853+
// json B;
854+
cout << "kidnap_ends: ";
819855
for( int i=0 ; i<kidnap_ends.size() ; i++ )
820856
{
821857
json _b;
822858
_b["stampNSec"] = kidnap_ends[i].toNSec();
823-
B.push_back( _b );
859+
// B.push_back( _b );
860+
obj["kidnap_ends"].push_back( _b );
861+
cout << "\t" << kidnap_ends[i];
824862
}
863+
cout << endl;
825864

826-
json obj;
827-
obj["kidnap_starts"] = A;
828-
obj["kidnap_ends"] = B;
865+
// obj["kidnap_starts"] = A;
866+
// obj["kidnap_ends"] = B;
867+
cout << "^^^^^ DONE NodeDataManager::kidnap_data_to_json ^^^^^\n";
829868
return obj;
830869
}
831870

@@ -869,12 +908,26 @@ bool NodeDataManager::load_kidnap_data_from_json( json obj )
869908
kidnap_ends.clear();
870909
for( int i=0 ; i<n_kidnap_ends ; i++ )
871910
{
872-
ros::Time _tmpt = ros::Time().fromNSec( obj["kidnap_starts"][i]["stampNSec"] );
911+
ros::Time _tmpt = ros::Time().fromNSec( obj["kidnap_ends"][i]["stampNSec"] );
873912
cout << "\t" << _tmpt;
874913
kidnap_ends.push_back( _tmpt );
875914
}
876915
cout << endl;
877916

917+
918+
cout << "n_kidnap_starts="<< n_kidnap_starts << " n_kidnap_ends=" << n_kidnap_ends << endl;
919+
if( n_kidnap_starts == n_kidnap_ends ) {
920+
cout << "[NodeDataManager::load_kidnap_data_from_json] Set current_kidnap_status=false\n";
921+
current_kidnap_status = false;
922+
}else if( n_kidnap_starts-1 == n_kidnap_ends )
923+
{
924+
cout << "[NodeDataManager::load_kidnap_data_from_json] Set current_kidnap_status=true\n";
925+
current_kidnap_status = true;
926+
} else {
927+
cout << TermColor::RED() << "[NodeDataManager::load_kidnap_data_from_json] ERROR `n_kidnap_starts` and `n_kidnap_ends` need to be either of equal size of n_kidnap_ends can be 1 less than n_kidnap_starts. EXIT...\n" << TermColor::RESET();
928+
exit(1);
929+
}
930+
878931
cout << TermColor::GREEN() << "^^^^^^^^^^^^^^ DONE NodeDataManager::load_kidnap_data_from_json ^^^^^^^^^^^^^^^\n" << TermColor::RESET();
879932
return true;
880933
}
@@ -932,18 +985,52 @@ bool NodeDataManager::load_solved_posegraph_data_from_json( json obj )
932985

933986
// I assume worlds (the disjointset) and kidnap timestamps are already populated before calling this.
934987
// This assumption is used in verifying the loaded worldid etc from what is there in the data
988+
std::map< int, int > world_freq_counts;
935989
for( int i=0 ; i<n_nodes ; i++ )
936990
{
937991
int _seq = obj.at("SolvedPoseGraph").at(i).at("seq");
938992
int _setID_of_worldID = obj.at("SolvedPoseGraph").at(i).at("setID_of_worldID");
939993
int _worldID = obj.at("SolvedPoseGraph").at(i).at("worldID");
940994
ros::Time _tnode = ros::Time().fromNSec( obj.at("SolvedPoseGraph").at(i).at("stampNSec") );
941995

996+
Matrix4d ___w_T_c;
997+
bool poseloadstatus=RawFileIO::read_eigen_matrix4d_fromjson( obj.at("SolvedPoseGraph").at(i).at("w_T_c"), ___w_T_c );
998+
if( poseloadstatus == false )
999+
{
1000+
cout << TermColor::RED() << "When loading SolvedPoseGraph[" << i << "], RawFileIO::read_eigen_matrix4d_fromjson returned false. This means, I cannot read the pose from json file\n" << TermColor::RESET() ;
1001+
return false;
1002+
}
1003+
9421004
// verify worldID and setID_of_worldID
943-
1005+
#if 1
1006+
if( _worldID >= 0 && _setID_of_worldID>=0 ) {
1007+
if( this->which_world_is_this(_tnode) != _worldID ) {
1008+
cout << TermColor::RED() << "At i=" << i << " this->which_world_is_this(_tnode)=" << this->which_world_is_this(_tnode) << " but _worldID=" << _worldID << endl;
1009+
return false;
1010+
}
1011+
1012+
if( this->getWorldsConstPtr()->find_setID_of_world_i( this->which_world_is_this(_tnode) ) != _setID_of_worldID ) {
1013+
cout << TermColor::RED() << "At i=" << i << " this->getWorldsConstPtr()->find_setID_of_world_i( this->which_world_is_this(_tnode) )=" << this->getWorldsConstPtr()->find_setID_of_world_i( this->which_world_is_this(_tnode) ) << " but _setID_of_worldID=" << _setID_of_worldID << endl;
1014+
return false;
1015+
}
1016+
}
9441017

1018+
if( world_freq_counts.count( _worldID) > 0 )
1019+
world_freq_counts[_worldID]++;
1020+
else
1021+
world_freq_counts[_worldID] = 1;
1022+
#endif // verify
1023+
1024+
1025+
// new node creation
1026+
node_timestamps.push_back( _tnode );
1027+
node_pose.push_back( ___w_T_c );
1028+
Matrix<double, 6,6 > Cov = Matrix<double, 6,6 >::Zero();
1029+
node_pose_covariance.push_back( Cov );
9451030
}
9461031

1032+
for( auto itu=world_freq_counts.begin() ; itu!=world_freq_counts.end() ; itu++ )
1033+
cout << "\t\t>>> worldid=" << itu->first << ": counts=" << itu->second << endl;
9471034
cout << TermColor::GREEN() << "^^^^^^^^^^^^^^ DONE NodeDataManager::load_solved_posegraph_data_from_json ^^^^^^^^^^^^^^^\n" << TermColor::RESET();
9481035
return true;
9491036
}

‎src/NodeDataManager.h‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -160,6 +160,21 @@ class NodeDataManager
160160
ros::Time stamp_of_kidnap_i_ended( int i ) const;
161161
int n_kidnaps() const;
162162
json kidnap_data_to_json() const;
163+
void mark_as_kidnapped_and_signal_end_of_world() //this is needed to be done when CTRL+C is called ie. when saveStateToDisk is to be performed
164+
{
165+
cout << "[NodeDataManager::mark_as_kidnapped_and_signal_end_of_world]" << endl;
166+
ros::Time last_ti_stamp = * ( node_timestamps.rbegin() );
167+
mark_as_kidnapped( last_ti_stamp );
168+
worlds_handle_raw_ptr->world_ends( last_ti_stamp );
169+
}
170+
171+
void mark_as_unkidnapped_and_signal_start_of_world( ros::Time _t_begin) //this is called when loading data from disk
172+
{
173+
cout << "[NodeDataManager::mark_as_unkidnapped_and_signal_start_of_world] _t_begin=" << _t_begin << endl;
174+
175+
mark_as_unkidnapped( _t_begin );
176+
worlds_handle_raw_ptr->world_starts( _t_begin );
177+
}
163178
bool load_kidnap_data_from_json( json obj ); //< See comments in the implementation for json input format
164179
bool load_solved_posegraph_data_from_json( json obj ); //< See comments in the implementation for json input format
165180

‎src/PoseGraphSLAM.cpp‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -1073,14 +1073,14 @@ bool PoseGraphSLAM::saveAsJSON(const string base_path)
10731073
//--- to live merging trajectories and kidnap. Better to use this ---
10741074
//--- rather than new_optimize6DOF().
10751075
//----------------------------------------------------------------------------
1076-
// #define __reint_allocation_cout(msg) msg;
1077-
#define __reint_allocation_cout(msg) ;
1076+
#define __reint_allocation_cout(msg) msg;
1077+
// #define __reint_allocation_cout(msg) ;
10781078

1079-
// #define __reint_odom_cout(msg) msg;
1080-
#define __reint_odom_cout(msg) ;
1079+
#define __reint_odom_cout(msg) msg;
1080+
// #define __reint_odom_cout(msg) ;
10811081

1082-
// #define __reinit_loopedge_cout( msg ) msg;
1083-
#define __reinit_loopedge_cout( msg ) ;
1082+
#define __reinit_loopedge_cout( msg ) msg;
1083+
// #define __reinit_loopedge_cout( msg ) ;
10841084

10851085

10861086
// #define __reint_gueses_short_info(msg) msg;

‎src/Worlds.cpp‎

Lines changed: 9 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -396,6 +396,7 @@ json Worlds::saveStateToDisk() const
396396
//---
397397
// rel_pose_between_worlds__wb_T_wa and rel_pose_between_worlds__wb_T_wa___info_string
398398
cout << "[Worlds::saveStateToDisk]rel_pose_between_worlds__wb_T_wa\n"; int n_c=0;
399+
obb["rel_pose_between_worlds__wb_T_wa"] = {};
399400
for( auto it=rel_pose_between_worlds__wb_T_wa.begin() ; it!=rel_pose_between_worlds__wb_T_wa.end() ; it++ )
400401
{
401402
auto key = it->first; //this is std::pair<m,n>
@@ -418,21 +419,27 @@ json Worlds::saveStateToDisk() const
418419
//---
419420
// vec_world_starts, vec_world_ends
420421
json A;
422+
cout << "\tvec_world_starts: " ;
421423
for( int i=0 ; i<vec_world_starts.size() ; i++ )
422424
{
423425
json _a;
424426
_a["stampNSec"] = vec_world_starts.at(i).toNSec();
425427
A.push_back( _a );
428+
cout << "\t" << vec_world_starts.at(i);
426429
}
430+
cout << endl;
427431
cout << "[Worlds::saveStateToDisk]vec_world_starts.size=" << vec_world_starts.size() << endl;
428432

429433
json B;
434+
cout << "\tvec_world_ends: " ;
430435
for( int i=0 ; i<vec_world_ends.size() ; i++ )
431436
{
432437
json _b;
433438
_b["stampNSec"] = vec_world_ends.at(i).toNSec();
434439
B.push_back( _b );
440+
cout << "\t" << vec_world_ends.at(i);
435441
}
442+
cout << endl;
436443
cout << "[Worlds::saveStateToDisk]vec_world_ends.size=" << vec_world_ends.size() << endl;
437444

438445
obb["vec_world_starts"] = A;
@@ -492,7 +499,8 @@ bool Worlds::loadStateFromDisk( json _objk )
492499
this->disjoint_set_log = x_parse_str;
493500
vector<string> _cmds = RawFileIO::split( x_parse_str, ';' );
494501
cout << "log_string = " << x_parse_str << endl;
495-
cout << "For populating the disjoint set, I see " << _cmds.size() << " commands\n";
502+
cout << "For populating the disjoint set, I see " << _cmds.size()-1 << " commands\n";
503+
// because last statement will be empty, ';' separated ^^^
496504
for( int i =0 ; i<_cmds.size() ; i++ )
497505
{
498506
if( _cmds[i].length() < 4 ) //skip empty string

0 commit comments

Comments
 (0)