Skip to content

Commit

Permalink
[RSDK-3541] CartoFacade GetPosition (viam-modules#160)
Browse files Browse the repository at this point in the history
  • Loading branch information
@nicksanford
nicksanford authored Jun 28, 2023
1 parent e268c7c commit 14c8eaa
Show file tree
Hide file tree
Showing 7 changed files with 345 additions and 237 deletions.
16 changes: 0 additions & 16 deletions cartofacade/capi.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,11 +65,6 @@ type GetPosition struct {
Y float64
Z float64

Ox float64
Oy float64
Oz float64
Theta float64

Real float64
Imag float64
Jmag float64
Expand Down Expand Up @@ -295,15 +290,10 @@ func getTestGetPositionResponse() C.viam_carto_get_position_response {
gpr.y = C.double(200)
gpr.z = C.double(300)

gpr.o_x = C.double(400)
gpr.o_y = C.double(500)
gpr.o_z = C.double(600)

gpr.imag = C.double(700)
gpr.jmag = C.double(800)
gpr.kmag = C.double(900)

gpr.theta = C.double(1000)
gpr.real = C.double(1100)

gpr.component_reference = goStringToBstring("C++ component reference")
Expand Down Expand Up @@ -354,7 +344,6 @@ func getConfig(cfg CartoConfig) (C.viam_carto_config, error) {
vcc.sensors_len = C.int(sz)
vcc.map_rate_sec = C.int(cfg.MapRateSecond)
vcc.data_dir = goStringToBstring(cfg.DataDir)
vcc.component_reference = goStringToBstring(cfg.ComponentReference)
vcc.lidar_config = lidarCfg

return vcc, nil
Expand Down Expand Up @@ -384,11 +373,6 @@ func toGetPositionResponse(value C.viam_carto_get_position_response) GetPosition
Y: float64(value.y),
Z: float64(value.z),

Ox: float64(value.o_x),
Oy: float64(value.o_y),
Oz: float64(value.o_z),
Theta: float64(value.theta),

Real: float64(value.real),
Imag: float64(value.imag),
Jmag: float64(value.jmag),
Expand Down
32 changes: 8 additions & 24 deletions cartofacade/capi_test.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,8 +29,6 @@ func TestGetConfig(t *testing.T) {
dataDir := bstringToGoString(vcc.data_dir)
test.That(t, dataDir, test.ShouldResemble, dir)

componentReference := bstringToGoString(vcc.component_reference)
test.That(t, componentReference, test.ShouldResemble, "component")
freeBstringArray(vcc.sensors, vcc.sensors_len)

test.That(t, vcc.lidar_config, test.ShouldEqual, twoD)
Expand All @@ -46,16 +44,9 @@ func TestGetPositionResponse(t *testing.T) {
test.That(t, holder.X, test.ShouldEqual, 100)
test.That(t, holder.Y, test.ShouldEqual, 200)
test.That(t, holder.Z, test.ShouldEqual, 300)

test.That(t, holder.Ox, test.ShouldEqual, 400)
test.That(t, holder.Oy, test.ShouldEqual, 500)
test.That(t, holder.Oz, test.ShouldEqual, 600)

test.That(t, holder.Imag, test.ShouldEqual, 700)
test.That(t, holder.Jmag, test.ShouldEqual, 800)
test.That(t, holder.Kmag, test.ShouldEqual, 900)

test.That(t, holder.Theta, test.ShouldEqual, 1000)
test.That(t, holder.Real, test.ShouldEqual, 1100)
})
}
Expand Down Expand Up @@ -161,23 +152,16 @@ func TestCGoAPI(t *testing.T) {
holder, err := vc.getPosition()

test.That(t, err, test.ShouldBeNil)
test.That(t, holder.ComponentReference, test.ShouldEqual, "C++ component reference")

test.That(t, holder.X, test.ShouldEqual, 100)
test.That(t, holder.X, test.ShouldEqual, 100)
test.That(t, holder.Y, test.ShouldEqual, 200)
test.That(t, holder.Z, test.ShouldEqual, 300)
test.That(t, holder.ComponentReference, test.ShouldEqual, "mysensor")

test.That(t, holder.Ox, test.ShouldEqual, 400)
test.That(t, holder.Oy, test.ShouldEqual, 500)
test.That(t, holder.Oz, test.ShouldEqual, 600)
test.That(t, holder.X, test.ShouldEqual, 0)
test.That(t, holder.Y, test.ShouldEqual, 0)
test.That(t, holder.Z, test.ShouldEqual, 0)

test.That(t, holder.Imag, test.ShouldEqual, 700)
test.That(t, holder.Jmag, test.ShouldEqual, 800)
test.That(t, holder.Kmag, test.ShouldEqual, 900)

test.That(t, holder.Theta, test.ShouldEqual, 1000)
test.That(t, holder.Real, test.ShouldEqual, 1100)
test.That(t, holder.Imag, test.ShouldEqual, 0)
test.That(t, holder.Jmag, test.ShouldEqual, 0)
test.That(t, holder.Kmag, test.ShouldEqual, 0)
test.That(t, holder.Real, test.ShouldEqual, 1)

// test getPointCloudMap
_, err = vc.getPointCloudMap()
Expand Down
152 changes: 100 additions & 52 deletions viam-cartographer/src/carto_facade/carto_facade.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,17 @@ bool check_if_empty_pixel(ColorARGB pixel_color) {
return (pixel_color.G == 0);
}

std::string to_std_string(bstring b_str) {
int len = blength(b_str);
char *tmp = bstr2cstr(b_str, 0);
if (tmp == NULL) {
throw VIAM_CARTO_OUT_OF_MEMORY;
}
std::string std_str(tmp, len);
bcstrfree(tmp);
return std_str;
}

// Convert the scale of a specified color channel from the given UCHAR
// range of 0 - 255 to an inverse probability range of 100 - 0.
int calculate_probability_from_color_channels(ColorARGB pixel_color) {
Expand Down Expand Up @@ -60,14 +71,6 @@ std::ostream &operator<<(std::ostream &os, const ActionMode &action_mode) {
return os;
}

std::string to_std_string(bstring b_str) {
int len = blength(b_str);
char *tmp = bstr2cstr(b_str, 0);
std::string std_str(tmp, len);
bcstrfree(tmp);
return std_str;
}

void validate_lidar_config(viam_carto_LIDAR_CONFIG lidar_config) {
switch (lidar_config) {
case VIAM_CARTO_TWO_D:
Expand All @@ -84,9 +87,7 @@ config from_viam_carto_config(viam_carto_config vcc) {
for (int i = 0; i < vcc.sensors_len; i++) {
c.sensors.push_back(to_std_string(vcc.sensors[i]));
}

c.data_dir = to_std_string(vcc.data_dir);
c.component_reference = to_std_string(vcc.component_reference);
c.map_rate_sec = std::chrono::seconds(vcc.map_rate_sec);
c.lidar_config = vcc.lidar_config;
if (c.sensors.size() == 0) {
Expand All @@ -98,10 +99,11 @@ config from_viam_carto_config(viam_carto_config vcc) {
if (vcc.map_rate_sec < 0) {
throw VIAM_CARTO_MAP_RATE_SEC_INVALID;
}
if (c.component_reference.empty()) {
if (c.sensors[0].empty()) {
throw VIAM_CARTO_COMPONENT_REFERENCE_INVALID;
}
validate_lidar_config(c.lidar_config);
c.component_reference = bstrcpy(vcc.sensors[0]);

return c;
};
Expand Down Expand Up @@ -160,6 +162,8 @@ CartoFacade::CartoFacade(viam_carto_lib *pVCL, const viam_carto_config c,
path_to_internal_state = config.data_dir + "/internal_state";
};

CartoFacade::~CartoFacade() { bdestroy(config.component_reference); }

void setup_filesystem(std::string data_dir,
std::string path_to_internal_state) {
// setup internal state directory if it doesn't exist
Expand Down Expand Up @@ -530,34 +534,34 @@ void CartoFacade::GetLatestSampledPointCloudMapString(std::string &pointcloud) {
return;
}

int CartoFacade::GetPosition(viam_carto_get_position_response *r) {
bstring cr = bfromcstr("C++ component reference");
r->x = 100;
r->y = 200;
r->z = 300;
r->o_x = 400;
r->o_y = 500;
r->o_z = 600;
r->imag = 700;
r->jmag = 800;
r->kmag = 900;
r->theta = 1000;
r->real = 1100;
r->component_reference = cr;
return VIAM_CARTO_SUCCESS;
};
void CartoFacade::GetPosition(viam_carto_get_position_response *r) {
cartographer::transform::Rigid3d global_pose;
{
std::lock_guard<std::mutex> lk(viam_response_mutex);
global_pose = latest_global_pose;
}

int CartoFacade::GetPointCloudMap(viam_carto_get_point_cloud_map_response *r) {
return VIAM_CARTO_SUCCESS;
};
int CartoFacade::GetInternalState(viam_carto_get_internal_state_response *r) {
return VIAM_CARTO_SUCCESS;
auto pos_vector = global_pose.translation();
auto pos_quat = global_pose.rotation();

r->x = pos_vector.x() * 1000;
r->y = pos_vector.y() * 1000;
r->z = pos_vector.z() * 1000;
r->real = pos_quat.w();
r->imag = pos_quat.x();
r->jmag = pos_quat.y();
r->kmag = pos_quat.z();
r->component_reference = bstrcpy(config.component_reference);
};

int CartoFacade::Start() {
void CartoFacade::GetPointCloudMap(
viam_carto_get_point_cloud_map_response *r){};

void CartoFacade::GetInternalState(viam_carto_get_internal_state_response *r){};

void CartoFacade::Start() {
started = true;
StartSaveInternalState();
return VIAM_CARTO_SUCCESS;
};

void CartoFacade::StartSaveInternalState() {
Expand Down Expand Up @@ -622,16 +626,12 @@ void CartoFacade::SaveInternalStateOnInterval() {
}
}

int CartoFacade::Stop() {
StopSaveInternalState();
return VIAM_CARTO_SUCCESS;
};
void CartoFacade::Stop() { StopSaveInternalState(); };

void CartoFacade::AddSensorReading(const viam_carto_sensor_reading *sr) {
std::string sensor = to_std_string(sr->sensor);
if (config.sensors[0] != sensor) {
VLOG(1) << "expected sensor: " << sensor << " to be "
<< config.sensors[0];
if (biseq(config.component_reference, sr->sensor) == false) {
VLOG(1) << "expected sensor: " << to_std_string(sr->sensor) << " to be "
<< config.component_reference;
throw VIAM_CARTO_SENSOR_NOT_IN_SENSOR_LIST;
}
std::string sensor_reading = to_std_string(sr->sensor_reading);
Expand All @@ -653,9 +653,9 @@ void CartoFacade::AddSensorReading(const viam_carto_sensor_reading *sr) {
if (map_builder_mutex.try_lock()) {
map_builder.AddSensorData(measurement);
auto local_poses = map_builder.GetLocalSlamResultPoses();
VLOG(1) << "local_poses.size(): " << local_poses.size();
if (local_poses.size() > 0) {
update_latest_global_pose = true;
VLOG(1) << "local_poses.size(): " << local_poses.size();
tmp_global_pose = map_builder.GetGlobalPose(local_poses.back());
}
map_builder_mutex.unlock();
Expand Down Expand Up @@ -731,6 +731,14 @@ extern int viam_carto_lib_init(viam_carto_lib **ppVCL, int minloglevel,
};

extern int viam_carto_lib_terminate(viam_carto_lib **ppVCL) {
if (ppVCL == nullptr) {
return VIAM_CARTO_LIB_INVALID;
}

if (*ppVCL == nullptr) {
return VIAM_CARTO_LIB_INVALID;
}

FLAGS_logtostderr = 0;
FLAGS_minloglevel = 0;
FLAGS_v = 0;
Expand Down Expand Up @@ -792,9 +800,9 @@ extern int viam_carto_start(viam_carto *vc) {
if (vc == nullptr) {
return VIAM_CARTO_VC_INVALID;
}
viam::carto_facade::CartoFacade *cf =
static_cast<viam::carto_facade::CartoFacade *>(vc->carto_obj);
try {
viam::carto_facade::CartoFacade *cf =
static_cast<viam::carto_facade::CartoFacade *>(vc->carto_obj);
cf->Start();
} catch (int err) {
return err;
Expand All @@ -809,9 +817,10 @@ extern int viam_carto_stop(viam_carto *vc) {
if (vc == nullptr) {
return VIAM_CARTO_VC_INVALID;
}
viam::carto_facade::CartoFacade *cf =
static_cast<viam::carto_facade::CartoFacade *>(vc->carto_obj);

try {
viam::carto_facade::CartoFacade *cf =
static_cast<viam::carto_facade::CartoFacade *>(vc->carto_obj);
cf->Stop();
} catch (int err) {
return err;
Expand All @@ -823,6 +832,13 @@ extern int viam_carto_stop(viam_carto *vc) {
};

extern int viam_carto_terminate(viam_carto **ppVC) {
if (ppVC == nullptr) {
return VIAM_CARTO_VC_INVALID;
}

if (*ppVC == nullptr) {
return VIAM_CARTO_VC_INVALID;
}
viam::carto_facade::CartoFacade *cf =
static_cast<viam::carto_facade::CartoFacade *>((*ppVC)->carto_obj);
delete cf;
Expand All @@ -840,9 +856,9 @@ extern int viam_carto_add_sensor_reading(viam_carto *vc,
return VIAM_CARTO_SENSOR_READING_INVALID;
}

viam::carto_facade::CartoFacade *cf =
static_cast<viam::carto_facade::CartoFacade *>(vc->carto_obj);
try {
viam::carto_facade::CartoFacade *cf =
static_cast<viam::carto_facade::CartoFacade *>(vc->carto_obj);
cf->AddSensorReading(sr);
} catch (int err) {
return err;
Expand All @@ -855,6 +871,9 @@ extern int viam_carto_add_sensor_reading(viam_carto *vc,

extern int viam_carto_add_sensor_reading_destroy(
viam_carto_sensor_reading *sr) {
if (sr == nullptr) {
return VIAM_CARTO_SENSOR_READING_INVALID;
}
int return_code = VIAM_CARTO_SUCCESS;
int rc = BSTR_OK;
// destroy sensor_reading
Expand All @@ -876,14 +895,32 @@ extern int viam_carto_add_sensor_reading_destroy(

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);
cf->GetPosition(r);
if (vc == nullptr) {
return VIAM_CARTO_VC_INVALID;
}

if (r == nullptr) {
return VIAM_CARTO_GET_POSITION_RESPONSE_INVALID;
}

try {
viam::carto_facade::CartoFacade *cf =
static_cast<viam::carto_facade::CartoFacade *>((vc)->carto_obj);
cf->GetPosition(r);
} catch (int err) {
return err;
} catch (std::exception &e) {
LOG(ERROR) << e.what();
return VIAM_CARTO_UNKNOWN_ERROR;
}
return VIAM_CARTO_SUCCESS;
};

extern int viam_carto_get_position_response_destroy(
viam_carto_get_position_response *r) {
if (r == nullptr) {
return VIAM_CARTO_GET_POSITION_RESPONSE_INVALID;
}
int return_code = VIAM_CARTO_SUCCESS;
int rc = BSTR_OK;
rc = bdestroy(r->component_reference);
Expand All @@ -896,6 +933,17 @@ extern int viam_carto_get_position_response_destroy(

extern int viam_carto_get_point_cloud_map(
viam_carto *vc, viam_carto_get_point_cloud_map_response *r) {
try {
viam::carto_facade::CartoFacade *cf =
static_cast<viam::carto_facade::CartoFacade *>((vc)->carto_obj);
cf->GetPointCloudMap(r);
} catch (int err) {
return err;
} catch (std::exception &e) {
LOG(ERROR) << e.what();
return VIAM_CARTO_UNKNOWN_ERROR;
}

return VIAM_CARTO_SUCCESS;
};

Expand Down
Loading

0 comments on commit 14c8eaa

Please sign in to comment.