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));
102 if ( strncmp(
"SetInitialPoseMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
106 "message type for this interface type.", type);
122 memcpy(data, oi->data,
sizeof(LocalizationInterface_data_t));
147 data_size =
sizeof(SetInitialPoseMessage_data_t);
150 data = (SetInitialPoseMessage_data_t *)
data_ptr;
152 strncpy(data->frame, ini_frame, 32);
153 memcpy(data->rotation, ini_rotation,
sizeof(
double) * 4);
154 memcpy(data->translation, ini_translation,
sizeof(
double) * 3);
155 memcpy(data->covariance, ini_covariance,
sizeof(
double) * 36);
164 data_size =
sizeof(SetInitialPoseMessage_data_t);
167 data = (SetInitialPoseMessage_data_t *)
data_ptr;
189 data = (SetInitialPoseMessage_data_t *)
data_ptr;
225 strncpy(data->frame, new_frame,
sizeof(data->frame));
237 return data->rotation;
252 throw Exception(
"Index value %u out of bounds (0..4)", index);
254 return data->rotation[index];
276 memcpy(data->rotation, new_rotation,
sizeof(
double) * 4);
290 throw Exception(
"Index value %u out of bounds (0..4)", index);
292 data->rotation[index] = new_rotation;
303 return data->translation;
318 throw Exception(
"Index value %u out of bounds (0..3)", index);
320 return data->translation[index];
342 memcpy(data->translation, new_translation,
sizeof(
double) * 3);
356 throw Exception(
"Index value %u out of bounds (0..3)", index);
358 data->translation[index] = new_translation;
372 return data->covariance;
390 throw Exception(
"Index value %u out of bounds (0..36)", index);
392 return data->covariance[index];
417 memcpy(data->covariance, new_covariance,
sizeof(
double) * 36);
434 throw Exception(
"Index value %u out of bounds (0..36)", index);
436 data->covariance[index] = new_covariance;
LocalizationInterface Fawkes BlackBoard Interface.
void * data_ptr
Pointer to memory that contains local data.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
void set_rotation(unsigned int index, const double new_rotation)
Set rotation value at given index.
virtual const char * enum_tostring(const char *enumtype, int val) const
Convert arbitrary enum value to string.
void set_hash(unsigned char *ihash)
Set hash.
Fawkes library namespace.
Timestamp data, must be present and first entries for each interface data structs! This leans on time...
void set_map(const char *new_map)
Set map value.
size_t maxlenof_map() const
Get maximum length of map value.
size_t maxlenof_translation() const
Get maximum length of translation value.
Base class for all Fawkes BlackBoard interfaces.
virtual Message * create_message(const char *type) const
Create message based on type name.
message_data_ts_t * data_ts
data timestamp aliasing pointer
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
unsigned int data_size
Size of memory needed to hold all data.
double * covariance() const
Get covariance value.
void add_messageinfo(const char *name)
Add an entry to the message info list.
SetInitialPoseMessage()
Constructor.
bool data_changed
Indicator if data has changed.
double * translation() const
Get translation value.
const char * type() const
Get type of interface.
Base class for exceptions in Fawkes.
char * frame() const
Get frame value.
SetInitialPoseMessage Fawkes BlackBoard Interface Message.
virtual void copy_values(const Interface *other)
Copy values from other interface.
size_t maxlenof_frame() const
Get maximum length of frame value.
size_t maxlenof_rotation() const
Get maximum length of rotation value.
char * map() const
Get map value.
size_t maxlenof_covariance() const
Get maximum length of covariance value.
void set_frame(const char *new_frame)
Set frame value.
virtual Message * clone() const
Clone this message.
void add_fieldinfo(interface_fieldtype_t type, const char *name, size_t length, void *value, const char *enumtype=0, const interface_enum_map_t *enum_map=0)
Add an entry to the info list.
double * rotation() const
Get rotation value.
~SetInitialPoseMessage()
Destructor.
const char * type() const
Get message type.
void set_covariance(unsigned int index, const double new_covariance)
Set covariance value at given index.
virtual bool message_valid(const Message *message) const
Check if message is valid and can be enqueued.