Skip to content

Commit 6f8a3ae

Browse files
mpkusempkuse
committed
Save to json file and load from file
SAVE: saved to disk from the class composer - save solved pose graph - save kidnap data - save disjointset data (using class Worlds) LOAD: load from disk from class composer. TODO> correctly load the solved pose graph and verify correctness - Load disjoint set data (using class Worlds) - load kidnap start end data - pose graph (TODO) - update variables in class PoseGraphSLAM on load (TODO)
1 parent 518149b commit 6f8a3ae

9 files changed

Lines changed: 733 additions & 9 deletions

‎src/Composer.cpp‎

Lines changed: 144 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -627,3 +627,147 @@ void Composer::imu_propagate_callback( const nav_msgs::Odometry::ConstPtr& msg )
627627

628628

629629
//------ END Publish body pose @200Hz ------//
630+
631+
632+
633+
634+
635+
bool Composer::saveStateToDisk( string save_dir_path )
636+
{
637+
//---
638+
// rm -rf && mkdir
639+
cout << TermColor::GREEN();
640+
cout << "\n^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^\n";
641+
cout << "^^^^^^^^^^ Composer::saveStateToDisk ^^^^^^^^^^\n";
642+
cout << "^^^^^^^^^^ DIR=" << save_dir_path ;
643+
cout << "\n^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^\n";
644+
cout << TermColor::RESET();
645+
646+
// TODO: rm -rf save_folder_name ; mkdir save_folder_name
647+
string system_cmd0 = string( "rm -rf ") + save_dir_path + " && mkdir "+ save_dir_path;
648+
const int rm_dir_err0 = RawFileIO::exec_cmd( system_cmd0 );
649+
if ( rm_dir_err0 == -1 )
650+
{
651+
cout << TermColor::RED() << "[Composer::saveStateToDiskr] Cannot mkdir folder: " << save_dir_path << "!\n" << TermColor::RESET() << endl;
652+
cout << "So not saveing state to disk...return false\n";
653+
return false;
654+
}
655+
656+
657+
//---
658+
// save pose graph (corrected poses)
659+
IOFormat CSVFormat(FullPrecision, DontAlignCols, ", ", "\n");
660+
json obj;
661+
662+
json pose_graph;
663+
for( int i=0 ; i<global_lmb.size() ; i++ )
664+
{
665+
json this_node;
666+
Matrix4d wTc = global_lmb[i];
667+
668+
// wj_T_c : Corrected poses
669+
this_node["w_T_c"]["rows"] = wTc.rows();
670+
this_node["w_T_c"]["cols"] = wTc.rows();
671+
std::stringstream ss;
672+
ss << wTc.format(CSVFormat);
673+
this_node["w_T_c"]["data"] = ss.str();
674+
this_node["w_T_c"]["data_pretty"] = PoseManipUtils::prettyprintMatrix4d(wTc);
675+
676+
677+
// j : setID of the world
678+
// k : worldID of this pose
679+
int worldid = manager->which_world_is_this( manager->getNodeTimestamp(i) );
680+
this_node["worldID"] = worldid;
681+
this_node["setID_of_worldID"] = manager->getWorldsConstPtr()->find_setID_of_world_i( worldid );
682+
this_node["stampNSec"] = manager->getNodeTimestamp(i).toNSec();
683+
this_node["seq"] = i;
684+
685+
pose_graph.push_back( this_node );
686+
}
687+
obj["SolvedPoseGraph"] = pose_graph;
688+
689+
//---
690+
// Kidnap Timestamps
691+
obj["KidnapTimestamps"] = manager->kidnap_data_to_json();
692+
cout << "In len( obj[\"KidnapTimestamps\"][\"kidnap_starts\"] ) = " << obj["KidnapTimestamps"]["kidnap_starts"].size() << endl;
693+
cout << "In len( obj[\"KidnapTimestamps\"][\"kidnap_ends\"] ) = " << obj["KidnapTimestamps"]["kidnap_ends"].size() << endl;
694+
695+
//---
696+
// World poses data and disjoint_set.
697+
obj["WorldsData"] = manager->getWorldsConstPtr()->saveStateToDisk();
698+
699+
700+
701+
702+
//---
703+
// Save JSON
704+
cout << "obj[\"SolvedPoseGraph\"].size()=" << obj["SolvedPoseGraph"].size() << endl;
705+
RawFileIO::write_string( save_dir_path+"/solved_posegraph.json", obj.dump(4) );
706+
cout << TermColor::GREEN() << "DONE........ Composer::saveStateToDisk ^^^^^^^^^^\n" << TermColor::RESET();
707+
return true;
708+
}
709+
710+
711+
bool Composer::loadStateFromDisk( string save_dir_path )
712+
{
713+
cout << TermColor::GREEN();
714+
cout << "\n^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^\n";
715+
cout << "^^^^^^^^^^ Composer::loadStateFromDisk ^^^^^^^^^^\n";
716+
cout << "^^^^^^^^^^ DIR=" << save_dir_path ;
717+
cout << "\n^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^\n";
718+
cout << TermColor::RESET();
719+
720+
//---
721+
// Load JSON
722+
string json_fname = save_dir_path+"/solved_posegraph.json";
723+
cout << TermColor::GREEN() << "[Composer::loadStateFromDisk]Open file: " << json_fname << TermColor::RESET() << endl;
724+
std::ifstream json_fileptr(json_fname);
725+
if( !json_fileptr )
726+
{
727+
ROS_ERROR( "[Composer::loadStateFromDisk]Cannot load from previous state" );
728+
cout << TermColor::RED() << "[Composer::loadStateFromDisk]Fail to open" << json_fname << " file, perhaps it doesnt exist. Cannot load from previous state.\nEXIT(1)"<< endl;
729+
exit(1);
730+
}
731+
json json_obj;
732+
json_fileptr >> json_obj;
733+
cout << "[DataManager::loadStateFromDisk]Successfully opened file and loaded data "<< json_fname << endl;
734+
json_fileptr.close();
735+
736+
737+
//---
738+
// Load World Data into class Worlds
739+
bool status_w = manager->getWorldsPtr()->loadStateFromDisk( json_obj["WorldsData"] );
740+
if( status_w == false ) {
741+
cout << TermColor::RED() << "[Composer::loadStateFromDisk] manager->getWorldsPtr()->loadStateFromDisk returned false\nFATAL ERROR..." << TermColor::RESET() << endl;
742+
exit(1);
743+
}
744+
manager->getWorldsPtr()->print_summary();
745+
746+
747+
748+
//---
749+
// Load kidnap timestamps into manager->kidnap_data
750+
bool status_k = manager->load_kidnap_data_from_json( json_obj["KidnapTimestamps"] );
751+
if( status_k == false )
752+
{
753+
cout << TermColor::RED() << "[Composer::loadStateFromDisk] manager->load_kidnap_data_from_json() returned false\nFATAL ERROR..." << TermColor::RESET() << endl;
754+
exit(1);
755+
}
756+
757+
758+
//---
759+
// Load Pose Graph
760+
bool status_pg = manager->load_solved_posegraph_data_from_json( json_obj );
761+
if( status_pg == false )
762+
{
763+
cout << TermColor::RED() << "[Composer::loadStateFromDisk] manager->load_solved_posegraph_data_from_json() returned false\nFATAL ERROR..." << TermColor::RESET() << endl;
764+
exit(1);
765+
}
766+
767+
//---
768+
// Adjust variables in object slam (in PoseGraphSLAM), especially the solved_until
769+
770+
771+
772+
773+
}

