C++ (Cpp) urg_get_distance_intensity Examples

Example #1

File: get_distance_intensity.c Project: Fuzyou/urg

int main(int argc, char *argv[])
{
    enum {
        CAPTURE_TIMES = 10,
    };
    urg_t urg;
    int max_data_size;
    long *data = NULL;
    unsigned short *intensity = NULL;
    long time_stamp;
    int n;
    int i;

    if (open_urg_sensor(&urg, argc, argv) < 0) {
        return 1;
    }

    max_data_size = urg_max_data_size(&urg);
    data = (long *)malloc(max_data_size * sizeof(data[0]));
    if (!data) {
        perror("urg_max_index()");
        return 1;
    }
    intensity = malloc(max_data_size * sizeof(intensity[0]));
    if (!intensity) {
        perror("urg_max_index()");
        return 1;
    }

    // データ取得
    urg_start_measurement(&urg, URG_DISTANCE_INTENSITY, URG_SCAN_INFINITY, 0);
    for (i = 0; i < CAPTURE_TIMES; ++i) {
        n = urg_get_distance_intensity(&urg, data, intensity, &time_stamp);
        if (n <= 0) {
            printf("urg_get_distance_intensity: %s\n", urg_error(&urg));
            free(data);
            urg_close(&urg);
            return 1;
        }
        print_data(&urg, data, intensity, n, time_stamp);
    }

    // 切断
    free(intensity);
    free(data);
    urg_close(&urg);

#if defined(URG_MSC)
    getchar();
#endif
    return 0;
}

Example #2

File: lrf_scan_node_wrapper.cpp Project: RyodoTanaka/lrf_scan

bool LRF::GetLRFData(ros::Time& t, sensor_msgs::LaserScan &data)
{
  if(publish_intensity){
    t = ros::Time::now();
    if(urg_get_distance_intensity(&urg, &data.ranges[0], &data.intensities[0], &timestamp) <= 0){
      ROS_WARN("Disable to get LRF data.");
      for(int i=0; i<step; i++){
		data.ranges[i] = 0;
		data.intensities[i] = 0;
      } 
      return false;
    }
  }
  else{
    t = ros::Time::now();
    if(urg_get_distance(&urg, &data.ranges[0], &timestamp) <= 0){
      ROS_WARN("Disable to get LRF data.");
      for(int i=0; i<step; i++)
		data.ranges[i] = 0;
      return false;
    }
  }
  return true;
}