From 116f11bfe36ea3712ab75f7da36a474cdff520cd Mon Sep 17 00:00:00 2001 From: Katharina Xenia Kufieta Date: Mon, 8 Jan 2024 14:03:24 +0100 Subject: [PATCH] Remove unused functions --- .../src/carto_facade/carto_facade.cc | 1 - viam-cartographer/src/carto_facade/io.cc | 102 --------------- viam-cartographer/src/carto_facade/io.h | 33 ----- viam-cartographer/src/carto_facade/io_test.cc | 119 ------------------ .../src/carto_facade/map_builder.cc | 1 - .../src/carto_facade/util_test.cc | 43 ------- 6 files changed, 299 deletions(-) delete mode 100644 viam-cartographer/src/carto_facade/io.cc delete mode 100644 viam-cartographer/src/carto_facade/io.h delete mode 100644 viam-cartographer/src/carto_facade/io_test.cc diff --git a/viam-cartographer/src/carto_facade/carto_facade.cc b/viam-cartographer/src/carto_facade/carto_facade.cc index 76f7ce39..c5402af8 100644 --- a/viam-cartographer/src/carto_facade/carto_facade.cc +++ b/viam-cartographer/src/carto_facade/carto_facade.cc @@ -9,7 +9,6 @@ #include #include "glog/logging.h" -#include "io.h" #include "map_builder.h" #include "util.h" diff --git a/viam-cartographer/src/carto_facade/io.cc b/viam-cartographer/src/carto_facade/io.cc deleted file mode 100644 index 5ff0f739..00000000 --- a/viam-cartographer/src/carto_facade/io.cc +++ /dev/null @@ -1,102 +0,0 @@ -// This is an experimental integration of cartographer into RDK. -#include "io.h" - -#include // pcl::PCDReader -#include - -#include -#include // std::ifstream -#include -#include - -#include "glog/logging.h" - -namespace viam { -namespace carto_facade { -namespace io { - -namespace fs = boost::filesystem; - -cartographer::sensor::TimedPointCloudData TimedPointCloudDataFromPCDBuilder( - std::string file_path, double start_time) { - pcl::console::setVerbosityLevel(pcl::console::L_ALWAYS); - - cartographer::sensor::TimedPointCloudData timed_pcd; - cartographer::sensor::TimedPointCloud ranges; - - // Open the point cloud file - pcl::PointCloud::Ptr cloud( - new pcl::PointCloud); - auto err = pcl::io::loadPCDFile(file_path, *cloud); - - if (err == -1) { - return timed_pcd; - } - - double current_time = ReadTimeFromTimestamp(file_path.substr( - file_path.find(filename_prefix) + filename_prefix.length(), - file_path.find(".pcd"))); - double time_delta = current_time - start_time; - - VLOG(1) << "Accessing file " << file_path << " ... "; - VLOG(1) << "Loaded " << cloud->width * cloud->height << " data points"; - - for (size_t i = 0; i < cloud->points.size(); ++i) { - cartographer::sensor::TimedRangefinderPoint timed_rangefinder_point; - timed_rangefinder_point.position = Eigen::Vector3f( - cloud->points[i].x, cloud->points[i].y, cloud->points[i].z); - timed_rangefinder_point.time = 0 - i * 0.0001; - - ranges.push_back(timed_rangefinder_point); - } - - timed_pcd.time = cartographer::common::FromUniversal(123) + - cartographer::common::FromSeconds(double(time_delta)); - timed_pcd.origin = Eigen::Vector3f::Zero(); - timed_pcd.ranges = ranges; - - return timed_pcd; -} - -double ReadTimeFromTimestamp(std::string timestamp) { - std::string::size_type sz; - auto partial_time_format = time_format.substr(0, time_format.find(".")); - // Create a stream which we will use to parse the string - std::istringstream ss(timestamp); - - // Create a tm object to store the parsed date and time. - std::tm dt = {0}; - - // Now we read from buffer using get_time manipulator - // and formatting the input appropriately. - ss >> std::get_time(&dt, partial_time_format.c_str()); - if (ss.fail()) { - throw std::runtime_error( - "timestamp cannot be parsed into a std::tm object: " + timestamp); - } - double timestamp_time = (double)timegm(&dt); - if (timestamp_time == -1) { - throw std::runtime_error( - "timestamp cannot be represented as a std::time_t object: " + - timestamp); - } - auto sub_sec_index = timestamp.find("."); - if ((sub_sec_index != std::string::npos)) { - double sub_sec = 0; - try { - sub_sec = (double)std::stof(timestamp.substr(sub_sec_index), &sz); - } catch (std::exception &e) { - LOG(FATAL) << e.what(); - throw std::runtime_error( - "could not extract sub seconds from timestamp: " + timestamp); - } - double timestamp_time_w_sub_sec = timestamp_time + sub_sec; - return timestamp_time_w_sub_sec; - } else { - return timestamp_time; - } -} - -} // namespace io -} // namespace carto_facade -} // namespace viam diff --git a/viam-cartographer/src/carto_facade/io.h b/viam-cartographer/src/carto_facade/io.h deleted file mode 100644 index 18d4e77f..00000000 --- a/viam-cartographer/src/carto_facade/io.h +++ /dev/null @@ -1,33 +0,0 @@ -// This is an experimental integration of cartographer into RDK. -#ifndef VIAM_CARTO_FACADE_IO_H -#define VIAM_CARTO_FACADE_IO_H - -#include - -#include -#include -#include -#include -#include - -#include "cartographer/sensor/timed_point_cloud_data.h" - -namespace viam { -namespace carto_facade { -namespace io { -static const std::string filename_prefix = "_data_"; -static const std::string time_format = "%Y-%m-%dT%H:%M:%S.0000Z"; - -// TimedPointCloudDataFromPCDBuilder creates a TimedPointCloudData object -// from a PCD file. -cartographer::sensor::TimedPointCloudData TimedPointCloudDataFromPCDBuilder( - std::string file_path, double start_time); - -// Converts UTC time string to a double value. -double ReadTimeFromTimestamp(std::string timestamp); - -} // namespace io -} // namespace carto_facade -} // namespace viam - -#endif // VIAM_CARTO_FACADE_IO_H diff --git a/viam-cartographer/src/carto_facade/io_test.cc b/viam-cartographer/src/carto_facade/io_test.cc deleted file mode 100644 index 0d354e06..00000000 --- a/viam-cartographer/src/carto_facade/io_test.cc +++ /dev/null @@ -1,119 +0,0 @@ -#include "io.h" - -#include -#include -#include -#include -#include - -namespace viam { -namespace carto_facade { -namespace io { - -BOOST_AUTO_TEST_SUITE(CartoFacade_io) - -BOOST_AUTO_TEST_CASE(TimedPointCloudDataFromPCDBuilder_success) { - // Create a mini PCD file and save it in a tmp directory - std::string filename = "rplidar_data_2022-01-01T01:00:00.0001Z.pcd"; - std::vector> points = { - {-0.001000, 0.002000, 0.005000, 16711938}, - {0.582000, 0.012000, 0.000000, 16711938}, - {0.007000, 0.006000, 0.001000, 16711938}}; - std::string pcd = - "VERSION .7\n" - "FIELDS x y z rgb\n" - "SIZE 4 4 4 4\n" - "TYPE F F F I\n" - "COUNT 1 1 1 1\n" - "WIDTH 3\n" - "HEIGHT 1\n" - "VIEWPOINT 0 0 0 1 0 0 0\n" - "POINTS 3\n" - "DATA ascii\n"; - for (std::vector point : points) { - for (int i = 0; i < 3; i++) { - pcd = pcd + std::to_string(point.at(i)) + " "; - } - pcd = pcd + std::to_string(point.at(3)) + "\n"; - } - // Create a unique path in the temp directory and add the PCD file - boost::filesystem::path tmp_dir = boost::filesystem::temp_directory_path() / - boost::filesystem::unique_path(); - bool ok = boost::filesystem::create_directory(tmp_dir); - if (!ok) { - throw std::runtime_error("could not create directory: " + - tmp_dir.string()); - } - boost::filesystem::ofstream ofs(tmp_dir / filename); - ofs << pcd; - ofs.close(); - // Read it in and check if the data in the TimedPointCloudData is equivalent - // to what we had in the pcd file - cartographer::sensor::TimedPointCloudData timed_pcd = - TimedPointCloudDataFromPCDBuilder(tmp_dir.string() + "/" + filename, 0); - - auto tolerance = boost::test_tools::tolerance(0.00001); - BOOST_TEST(timed_pcd.ranges.size() == points.size()); - for (int i = 0; i < points.size(); i++) { - cartographer::sensor::TimedRangefinderPoint timed_rangefinder_point = - timed_pcd.ranges.at(i); - for (int j = 0; j < 3; j++) { - BOOST_TEST( - timed_rangefinder_point.position(j, 0) == points.at(i).at(j), - tolerance); - } - } - - // Remove the temporary directory and its contents - boost::filesystem::remove_all(tmp_dir); -} - -BOOST_AUTO_TEST_CASE(ReadTimeFromTimestamp_missing_timestamp) { - // Provide a filename with a missing timestamp - std::string timestamp = "no-timestamp"; - const std::string message = - "timestamp cannot be parsed into a std::tm object: " + timestamp; - BOOST_CHECK_EXCEPTION(ReadTimeFromTimestamp(timestamp), std::runtime_error, - [&message](const std::runtime_error& ex) { - BOOST_CHECK_EQUAL(ex.what(), message); - return true; - }); -} - -BOOST_AUTO_TEST_CASE(ReadTimeFromTimestamp_success) { - // Provide a filename with a timestamp - std::time_t t = std::time(nullptr); - char timestamp[100]; - std::strftime(timestamp, sizeof(timestamp), time_format.c_str(), - std::gmtime(&t)); - std::string filename_prefix = "rplidar_data_"; - std::string filename_type = ".pcd"; - std::string filename = - filename_prefix + std::string(timestamp) + filename_type; - // Read the time - std::string timestamp_str = filename.substr( - filename.find(filename_prefix) + filename_prefix.length(), - filename.find(filename_type)); - double filename_time = ReadTimeFromTimestamp(timestamp_str); - auto tolerance = boost::test_tools::tolerance(0.0001); - // Make sure the time read from the filename equals what we put into the - // filename - BOOST_TEST((double)t == filename_time, tolerance); -} - -BOOST_AUTO_TEST_CASE(ReadTimeFromTimestamp_comparison) { - const std::string timestamp_1 = "2022-01-01T01:00:00.0000Z"; - const std::string timestamp_2 = "2022-01-01T01:00:00.0001Z"; - const std::string timestamp_3 = "2022-01-01T01:00:01.0000Z"; - const auto time_1 = ReadTimeFromTimestamp(timestamp_1); - const auto time_2 = ReadTimeFromTimestamp(timestamp_2); - const auto time_3 = ReadTimeFromTimestamp(timestamp_3); - BOOST_TEST(time_1 < time_2); - BOOST_TEST(time_2 < time_3); -} - -BOOST_AUTO_TEST_SUITE_END() - -} // namespace io -} // namespace carto_facade -} // namespace viam diff --git a/viam-cartographer/src/carto_facade/map_builder.cc b/viam-cartographer/src/carto_facade/map_builder.cc index caff4894..a0482e0b 100644 --- a/viam-cartographer/src/carto_facade/map_builder.cc +++ b/viam-cartographer/src/carto_facade/map_builder.cc @@ -11,7 +11,6 @@ #include "cartographer/mapping/map_builder_interface.h" #include "cartographer/mapping/trajectory_builder_interface.h" #include "glog/logging.h" -#include "io.h" #include "map_builder.h" namespace viam { diff --git a/viam-cartographer/src/carto_facade/util_test.cc b/viam-cartographer/src/carto_facade/util_test.cc index 5ae76bbb..4e7757ab 100644 --- a/viam-cartographer/src/carto_facade/util_test.cc +++ b/viam-cartographer/src/carto_facade/util_test.cc @@ -8,7 +8,6 @@ #include #include -#include "io.h" #include "test_helpers.h" namespace help = viam::carto_facade::test_helpers; @@ -133,32 +132,11 @@ BOOST_AUTO_TEST_CASE(carto_lidar_reading_wrong_shape_binary_failure) { } BOOST_AUTO_TEST_CASE(carto_lidar_reading_binary_success) { - // Create a mini PCD file and save it in a tmp directory - std::string filename = "rplidar_data_2022-01-01T01:00:00.0001Z.pcd"; std::vector> points = { {-0.001000, 0.002000, 0.005000, 16711938}, {0.582000, 0.012000, 0.000000, 16711938}, {0.007000, 0.006000, 0.001000, 16711938}}; std::string pcd = help::binary_pcd(points); - boost::filesystem::path tmp_dir = help::make_tmp_dir(); - // Create a unique path in the temp directory and add the PCD file - boost::filesystem::ofstream ofs(tmp_dir / filename); - ofs << pcd; - ofs.close(); - // Read it in and check if the data in the TimedPointCloudData is equivalent - // to what we had in the pcd file - cartographer::sensor::TimedPointCloudData timed_pcd = - viam::carto_facade::io::TimedPointCloudDataFromPCDBuilder( - tmp_dir.string() + "/" + filename, 0); - - BOOST_TEST(timed_pcd.ranges.size() == points.size()); - help::timed_pcd_contains(timed_pcd, points); - BOOST_TEST(timed_pcd.origin == Eigen::Vector3f::Zero()); - BOOST_TEST(timed_pcd.time == - cartographer::common::FromUniversal(16409988000001121)); - // Remove the temporary directory and its contents - boost::filesystem::remove_all(tmp_dir); - auto [success, timed_pcd_from_string] = carto_lidar_reading(pcd, 16409988000001121); BOOST_TEST(success); @@ -170,32 +148,11 @@ BOOST_AUTO_TEST_CASE(carto_lidar_reading_binary_success) { } BOOST_AUTO_TEST_CASE(carto_lidar_reading_ascii_success) { - // Create a mini PCD file and save it in a tmp directory - std::string filename = "rplidar_data_2022-01-01T01:00:00.0001Z.pcd"; std::vector> points = { {-0.001000, 0.002000, 0.005000, 16711938}, {0.582000, 0.012000, 0.000000, 16711938}, {0.007000, 0.006000, 0.001000, 16711938}}; std::string pcd = help::ascii_pcd(points); - // Create a unique path in the temp directory and add the PCD file - boost::filesystem::path tmp_dir = help::make_tmp_dir(); - boost::filesystem::ofstream ofs(tmp_dir / filename); - ofs << pcd; - ofs.close(); - // Read it in and check if the data in the TimedPointCloudData is equivalent - // to what we had in the pcd file - cartographer::sensor::TimedPointCloudData timed_pcd = - viam::carto_facade::io::TimedPointCloudDataFromPCDBuilder( - tmp_dir.string() + "/" + filename, 0); - - BOOST_TEST(timed_pcd.ranges.size() == points.size()); - help::timed_pcd_contains(timed_pcd, points); - BOOST_TEST(timed_pcd.origin == Eigen::Vector3f::Zero()); - BOOST_TEST(timed_pcd.time == - cartographer::common::FromUniversal(16409988000001121)); - // Remove the temporary directory and its contents - boost::filesystem::remove_all(tmp_dir); - auto [success, timed_pcd_from_string] = carto_lidar_reading(pcd, 16409988000001121); BOOST_TEST(success);