void TinkerforgeSensors::callbackEnumerate(const char *uid, const char *connected_uid,
char position, uint8_t hardware_version[3],
uint8_t firmware_version[3], uint16_t device_identifier,
uint8_t enumeration_type, void *user_data)
{
TinkerforgeSensors *tfs = (TinkerforgeSensors*) user_data;
std::string topic("");
if(enumeration_type == IPCON_ENUMERATION_TYPE_DISCONNECTED)
{
return;
}
// search for predefined topic
for (std::vector<SensorConf>::iterator it = tfs->sensor_conf.begin(); it != tfs->sensor_conf.end(); ++it)
{
if (it->uid.compare((std::string)uid) == 0)
{
topic = it->topic;
break;
}
}
// check if device is an imu
if(device_identifier == IMU_DEVICE_IDENTIFIER)
{
ROS_INFO_STREAM("found IMU with UID:" << uid);
// Create IMU device object
IMU *imu = new IMU();
imu_create(imu, uid, &(tfs->ipcon));
imu_set_convergence_speed(imu,tfs->imu_convergence_speed);
imu_leds_on(imu);
tfs->imu_init_time = ros::Time::now();
SensorDevice *imu_dev = new SensorDevice(imu, uid, topic, IMU_DEVICE_IDENTIFIER, SensorClass::IMU, 10);
tfs->sensors.push_back(imu_dev);
}
else if (device_identifier == IMU_V2_DEVICE_IDENTIFIER)
{
ROS_INFO_STREAM("found IMU_v2 with UID:" << uid);
// Create IMU_v2 device object
IMUV2 *imu_v2 = new IMUV2();
imu_v2_create(imu_v2, uid, &(tfs->ipcon));
imu_leds_on(imu_v2);
SensorDevice *imu_dev = new SensorDevice(imu_v2, uid, topic, IMU_V2_DEVICE_IDENTIFIER, SensorClass::IMU, 10);
tfs->sensors.push_back(imu_dev);
imu_dev = new SensorDevice(imu_v2, uid, std::string(""), IMU_V2_MAGNETIC_DEVICE_IDENTIFIER, SensorClass::MAGNETIC, 10);
tfs->sensors.push_back(imu_dev);
return;
}
else if (device_identifier == GPS_DEVICE_IDENTIFIER)
{
ROS_INFO_STREAM("found GPS with UID:" << uid);
// Create GPS device object
GPS *gps = new GPS();
gps_create(gps, uid, &(tfs->ipcon));
SensorDevice *gps_dev = new SensorDevice(gps, uid, topic, GPS_DEVICE_IDENTIFIER, SensorClass::GPS, 10);
tfs->sensors.push_back(gps_dev);
}
else if (device_identifier == DUAL_BUTTON_DEVICE_IDENTIFIER)
{
ROS_INFO_STREAM("found DualButton with UID:" << uid);
DualButton *db = new DualButton();
dual_button_create(db, uid, &(tfs->ipcon));
SensorDevice *db_dev = new SensorDevice(db, uid, topic, DUAL_BUTTON_DEVICE_IDENTIFIER, SensorClass::MISC, 10);
tfs->sensors.push_back(db_dev);
}
else if (device_identifier == TEMPERATURE_DEVICE_IDENTIFIER)
{
ROS_INFO_STREAM("found Temperature with UID:" << uid);
Temperature *temp = new Temperature();
// Create Temperature device object
temperature_create(temp, uid, &(tfs->ipcon));
SensorDevice *temp_dev = new SensorDevice(temp, uid, topic, TEMPERATURE_DEVICE_IDENTIFIER, SensorClass::TEMPERATURE, 10);
tfs->sensors.push_back(temp_dev);
}
else if (device_identifier == AMBIENT_LIGHT_DEVICE_IDENTIFIER)
{
ROS_INFO_STREAM("found Ambient Light with UID:" << uid);
// Create Ambient Light device object
AmbientLight *ambient_light = new AmbientLight();
ambient_light_create(ambient_light, uid, &(tfs->ipcon));
SensorDevice *ambient_light_dev = new SensorDevice(ambient_light, uid, topic, AMBIENT_LIGHT_DEVICE_IDENTIFIER, SensorClass::LIGHT, 10);
tfs->sensors.push_back(ambient_light_dev);
}
else if (device_identifier == AMBIENT_LIGHT_V2_DEVICE_IDENTIFIER)
{
ROS_INFO_STREAM("found Ambient Light v2 with UID:" << uid);
// Create Ambient Light device object
AmbientLightV2 *ambient_v2_light = new AmbientLightV2();
ambient_light_create(ambient_v2_light, uid, &(tfs->ipcon));
SensorDevice *ambient_light_v2_dev = new SensorDevice(ambient_v2_light, uid, topic, AMBIENT_LIGHT_V2_DEVICE_IDENTIFIER, SensorClass::LIGHT, 10);
tfs->sensors.push_back(ambient_light_v2_dev);
}
else if (device_identifier == DISTANCE_IR_DEVICE_IDENTIFIER)
{
ROS_INFO_STREAM("found Distance IR with UID:" << uid);
// Create Distance IR device object
DistanceIR *distance_ir = new DistanceIR();
distance_ir_create(distance_ir, uid, &(tfs->ipcon));
SensorDevice *distance_ir_dev = new SensorDevice(distance_ir, uid, topic, DISTANCE_IR_DEVICE_IDENTIFIER, SensorClass::RANGE, 10);
tfs->sensors.push_back(distance_ir_dev);
}
else if (device_identifier == DISTANCE_US_DEVICE_IDENTIFIER)
{
ROS_INFO_STREAM("found Distance US with UID:" << uid);
// Create Distance US device object
DistanceUS *distance_us = new DistanceUS();
distance_us_create(distance_us, uid, &(tfs->ipcon));
SensorDevice *distance_us_dev = new SensorDevice(distance_us, uid, topic, DISTANCE_US_DEVICE_IDENTIFIER, SensorClass::RANGE, 10);
tfs->sensors.push_back(distance_us_dev);
}
else if (device_identifier == MOTION_DETECTOR_DEVICE_IDENTIFIER)
{
ROS_INFO_STREAM("found Motion Detector with UID:" << uid);
// Create Motion Detector device object
MotionDetector * md = new MotionDetector();
motion_detector_create(md, uid, &(tfs->ipcon));
SensorDevice *md_dev = new SensorDevice(md, uid, topic, MOTION_DETECTOR_DEVICE_IDENTIFIER, SensorClass::MISC, 10);
tfs->sensors.push_back(md_dev);
}
}
status_t director_create(
size_t max_layers,
size_t max_bytes,
size_t bytes_per_video_frame,
size_t bytes_per_depth_frame,
size_t bytes_per_depth_pixel,
float depth_scale,
director_handle_t* p_handle)
{
int pthreadErr = 0;
status_t status = NO_ERROR;
director_t* p_director = NULL;
/* check inputs */
if (NULL == p_handle) {
return ERR_NULL_POINTER;
}
if (max_layers > DIRECTOR_MAX_LAYERS) {
return ERR_INVALID_ARGUMENT;
}
/* allocate / initialize structure */
p_director = malloc(sizeof(director_t));
if (NULL == p_director) {
return ERR_FAILED_ALLOC;
}
memset(p_director, 0, sizeof(director_t));
p_director->max_layers = max_layers;
p_director->max_bytes = max_bytes;
p_director->bytes_per_video_frame = bytes_per_video_frame;
p_director->bytes_per_depth_frame = bytes_per_depth_frame;
p_director->bytes_per_depth_pixel = bytes_per_depth_pixel;
p_director->depth_scale = depth_scale;
p_director->valid_frame_min_presence = 0.05;
p_director->valid_frame_min_motion = 0.01;
p_director->valid_frame_patience = 5;
p_director->invalid_frame_count = 0;
p_director->loop_min_frame_count = 10;
p_director->is_recording = FALSE;
/* create internal storage */
status = frame_store_create(
bytes_per_video_frame,
bytes_per_depth_frame,
sizeof(float), // depth cutoff storage
max_bytes,
&(p_director->frame_store));
if (NO_ERROR != status) {
director_release(p_director);
return status;
}
status = vector_create(32, sizeof(loop_t*), &(p_director->loops));
if (NO_ERROR != status) {
director_release(p_director);
return status;
}
p_director->playing_loops = (loop_t**)malloc(sizeof(loop_t*) * max_layers);
if (NULL == p_director->playing_loops) {
director_release(p_director);
return ERR_FAILED_ALLOC;
}
memset(p_director->playing_loops, 0, sizeof(loop_t*) * max_layers);
status = _loop_create(&(p_director->p_current_loop));
if (NO_ERROR != status) {
director_release(p_director);
return status;
}
/* create motion detector to trigger start of recording */
status = motion_detector_create(
bytes_per_depth_pixel,
bytes_per_depth_frame / bytes_per_depth_pixel,
TRUE,
&(p_director->motion_detector));
if (NO_ERROR != status) {
director_release(p_director);
return status;
}
/* create mutex for handling new loops */
pthreadErr = pthread_mutex_init(&(p_director->loops_mutex), NULL);
if (pthreadErr) {
director_release(p_director);
return ERR_FAILED_THREAD_CREATE;
}
srand(time(NULL));
*p_handle = p_director;
return NO_ERROR;
}
