예제 #1
0
void LaserTransform::checkConvergenceSpeed()
{
  if (is_imu_connected)
  {
    int16_t ang_x, ang_y, ang_z;
    imu_get_angular_velocity(&imu, &ang_x, &ang_y, &ang_z);
    // Realistische Drehraten: Siehe IPython Notebook,
    // Kapitel "Roll-/Pitch-/Yawrate"
    // http://nbviewer.ipython.org/github/balzer82/ICINCO-2014/blob/master/Extended-Kalman-Filter-CTRV-Attitude.ipynb
    if (ang_x > 5 || ang_x < -5 || ang_y > 5 || ang_y < -5 || ang_z > 5 || ang_z < -5)
    {
      // Fast turning: No Magnetic Field, just RotationRate
      imu_set_convergence_speed(&imu, 0);
    }
    else
      imu_set_convergence_speed(&imu, imu_convergence_speed);
  }
}
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);
  }
}
예제 #3
0
void LaserTransform::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)
{
  LaserTransform *lt = (LaserTransform*) user_data;

  if(enumeration_type == IPCON_ENUMERATION_TYPE_DISCONNECTED)
  {
    return;
  }

  // check if device is an imu
  if(device_identifier == IMU_DEVICE_IDENTIFIER)
  {
    if (lt->is_imu_v2_connected)
      return;
    ROS_INFO_STREAM("found IMU with UID:" << uid);
    // Create IMU device object
    imu_create(&(lt->imu), uid, &(lt->ipcon));
    imu_set_convergence_speed(&(lt->imu),lt->imu_convergence_speed);
    imu_leds_on(&(lt->imu));
    lt->imu_init_time = ros::Time::now();
    lt->is_imu_connected = true;
  }
  else if (device_identifier == IMU_V2_DEVICE_IDENTIFIER)
  {
    if (lt->is_imu_connected)
      return;
    ROS_INFO_STREAM("found IMU_v2 with UID:" << uid);
    // Create IMU_v2 device object
    imu_v2_create(&(lt->imu_v2), uid, &(lt->ipcon));
    imu_leds_on(&(lt->imu_v2));
    lt->is_imu_v2_connected = true;
  }
  else if (device_identifier == GPS_DEVICE_IDENTIFIER)
  {
    ROS_INFO_STREAM("found GPS with UID:" << uid);
    // Create GPS device object
    gps_create(&(lt->gps), uid, &(lt->ipcon));
    lt->is_gps_connected = true;
  }
  else if (device_identifier == INDUSTRIAL_DIGITAL_IN_4_DEVICE_IDENTIFIER)
  {
    ROS_INFO_STREAM("found IDI4 with UID:" << uid);
    // Create IndustrialDigitalIn4 device object
    industrial_digital_in_4_create(&(lt->idi4), uid, &(lt->ipcon));

    // Register callback for interrupts
    industrial_digital_in_4_register_callback(&(lt->idi4),
      INDUSTRIAL_DIGITAL_IN_4_CALLBACK_INTERRUPT,
      (void*)callbackIdi4,
      lt);

    // set debounce period
    industrial_digital_in_4_set_debounce_period(&(lt->idi4),10);

    // Enable interrupt on pin 0
    industrial_digital_in_4_set_interrupt(&(lt->idi4), 1 << 0);

    lt->is_idi4_connected = true;
  }
  else if (device_identifier == DUAL_BUTTON_DEVICE_IDENTIFIER)
  {
    ROS_INFO_STREAM("found DualButton with UID:" << uid);
    dual_button_create(&(lt->db), uid, &(lt->ipcon));

    // Register state changed callback to function cb_state_changed
    dual_button_register_callback(&(lt->db),
                                  DUAL_BUTTON_CALLBACK_STATE_CHANGED,
                                  (void *)callbackDb,
                                  lt);
  }
}