Only jsf parts of #60

docs/data_tools.rst

@@ -38,3 +38,5 @@ bathymetric map as an image.
    csv_data
 
    xyz_data
+
+   jsf_data

docs/jsf_data.rst

@@ -0,0 +1,6 @@
+jsf_data
+========
+
+.. automodule:: auvlib.data_tools.jsf_data
+     :members:
+     :undoc-members:

src/data_tools/include/data_tools/jsf_data.h

@@ -57,11 +57,15 @@ struct jsf_sss_ping
     double sound_vel;
     uint16_t frequency;
     Eigen::Vector3d pos_;
+    double sample_interval;
+    std::unordered_map<std::string,bool> flag_; // flag indicates which values are valid
+    double altitude_;
+    double slant_; 
 
 	template <class Archive>
     void serialize( Archive & ar )
     {
-        ar(CEREAL_NVP(time_string_), CEREAL_NVP(time_stamp_), CEREAL_NVP(port), CEREAL_NVP(stbd), CEREAL_NVP(first_in_file_), CEREAL_NVP(rpy), CEREAL_NVP(lat_), CEREAL_NVP(long_), CEREAL_NVP(utm_zone), CEREAL_NVP(sound_vel), CEREAL_NVP(frequency), CEREAL_NVP(pos_));
+        ar(CEREAL_NVP(time_string_), CEREAL_NVP(time_stamp_), CEREAL_NVP(port), CEREAL_NVP(stbd), CEREAL_NVP(first_in_file_), CEREAL_NVP(rpy), CEREAL_NVP(lat_), CEREAL_NVP(long_), CEREAL_NVP(utm_zone), CEREAL_NVP(sound_vel), CEREAL_NVP(frequency), CEREAL_NVP(pos_), CEREAL_NVP(sample_interval), CEREAL_NVP(flag_), CEREAL_NVP(altitude_), CEREAL_NVP(slant_));
     }
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
@@ -105,6 +109,7 @@ cv::Mat make_waterfall_image(const jsf_sss_ping::PingsT& pings);
 void show_waterfall_image(const jsf_sss_ping::PingsT& pings);
 jsf_sss_ping::PingsT filter_frequency(const jsf_sss_ping::PingsT& pings, int desired_freq);
 std_data::sss_ping::PingsT convert_to_xtf_pings(const jsf_sss_ping::PingsT& pings);
+jsf_sss_ping::PingsT match_sound_vel(jsf_sss_ping::PingsT& sss_pings, jsf_dvl_ping::PingsT& dvl_pings);
 
 
 } // namespace jsf_data

src/data_tools/src/jsf_data.cpp

@@ -68,7 +68,7 @@ cv::Mat make_waterfall_image(const jsf_sss_ping::PingsT& pings)
     }
     cv::Mat resized_swath_img;//dst image
     cv::resize(swath_img, resized_swath_img, cv::Size(512, rows));//resize image
-    
+
     return resized_swath_img;
 }
 
