24 #include <interfaces/LocalizationInterface.h>
26 #include <core/exceptions/software.h>
48 LocalizationInterface::LocalizationInterface() : Interface()
50 data_size =
sizeof(LocalizationInterface_data_t);
52 data = (LocalizationInterface_data_t *)
data_ptr;
57 unsigned char tmp_hash[] = {0x7f, 0x9, 0xec, 0xd1, 00, 0x3f, 0x3, 0xb7, 0x95, 0xce, 0xe, 0x1d, 0x6f, 0x48, 0x6c, 0xad};
62 LocalizationInterface::~LocalizationInterface()
94 strncpy(data->map, new_map,
sizeof(data->map)-1);
95 data->map[
sizeof(data->map)-1] = 0;
103 if ( strncmp(
"SetInitialPoseMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
107 "message type for this interface type.",
type);
118 const LocalizationInterface *oi =
dynamic_cast<const LocalizationInterface *
>(other);
120 throw TypeMismatchException(
"Can only copy values from interface of same type (%s vs. %s)",
121 type(), other->type());
123 memcpy(data, oi->data,
sizeof(LocalizationInterface_data_t));
129 throw UnknownTypeException(
"Unknown enum type %s", enumtype);
148 data_size =
sizeof(SetInitialPoseMessage_data_t);
151 data = (SetInitialPoseMessage_data_t *)
data_ptr;
153 strncpy(data->frame, ini_frame, 32-1);
154 data->frame[32-1] = 0;
155 memcpy(data->rotation, ini_rotation,
sizeof(
double) * 4);
156 memcpy(data->translation, ini_translation,
sizeof(
double) * 3);
157 memcpy(data->covariance, ini_covariance,
sizeof(
double) * 36);
166 data_size =
sizeof(SetInitialPoseMessage_data_t);
169 data = (SetInitialPoseMessage_data_t *)
data_ptr;
191 data = (SetInitialPoseMessage_data_t *)
data_ptr;
227 strncpy(data->frame, new_frame,
sizeof(data->frame)-1);
228 data->frame[
sizeof(data->frame)-1] = 0;
240 return data->rotation;
255 throw Exception(
"Index value %u out of bounds (0..3)", index);
257 return data->rotation[index];
279 memcpy(data->rotation, new_rotation,
sizeof(
double) * 4);
293 throw Exception(
"Index value %u out of bounds (0..3)", index);
295 data->rotation[index] = new_rotation;
306 return data->translation;
321 throw Exception(
"Index value %u out of bounds (0..2)", index);
323 return data->translation[index];
345 memcpy(data->translation, new_translation,
sizeof(
double) * 3);
359 throw Exception(
"Index value %u out of bounds (0..2)", index);
361 data->translation[index] = new_translation;
375 return data->covariance;
393 throw Exception(
"Index value %u out of bounds (0..35)", index);
395 return data->covariance[index];
420 memcpy(data->covariance, new_covariance,
sizeof(
double) * 36);
437 throw Exception(
"Index value %u out of bounds (0..35)", index);
439 data->covariance[index] = new_covariance;