Skip to content

Commit d5b34aa

Browse files
mpkusempkuse
authored
Merge pull request #5 from mpkuse/dev-loadstate
Dev loadstate
2 parents 518149b + c405046 commit d5b34aa

11 files changed

Lines changed: 1678 additions & 44 deletions

‎src/Composer.cpp‎

Lines changed: 568 additions & 20 deletions
Large diffs are not rendered by default.

‎src/Composer.h‎

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,14 @@ 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+
38+
#include <nav_msgs/Path.h>
39+
#include <std_msgs/String.h>
40+
3341
class Composer
3442
{
3543
public:
@@ -102,6 +110,10 @@ class Composer
102110
// makes use of global_jmb and publish last cam
103111
public:
104112
void cam_visual_publish_thread( int looprate = 30 ) const;
113+
// vvvv I am publishing just as it is, this might need work when having multiple co-ordinate systems
114+
void path_publish_thread( int looprate=30 );
115+
void detailed_path_publish_thread( int looprate=30 );
116+
void w0_T_w1_publish_thread( int looprate=3 ); //if world exists(0,1) will publish else nothing
105117
void cam_visual_publish_enable() { b_cam_visual_publish = true;}
106118
void cam_visual_publish_disable() {b_cam_visual_publish = false; }
107119

@@ -131,6 +143,7 @@ class Composer
131143
// makes use of manager->getEdgeLen() and global_lmb and publish edges.
132144
public:
133145
void disjointset_statusimage_publish_thread( int looprate = 10 ) const;
146+
void disjointset_statusjson_publish_thread( int looprate = 10 ) ; //since i define the ros::Publisher inside this thread, I cannot set this as const. :(
134147
void disjointset_statusimage_publish_enable() { b_disjointset_statusimage_publish = true;}
135148
void disjointset_statusimage_publish_disable() {b_disjointset_statusimage_publish = false; }
136149

@@ -148,5 +161,12 @@ class Composer
148161

149162
private:
150163
ros::Publisher pub_hz200_marker;
164+
ros::Publisher pub_hz200_pose, pub_hz200_posestamped;
165+
151166

167+
168+
/////////////// Save State ////////////////////
169+
public:
170+
bool saveStateToDisk( string save_dir_path );
171+
bool loadStateFromDisk( string save_dir_path );
152172
};

‎src/NodeDataManager.cpp‎

Lines changed: 288 additions & 2 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

@@ -151,7 +179,9 @@ void NodeDataManager::loopclosure_pose_callback( const cerebro::LoopEdge::Const
151179
loopclosure_edges_timestamps.push_back( std::make_pair( t_a, t_b) );
152180
}
153181
else {
154-
cout << TermColor::YELLOW() << "[NodeDataManager::loopclosure_pose_callback] This edge's end points cannot be found in vector of nodes. This is not FATAL, I am ignoring this edge candidate as a fix.Ideally this should not be happening.\n" << TermColor::RESET() << endl;
182+
cout << TermColor::YELLOW() << "[NodeDataManager::loopclosure_pose_callback] This edge's end points( t_a=" << t_a << ", t_b=" << t_b << ") cannot be found in vector of nodes. This is not FATAL, I am ignoring this edge candidate as a fix.Ideally this should not be happening." << TermColor::RESET() << endl;
183+
cout << "t_a=" << t_a << " t_b=" << t_b << endl;
184+
cout << "index_t_a=" << index_t_a << " index_t_b=" << index_t_b << endl;
155185
}
156186

157187

@@ -805,6 +835,262 @@ const ros::Time NodeDataManager::last_kidnap_started() const
805835
}
806836

807837