‎src/Composer.h‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,11 @@ using namespace std;
3030
#include "utils/RosMarkerUtils.h"
3131
#include "utils/TermColor.h"
3232

33+
// For saveStateToDisk
34+
#include "nlohmann/json.hpp"
35+
using json = nlohmann::json;
36+
#include "utils/RawFileIO.h"
37+
3338
class Composer
3439
{
3540
public:
@@ -149,4 +154,9 @@ class Composer
149154
private:
150155
ros::Publisher pub_hz200_marker;
151156

157+
158+
/////////////// Save State ////////////////////
159+
public:
160+
bool saveStateToDisk( string save_dir_path );
161+
bool loadStateFromDisk( string save_dir_path );
152162
};

‎src/NodeDataManager.cpp‎

Lines changed: 144 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -804,6 +804,150 @@ const ros::Time NodeDataManager::last_kidnap_started() const
804804
}
805805
}
806806

807+
json NodeDataManager::kidnap_data_to_json() const
808+
{
809+
std::lock_guard<std::mutex> lk(mutex_kidnap);
810+
json A;
811+
for( int i=0 ; i<kidnap_starts.size() ; i++ )
812+
{
813+
json _a;
814+
_a["stampNSec"] = kidnap_starts[i].toNSec();
815+
A.push_back( _a );
816+
}
817+
818+
json B;
819+
for( int i=0 ; i<kidnap_ends.size() ; i++ )
820+
{
821+
json _b;
822+
_b["stampNSec"] = kidnap_ends[i].toNSec();
823+
B.push_back( _b );
824+
}
825+
826+
json obj;
827+
obj["kidnap_starts"] = A;
828+
obj["kidnap_ends"] = B;
829+
return obj;
830+
}
831+
832+
833+
//
834+
// Sample input
835+
// "kidnap_ends": [
836+
// {
837+
// "stampNSec": 1550116863137285233
838+
// },
839+
// {
840+
// "stampNSec": 1550116901956894159
841+
// }
842+
// ],
843+
// "kidnap_starts": [
844+
// {
845+
// "stampNSec": 1550116842603365660
846+
// },
847+
// {
848+
// "stampNSec": 1550116885619684696
849+
// }
850+
// ]
851+
bool NodeDataManager::load_kidnap_data_from_json( json obj )
852+
{
853+
cout << TermColor::GREEN() << "^^^^^^^^^^^^^^ NodeDataManager::load_kidnap_data_from_json ^^^^^^^^^^^^^^^\n" << TermColor::RESET();
854+
855+
int n_kidnap_starts = obj.at("kidnap_starts").size();
856+
cout << "[NodeDataManager::load_kidnap_data_from_json] I found n_kidnap_starts=" << n_kidnap_starts << endl;
857+
kidnap_starts.clear();
858+
for( int i=0 ; i<n_kidnap_starts ; i++ )
859+
{
860+
ros::Time _tmpt = ros::Time().fromNSec( obj["kidnap_starts"][i]["stampNSec"] );
861+
cout << "\t" << _tmpt ;
862+
kidnap_starts.push_back( _tmpt );
863+
}
864+
cout << endl;
865+
866+
867+
int n_kidnap_ends = obj.at("kidnap_ends").size();
868+
cout << "[NodeDataManager::load_kidnap_data_from_json] I found n_kidnap_ends=" << n_kidnap_ends << endl;
869+
kidnap_ends.clear();
870+
for( int i=0 ; i<n_kidnap_ends ; i++ )
871+
{
872+
ros::Time _tmpt = ros::Time().fromNSec( obj["kidnap_starts"][i]["stampNSec"] );
873+
cout << "\t" << _tmpt;
874+
kidnap_ends.push_back( _tmpt );
875+
}
876+
cout << endl;
877+
878+
cout << TermColor::GREEN() << "^^^^^^^^^^^^^^ DONE NodeDataManager::load_kidnap_data_from_json ^^^^^^^^^^^^^^^\n" << TermColor::RESET();
879+
return true;
880+
}
881+
882+
883+
// Sample
884+
// "SolvedPoseGraph":
885+
// [
886+
// {
887+
// "seq": 0,
888+
// "setID_of_worldID": 0,
889+
// "stampNSec": 1550116822619016171,
890+
// "w_T_c": {
891+
// "cols": 4,
892+
// "data": "-0.5126642798427126, 0.02348155480372338, 0.8582679958831929, 0.1522650121223336\n-0.8550100366965891, -0.1051413903718039, -0.5078416339556862, 0.06817469369769494\n0.07831457923943769, -0.9941800162016352, 0.07397920021104687, 0.07354274867141557\n0, 0, 0, 1",
893+
// "data_pretty": ":YPR(deg)=(-120.947,-4.492,-85.744) :TxTyTz=(0.152,0.068,0.074)",
894+
// "rows": 4
895+
// },
896+
// "worldID": 0
897+
// },
898+
// {
899+
// "seq": 1,
900+
// "setID_of_worldID": 0,
901+
// "stampNSec": 1550116822718936205,
902+
// "w_T_c": {
903+
// "cols": 4,
904+
// "data": "-0.5138138579767237, 0.02259357261832001, 0.8576041335181481, 0.1867067427757811\n-0.8534146182324505, -0.1155687086714443, -0.5082591494141291, 0.08460685550314911\n0.08762881226077235, -0.9930424986134048, 0.07866248921443475, 0.07945286857626968\n0, 0, 0, 1",
905+
// "data_pretty": ":YPR(deg)=(-121.051,-5.027,-85.471) :TxTyTz=(0.187,0.085,0.079)",
906+
// "rows": 4
907+
// },
908+
// "worldID": 0
909+
// },
910+
// {
911+
// "seq": 2,
912+
// "setID_of_worldID": 0,
913+
// "stampNSec": 1550116822818856239,
914+
// "w_T_c": {
915+
// "cols": 4,
916+
// "data": "-0.603018213051328, 0.02022099545331435, 0.7974710941916718, 0.2303592300779559\n-0.791549852842599, -0.1393364478125909, -0.5950077182489573, 0.0736287467796735\n0.09908514113248795, -0.9900386182638774, 0.1000283417482688, 0.08264907108338827\n0, 0, 0, 1",
917+
// "data_pretty": ":YPR(deg)=(-127.301,-5.686,-84.231) :TxTyTz=(0.230,0.074,0.083)",
918+
// "rows": 4
919+
// },
920+
// "worldID": 0
921+
// },
922+
// .
923+
// .
924+
// .
925+
// ]
926+
bool NodeDataManager::load_solved_posegraph_data_from_json( json obj )
927+
{
928+
cout << TermColor::GREEN() << "^^^^^^^^^^^^^^ NodeDataManager::load_solved_posegraph_data_from_json ^^^^^^^^^^^^^^^\n" << TermColor::RESET();
929+
930+
int n_nodes = obj.at("SolvedPoseGraph").size();
931+
cout << "[NodeDataManager::load_solved_posegraph_data_from_json] I see "<< n_nodes << " nodes in the json\n";
932+
933+
// I assume worlds (the disjointset) and kidnap timestamps are already populated before calling this.
934+
// This assumption is used in verifying the loaded worldid etc from what is there in the data
935+
for( int i=0 ; i<n_nodes ; i++ )
936+
{
937+
int _seq = obj.at("SolvedPoseGraph").at(i).at("seq");
938+
int _setID_of_worldID = obj.at("SolvedPoseGraph").at(i).at("setID_of_worldID");
939+
int _worldID = obj.at("SolvedPoseGraph").at(i).at("worldID");
940+
ros::Time _tnode = ros::Time().fromNSec( obj.at("SolvedPoseGraph").at(i).at("stampNSec") );
941+
942+
// verify worldID and setID_of_worldID
943+
944+
945+
}
946+
947+
cout << TermColor::GREEN() << "^^^^^^^^^^^^^^ DONE NodeDataManager::load_solved_posegraph_data_from_json ^^^^^^^^^^^^^^^\n" << TermColor::RESET();
948+
return true;
949+
}
950+
807951

808952
int NodeDataManager::n_kidnaps() const
809953
{

‎src/NodeDataManager.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -159,6 +159,9 @@ class NodeDataManager
159159
ros::Time stamp_of_kidnap_i_started( int i ) const;
160160
ros::Time stamp_of_kidnap_i_ended( int i ) const;
161161
int n_kidnaps() const;
162+
json kidnap_data_to_json() const;
163+
bool load_kidnap_data_from_json( json obj ); //< See comments in the implementation for json input format
164+
bool load_solved_posegraph_data_from_json( json obj ); //< See comments in the implementation for json input format
162165

163166

164167
int nodeidx_of_world_i_started( int i ) const;

0 commit comments

Comments
 (0)