Skip to content

Commit

Permalink
Merge branch 'main' into RSDK-3345-refactor-get-pointcloud-map
Browse files Browse the repository at this point in the history
  • Loading branch information
@kim-mishra
kim-mishra authored Jun 6, 2023
2 parents 25357ee + ed04293 commit c0aca67
Show file tree
Hide file tree
Showing 12 changed files with 367 additions and 218 deletions.
2 changes: 1 addition & 1 deletion integration_test.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -224,7 +224,7 @@ func integrationtestHelperCartographer(t *testing.T, subAlgo viamcartographer.Su
deleteProcessedData,
useLiveData)
}
if strings.Contains(line, "Finished optimizing final map") {
if strings.Contains(line, "No new data found") {
break
}
}
Expand Down
12 changes: 6 additions & 6 deletions internal/dim-2d/dim-2d.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -118,18 +118,18 @@ func GetAndSaveData(ctx context.Context, dataDirectory string, lidar lidar.Lidar
}

// If the server provided timestamps correlated with the point cloud, extract the time
// received from the metadata and use that instead of the current time.
timeRec := time.Now()
timeReceivedMetadata, ok := md[contextutils.TimeReceivedMetadataKey]
// requested from the metadata and use that instead of the current time.
timeReq := time.Now()
timeRequestedMetadata, ok := md[contextutils.TimeRequestedMetadataKey]
if ok {
timeRec, err = time.Parse(time.RFC3339Nano, timeReceivedMetadata[0])
timeReq, err = time.Parse(time.RFC3339Nano, timeRequestedMetadata[0])
if err != nil {
logger.Warnw("couldn't parse time received", "error", err)
logger.Warnw("couldn't parse time", "error", err)
return "", err
}
}

