RSDK-4664 - Return grpc errors for Position until cartographer has se…

…t an initial position (#264)

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:

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()

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<std::mutex> 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<std::mutex> lk(viam_response_mutex);

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<std::string> file_list_offline;
-    // size_t current_file_offline = 0;
-    // std::string current_file_online;
-
     std::shared_mutex optimization_shared_mutex;
 
-    // std::atomic<bool> finished_processing_offline{false};
     std::unique_ptr<std::thread> thread_save_internal_state;
 
     std::mutex viam_response_mutex;

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<std::vector<double>> 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);
     }
 
     {

viam-cartographer/src/carto_facade/map_builder.cc

@@ -132,6 +132,7 @@ MapBuilder::GetLocalSlamResultCallback() {
             std::lock_guard<std::mutex> lk(local_slam_result_pose_mutex);
             local_slam_result_pose = local_pose;
         }
+        local_pose_initialized = true;
     };
 }
 

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<bool> local_pose_initialized{false};
 
    private:
     std::mutex local_slam_result_pose_mutex;

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)