@@ -154,6 +154,8 @@ vector<ReturnType, Eigen::aligned_allocator<ReturnType> > parse_file_impl(const
     }
     while (!input.eof()) {
         jsf_msg_header jsf_hdr;
+        int16_t prot_ver = jsf_hdr.prot_ver;
+        // cout << "in while Protocol Version: "  << prot_ver << endl;
         input.read(reinterpret_cast<char*>(&jsf_hdr),sizeof(jsf_hdr));
         if (jsf_hdr.start_marker != SONAR_MESSAGE_HEADER_START) {
             cout << "Invalid file format! start marker: " << jsf_hdr.start_marker << endl;
@@ -189,10 +191,18 @@ jsf_sss_ping read_datagram<jsf_sss_ping, jsf_sonar_data_msg_header>(std::ifstrea
     ping.rpy = Eigen::Vector3d(jsf_sonar_data_hdr.roll, jsf_sonar_data_hdr.pitch, jsf_sonar_data_hdr.compass_heading);
     ping.rpy.head<2>() = M_PI/32768.*ping.rpy.head<2>();
     ping.rpy[2] = .5*M_PI - M_PI/180.*0.01*ping.rpy[2];
-
+    double interval = jsf_sonar_data_hdr.spl_intvl_in_ns*0.000000001;
+    ping.sample_interval = interval;
+    // cout << "samples interval in s: " << ping.sample_interval << endl;
+    // cout << "Num of samples in this pocket: "<< jsf_sonar_data_hdr.spls_num_in_pkt << endl;
+    if (ping.sound_vel>0) cout << "Sound velocity: " << ping.sound_vel << endl;
     // NOTE: this is only valid if coord_units == 2
     ping.lat_ = 0.0001/60.*double(jsf_sonar_data_hdr.y_coord);
     ping.long_ = 0.0001/60.*double(jsf_sonar_data_hdr.x_coord);
+
+    ping.altitude_ = 0.001*double(jsf_sonar_data_hdr.al_in_mm);
+    // cout << "altitude in m: " << ping.altitude_ << endl;
+
     double easting, northing;
     string utm_zone;
     tie(northing, easting, utm_zone) = lat_long_utm::lat_long_to_UTM(ping.lat_, ping.long_);
@@ -201,12 +211,80 @@ jsf_sss_ping read_datagram<jsf_sss_ping, jsf_sonar_data_msg_header>(std::ifstrea
     ping.pos_ = Eigen::Vector3d(easting, northing, -0.001*jsf_sonar_data_hdr.depth_in_mm);
     ping_side = process_side_scan_ping_side(input, jsf_hdr, jsf_sonar_data_hdr);
 
-    //cout << "Coord units: " << jsf_sonar_data_hdr.coord_units << endl;
 
+ // initialize the flag
+    ping.flag_["latlon_XY"] = false; // bit 0
+    ping.flag_["course"] = false; // bit 1
+
+    ping.flag_["speed"] = false; // bit 2
+    ping.flag_["heading"] = false; // bit 3
+    ping.flag_["pressure"] = false; // bit 4
+    ping.flag_["pitchroll"] = false; // bit 5
+    ping.flag_["altitude"] = false; // bit 6
+    //  bit 7 reserved
+    ping.flag_["water_temp"] = false; // bit 8
+    ping.flag_["depth"] = false; // bit 9
+    ping.flag_["annotation"] = false; // bit 10
+    ping.flag_["cable_counter"] = false; // bit 11
+    ping.flag_["kp"] = false; // bit 12
+    ping.flag_["position_interpolated"] = false; // bit 13
+    ping.flag_["sound_vel"] = false; // bit 14
+
+    // bit 0
+    if (jsf_sonar_data_hdr.val_flag & 0x01) ping.flag_["latlon_XY"] = true;
+
+    // bit 1
+    if (jsf_sonar_data_hdr.val_flag & 0x02) ping.flag_["course"] = true;
+
+
+    // bit 2
+    if (jsf_sonar_data_hdr.val_flag & 0x04) ping.flag_["speed"] = true;
+
+    // bit 3
+    if (jsf_sonar_data_hdr.val_flag & 0x08) ping.flag_["heading"] = true;
+
+    // bit 4
+    if (jsf_sonar_data_hdr.val_flag & 0x10) ping.flag_["pressure"] = true;
+
+    // bit 5
+    if (jsf_sonar_data_hdr.val_flag & 0x20) ping.flag_["pitchroll"] = true;
+
+    // bit 6
+    if (jsf_sonar_data_hdr.val_flag & 0x40) ping.flag_["altitude"] = true;
+
+    // bit 7
+    // if (jsf_sonar_data_hdr.val_flag & 0x80) ping.flag_["pitch"] = true;
+
+    // bit 8
+    if (jsf_sonar_data_hdr.val_flag & 0x100) ping.flag_["water_temp"] = true;
+
+    // bit 9
+    if (jsf_sonar_data_hdr.val_flag & 0x200) ping.flag_["depth"] = true;
+
+    // bit 10
+    if (jsf_sonar_data_hdr.val_flag & 0x400) ping.flag_["annotation"] = true;
+
+    // bit 11
+    if (jsf_sonar_data_hdr.val_flag & 0x800) ping.flag_["cable_counter"] = true;
+
+    // bit 12
+    if (jsf_sonar_data_hdr.val_flag & 0x1000) ping.flag_["kp"] = true; 
+
+    // bit 13
+    if (jsf_sonar_data_hdr.val_flag & 0x2000) ping.flag_["position_interpolated"] = true; 
+
+    // bit 14
+    if (jsf_sonar_data_hdr.val_flag & 0x4000) ping.flag_["sound_vel"] = true; 
+
+
+    // cout << "Protocol Version: " << std::hex << jsf_hdr.prot_ver << endl;
+    //cout << "Coord units: " << jsf_sonar_data_hdr.coord_units << endl;
+    // cout << "Starting depth: " << jsf_sonar_data_hdr.starting_depth << "depth in m: " << 0.001*jsf_sonar_data_hdr.depth_in_mm << endl;
     const boost::posix_time::ptime epoch = boost::posix_time::time_from_string("1970-01-01 00:00:00.000");
     boost::posix_time::ptime data_time;
 
     if (jsf_hdr.prot_ver >= 8) {
+
         data_time = epoch + boost::posix_time::seconds(jsf_sonar_data_hdr.ping_time_in_sec) + boost::posix_time::milliseconds(jsf_sonar_data_hdr.today_in_ms%1000);
     }
     else {
@@ -363,6 +441,25 @@ std_data::sss_ping::PingsT convert_to_xtf_pings(const jsf_sss_ping::PingsT& ping
 }
 
 
+jsf_sss_ping::PingsT match_sound_vel(jsf_sss_ping::PingsT& sss_pings, jsf_dvl_ping::PingsT& dvl_pings)
+{
+    std::stable_sort(dvl_pings.begin(), dvl_pings.end(), [](const jsf_dvl_ping& dvl_ping1, const jsf_dvl_ping& dvl_ping2) {
+        return dvl_ping1.time_stamp_ < dvl_ping2.time_stamp_;
+    });
+    std::stable_sort(sss_pings.begin(), sss_pings.end(), [](const jsf_sss_ping& sss_ping1, const jsf_sss_ping& sss_ping2) {
+        return sss_ping1.time_stamp_ < sss_ping2.time_stamp_;
+    });
+    auto pos = dvl_pings.begin();
+    for (jsf_sss_ping& sss_ping: sss_pings){
+        pos = std::find_if(pos, dvl_pings.end(),[&](const jsf_dvl_ping& dvl_ping){
+            return dvl_ping.time_stamp_ > sss_ping.time_stamp_;
+        });
+        sss_ping.sound_vel = pos->sound_vel_;
+        sss_ping.slant_ = sss_ping.sound_vel * sss_ping.sample_interval* sss_ping.port.pings.size()/2;
+    }
+    return sss_pings;
+}
+
 } // namespace jsf_data
 
 

src/data_tools/src/test_jsf.cpp

@@ -20,27 +20,27 @@ using namespace std;
 using namespace jsf_data;
 using namespace std_data;
 
-void test_parse_sss(const boost::filesystem::path& path)
+jsf_sss_ping::PingsT test_parse_sss(const boost::filesystem::path& path)
 {
     jsf_sss_ping::PingsT pings = parse_file<jsf_sss_ping>(path);
     int rows = pings.size();
     int cols = pings[0].port.pings.size() + pings[0].stbd.pings.size();
 
     for (const jsf_data::jsf_sss_ping& ping : pings) {
-        cout << "Ping duration:" << ping.port.time_duration << endl;
-        cout << "Ping pos: " << ping.pos_.transpose() << endl;
-        cout << "Ping rpy: " << ping.rpy.transpose() << endl;
+        // cout << "Ping duration:" << ping.port.time_duration << endl;
+        // cout << "Ping pos: " << ping.pos_.transpose() << endl;
+        // cout << "Ping rpy: " << ping.rpy.transpose() << endl;
     }
 
-    jsf_sss_ping::PingsT filtered_pings = filter_frequency(pings, 21269);
+    // jsf_sss_ping::PingsT filtered_pings = filter_frequency(pings, 21269);
 
     printf("rows num: %d\n", rows);
     printf("cols num: %d\n", cols);
-    show_waterfall_image(filtered_pings);
-
+    // show_waterfall_image(filtered_pings);
+    return pings;
 }
 
-void test_parse_dvl(const boost::filesystem::path& path)
+jsf_dvl_ping::PingsT test_parse_dvl(const boost::filesystem::path& path)
 {
     jsf_dvl_ping::PingsT pings = parse_file<jsf_dvl_ping>(path);
     cout << "Number of dvl pings: " << pings.size() << endl;
@@ -58,15 +58,47 @@ void test_parse_dvl(const boost::filesystem::path& path)
     if (!pings.empty()) {
         cout << "Sound velocity from the first dvl data: " << pings[0].sound_vel_ << " m/s" << endl;
     }
+    return pings;
+}
+
+
+void test_match_sound_vel(const boost::filesystem::path& path){
+    jsf_sss_ping::PingsT sss_pings = parse_file<jsf_sss_ping>(path);
+    jsf_dvl_ping::PingsT dvl_pings = parse_file<jsf_dvl_ping>(path);
+
+
+    sss_pings = match_sound_vel(sss_pings, dvl_pings);
+
+    for (auto i : sss_pings) {
+        // if (i.sound_vel==1477)
+        cout << "Sound velocity from the sss data at time_stamp: " << i.time_string_ << ", "<< i.sound_vel << " m/s" << endl;
+    }
+    for (auto i : dvl_pings) {
+        // if (i.sound_vel_==1477)
+    cout << "Sound velocity from the dvl data at time_stamp: " << i.time_string_ << ", "<<i.sound_vel_ << " m/s" << endl;
+    }
+    cout << "slant range: " << sss_pings[0].slant_ << endl;
+
 }
 
 int main(int argc, char** argv){
     int rtn=0;
     boost::filesystem::path path(argv[1]);
 
-    test_parse_dvl(path);
+    jsf_dvl_ping::PingsT dvl_pings = test_parse_dvl(path);
 
-    test_parse_sss(path);
+    jsf_sss_ping::PingsT sss_pings = test_parse_sss(path);
 
+    double time = sss_pings[0].sample_interval * sss_pings[0].port.pings.size();
+    double sound_vel = dvl_pings[0].sound_vel_;
+    double slant_range = sound_vel * time/2;
+    cout << "time: " << time << endl;
+    cout << "slant range: " << slant_range << endl;
+    cout << "port time duration: " << sss_pings[0].port.time_duration << endl;
+    cout << "stbd time duration: " << sss_pings[0].stbd.time_duration << endl;
+    cout << "port.pings.size(): "<< sss_pings[0].port.pings.size()<<endl;
+    cout << "dvl depth: " << dvl_pings[0].depth_ << endl;
+
+    test_match_sound_vel(path);
     return rtn;
 }

src/pydata_tools/src/pyjsf_data.cpp

@@ -43,15 +43,45 @@ PYBIND11_MODULE(jsf_data, m) {
         .def_readwrite("sound_vel_", &jsf_sss_ping::sound_vel, "Sound speed in m/s")
         .def_readwrite("frequency", &jsf_sss_ping::frequency, "Frequency of sampling")
         .def_readwrite("pos_", &jsf_sss_ping::pos_, "Position in ENU coordinates")
+        .def_readwrite("sample_interval_", &jsf_sss_ping::sample_interval, "Sample interval in s")
+        .def_readwrite("flag_", &jsf_sss_ping::flag_, "Flag indicates which values are valid")
+        .def_readwrite("altitude_", &jsf_sss_ping::altitude_, "Altitude")
+        .def_readwrite("slant_", &jsf_sss_ping::slant_, "Max Slant Range")
+
         .def_static("parse_file", &parse_file_from_str<jsf_sss_ping>, "Parse jsf_sss_ping from .jsf file")
         .def_static("parse_folder", &parse_folder_from_str<jsf_sss_ping>, "Parse jsf_sss_ping from folder of .jsf files")
         .def_static("read_data", &read_data_from_str<jsf_sss_ping::PingsT>, "Read jsf_sss_ping::PingsT from .cereal file");
 
+    py::class_<jsf_dvl_ping>(m, "jsf_dvl_ping", "Class for jsf dvl type")
+        .def(py::init<>())
+        .def_readwrite("time_string_", &jsf_dvl_ping::time_string_, "Readable date of measurement")
+        .def_readwrite("time_stamp_", &jsf_dvl_ping::time_stamp_, "UNIX timestamp")
+        .def_readwrite("dist_to_bottom_", &jsf_dvl_ping::dist_to_bottom_, "Disctance to bottom in meter for up to 4 beams")
+        .def_readwrite("vel_wrt_bottom_", &jsf_dvl_ping::vel_wrt_bottom_, "Velocity with respect to the bottom in m/s")
+        .def_readwrite("vel_wrt_water_", &jsf_dvl_ping::vel_wrt_water_, "Velocity with respect to the water layer in m/s")
+        .def_readwrite("first_in_file_", &jsf_dvl_ping::first_in_file_, "Is first measurement in file?")
+        .def_readwrite("ship_coord_", &jsf_dvl_ping::ship_coord_, "Is velocity in ship coordinates?")
+        .def_readwrite("depth_", &jsf_dvl_ping::depth_, "Depth from depth sensor in meters")
+        .def_readwrite("pitch_", &jsf_dvl_ping::pitch_, "Radian pitch in ENU coordinates")
+        .def_readwrite("roll_", &jsf_dvl_ping::roll_, "Radian roll in ENU coordinatese")
+        .def_readwrite("heading_", &jsf_dvl_ping::heading_, "Radian heading in ENU coordinates")
+        .def_readwrite("sound_vel_", &jsf_dvl_ping::sound_vel_, "Sound speed in m/s")
+        .def_readwrite("temp_", &jsf_dvl_ping::temp_, "Temperature in degree Celsius")
+        .def_readwrite("salinity_", &jsf_dvl_ping::salinity_, "Salinity in 1 part per thousand")
+        .def_readwrite("flag_", &jsf_dvl_ping::flag_, "Flag indicates which values present")
+        .def_readwrite("error_", &jsf_dvl_ping::error_, "Is error detected?")
+        .def_static("parse_file", &parse_file_from_str<jsf_dvl_ping>, "Parse jsf_dvl_ping from .jsf file")
+        .def_static("parse_folder", &parse_folder_from_str<jsf_dvl_ping>, "Parse jsf_dvl_ping from folder of .jsf files")
+        .def_static("read_data", &read_data_from_str<jsf_dvl_ping::PingsT>, "Read jsf_dvl_ping::PingsT from .cereal file");
+
     m.def("write_data", &write_data_from_str<jsf_sss_ping::PingsT>, "Write jsf pings to .cereal file");
+    m.def("write_data", &write_data_from_str<jsf_dvl_ping::PingsT>, "Write dvl pings to .cereal file");
     m.def("make_waterfall_image", &make_waterfall_image, "Create a cv2 waterfall image from jsf_sss_ping::PingsT");
     m.def("show_waterfall_image", &show_waterfall_image, "Show a waterfall image created from jsf_sss_ping::PingsT");
     m.def("filter_frequency", &filter_frequency, "Filter to keep only jsf_sss_ping::PingsT with certain frequency");
     m.def("convert_to_xtf_pings", &convert_to_xtf_pings, "Convert jsf_sss_ping::PingsT to std_data::sss_ping::PingsT");
+    m.def("match_sound_vel", &match_sound_vel, "Match sound velocity and slant range from jsf_dvl_ping::PingsT to jsf_sss_ping::PingsT");
+
 
     // from http://alexsm.com/pybind11-buffer-protocol-opencv-to-numpy/
     pybind11::class_<cv::Mat>(m, "Image", pybind11::buffer_protocol())