dataDir := filepath.Join(dataDirectory, "data")
filename := dataprocess.CreateTimestampFilename(dataDir, lidar.Name, ".pcd", timeRec)
filename := dataprocess.CreateTimestampFilename(dataDir, lidar.Name, ".pcd", timeReq)
return filename, dataprocess.WritePCDToFile(pointcloud, filename)
}
26 changes: 26 additions & 0 deletions internal/testhelper/testhelper.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@ import (
"go.viam.com/rdk/rimage/transform"
"go.viam.com/rdk/services/slam"
"go.viam.com/rdk/testutils/inject"
"go.viam.com/rdk/utils/contextutils"
"go.viam.com/test"
"go.viam.com/utils/artifact"
"go.viam.com/utils/pexec"
Expand All @@ -42,6 +43,8 @@ const (
// sensor that is used in the GetAndSaveData function.
SensorValidationIntervalSecForTest = 1
testDialMaxTimeoutSec = 1
// TestTime can be used to test specific timestamps provided by a replay sensor.
TestTime = "2006-01-02T15:04:05.9999Z"
)

// IntegrationLidarReleasePointCloudChan is the lidar pointcloud release
Expand All @@ -68,6 +71,8 @@ func SetupDeps(sensors []string) resource.Dependencies {
switch sensor {
case "good_lidar":
deps[camera.Named(sensor)] = getGoodLidar()
case "replay_sensor":
deps[camera.Named(sensor)] = getReplaySensor()
case "invalid_sensor":
deps[camera.Named(sensor)] = getInvalidSensor()
case "gibberish":
Expand Down Expand Up @@ -98,6 +103,27 @@ func getGoodLidar() *inject.Camera {
return cam
}

func getReplaySensor() *inject.Camera {
cam := &inject.Camera{}
cam.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) {
md := ctx.Value(contextutils.MetadataContextKey)
if mdMap, ok := md.(map[string][]string); ok {
mdMap[contextutils.TimeRequestedMetadataKey] = []string{TestTime}
}
return pointcloud.New(), nil
}
cam.StreamFunc = func(ctx context.Context, errHandlers ...gostream.ErrorHandler) (gostream.VideoStream, error) {
return nil, errors.New("lidar not camera")
}
cam.ProjectorFunc = func(ctx context.Context) (transform.Projector, error) {
return nil, transform.NewNoIntrinsicsError("")
}
cam.PropertiesFunc = func(ctx context.Context) (camera.Properties, error) {
return camera.Properties{}, nil
}
return cam
}

func getInvalidSensor() *inject.Camera {
cam := &inject.Camera{}
cam.NextPointCloudFunc = func(ctx context.Context) (pointcloud.PointCloud, error) {
Expand Down
94 changes: 60 additions & 34 deletions viam-cartographer.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -153,15 +153,13 @@ func New(
}
}()

if cartoSvc.useLiveData {
if cartoSvc.primarySensorName != "" {
if err := dim2d.ValidateGetAndSaveData(cancelCtx, cartoSvc.dataDirectory, lidar,
sensorValidationMaxTimeoutSec, sensorValidationIntervalSec, cartoSvc.logger); err != nil {
return nil, errors.Wrap(err, "getting and saving data failed")
}
cartoSvc.StartDataProcess(cancelCtx, lidar, nil)
logger.Debug("Running in live mode")
} else {
logger.Debug("Running in offline mode")
logger.Debugf("Reading data from sensor: %v", cartoSvc.primarySensorName)
}

if err := cartoSvc.StartSLAMProcess(ctx); err != nil {
Expand Down Expand Up @@ -248,55 +246,83 @@ func (cartoSvc *cartographerService) GetInternalState(ctx context.Context) (func

// StartDataProcess starts a go routine that saves data from the lidar to the user-defined data directory.
func (cartoSvc *cartographerService) StartDataProcess(
cancelCtx context.Context,
ctx context.Context,
lidar lidar.Lidar,
c chan int,
) {
cartoSvc.activeBackgroundWorkers.Add(1)
if err := cancelCtx.Err(); err != nil {
if err := ctx.Err(); err != nil {
if !errors.Is(err, context.Canceled) {
cartoSvc.logger.Errorw("unexpected error in SLAM service", "error", err)
}
cartoSvc.activeBackgroundWorkers.Done()
return
}

goutils.PanicCapturingGo(func() {
ticker := time.NewTicker(time.Millisecond * time.Duration(cartoSvc.dataRateMs))
defer ticker.Stop()
defer cartoSvc.activeBackgroundWorkers.Done()
if !cartoSvc.useLiveData {
// If we're not using live data, we read from the sensor as fast as
// possible, since the sensor is just playing back pre-captured data.
cartoSvc.readData(ctx, lidar, c)
} else {
cartoSvc.readDataOnInterval(ctx, lidar, c)
}
})
}

for {
if err := cancelCtx.Err(); err != nil {
func (cartoSvc *cartographerService) readData(ctx context.Context, lidar lidar.Lidar, c chan int) {
for {
if err := ctx.Err(); err != nil {
if !errors.Is(err, context.Canceled) {
cartoSvc.logger.Errorw("unexpected error in SLAM data process", "error", err)
}
return
}

cartoSvc.getNextDataPoint(ctx, lidar, c)
}
}

func (cartoSvc *cartographerService) readDataOnInterval(ctx context.Context, lidar lidar.Lidar, c chan int) {
ticker := time.NewTicker(time.Millisecond * time.Duration(cartoSvc.dataRateMs))
defer ticker.Stop()
defer cartoSvc.activeBackgroundWorkers.Done()

for {
if err := ctx.Err(); err != nil {
if !errors.Is(err, context.Canceled) {
cartoSvc.logger.Errorw("unexpected error in SLAM data process", "error", err)
}
return
}

select {
case <-ctx.Done():
return
case <-ticker.C:
if err := ctx.Err(); err != nil {
if !errors.Is(err, context.Canceled) {
cartoSvc.logger.Errorw("unexpected error in SLAM data process", "error", err)
}
return
}

select {
case <-cancelCtx.Done():
return
case <-ticker.C:
cartoSvc.activeBackgroundWorkers.Add(1)
if err := cancelCtx.Err(); err != nil {
if !errors.Is(err, context.Canceled) {
cartoSvc.logger.Errorw("unexpected error in SLAM data process", "error", err)
}
cartoSvc.activeBackgroundWorkers.Done()
return
}
goutils.PanicCapturingGo(func() {
defer cartoSvc.activeBackgroundWorkers.Done()
if _, err := dim2d.GetAndSaveData(cancelCtx, cartoSvc.dataDirectory, lidar, cartoSvc.logger); err != nil {
cartoSvc.logger.Warn(err)
}
if c != nil {
c <- 1
}
})
}
cartoSvc.activeBackgroundWorkers.Add(1)
goutils.PanicCapturingGo(func() {
defer cartoSvc.activeBackgroundWorkers.Done()
cartoSvc.getNextDataPoint(ctx, lidar, c)
})
}
})
}
}

func (cartoSvc *cartographerService) getNextDataPoint(ctx context.Context, lidar lidar.Lidar, c chan int) {
if _, err := dim2d.GetAndSaveData(ctx, cartoSvc.dataDirectory, lidar, cartoSvc.logger); err != nil {
cartoSvc.logger.Warn(err)
}
if c != nil {
c <- 1
}
}

func (cartoSvc *cartographerService) DoCommand(ctx context.Context, req map[string]interface{}) (map[string]interface{}, error) {
Expand Down
93 changes: 64 additions & 29 deletions viam-cartographer/src/carto_facade/carto_facade.cc
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,8 @@
// This is an experimental integration of cartographer into RDK.
#include "carto_facade.h"

#include "glog/logging.h"

namespace viam {
namespace carto_facade {

Expand All @@ -26,8 +28,9 @@ config from_viam_carto_config(viam_carto_config vcc) {
return c;
};

CartoFacade::CartoFacade(const viam_carto_config c,
CartoFacade::CartoFacade(viam_carto_lib *pVCL, const viam_carto_config c,
const viam_carto_algo_config ac) {
lib = pVCL;
config = from_viam_carto_config(c);
algo_config = ac;
};
Expand Down Expand Up @@ -66,90 +69,122 @@ int CartoFacade::AddSensorReading(viam_carto_sensor_reading *sr) {
} // namespace carto_facade
} // namespace viam

extern int viam_carto_init(const viam_carto **ppVC, const viam_carto_config c,
const viam_carto_algo_config ac, char **errmsg) {
extern int viam_carto_lib_init(viam_carto_lib **ppVCL, int minloglevel,
int verbose) {
if (ppVCL == nullptr) {
return VIAM_CARTO_LIB_INVALID;
}
if (!((sizeof(float) == 4) && (CHAR_BIT == 8) && (sizeof(int) == 4))) {
return VIAM_CARTO_LIB_PLATFORM_INVALID;
}
viam_carto_lib *vcl = (viam_carto_lib *)malloc(sizeof(viam_carto_lib));
if (vcl == nullptr) {
return VIAM_CARTO_OUT_OF_MEMORY;
}
google::InitGoogleLogging("cartographer");
FLAGS_logtostderr = 1;
FLAGS_minloglevel = minloglevel;
FLAGS_v = verbose;
vcl->minloglevel = minloglevel;
vcl->verbose = verbose;

*ppVCL = vcl;

return VIAM_CARTO_SUCCESS;
};

extern int viam_carto_lib_terminate(viam_carto_lib **ppVCL) {
FLAGS_logtostderr = 0;
FLAGS_minloglevel = 0;
FLAGS_v = 0;
google::ShutdownGoogleLogging();
free(*ppVCL);
*ppVCL = nullptr;
return VIAM_CARTO_SUCCESS;
};

extern int viam_carto_init(viam_carto **ppVC, viam_carto_lib *pVCL,
const viam_carto_config c,
const viam_carto_algo_config ac) {
if (ppVC == nullptr) {
*errmsg = "viam_carto pointer should not be NULL";
return VIAM_CARTO_VC_INVALID;
}

if (pVCL == nullptr) {
return VIAM_CARTO_LIB_INVALID;
}

// allocate viam_carto struct
viam_carto *vc = (viam_carto *)malloc(sizeof(viam_carto));
if (vc == nullptr) {
return VIAM_CARTO_OUT_OF_MEMORY;
}

vc->carto_obj = new viam::carto_facade::CartoFacade(c, ac);
vc->carto_obj = new viam::carto_facade::CartoFacade(pVCL, c, ac);

// point to newly created viam_carto struct
*ppVC = vc;
return VIAM_CARTO_SUCCESS;
};

extern int viam_carto_start(const viam_carto **vc, char **errmsg) {
return VIAM_CARTO_SUCCESS;
};
extern int viam_carto_start(viam_carto *vc) { return VIAM_CARTO_SUCCESS; };

extern int viam_carto_stop(const viam_carto **vc, char **errmsg) {
return VIAM_CARTO_SUCCESS;
};
extern int viam_carto_stop(viam_carto *vc) { return VIAM_CARTO_SUCCESS; };

extern int viam_carto_terminate(const viam_carto **ppVC, char **errmsg) {
extern int viam_carto_terminate(viam_carto **ppVC) {
viam::carto_facade::CartoFacade *cf =
static_cast<viam::carto_facade::CartoFacade *>((*ppVC)->carto_obj);
delete cf;
free((viam_carto *)*ppVC);
return VIAM_CARTO_SUCCESS;
};

extern int viam_carto_add_sensor_reading(const viam_carto **vc,
const viam_carto_sensor_reading *sr,
char **errmsg) {
extern int viam_carto_add_sensor_reading(viam_carto *vc,
const viam_carto_sensor_reading *sr) {
return VIAM_CARTO_SUCCESS;
};

extern int viam_carto_add_sensor_reading_destroy(viam_carto_sensor_reading *sr,
char **errmsg) {
extern int viam_carto_add_sensor_reading_destroy(
viam_carto_sensor_reading *sr) {
return VIAM_CARTO_SUCCESS;
};

extern int viam_carto_get_position(const viam_carto **vc,
viam_carto_get_position_response *r,
char **errmsg) {
extern int viam_carto_get_position(viam_carto *vc,
viam_carto_get_position_response *r) {
viam::carto_facade::CartoFacade *cf =
static_cast<viam::carto_facade::CartoFacade *>((*vc)->carto_obj);
static_cast<viam::carto_facade::CartoFacade *>((vc)->carto_obj);
cf->GetPosition(r);
return VIAM_CARTO_SUCCESS;
};

extern int viam_carto_get_position_response_destroy(
viam_carto_get_position_response *r, char **errmsg) {
viam_carto_get_position_response *r) {
int return_code = VIAM_CARTO_SUCCESS;
int rc = BSTR_OK;
rc = bdestroy(r->component_reference);
if (rc != BSTR_OK) {
// TODO: Write error messages
return_code = VIAM_CARTO_DESTRUCTOR_ERROR;
}
r->component_reference = nullptr;
return return_code;
};

extern int viam_carto_get_point_cloud_map(
const viam_carto **vc, viam_carto_get_point_cloud_map_response *r,
char **errmsg) {
viam_carto *vc, viam_carto_get_point_cloud_map_response *r) {
return VIAM_CARTO_SUCCESS;
};

extern int viam_carto_get_point_cloud_map_response_destroy(
viam_carto_get_point_cloud_map_response *r, char **errmsg) {
viam_carto_get_point_cloud_map_response *r) {
return VIAM_CARTO_SUCCESS;
};

extern int viam_carto_get_internal_state(
const viam_carto **vc, viam_carto_get_internal_state_response *r,
char **errmsg) {
viam_carto *vc, viam_carto_get_internal_state_response *r) {
return VIAM_CARTO_SUCCESS;
};

extern int viam_carto_get_internal_state_response_destroy(
viam_carto_get_internal_state_response *r, char **errmsg) {
viam_carto_get_internal_state_response *r) {
return VIAM_CARTO_SUCCESS;
};
Loading

0 comments on commit c0aca67

Please sign in to comment.