838+
void NodeDataManager::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
839+
{
840+
cout << "[NodeDataManager::mark_as_kidnapped_and_signal_end_of_world]" << endl;
841+
ros::Time last_ti_stamp = * ( node_timestamps.rbegin() );
842+
mark_as_kidnapped( last_ti_stamp );
843+
worlds_handle_raw_ptr->world_ends( last_ti_stamp );
844+
}
845+
846+
void NodeDataManager::mark_as_unkidnapped_and_signal_start_of_world( ros::Time _t_begin) //this is called when loading data from disk
847+
{
848+
cout << "[NodeDataManager::mark_as_unkidnapped_and_signal_start_of_world] _t_begin=" << _t_begin << endl;
849+
850+
mark_as_unkidnapped( _t_begin );
851+
worlds_handle_raw_ptr->world_starts( _t_begin );
852+
}
853+
854+
json NodeDataManager::kidnap_data_to_json() const
855+
{
856+
std::lock_guard<std::mutex> lk(mutex_kidnap);
857+
json obj;
858+
cout << "^^^^^ NodeDataManager::kidnap_data_to_json ^^^^^\n";
859+
860+
// json A;
861+
cout << "kidnap_starts: ";
862+
for( int i=0 ; i<kidnap_starts.size() ; i++ )
863+
{
864+
json _a;
865+
_a["stampNSec"] = kidnap_starts[i].toNSec();
866+
// A.push_back( _a );
867+
obj["kidnap_starts"].push_back( _a );
868+
cout << "\t" << kidnap_starts[i];
869+
}
870+
cout << endl;
871+
872+
// json B;
873+
cout << "kidnap_ends: ";
874+
for( int i=0 ; i<kidnap_ends.size() ; i++ )
875+
{
876+
json _b;
877+
_b["stampNSec"] = kidnap_ends[i].toNSec();
878+
// B.push_back( _b );
879+
obj["kidnap_ends"].push_back( _b );
880+
cout << "\t" << kidnap_ends[i];
881+
}
882+
cout << endl;
883+
884+
// obj["kidnap_starts"] = A;
885+
// obj["kidnap_ends"] = B;
886+
cout << "^^^^^ DONE NodeDataManager::kidnap_data_to_json ^^^^^\n";
887+
return obj;
888+
}
889+
890+
891+
//
892+
// Sample input
893+
// "kidnap_ends": [
894+
// {
895+
// "stampNSec": 1550116863137285233
896+
// },
897+
// {
898+
// "stampNSec": 1550116901956894159
899+
// }
900+
// ],
901+
// "kidnap_starts": [
902+
// {
903+
// "stampNSec": 1550116842603365660
904+
// },
905+
// {
906+
// "stampNSec": 1550116885619684696
907+
// }
908+
// ]
909+
bool NodeDataManager::load_kidnap_data_from_json( json obj )
910+
{
911+
cout << TermColor::GREEN() << "^^^^^^^^^^^^^^ NodeDataManager::load_kidnap_data_from_json ^^^^^^^^^^^^^^^\n" << TermColor::RESET();
912+
913+
int n_kidnap_starts = obj.at("kidnap_starts").size();
914+
cout << "[NodeDataManager::load_kidnap_data_from_json] I found n_kidnap_starts=" << n_kidnap_starts << endl;
915+
kidnap_starts.clear();
916+
for( int i=0 ; i<n_kidnap_starts ; i++ )
917+
{
918+
ros::Time _tmpt = ros::Time().fromNSec( obj["kidnap_starts"][i]["stampNSec"] );
919+
cout << "\t" << _tmpt ;
920+
kidnap_starts.push_back( _tmpt );
921+
}
922+
cout << endl;
923+
924+
925+
int n_kidnap_ends = obj.at("kidnap_ends").size();
926+
cout << "[NodeDataManager::load_kidnap_data_from_json] I found n_kidnap_ends=" << n_kidnap_ends << endl;
927+
kidnap_ends.clear();
928+
for( int i=0 ; i<n_kidnap_ends ; i++ )
929+
{
930+
ros::Time _tmpt = ros::Time().fromNSec( obj["kidnap_ends"][i]["stampNSec"] );
931+
cout << "\t" << _tmpt;
932+
kidnap_ends.push_back( _tmpt );
933+
}
934+
cout << endl;
935+
936+
937+
cout << "n_kidnap_starts="<< n_kidnap_starts << " n_kidnap_ends=" << n_kidnap_ends << endl;
938+
if( n_kidnap_starts == n_kidnap_ends ) {
939+
cout << "[NodeDataManager::load_kidnap_data_from_json] Set current_kidnap_status=false\n";
940+
current_kidnap_status = false;
941+
}else if( n_kidnap_starts-1 == n_kidnap_ends )
942+
{
943+
cout << "[NodeDataManager::load_kidnap_data_from_json] Set current_kidnap_status=true\n";
944+
current_kidnap_status = true;
945+
} else {
946+
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();
947+
exit(1);
948+
}
949+
950+
cout << TermColor::GREEN() << "^^^^^^^^^^^^^^ DONE NodeDataManager::load_kidnap_data_from_json ^^^^^^^^^^^^^^^\n" << TermColor::RESET();
951+
return true;
952+
}
953+
954+
955+
// Sample
956+
// "SolvedPoseGraph":
957+
// [
958+
// {
959+
// "seq": 0,
960+
// "setID_of_worldID": 0,
961+
// "stampNSec": 1550116822619016171,
962+
// "w_T_c": {
963+
// "cols": 4,
964+
// "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",
965+
// "data_pretty": ":YPR(deg)=(-120.947,-4.492,-85.744) :TxTyTz=(0.152,0.068,0.074)",
966+
// "rows": 4
967+
// },
968+
// "worldID": 0
969+
// },
970+
// {
971+
// "seq": 1,
972+
// "setID_of_worldID": 0,
973+
// "stampNSec": 1550116822718936205,
974+
// "w_T_c": {
975+
// "cols": 4,
976+
// "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",
977+
// "data_pretty": ":YPR(deg)=(-121.051,-5.027,-85.471) :TxTyTz=(0.187,0.085,0.079)",
978+
// "rows": 4
979+
// },
980+
// "worldID": 0
981+
// },
982+
// {
983+
// "seq": 2,
984+
// "setID_of_worldID": 0,
985+
// "stampNSec": 1550116822818856239,
986+
// "w_T_c": {
987+
// "cols": 4,
988+
// "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",
989+
// "data_pretty": ":YPR(deg)=(-127.301,-5.686,-84.231) :TxTyTz=(0.230,0.074,0.083)",
990+
// "rows": 4
991+
// },
992+
// "worldID": 0
993+
// },
994+
// .
995+
// .
996+
// .
997+
// ]
998+
bool NodeDataManager::load_solved_posegraph_data_from_json( json obj )
999+
{
1000+
cout << TermColor::GREEN() << "^^^^^^^^^^^^^^ NodeDataManager::load_solved_posegraph_data_from_json ^^^^^^^^^^^^^^^\n" << TermColor::RESET();
1001+
1002+
int n_nodes = obj.at("SolvedPoseGraph").size();
1003+
cout << "[NodeDataManager::load_solved_posegraph_data_from_json] I see "<< n_nodes << " nodes in the json\n";
1004+
1005+
// I assume worlds (the disjointset) and kidnap timestamps are already populated before calling this.
1006+
// This assumption is used in verifying the loaded worldid etc from what is there in the data
1007+
std::map< int, int > world_freq_counts;
1008+
for( int i=0 ; i<n_nodes ; i++ )
1009+
{
1010+
int _seq = obj.at("SolvedPoseGraph").at(i).at("seq");
1011+
int _setID_of_worldID = obj.at("SolvedPoseGraph").at(i).at("setID_of_worldID");
1012+
int _worldID = obj.at("SolvedPoseGraph").at(i).at("worldID");
1013+
ros::Time _tnode = ros::Time().fromNSec( obj.at("SolvedPoseGraph").at(i).at("stampNSec") );
1014+
1015+
Matrix4d ___w_T_c; //< this is actually ws_T_cam (cam position in world-set-id, )
1016+
bool poseloadstatus=RawFileIO::read_eigen_matrix4d_fromjson( obj.at("SolvedPoseGraph").at(i).at("w_T_c"), ___w_T_c );
1017+
if( poseloadstatus == false )
1018+
{
1019+
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() ;
1020+
return false;
1021+
}
1022+
1023+
1024+
if( i==0 || i==1 || i==2 || i==n_nodes-1 || i== n_nodes-2) {
1025+
cout << "i=" << i << "\t";
1026+
cout << "t=" << _tnode << "\t";
1027+
cout << "_worldID=" << _worldID << "\t";
1028+
cout << "_setID_of_worldID=" << _setID_of_worldID << "\t";
1029+
cout << "wset_T_cam=" << PoseManipUtils::prettyprintMatrix4d( ___w_T_c ) << endl;
1030+
}
1031+
if( i==3 ) {
1032+
cout << "\t.\n\t.\n\t.\n";
1033+
}
1034+
1035+
// For everything to be working correctly, I need to store (in NodeDataManager)
1036+
// the wTc and not wsTc
1037+
// w_T_c := w_T_ws * ws_T_c
1038+
if( _worldID >= 0 && _worldID != _setID_of_worldID )
1039+
{
1040+
Matrix4d wTws = Matrix4d::Identity();
1041+
if( getWorldsConstPtr()->is_exist(_worldID,_setID_of_worldID) )
1042+
wTws = getWorldsConstPtr()->getPoseBetweenWorlds( _worldID,_setID_of_worldID );
1043+
else {
1044+
cout << "[NodeDataManager::load_solved_posegraph_data_from_json] ERROR";
1045+
cout << "at i="<< i << "you requesting a pose between the worlds "<< _worldID << " and " << _setID_of_worldID << " that does not exist. This cannot be happening\n";
1046+
getWorldsConstPtr()->print_summary();
1047+
exit(1);
1048+
}
1049+
Matrix4d f_wTc = wTws * ___w_T_c;
1050+
___w_T_c = f_wTc;
1051+
} else if( _worldID < 0 && _worldID != _setID_of_worldID )
1052+
{
1053+
// TODO: the kidnapped sections need to be shifted from setID_of_worldID of their world's to worldIDs of their worlds
1054+
// Also note, the worlds of kidnap will be -worldid +1. For example, for worldID=-1 the worldid is actually 0, ie. -1 is adjacent to the world0.
1055+
1056+
}
1057+
1058+
1059+
// verify worldID and setID_of_worldID
1060+
#if 1
1061+
if( _worldID >= 0 && _setID_of_worldID>=0 ) {
1062+
if( this->which_world_is_this(_tnode) != _worldID ) {
1063+
cout << TermColor::RED() << "At i=" << i << " this->which_world_is_this(_tnode)=" << this->which_world_is_this(_tnode) << " but _worldID=" << _worldID << endl;
1064+
return false;
1065+
}
1066+
1067+
if( this->getWorldsConstPtr()->find_setID_of_world_i( this->which_world_is_this(_tnode) ) != _setID_of_worldID ) {
1068+
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;
1069+
return false;
1070+
}
1071+
}
1072+
1073+
if( world_freq_counts.count( _worldID) > 0 )
1074+
world_freq_counts[_worldID]++;
1075+
else
1076+
world_freq_counts[_worldID] = 1;
1077+
#endif // verify
1078+
1079+
1080+
// new node creation
1081+
node_timestamps.push_back( _tnode );
1082+
node_pose.push_back( ___w_T_c );
1083+
Matrix<double, 6,6 > Cov = Matrix<double, 6,6 >::Zero();
1084+
node_pose_covariance.push_back( Cov );
1085+
}
1086+
1087+
for( auto itu=world_freq_counts.begin() ; itu!=world_freq_counts.end() ; itu++ )
1088+
cout << "\t\t>>> worldid=" << itu->first << ": counts=" << itu->second << endl;
1089+
cout << TermColor::GREEN() << "^^^^^^^^^^^^^^ DONE NodeDataManager::load_solved_posegraph_data_from_json ^^^^^^^^^^^^^^^\n" << TermColor::RESET();
1090+
return true;
1091+
}
1092+
1093+
8081094
int NodeDataManager::n_kidnaps() const
8091095
{
8101096
std::lock_guard<std::mutex> lk(mutex_kidnap);

‎src/NodeDataManager.h‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -160,6 +160,13 @@ class NodeDataManager
160160
ros::Time stamp_of_kidnap_i_ended( int i ) const;
161161
int n_kidnaps() const;
162162

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+
void mark_as_unkidnapped_and_signal_start_of_world( ros::Time _t_begin); //this is called when loading data from disk
165+
166+
json kidnap_data_to_json() const;
167+
bool load_kidnap_data_from_json( json obj ); //< See comments in the implementation for json input format
168+
bool load_solved_posegraph_data_from_json( json obj ); //< See comments in the implementation for json input format
169+
163170

164171
int nodeidx_of_world_i_started( int i ) const;
165172
int nodeidx_of_world_i_ended( int i ) const;

0 commit comments

Comments
 (0)