int open_urg_sensor(urg_t *urg, int argc, char *argv[])
{
#if defined(URG_WINDOWS_OS)
const char *device = "COM4";
#elif defined(URG_LINUX_OS)
const char *device = "/dev/ttyACM0";
//const char *device = "/dev/ttyUSB0";
#else
const char *device = "/dev/tty.usbmodemfd333131";
#endif
urg_connection_type_t connection_type = URG_SERIAL;
long baudrate_or_port = 115200;
//const char *ip_address = "localhost";
const char *ip_address = "192.168.0.10";
int i;
// \~japanese 接続タイプの切替え
for (i = 1; i < argc; ++i) {
if (!strcmp(argv[i], "-e")) {
connection_type = URG_ETHERNET;
baudrate_or_port = 10940;
device = ip_address;
}
}
// \~japanese 接続
if (urg_open(urg, connection_type, device, baudrate_or_port) < 0) {
printf("urg_open: %s, %ld: %s\n",
device, baudrate_or_port, urg_error(urg));
return -1;
}
return 0;
}
void checkAndConnect(Hok_t *hok) {
if (!hok->isWorking) {
printf("%sHokuyo not connected, trying to connect to %s\n", PREFIX, hok->path);
int error = urg_connect(hok->urg, hok->path, 115200);
if (error < 0) {
printf("%sCan't connect to hokuyo : %s\n", PREFIX, urg_error(hok->urg));
hok->isWorking = 0;
} else {
hok->imin = urg_rad2index(hok->urg, hok->cone_min);
hok->imax = urg_rad2index(hok->urg, hok->cone_max);
urg_setCaptureTimes(hok->urg, UrgInfinityTimes);
error = urg_requestData(hok->urg, URG_MD, hok->imin, hok->imax);
if (error < 0) {
printf("%sCan't connect to hokuyo\n", PREFIX);
hok->isWorking = 0;
} else {
printf("%sHokuyo connected\n", PREFIX);
hok->isWorking = 1;
printf("%sRequesting data on indexes %d to %d from %s OK\n", PREFIX, hok->imin, hok->imax, hok->path);
hok->nb_data = urg_dataMax(hok->urg);
double *angles = malloc(hok->nb_data * sizeof(double));
int i;
for (i=0; i<hok->nb_data; i++) {
angles[i] = modTwoPi(urg_index2rad(hok->urg, i) + hok->orientation);
}
hok->fm = initFastmath(hok->nb_data, angles);
free(angles);
printf("%sCalculted sin/cos data for %s\n", PREFIX, hok->path);
}
}
}
}
/*!
\~japanese
\brief PC のタイムスタンプに補正するための値を返す
*/
static long print_time_stamp(urg_t *urg, long time_stamp_offset)
{
long sensor_time_stamp;
long pc_time_stamp;
long before_pc_time_stamp;
long after_pc_time_stamp;
long delay;
urg_start_time_stamp_mode(urg);
before_pc_time_stamp = pc_msec_time();
sensor_time_stamp = urg_time_stamp(urg);
after_pc_time_stamp = pc_msec_time();
delay = (after_pc_time_stamp - before_pc_time_stamp) / 2;
if (sensor_time_stamp < 0) {
printf("urg_time_stamp: %s\n", urg_error(urg));
return -1;
}
sensor_time_stamp -= time_stamp_offset;
pc_time_stamp = pc_msec_time();
urg_stop_time_stamp_mode(urg);
printf("%ld,\t%ld\n", pc_time_stamp, sensor_time_stamp);
return sensor_time_stamp - (pc_time_stamp - delay);
}
void urg_exit(urg_t *urg, const char *message){
printf("%s: %s\n", message, urg_error(urg));
urg_disconnect(urg);
#ifdef MSC
getchar();
#endif
}
int main(int argc, char *argv[])
{
urg_t urg;
long *data;
long max_distance;
long min_distance;
long time_stamp;
int i;
int n;
if (open_urg_sensor(&urg, argc, argv) < 0) {
return 1;
}
data = (long *)malloc(urg_max_data_size(&urg) * sizeof(data[0]));
if (!data) {
perror("urg_max_index()");
return 1;
}
// \~japanese データ取得
urg_start_measurement(&urg, URG_DISTANCE, 1, 0);
n = urg_get_distance(&urg, data, &time_stamp);
if (n < 0) {
printf("urg_get_distance: %s\n", urg_error(&urg));
urg_close(&urg);
return 1;
}
// \~japanese X-Y 座標系の値を出力
urg_distance_min_max(&urg, &min_distance, &max_distance);
for (i = 0; i < n; ++i) {
long distance = data[i];
double radian;
long x;
long y;
if ((distance < min_distance) || (distance > max_distance)) {
continue;
}
radian = urg_index2rad(&urg, i);
x = (long)(distance * cos(radian));
y = (long)(distance * sin(radian));
printf("%ld, %ld\n", x, y);
}
printf("\n");
// \~japanese 切断
free(data);
urg_close(&urg);
#if defined(URG_MSC)
getchar();
#endif
return 0;
}
int main(int argc, char *argv[])
{
enum {
CAPTURE_TIMES = 0,
};
urg_t urg;
long *data = NULL;
long time_stamp;
int n;
int i;
if (open_urg_sensor(&urg, argc, argv) < 0) {
return 1;
}
data = (long *)malloc(urg_max_data_size(&urg) * sizeof(data[0]));
if (!data) {
perror("urg_max_index()");
return 1;
}
// \~japanese データ取得
#if 0
// \~japanese データの取得範囲を変更する場合
urg_set_scanning_parameter(&urg,
urg_deg2step(&urg, -90),
urg_deg2step(&urg, +90), 0);
#endif
int k;
for (k = 0; k < 3; k++) {
urg_start_measurement(&urg, URG_DISTANCE, CAPTURE_TIMES, 0);
for (i = 0; i < 25; ++i) {
n = urg_get_distance(&urg, data, &time_stamp);
if (n <= 0) {
printf("urg_get_distance: %s\n", urg_error(&urg));
free(data);
urg_close(&urg);
return 1;
}
print_data(&urg, data, n, time_stamp);
}
urg_stop_measurement(&urg);
sleep(10);
}
// \~japanese 切断
free(data);
urg_close(&urg);
#if defined(URG_MSC)
getchar();
#endif
return 0;
}
int scan_center(double walling) {
long time_stamp; // Timestamp of the received data frame (not used)
long data[700]; // Array to hold received distance data, index corresponds to angle. Max size is 681
int n = urg_get_distance(&urg, data, &time_stamp); // Populates data[] with n entries, returns -1 for error
if (n <= 0) {
printf("urg_get_distance: %s\n", urg_error(&urg));
return -1;
}
int check = processData(&urg, data, n, walling); // Process the data array, (convert to xy, find walls, obstacles, etc.)
return check;
}
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;
}
int main(int argc, char *argv[]){
enum {
CAPTURE_TIMES = 1, //modificat_numarul_de_afisari_ale_citirii
};
urg_t urg;
long *data = NULL;
long time_stamp;
int n;
int i;
if (open_urg_sensor(&urg, argc, argv) < 0) {
return 1;
}
data = (long *)malloc(urg_max_data_size(&urg) * sizeof(data[0]));
if (!data) {
perror("1urg_max_index()");
return 1;
}
// \~japanese データ取得
#if 0
// \~japanese データの取得範囲を変更する場合
urg_set_scanning_parameter(&urg,
urg_deg2step(&urg, -90),
urg_deg2step(&urg, +90), 0);
#endif
urg_start_measurement(&urg, URG_DISTANCE, URG_SCAN_INFINITY, 0);
for (i = 0; i < CAPTURE_TIMES; ++i) {
n = urg_get_distance(&urg, data, &time_stamp);
if (n <= 0) {
printf("urg_get_distance: %s\n", urg_error(&urg));
free(data);
urg_close(&urg);
return 1;
}
print_data(&urg, data, n, time_stamp);
}
// \~japanese 切断
free(data);
urg_close(&urg);
#if defined(URG_MSC)
getchar();
#endif
return 0;
}
//! main
int main(int argc, char *argv[])
{
#ifdef WINDOWS_OS
const char device[] = "COM3"; /* For Windows */
#else
const char device[] = "/dev/ttyACM0"; /* For Linux */
#endif
urg_t urg;
urg_parameter_t parameters;
int ret;
/* Connection */
urg_initialize(&urg);
ret = urg_connect(&urg, device, 115200);
if (ret < 0) {
urg_exit(&urg, "urg_connect()");
}
/* Get sensor parameter */
ret = urg_parameters(&urg, ¶meters);
printf("urg_getParameters: %s\n", urg_error(&urg));
if (ret < 0) {
urg_disconnect(&urg);
exit(1);
}
/* Display */
printf("distance_min: %ld\n", parameters.distance_min_);
printf("distance_max: %ld\n", parameters.distance_max_);
printf("area_total: %d\n", parameters.area_total_);
printf("area_min: %d\n", parameters.area_min_);
printf("area_max: %d\n", parameters.area_max_);
printf("area_front: %d\n", parameters.area_front_);
printf("scan_rpm: %d\n", parameters.scan_rpm_);
printf("\n");
/* Display information from URG structure (same resource as above) */
printf("urg_getDistanceMax(): %ld\n", urg_maxDistance(&urg));
printf("urg_getDistanceMin(): %ld\n", urg_minDistance(&urg));
printf("urg_getScanMsec(): %d\n", urg_scanMsec(&urg));
printf("urg_getDataMax(): %d\n", urg_dataMax(&urg));
#ifdef MSC
getchar();
#endif
return 0;
}
int main(int argc, char *argv[])
{
enum {
CAPTURE_TIMES = 10,
};
urg_t urg;
long *data = NULL;
long time_stamp;
int n;
int i;
if (open_urg_sensor(&urg, argc, argv) < 0) {
return 1;
}
data = (long *)malloc(urg_max_data_size(&urg) * 3 * sizeof(data[0]));
if (!data) {
perror("urg_max_index()");
return 1;
}
// データ取得
urg_start_measurement(&urg, URG_MULTIECHO, URG_SCAN_INFINITY, 0);
for (i = 0; i < CAPTURE_TIMES; ++i) {
n = urg_get_multiecho(&urg, data, &time_stamp);
if (n <= 0) {
printf("urg_get_multiecho: %s\n", urg_error(&urg));
free(data);
urg_close(&urg);
return 1;
}
print_data(&urg, data, n, time_stamp);
}
// 切断
free(data);
urg_close(&urg);
#if defined(URG_MSC)
getchar();
#endif
return 0;
}
int main(int argc, char *argv[])
{
enum {
LinesMax = 5,
};
//const char device[] = "COM3"; /* Example for Windows */
const char device[] = "/dev/ttyACM0"; /* Example for Linux */
char buffer[LinesMax][UrgLineWidth];
char *lines[LinesMax];
int ret;
int i;
/* Connection */
urg_t urg;
ret = urg_connect(&urg, device, 115200);
if (ret < 0) {
urg_exit(&urg, "urg_connect()");
}
/* Get version information */
for (i = 0; i < LinesMax; ++i) {
lines[i] = buffer[i];
}
ret = urg_versionLines(&urg, lines, LinesMax);
printf("urg_getVersion: %s\n", urg_error(&urg));
if (ret < 0) {
urg_disconnect(&urg);
exit(1);
}
/* Display */
for (i = 0; i < LinesMax; ++i) {
printf("%s\n", lines[i]);
}
/* Disconnect */
urg_disconnect(&urg);
return 0;
}
/*! main */
int main(int argc, char *argv[])
{
enum {
Times = 10,
Urgs = 2,
};
urg_t urg[Urgs];
long *data[Urgs];
int data_max[Urgs];
int timestamp;
int ret;
int n;
int i;
int k;
#ifdef WINDOWS_OS
const char *devices[] = { "COM3", "COM4" }; /* For Windows */
#else
const char *devices[] = { "/dev/ttyACM0", "/dev/ttyACM1" }; /* For Linux */
#endif
/* Connection */
for (i = 0; i < Urgs; ++i) {
urg_initialize(&urg[i]);
ret = urg_connect(&urg[i], devices[i], 115200);
if (ret < 0) {
urg_exit(&urg[i], "urg_connect()");
}
/* To clear existing MD command*/
urg_laserOff(&urg[i]);
/* It will become easy if some frames are skipped. */
/* If specified skip is 2, then transferred data becomes half. */
/* urg_setSkipLines(&urg[i], 2); */
/* Reserve for receive buffer */
data_max[i] = urg_dataMax(&urg[i]);
data[i] = (long*)malloc(sizeof(long) * data_max[i]);
if (data[i] == NULL) {
perror("data buffer");
exit(1);
}
}
/* Request for MD data */
for (i = 0; i < Urgs; ++i) {
urg_setCaptureTimes(&urg[i], Times);
/* Request for data */
ret = urg_requestData(&urg[i], URG_MD, URG_FIRST, URG_LAST);
if (ret < 0) {
urg_exit(&urg[i], "urg_requestData()");
}
}
for (k = 0; k < Times; ++k) {
for (i = 0; i < Urgs; ++i) {
/* Ends when data reception is completed */
int remain_times = urg_remainCaptureTimes(&urg[i]);
printf(" %d: ", i);
printf("(%03d/%03d): ", remain_times, Times);
if (remain_times <= 0) {
printf("\n");
continue;
}
/* Reception */
n = urg_receiveData(&urg[i], data[i], data_max[i]);
if (n < 0) {
/* Continue processing, because there is chances of receiving the
data next time. */
printf("%s: %s\n", "urg_receiveData()", urg_error(urg));
} else {
/* Display */
timestamp = urg_recentTimestamp(&urg[i]);
printf("timestamp: %d, ", timestamp);
#if 0
{
int j;
for (j = 0; j < n; ++j) {
/* Neglect if distance data is less than urg_minDistance() */
printf("%d:%ld, ", j, data[i][j]);
}
printf("\n");
}
#endif
printf("\n");
}
}
}
/* Disconnect */
for (i = 0; i < Urgs; ++i) {
urg_disconnect(&urg[i]);
free(data[i]);
}
#ifdef MSC
getchar();
#endif
return 0;
}
void Hokuyo::outError(const char *str) {
cvgException e = cvgException(cvgString("[Hokuyo] ") + str + ". Error: " + urg_error(&urg));
close();
throw e;
}
void error_func(urg_t *urg, const char *message){
urg_close(urg);
fprintf(stderr, "%s :: %s\n", message, urg_error(urg)); //print le message d'erreur perso et celui issu de l'hokuyo
}
void urg_exit(urg_t *urg, const char *message){
printf("%s: %s\n", message, urg_error(urg));
urg_disconnect(urg);
}
int main(int argc, char *argv[])
{
enum {
CAPTURE_TIMES = 1,
};
urg_t urg;
urg_connection_type_t connection_type = URG_SERIAL;
long *data = NULL;
long time_stamp;
int n;
int i;
#if defined(URG_WINDOWS_OS)
const char *device = "COM3";
#elif defined(URG_LINUX_OS)
const char *device = "/dev/ttyACM0";
#else
#endif
long baudrate_or_port = 115200;
const char *ip_address = "192.168.0.10";
// 接続タイプの切替え
for (i = 1; i < argc; ++i) {
if (!strcmp(argv[i], "-e")) {
connection_type = URG_ETHERNET;
baudrate_or_port = 10940;
device = ip_address;
}
}
for (i = 1; i < argc; ++i) {
if (!strcmp(argv[i], "-e")) {
connection_type = URG_ETHERNET;
}
}
// \~japanese 接続
if (urg_open(&urg, connection_type, device, baudrate_or_port) < 0) {
printf("urg_open: %s\n", urg_error(&urg));
return 1;
}
data = (long *)malloc(urg_max_data_size(&urg) * 3 * sizeof(data[0]));
if (!data) {
perror("urg_max_index()");
return 1;
}
// \~japanese データ取得
urg_start_measurement(&urg, URG_MULTIECHO, CAPTURE_TIMES, 0);
for (i = 0; i < CAPTURE_TIMES; ++i) {
n = urg_get_distance(&urg, data, &time_stamp);
if (n <= 0) {
printf("urg_distance: %s\n", urg_error(&urg));
free(data);
urg_close(&urg);
return 1;
}
print_data(&urg, data, n, time_stamp);
}
// \~japanese 切断
free(data);
urg_close(&urg);
#if defined(URG_MSC)
getchar();
#endif
return 0;
}
