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; }