From 51011a936ce03a9964df3b8b57e9b95c37f9ca42 Mon Sep 17 00:00:00 2001 From: Katharina Xenia Kufieta Date: Wed, 13 Sep 2023 13:56:52 -0400 Subject: [PATCH] RSDK-4664 - Return grpc errors for Position until cartographer has set an initial position (#264) --- cartofacade/capi.go | 2 + cartofacade/capi_test.go | 20 +++--- .../src/carto_facade/carto_facade.cc | 7 ++ .../src/carto_facade/carto_facade.h | 69 ++++--------------- .../src/carto_facade/carto_facade_test.cc | 46 +++---------- .../src/carto_facade/map_builder.cc | 1 + .../src/carto_facade/map_builder.h | 1 + viam_cartographer_test.go | 32 +++++---- 8 files changed, 63 insertions(+), 115 deletions(-) diff --git a/cartofacade/capi.go b/cartofacade/capi.go index d3eab026..5395934f 100644 --- a/cartofacade/capi.go +++ b/cartofacade/capi.go @@ -500,6 +500,8 @@ func toError(status C.int) error { return errors.New("VIAM_CARTO_LIDAR_READING_INVALID") case C.VIAM_CARTO_GET_POSITION_RESPONSE_INVALID: return errors.New("VIAM_CARTO_GET_POSITION_RESPONSE_INVALID") + case C.VIAM_CARTO_GET_POSITION_NOT_INITIALIZED: + return errors.New("VIAM_CARTO_GET_POSITION_NOT_INITIALIZED") case C.VIAM_CARTO_POINTCLOUD_MAP_EMPTY: return errors.New("VIAM_CARTO_POINTCLOUD_MAP_EMPTY") case C.VIAM_CARTO_GET_POINT_CLOUD_MAP_RESPONSE_INVALID: diff --git a/cartofacade/capi_test.go b/cartofacade/capi_test.go index 1741253c..10913645 100644 --- a/cartofacade/capi_test.go +++ b/cartofacade/capi_test.go @@ -199,9 +199,9 @@ func TestCGoAPIWithoutIMU(t *testing.T) { // test position before sensor data is added position, err := vc.position() - test.That(t, err, test.ShouldBeNil) - test.That(t, position.ComponentReference, test.ShouldEqual, "mylidar") - positionIsZero(t, position) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err, test.ShouldResemble, errors.New("VIAM_CARTO_GET_POSITION_NOT_INITIALIZED")) + test.That(t, position, test.ShouldResemble, Position{}) // test pointCloudMap before sensor data is added pcd, err := vc.pointCloudMap() @@ -250,11 +250,11 @@ func TestCGoAPIWithoutIMU(t *testing.T) { timestamp = timestamp.Add(time.Second * 2) testAddLidarReading(t, vc, "viam-cartographer/mock_lidar/0.pcd", timestamp, pointcloud.PCDAscii) - // test position zeroed if not enough sensor data has been provided + // test position not initialized after first sensor data has been provided position, err = vc.position() - test.That(t, err, test.ShouldBeNil) - test.That(t, position.ComponentReference, test.ShouldEqual, "mylidar") - positionIsZero(t, position) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err, test.ShouldResemble, errors.New("VIAM_CARTO_GET_POSITION_NOT_INITIALIZED")) + test.That(t, position, test.ShouldResemble, Position{}) // test pointCloudMap returns error if not enough sensor data has been provided pcd, err = vc.pointCloudMap() @@ -390,9 +390,9 @@ func TestCGoAPIWithIMU(t *testing.T) { // test position before sensor data is added position, err := vc.position() - test.That(t, err, test.ShouldBeNil) - test.That(t, position.ComponentReference, test.ShouldEqual, "mylidar") - positionIsZero(t, position) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err, test.ShouldResemble, errors.New("VIAM_CARTO_GET_POSITION_NOT_INITIALIZED")) + test.That(t, position, test.ShouldResemble, Position{}) // test pointCloudMap before sensor data is added pcd, err := vc.pointCloudMap() diff --git a/viam-cartographer/src/carto_facade/carto_facade.cc b/viam-cartographer/src/carto_facade/carto_facade.cc index 7ecc7de0..9b0cd5fa 100644 --- a/viam-cartographer/src/carto_facade/carto_facade.cc +++ b/viam-cartographer/src/carto_facade/carto_facade.cc @@ -604,6 +604,13 @@ void CartoFacade::GetPosition(viam_carto_get_position_response *r) { << CartoFacadeState::STARTED; throw VIAM_CARTO_NOT_IN_STARTED_STATE; } + { + std::lock_guard lk(map_builder_mutex); + if (!map_builder.local_pose_initialized) { + LOG(ERROR) << "position is not yet initialized"; + throw VIAM_CARTO_GET_POSITION_NOT_INITIALIZED; + } + } cartographer::transform::Rigid3d global_pose; { std::lock_guard lk(viam_response_mutex); diff --git a/viam-cartographer/src/carto_facade/carto_facade.h b/viam-cartographer/src/carto_facade/carto_facade.h index b2eabd92..5c8b216f 100644 --- a/viam-cartographer/src/carto_facade/carto_facade.h +++ b/viam-cartographer/src/carto_facade/carto_facade.h @@ -112,19 +112,20 @@ typedef struct viam_carto_imu_reading { #define VIAM_CARTO_LIDAR_READING_EMPTY 20 #define VIAM_CARTO_LIDAR_READING_INVALID 21 #define VIAM_CARTO_GET_POSITION_RESPONSE_INVALID 22 -#define VIAM_CARTO_POINTCLOUD_MAP_EMPTY 23 -#define VIAM_CARTO_GET_POINT_CLOUD_MAP_RESPONSE_INVALID 24 -#define VIAM_CARTO_LIB_ALREADY_INITIALIZED 25 -#define VIAM_CARTO_GET_INTERNAL_STATE_RESPONSE_INVALID 26 -#define VIAM_CARTO_GET_INTERNAL_STATE_FILE_WRITE_IO_ERROR 27 -#define VIAM_CARTO_GET_INTERNAL_STATE_FILE_READ_IO_ERROR 28 -#define VIAM_CARTO_NOT_IN_INITIALIZED_STATE 29 -#define VIAM_CARTO_NOT_IN_IO_INITIALIZED_STATE 30 -#define VIAM_CARTO_NOT_IN_STARTED_STATE 31 -#define VIAM_CARTO_NOT_IN_TERMINATABLE_STATE 32 -#define VIAM_CARTO_IMU_PROVIDED_AND_IMU_ENABLED_MISMATCH 33 -#define VIAM_CARTO_IMU_READING_EMPTY 34 -#define VIAM_CARTO_IMU_READING_INVALID 35 +#define VIAM_CARTO_GET_POSITION_NOT_INITIALIZED 23 +#define VIAM_CARTO_POINTCLOUD_MAP_EMPTY 24 +#define VIAM_CARTO_GET_POINT_CLOUD_MAP_RESPONSE_INVALID 25 +#define VIAM_CARTO_LIB_ALREADY_INITIALIZED 26 +#define VIAM_CARTO_GET_INTERNAL_STATE_RESPONSE_INVALID 27 +#define VIAM_CARTO_GET_INTERNAL_STATE_FILE_WRITE_IO_ERROR 28 +#define VIAM_CARTO_GET_INTERNAL_STATE_FILE_READ_IO_ERROR 29 +#define VIAM_CARTO_NOT_IN_INITIALIZED_STATE 30 +#define VIAM_CARTO_NOT_IN_IO_INITIALIZED_STATE 31 +#define VIAM_CARTO_NOT_IN_STARTED_STATE 32 +#define VIAM_CARTO_NOT_IN_TERMINATABLE_STATE 33 +#define VIAM_CARTO_IMU_PROVIDED_AND_IMU_ENABLED_MISMATCH 34 +#define VIAM_CARTO_IMU_READING_EMPTY 35 +#define VIAM_CARTO_IMU_READING_INVALID 36 typedef struct viam_carto_algo_config { bool optimize_on_start; @@ -441,50 +442,8 @@ class CartoFacade { // includes the timestamp of the time when the map is saved. void SaveInternalStateOnInterval(); - // ConvertSavedMapToStream converted the saved pbstream to the passed in - // string and deletes the file. - // void ConvertSavedMapToStream(const std::string filename_with_timestamp, - // std::string *buffer); - - // TryFileClose attempts to close an opened ifstream, returning an error - // string if it fails. - // std::string TryFileClose(std::ifstream &file, std::string filename); - - // ProcessDataAndStartSavingMaps processes the data in the data directory - // that is newer than the provided data_cutoff_time - // and starts the process to save maps in parallel. In offline mode, - // all data in the directory is processed. In online mode, the most - // recently generated data is processed until a shutdown signal is - // received. - // void ProcessDataAndStartSavingMaps(double data_cutoff_time); - - // GetLatestPaintedMapSlices paints and returns the current map of - // Cartographer - // cartographer::io::PaintSubmapSlicesResult GetLatestPaintedMapSlices(); - - // GetLatestSampledPointCloudMapString paints and returns the latest map as - // a pcd string with probability estimates written to the color field. The - // pcd is generated from PaintedMapSlices() and sampled to fit the 32 MB - // limit on gRPC messages. The sampled behavior may change when moving to - // streamed point clouds. - // void GetLatestSampledPointCloudMapString(std::string &pointcloud); - - // CacheLatestMap extracts and saves the latest map as a backup in - // the respective member variables. - // void CacheLatestMap(); - - // If using the LOCALIZING slam mode, cache a copy of the map before - // beginning to process data. If cartographer fails to do this, - // terminate the program. - // void CacheMapInLocalizationMode(); - - // std::vector file_list_offline; - // size_t current_file_offline = 0; - // std::string current_file_online; - std::shared_mutex optimization_shared_mutex; - // std::atomic finished_processing_offline{false}; std::unique_ptr thread_save_internal_state; std::mutex viam_response_mutex; diff --git a/viam-cartographer/src/carto_facade/carto_facade_test.cc b/viam-cartographer/src/carto_facade/carto_facade_test.cc index b876d4c8..b44dcaab 100644 --- a/viam-cartographer/src/carto_facade/carto_facade_test.cc +++ b/viam-cartographer/src/carto_facade/carto_facade_test.cc @@ -791,19 +791,9 @@ BOOST_AUTO_TEST_CASE(CartoFacade_demo_without_imu) { { viam_carto_get_position_response pr; // Test get position before any data is provided - // it should be all zeroed out - BOOST_TEST(viam_carto_get_position(vc, &pr) == VIAM_CARTO_SUCCESS); - BOOST_TEST(pr.x == 0); - BOOST_TEST(pr.y == 0); - BOOST_TEST(pr.z == 0); - BOOST_TEST(pr.imag == 0); - BOOST_TEST(pr.jmag == 0); - BOOST_TEST(pr.kmag == 0); - BOOST_TEST(pr.real == 1); - BOOST_TEST(to_std_string(pr.component_reference) == "lidar"); - - BOOST_TEST(viam_carto_get_position_response_destroy(&pr) == - VIAM_CARTO_SUCCESS); + // it should return the position uninitialized error + BOOST_TEST(viam_carto_get_position(vc, &pr) == + VIAM_CARTO_GET_POSITION_NOT_INITIALIZED); } std::vector> points = { @@ -937,19 +927,9 @@ BOOST_AUTO_TEST_CASE(CartoFacade_demo_without_imu) { { viam_carto_get_position_response pr; // Test get position before any data is provided - // it should be all zeroed out - BOOST_TEST(viam_carto_get_position(vc, &pr) == VIAM_CARTO_SUCCESS); - BOOST_TEST(pr.x == 0); - BOOST_TEST(pr.y == 0); - BOOST_TEST(pr.z == 0); - BOOST_TEST(pr.imag == 0); - BOOST_TEST(pr.jmag == 0); - BOOST_TEST(pr.kmag == 0); - BOOST_TEST(pr.real == 1); - BOOST_TEST(to_std_string(pr.component_reference) == "lidar"); - - BOOST_TEST(viam_carto_get_position_response_destroy(&pr) == - VIAM_CARTO_SUCCESS); + // it should return an error + BOOST_TEST(viam_carto_get_position(vc, &pr) == + VIAM_CARTO_GET_POSITION_NOT_INITIALIZED); } // GetPosition is unchanged from first AddLidarReading request @@ -974,18 +954,8 @@ BOOST_AUTO_TEST_CASE(CartoFacade_demo_without_imu) { { viam_carto_get_position_response pr; - BOOST_TEST(viam_carto_get_position(vc, &pr) == VIAM_CARTO_SUCCESS); - BOOST_TEST(pr.x == 0); - BOOST_TEST(pr.y == 0); - BOOST_TEST(pr.z == 0); - BOOST_TEST(pr.imag == 0); - BOOST_TEST(pr.jmag == 0); - BOOST_TEST(pr.kmag == 0); - BOOST_TEST(pr.real == 1); - BOOST_TEST(to_std_string(pr.component_reference) == "lidar"); - - BOOST_TEST(viam_carto_get_position_response_destroy(&pr) == - VIAM_CARTO_SUCCESS); + BOOST_TEST(viam_carto_get_position(vc, &pr) == + VIAM_CARTO_GET_POSITION_NOT_INITIALIZED); } { diff --git a/viam-cartographer/src/carto_facade/map_builder.cc b/viam-cartographer/src/carto_facade/map_builder.cc index cafd14c7..76018ef6 100644 --- a/viam-cartographer/src/carto_facade/map_builder.cc +++ b/viam-cartographer/src/carto_facade/map_builder.cc @@ -132,6 +132,7 @@ MapBuilder::GetLocalSlamResultCallback() { std::lock_guard lk(local_slam_result_pose_mutex); local_slam_result_pose = local_pose; } + local_pose_initialized = true; }; } diff --git a/viam-cartographer/src/carto_facade/map_builder.h b/viam-cartographer/src/carto_facade/map_builder.h index 0fafa6e3..166319dd 100644 --- a/viam-cartographer/src/carto_facade/map_builder.h +++ b/viam-cartographer/src/carto_facade/map_builder.h @@ -118,6 +118,7 @@ class MapBuilder { cartographer::mapping::proto::MapBuilderOptions map_builder_options_; cartographer::mapping::proto::TrajectoryBuilderOptions trajectory_builder_options_; + std::atomic local_pose_initialized{false}; private: std::mutex local_slam_result_pose_mutex; diff --git a/viam_cartographer_test.go b/viam_cartographer_test.go index 3fcfe50d..f623105c 100644 --- a/viam_cartographer_test.go +++ b/viam_cartographer_test.go @@ -243,9 +243,11 @@ func TestNew(t *testing.T) { timestamp1, err := svc.LatestMapInfo(context.Background()) test.That(t, err, test.ShouldBeNil) - _, componentReference, err := svc.Position(context.Background()) - test.That(t, err, test.ShouldBeNil) - test.That(t, componentReference, test.ShouldEqual, "replay_lidar") + pose, componentReference, err := svc.Position(context.Background()) + test.That(t, pose, test.ShouldBeNil) + test.That(t, componentReference, test.ShouldBeEmpty) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, "VIAM_CARTO_GET_POSITION_NOT_INITIALIZED") pcmFunc, err := svc.PointCloudMap(context.Background()) test.That(t, err, test.ShouldBeNil) @@ -290,9 +292,11 @@ func TestNew(t *testing.T) { test.That(t, ok, test.ShouldBeTrue) test.That(t, cs.SlamMode, test.ShouldEqual, cartofacade.UpdatingMode) - _, componentReference, err := svc.Position(context.Background()) - test.That(t, err, test.ShouldBeNil) - test.That(t, componentReference, test.ShouldEqual, "good_lidar") + pose, componentReference, err := svc.Position(context.Background()) + test.That(t, pose, test.ShouldBeNil) + test.That(t, componentReference, test.ShouldBeEmpty) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, "VIAM_CARTO_GET_POSITION_NOT_INITIALIZED") pcmFunc, err := svc.PointCloudMap(context.Background()) test.That(t, err, test.ShouldBeNil) @@ -467,9 +471,11 @@ func TestNewFeatureFlag(t *testing.T) { timestamp1, err := svc.LatestMapInfo(context.Background()) test.That(t, err, test.ShouldBeNil) - _, componentReference, err := svc.Position(context.Background()) - test.That(t, err, test.ShouldBeNil) - test.That(t, componentReference, test.ShouldEqual, "good_lidar") + pose, componentReference, err := svc.Position(context.Background()) + test.That(t, pose, test.ShouldBeNil) + test.That(t, componentReference, test.ShouldBeEmpty) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, "VIAM_CARTO_GET_POSITION_NOT_INITIALIZED") pcmFunc, err := svc.PointCloudMap(context.Background()) test.That(t, err, test.ShouldBeNil) @@ -514,9 +520,11 @@ func TestNewFeatureFlag(t *testing.T) { test.That(t, ok, test.ShouldBeTrue) test.That(t, cs.SlamMode, test.ShouldEqual, cartofacade.UpdatingMode) - _, componentReference, err := svc.Position(context.Background()) - test.That(t, err, test.ShouldBeNil) - test.That(t, componentReference, test.ShouldEqual, "good_lidar") + pose, componentReference, err := svc.Position(context.Background()) + test.That(t, pose, test.ShouldBeNil) + test.That(t, componentReference, test.ShouldBeEmpty) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, "VIAM_CARTO_GET_POSITION_NOT_INITIALIZED") timestamp1, err := svc.LatestMapInfo(context.Background()) test.That(t, err, test.ShouldBeNil)