urg_sensor.c
Go to the documentation of this file.
1 
11 #include "urg_c/urg_sensor.h"
12 #include "urg_c/urg_errno.h"
13 #include <stddef.h>
14 #include <string.h>
15 #include <stdio.h>
16 #include <stdlib.h>
17 
18 #if defined(URG_MSC)
19 #define snprintf _snprintf
20 #endif
21 
22 
23 enum {
24  URG_FALSE = 0,
25  URG_TRUE = 1,
26 
27  BUFFER_SIZE = 64 + 2 + 6,
28 
29  EXPECTED_END = -1,
30 
31  RECEIVE_DATA_TIMEOUT,
32  RECEIVE_DATA_COMPLETE,
34  PP_RESPONSE_LINES = 10,
35  VV_RESPONSE_LINES = 7,
36  II_RESPONSE_LINES = 9,
37 
38  MAX_TIMEOUT = 140,
39 };
40 
41 
42 static const char NOT_CONNECTED_MESSAGE[] = "not connected.";
43 static const char RECEIVE_ERROR_MESSAGE[] = "receive error.";
44 
45 
47 static char scip_checksum(const char buffer[], int size)
48 {
49  unsigned char sum = 0x00;
50  int i;
51 
52  for (i = 0; i < size; ++i) {
53  sum += buffer[i];
54  }
55 
56  // 計算の意味は SCIP 仕様書を参照のこと return (sum & 0x3f) + 0x30; } static int set_errno_and_return(urg_t *urg, int urg_errno) { urg->last_errno = urg_errno; return urg_errno; } // 受信した応答の行数を返す static int scip_response(urg_t *urg, const char* command, const int expected_ret[], int timeout, char *receive_buffer, int receive_buffer_max_size) { char *p = receive_buffer; char buffer[BUFFER_SIZE]; int filled_size = 0; int line_number = 0; int ret = URG_UNKNOWN_ERROR; int write_size = (int)strlen(command); int n = connection_write(&urg->connection, command, write_size); if (n != write_size) { return set_errno_and_return(urg, URG_SEND_ERROR); } if (p) { *p = '\0'; } do { n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, timeout); if (n < 0) { return set_errno_and_return(urg, URG_NO_RESPONSE); } else if (p && (line_number > 0) && (n < (receive_buffer_max_size - filled_size))) { // エコーバックは完全一致のチェックを行うため、格納しない memcpy(p, buffer, n); p += n; *p++ = '\0'; filled_size += n; } if (line_number == 0) { // エコーバック文字列が、一致するかを確認する if (strncmp(buffer, command, write_size - 1)) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } else if (n > 0) { // エコーバック以外の行のチェックサムを評価する char checksum = buffer[n - 1]; if ((checksum != scip_checksum(buffer, n - 1)) && (checksum != scip_checksum(buffer, n - 2))) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } } // ステータス応答を評価して、戻り値を決定する if (line_number == 1) { if (n == 1) { // SCIP 1.1 応答の場合は、正常応答とみなす ret = 0; } else if (n != 3) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { int i; int actual_ret = strtol(buffer, NULL, 10); for (i = 0; expected_ret[i] != EXPECTED_END; ++i) { if (expected_ret[i] == actual_ret) { ret = 0; break; } } } } ++line_number; } while (n > 0); return (ret < 0) ? ret : (line_number - 1); } static void ignore_receive_data(urg_t *urg, int timeout) { char buffer[BUFFER_SIZE]; int n; if (urg->is_sending == URG_FALSE) { return; } do { n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, timeout); } while (n >= 0); urg->is_sending = URG_FALSE; } static void ignore_receive_data_with_qt(urg_t *urg, int timeout) { if ((urg->is_sending == URG_FALSE) && (urg->is_laser_on == URG_FALSE)) { return; } connection_write(&urg->connection, "QT\n", 3); urg->is_sending = URG_TRUE; urg->is_laser_on = URG_FALSE; ignore_receive_data(urg, timeout); } static int change_sensor_baudrate(urg_t *urg, long current_baudrate, long next_baudrate) { enum { SS_COMMAND_SIZE = 10 }; char buffer[SS_COMMAND_SIZE]; int ss_expected[] = { 0, 3, 4, EXPECTED_END }; int ret; if (current_baudrate == next_baudrate) { // 現在のボーレートと設定するボーレートが一緒ならば、戻る return set_errno_and_return(urg, URG_NO_ERROR); } // "SS" コマンドでボーレートを変更する snprintf(buffer, SS_COMMAND_SIZE, "SS%06ld\n", next_baudrate); ret = scip_response(urg, buffer, ss_expected, urg->timeout, NULL, 0); // 0F 応答のときは Ethernet 用のセンサとみなし、正常応答を返す if (ret == -15) { return set_errno_and_return(urg, URG_NO_ERROR); } if (ret <= 0) { return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // 正常応答ならば、ホスト側のボーレートを変更する ret = connection_set_baudrate(&urg->connection, next_baudrate); // センサ側の設定反映を待つために少しだけ待機する ignore_receive_data(urg, MAX_TIMEOUT); return set_errno_and_return(urg, ret); } // ボーレートを変更しながら接続する static int connect_urg_device(urg_t *urg, long baudrate) { long try_baudrate[] = { 19200, 38400, 115200 }; int try_times = sizeof(try_baudrate) / sizeof(try_baudrate[0]); int i; // 指示されたボーレートから接続する for (i = 0; i < try_times; ++i) { if (try_baudrate[i] == baudrate) { try_baudrate[i] = try_baudrate[0]; try_baudrate[0] = baudrate; break; } } for (i = 0; i < try_times; ++i) { enum { RECEIVE_BUFFER_SIZE = 4 }; int qt_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE + 1]; int ret; connection_set_baudrate(&urg->connection, try_baudrate[i]); // QT を送信し、応答が返されるかでボーレートが一致しているかを確認する ret = scip_response(urg, "QT\n", qt_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); if (ret > 0) { if (!strcmp(receive_buffer, "E")) { int scip20_expected[] = { 0, EXPECTED_END }; // QT 応答の最後の改行を読み飛ばす ignore_receive_data(urg, MAX_TIMEOUT); // "E" が返された場合は、SCIP 1.1 とみなし "SCIP2.0" を送信する ret = scip_response(urg, "SCIP2.0\n", scip20_expected, MAX_TIMEOUT, NULL, 0); // SCIP2.0 応答の最後の改行を読み飛ばす ignore_receive_data(urg, MAX_TIMEOUT); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } else if (!strcmp(receive_buffer, "0Ee")) { int tm2_expected[] = { 0, EXPECTED_END }; // "0Ee" が返された場合は、TM モードとみなし "TM2" を送信する scip_response(urg, "TM2\n", tm2_expected, MAX_TIMEOUT, NULL, 0); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } if (ret <= 0) { if (ret == URG_INVALID_RESPONSE) { // 異常なエコーバックのときは、距離データ受信中とみなして // データを読み飛ばす ignore_receive_data_with_qt(urg, MAX_TIMEOUT); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } else { // 応答がないときは、ボーレートを変更して、再度接続を行う ignore_receive_data_with_qt(urg, MAX_TIMEOUT); continue; } } else if (!strcmp("00P", receive_buffer)) { // センサとホストのボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } return set_errno_and_return(urg, URG_NOT_DETECT_BAUDRATE_ERROR); } // PP コマンドの応答を urg_t に格納する static int receive_parameter(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * 9, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; int pp_expected[] = { 0, EXPECTED_END }; unsigned short received_bits = 0x0000; char *p; int i; int ret = scip_response(urg, "PP\n", pp_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); if (ret < 0) { return ret; } else if (ret < PP_RESPONSE_LINES) { ignore_receive_data_with_qt(urg, MAX_TIMEOUT); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } p = receive_buffer; for (i = 0; i < (ret - 1); ++i) { if (!strncmp(p, "DMIN:", 5)) { urg->min_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0001; } else if (!strncmp(p, "DMAX:", 5)) { urg->max_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0002; } else if (!strncmp(p, "ARES:", 5)) { urg->area_resolution = strtol(p + 5, NULL, 10); received_bits |= 0x0004; } else if (!strncmp(p, "AMIN:", 5)) { urg->first_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0008; } else if (!strncmp(p, "AMAX:", 5)) { urg->last_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0010; } else if (!strncmp(p, "AFRT:", 5)) { urg->front_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0020; } else if (!strncmp(p, "SCAN:", 5)) { int rpm = strtol(p + 5, NULL, 10); // タイムアウト時間は、計測周期の 16 倍程度の値にする urg->scan_usec = 1000 * 1000 * 60 / rpm; urg->timeout = urg->scan_usec >> (10 - 4); received_bits |= 0x0040; } p += strlen(p) + 1; } // 全てのパラメータを受信したか確認 if (received_bits != 0x007f) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } urg_set_scanning_parameter(urg, urg->first_data_index - urg->front_data_index, urg->last_data_index - urg->front_data_index, 1); return set_errno_and_return(urg, URG_NO_ERROR); } //! SCIP 文字列のデコード long urg_scip_decode(const char data[], int size) { const char* p = data; const char* last_p = p + size; int value = 0; while (p < last_p) { value <<= 6; value &= ~0x3f; value |= *p++ - 0x30; } return value; } static int parse_parameter(const char *parameter, int size) { char buffer[5]; memcpy(buffer, parameter, size); buffer[size] = '\0'; return strtol(buffer, NULL, 10); } static urg_measurement_type_t parse_distance_parameter(urg_t *urg, const char echoback[]) { urg_measurement_type_t ret_type = URG_UNKNOWN; urg->received_range_data_byte = URG_COMMUNICATION_3_BYTE; if (echoback[1] == 'S') { urg->received_range_data_byte = URG_COMMUNICATION_2_BYTE; ret_type = URG_DISTANCE; } else if (echoback[1] == 'D') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO; } } else if (echoback[1] == 'E') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE_INTENSITY; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO_INTENSITY; } } else { return URG_UNKNOWN; } // パラメータの格納 urg->received_first_index = parse_parameter(&echoback[2], 4); urg->received_last_index = parse_parameter(&echoback[6], 4); urg->received_skip_step = parse_parameter(&echoback[10], 2); return ret_type; } static urg_measurement_type_t parse_distance_echoback(urg_t *urg, const char echoback[]) { size_t line_length; urg_measurement_type_t ret_type = URG_UNKNOWN; if (!strcmp("QT", echoback)) { return URG_STOP; } line_length = strlen(echoback); if ((line_length == 12) && ((echoback[0] == 'G') || (echoback[0] == 'H'))) { ret_type = parse_distance_parameter(urg, echoback); } else if ((line_length == 15) && ((echoback[0] == 'M') || (echoback[0] == 'N'))) { ret_type = parse_distance_parameter(urg, echoback); } return ret_type; } static int receive_length_data(urg_t *urg, long length[], unsigned short intensity[], urg_measurement_type_t type, char buffer[]) { int n; int step_filled = 0; int line_filled = 0; int multiecho_index = 0; int each_size = (urg->received_range_data_byte == URG_COMMUNICATION_2_BYTE) ? 2 : 3; int data_size = each_size; int is_intensity = URG_FALSE; int is_multiecho = URG_FALSE; int multiecho_max_size = 1; if ((type == URG_DISTANCE_INTENSITY) || (type == URG_MULTIECHO_INTENSITY)) { data_size *= 2; is_intensity = URG_TRUE; } if ((type == URG_MULTIECHO) || (type == URG_MULTIECHO_INTENSITY)) { is_multiecho = URG_TRUE; multiecho_max_size = URG_MAX_ECHO; } do { char *p = buffer; char *last_p; n = connection_readline(&urg->connection, &buffer[line_filled], BUFFER_SIZE - line_filled, urg->timeout); if (n > 0) { // チェックサムの評価 if (buffer[line_filled + n - 1] != scip_checksum(&buffer[line_filled], n - 1)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } } if (n > 0) { line_filled += n - 1; } last_p = p + line_filled; while ((last_p - p) >= data_size) { int index; if (*p == '&') { // 先頭文字が '&' だったときは、マルチエコーのデータとみなす if ((last_p - (p + 1)) < data_size) { // '&' を除いて、data_size 分データが無ければ抜ける break; } --step_filled; ++multiecho_index; ++p; --line_filled; } else { // 次のデータ multiecho_index = 0; } index = (step_filled * multiecho_max_size) + multiecho_index; if (step_filled > (urg->received_last_index - urg->received_first_index)) { // データが多過ぎる場合は、残りのデータを無視して戻る ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (is_multiecho && (multiecho_index == 0)) { // マルチエコーのデータ格納先をダミーデータで埋める int i; if (length) { for (i = 1; i < multiecho_max_size; ++i) { length[index + i] = 0; } } if (intensity) { for (i = 1; i < multiecho_max_size; ++i) { intensity[index + i] = 0; } } } // 距離データの格納 if (length) { length[index] = urg_scip_decode(p, 3); } p += 3; // 強度データの格納 if (is_intensity) { if (intensity) { intensity[index] = (unsigned short)urg_scip_decode(p, 3); } p += 3; } ++step_filled; line_filled -= data_size; } // 次に処理する文字を退避 memmove(buffer, p, line_filled); } while (n > 0); return step_filled; } //! 距離データの取得 static int receive_data(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { urg_measurement_type_t type; char buffer[BUFFER_SIZE]; int ret = 0; int n; int extended_timeout = urg->timeout + 2 * (urg->scan_usec * (urg->scanning_skip_scan) / 1000); // エコーバックの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, extended_timeout); if (n <= 0) { return set_errno_and_return(urg, URG_NO_RESPONSE); } // エコーバックの解析 type = parse_distance_echoback(urg, buffer); // 応答の取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 3) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } if (buffer[n - 1] != scip_checksum(buffer, n - 1)) { // チェックサムの評価 ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } if (type == URG_STOP) { // QT 応答の場合には、最後の改行を読み捨て、正常応答として処理する n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n == 0) { return 0; } else { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } if (urg->specified_scan_times != 1) { if (!strncmp(buffer, "00", 2)) { // "00" 応答の場合は、エコーバック応答とみなし、 // 最後の空行を読み捨て、次からのデータを返す n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 0) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } } } if (((urg->specified_scan_times == 1) && (strncmp(buffer, "00", 2))) || ((urg->specified_scan_times != 1) && (strncmp(buffer, "99", 2)))) { if (urg->error_handler) { type = urg->error_handler(buffer, urg); } if (type == URG_UNKNOWN) { // Gx, Hx のときは 00P が返されたときがデータ // Mx, Nx のときは 99b が返されたときがデータ ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } // タイムスタンプの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n > 0) { if (time_stamp) { *time_stamp = urg_scip_decode(buffer, 4); } if (system_time_stamp) { urg_get_walltime(system_time_stamp); } } // データの取得 switch (type) { case URG_DISTANCE: case URG_MULTIECHO: ret = receive_length_data(urg, data, NULL, type, buffer); break; case URG_DISTANCE_INTENSITY: case URG_MULTIECHO_INTENSITY: ret = receive_length_data(urg, data, intensity, type, buffer); break; case URG_STOP: case URG_UNKNOWN: ret = 0; break; } // specified_scan_times == 1 のときは Gx 系コマンドが使われるため // データを明示的に停止しなくてよい if ((urg->specified_scan_times > 1) && (urg->scanning_remain_times > 0)) { if (--urg->scanning_remain_times <= 0) { // データの停止のみを行う urg_stop_measurement(urg); } } return ret; } int urg_open(urg_t *urg, urg_connection_type_t connection_type, const char *device_or_address, long baudrate_or_port) { int ret; long baudrate = baudrate_or_port; urg->is_active = URG_FALSE; urg->is_sending = URG_TRUE; urg->last_errno = URG_NOT_CONNECTED; urg->timeout = MAX_TIMEOUT; urg->scanning_skip_scan = 0; urg->error_handler = NULL; // デバイスへの接続 ret = connection_open(&urg->connection, connection_type, device_or_address, baudrate_or_port); if (ret < 0) { switch (connection_type) { case URG_SERIAL: urg->last_errno = URG_SERIAL_OPEN_ERROR; break; case URG_ETHERNET: urg->last_errno = URG_ETHERNET_OPEN_ERROR; break; default: urg->last_errno = URG_INVALID_RESPONSE; break; } return urg->last_errno; } // 指定したボーレートで URG と通信できるように調整 if (connection_type == URG_ETHERNET) { // Ethernet のときは仮の通信速度を指定しておく baudrate = 115200; } if (connect_urg_device(urg, baudrate) != URG_NO_ERROR) { return set_errno_and_return(urg, ret); } urg->is_sending = URG_FALSE; // 変数の初期化 urg->last_errno = URG_NO_ERROR; urg->range_data_byte = URG_COMMUNICATION_3_BYTE; urg->specified_scan_times = 0; urg->scanning_remain_times = 0; urg->is_laser_on = URG_FALSE; // パラメータ情報を取得 ret = receive_parameter(urg); if (ret == URG_NO_ERROR) { urg->is_active = URG_TRUE; } return ret; } void urg_close(urg_t *urg) { if (urg->is_active) { ignore_receive_data_with_qt(urg, urg->timeout); } connection_close(&urg->connection); urg->is_active = URG_FALSE; } void urg_set_timeout_msec(urg_t *urg, int msec) { urg->timeout = msec; } int urg_start_time_stamp_mode(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM0 を発行する n = scip_response(urg, "TM0\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } long urg_time_stamp(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; char buffer[BUFFER_SIZE]; char *p; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } ret = scip_response(urg, "TM1\n", expected, urg->timeout, buffer, BUFFER_SIZE); if (ret < 0) { return ret; } // buffer からタイムスタンプを取得し、デコードして返す if (strcmp(buffer, "00P")) { // 最初の応答が "00P" でなければ戻る return set_errno_and_return(urg, URG_RECEIVE_ERROR); } p = buffer + 4; if (strlen(p) != 5) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (p[5] == scip_checksum(p, 4)) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } return urg_scip_decode(p, 4); } int urg_stop_time_stamp_mode(urg_t *urg) { int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM2 を発行する n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } static int send_distance_command(urg_t *urg, int scan_times, int skip_scan, char single_scan_ch, char continuous_scan_ch, char scan_type_ch) { char buffer[BUFFER_SIZE]; int write_size = 0; int front_index = urg->front_data_index; int n; urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times; urg->scanning_remain_times = urg->specified_scan_times; urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan; if (scan_times >= 100) { // 計測回数が 99 を越える場合は、無限回のスキャンを行う urg->specified_scan_times = 0; } if (urg->scanning_remain_times == 1) { // レーザ発光を指示 urg_laser_on(urg); write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n", single_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step); } else { write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n", continuous_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step, skip_scan, urg->specified_scan_times); urg->is_sending = URG_TRUE; } n = connection_write(&urg->connection, buffer, write_size); if (n != write_size) { return set_errno_and_return(urg, URG_SEND_ERROR); } return 0; } int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan) { char range_byte_ch; int ret = 0; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((skip_scan < 0) || (skip_scan > 9)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // !!! Mx 系, Nx 系の計測中のときは、QT を発行してから // !!! 計測開始コマンドを送信するようにする // !!! ただし、MD 計測中に MD を発行するように、同じコマンドの場合は // !!! Mx 系, Nx 系の計測は上書きすることができるようにする // 指定されたタイプのパケットを生成し、送信する switch (type) { case URG_DISTANCE: range_byte_ch = (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D'; ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', range_byte_ch); break; case URG_DISTANCE_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', 'E'); break; case URG_MULTIECHO: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'D'); break; case URG_MULTIECHO_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'E'); break; case URG_STOP: case URG_UNKNOWN: default: ignore_receive_data_with_qt(urg, urg->timeout); urg->last_errno = URG_INVALID_PARAMETER; ret = urg->last_errno; break; } return ret; } int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, NULL, time_stamp, system_time_stamp); } int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp); } int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp); } int urg_stop_measurement(urg_t *urg) { enum { MAX_READ_TIMES = 3 }; int ret = URG_INVALID_RESPONSE; int n; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // QT を発行する n = connection_write(&urg->connection, "QT\n", 3); if (n != 3) { return set_errno_and_return(urg, URG_SEND_ERROR); } for (i = 0; i < MAX_READ_TIMES; ++i) { // QT の応答が返されるまで、距離データを読み捨てる ret = receive_data(urg, NULL, NULL, NULL, NULL); if (ret == URG_NO_ERROR) { // 正常応答 urg->is_laser_on = URG_FALSE; urg->is_sending = URG_FALSE; return set_errno_and_return(urg, URG_NO_ERROR); } } return ret; } int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step) { // 設定の範囲外を指定したときは、エラーを返す if (((skip_step < 0) || (skip_step >= 100)) || (first_step > last_step) || (first_step < -urg->front_data_index) || (last_step > (urg->last_data_index - urg->front_data_index))) { return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR); } urg->scanning_first_step = first_step; urg->scanning_last_step = last_step; urg->scanning_skip_step = skip_step; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_set_connection_data_size(urg_t *urg, urg_range_data_byte_t data_byte) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((data_byte != URG_COMMUNICATION_3_BYTE) || (data_byte != URG_COMMUNICATION_2_BYTE)) { return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR); } urg->range_data_byte = data_byte; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_laser_on(urg_t *urg) { int expected[] = { 0, 2, EXPECTED_END }; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if (urg->is_laser_on != URG_FALSE) { // 既にレーザが発光しているときは、コマンドを送信しないようにする urg->last_errno = 0; return urg->last_errno; } ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0); if (ret >= 0) { urg->is_laser_on = URG_TRUE; ret = 0; } return ret; } int urg_laser_off(urg_t *urg) { return urg_stop_measurement(urg); } int urg_reboot(urg_t *urg) { int expected[] = { 0, 1, EXPECTED_END }; int ret; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // ２回目の RB 送信後、接続を切断する for (i = 0; i < 2; ++i) { ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0); if (ret < 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } urg->is_active = URG_FALSE; urg_close(urg); urg->last_errno = 0; return urg->last_errno; } void urg_sleep(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = 4 }; int sl_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE]; if (urg_stop_measurement(urg) != URG_NO_ERROR) { return; } scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); } void urg_wakeup(urg_t *urg) { urg_stop_measurement(urg); } int urg_is_stable(urg_t *urg) { const char *stat = urg_sensor_status(urg); return strncmp("Stable", stat, 6) ? 0 : 1; } static char *copy_token(char *dest, char *receive_buffer, const char *start_str, const char *end_ch, int lines) { size_t start_str_len = strlen(start_str); size_t end_ch_len = strlen(end_ch); int i; size_t j; for (j = 0; j < end_ch_len; ++j) { const char *p = receive_buffer; for (i = 0; i < lines; ++i) { if (!strncmp(p, start_str, start_str_len)) { char *last_p = strchr(p + start_str_len, end_ch[j]); if (last_p) { *last_p = '\0'; memcpy(dest, p + start_str_len, last_p - (p + start_str_len) + 1); return dest; } } p += strlen(p) + 1; } } return NULL; } static const char *receive_command_response(urg_t *urg, char *buffer, int buffer_size, const char* command, int response_lines) { const int vv_expected[] = { 0, EXPECTED_END }; int ret; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = scip_response(urg, command, vv_expected, urg->timeout, buffer, buffer_size); if (ret < response_lines) { return RECEIVE_ERROR_MESSAGE; } return NULL; } const char *urg_sensor_product_type(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROD:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_serial_id(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "SERI:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_vendor(urg_t *urg){ enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "VEND:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", "(", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_date(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } // Get the firmware version and append a '(', this will be what's before the date char firmware_version[50]; strcpy(firmware_version, urg_sensor_firmware_version(urg)); strcat(firmware_version, "("); ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } // Strip out the actual date from between the '(' and ')' char *date; p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", ";", VV_RESPONSE_LINES); date = copy_token(urg->return_buffer, p, firmware_version, ")", 1); return (date) ? date : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_protocol_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROT:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_status(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "STAT:", ";", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_state(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "MESM:", " (", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } void urg_set_error_handler(urg_t *urg, urg_error_handler handler) { urg->error_handler = handler; }
57  return (sum & 0x3f) + 0x30;
58 }
59 
60 
61 static int set_errno_and_return(urg_t *urg, int urg_errno)
62 {
63  urg->last_errno = urg_errno;
64  return urg_errno;
65 }
66 
67 
68 // 受信した応答の行数を返す
69 static int scip_response(urg_t *urg, const char* command,
70  const int expected_ret[], int timeout,
71  char *receive_buffer, int receive_buffer_max_size)
72 {
73  char *p = receive_buffer;
74  char buffer[BUFFER_SIZE];
75  int filled_size = 0;
76  int line_number = 0;
77  int ret = URG_UNKNOWN_ERROR;
78 
79  int write_size = (int)strlen(command);
80  int n = connection_write(&urg->connection, command, write_size);
81 
82  if (n != write_size) {
83  return set_errno_and_return(urg, URG_SEND_ERROR);
84  }
85 
86  if (p) {
87  *p = '\0';
88  }
89 
90  do {
91  n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, timeout);
92  if (n < 0) {
93  return set_errno_and_return(urg, URG_NO_RESPONSE);
94 
95  } else if (p && (line_number > 0)
96  && (n < (receive_buffer_max_size - filled_size))) {
97  // エコーバックは完全一致のチェックを行うため、格納しない
98  memcpy(p, buffer, n);
99  p += n;
100  *p++ = '\0';
101  filled_size += n;
102  }
103 
104  if (line_number == 0) {
105  // エコーバック文字列が、一致するかを確認する if (strncmp(buffer, command, write_size - 1)) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } else if (n > 0) { // エコーバック以外の行のチェックサムを評価する char checksum = buffer[n - 1]; if ((checksum != scip_checksum(buffer, n - 1)) && (checksum != scip_checksum(buffer, n - 2))) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } } // ステータス応答を評価して、戻り値を決定する if (line_number == 1) { if (n == 1) { // SCIP 1.1 応答の場合は、正常応答とみなす ret = 0; } else if (n != 3) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { int i; int actual_ret = strtol(buffer, NULL, 10); for (i = 0; expected_ret[i] != EXPECTED_END; ++i) { if (expected_ret[i] == actual_ret) { ret = 0; break; } } } } ++line_number; } while (n > 0); return (ret < 0) ? ret : (line_number - 1); } static void ignore_receive_data(urg_t *urg, int timeout) { char buffer[BUFFER_SIZE]; int n; if (urg->is_sending == URG_FALSE) { return; } do { n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, timeout); } while (n >= 0); urg->is_sending = URG_FALSE; } static void ignore_receive_data_with_qt(urg_t *urg, int timeout) { if ((urg->is_sending == URG_FALSE) && (urg->is_laser_on == URG_FALSE)) { return; } connection_write(&urg->connection, "QT\n", 3); urg->is_sending = URG_TRUE; urg->is_laser_on = URG_FALSE; ignore_receive_data(urg, timeout); } static int change_sensor_baudrate(urg_t *urg, long current_baudrate, long next_baudrate) { enum { SS_COMMAND_SIZE = 10 }; char buffer[SS_COMMAND_SIZE]; int ss_expected[] = { 0, 3, 4, EXPECTED_END }; int ret; if (current_baudrate == next_baudrate) { // 現在のボーレートと設定するボーレートが一緒ならば、戻る return set_errno_and_return(urg, URG_NO_ERROR); } // "SS" コマンドでボーレートを変更する snprintf(buffer, SS_COMMAND_SIZE, "SS%06ld\n", next_baudrate); ret = scip_response(urg, buffer, ss_expected, urg->timeout, NULL, 0); // 0F 応答のときは Ethernet 用のセンサとみなし、正常応答を返す if (ret == -15) { return set_errno_and_return(urg, URG_NO_ERROR); } if (ret <= 0) { return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // 正常応答ならば、ホスト側のボーレートを変更する ret = connection_set_baudrate(&urg->connection, next_baudrate); // センサ側の設定反映を待つために少しだけ待機する ignore_receive_data(urg, MAX_TIMEOUT); return set_errno_and_return(urg, ret); } // ボーレートを変更しながら接続する static int connect_urg_device(urg_t *urg, long baudrate) { long try_baudrate[] = { 19200, 38400, 115200 }; int try_times = sizeof(try_baudrate) / sizeof(try_baudrate[0]); int i; // 指示されたボーレートから接続する for (i = 0; i < try_times; ++i) { if (try_baudrate[i] == baudrate) { try_baudrate[i] = try_baudrate[0]; try_baudrate[0] = baudrate; break; } } for (i = 0; i < try_times; ++i) { enum { RECEIVE_BUFFER_SIZE = 4 }; int qt_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE + 1]; int ret; connection_set_baudrate(&urg->connection, try_baudrate[i]); // QT を送信し、応答が返されるかでボーレートが一致しているかを確認する ret = scip_response(urg, "QT\n", qt_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); if (ret > 0) { if (!strcmp(receive_buffer, "E")) { int scip20_expected[] = { 0, EXPECTED_END }; // QT 応答の最後の改行を読み飛ばす ignore_receive_data(urg, MAX_TIMEOUT); // "E" が返された場合は、SCIP 1.1 とみなし "SCIP2.0" を送信する ret = scip_response(urg, "SCIP2.0\n", scip20_expected, MAX_TIMEOUT, NULL, 0); // SCIP2.0 応答の最後の改行を読み飛ばす ignore_receive_data(urg, MAX_TIMEOUT); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } else if (!strcmp(receive_buffer, "0Ee")) { int tm2_expected[] = { 0, EXPECTED_END }; // "0Ee" が返された場合は、TM モードとみなし "TM2" を送信する scip_response(urg, "TM2\n", tm2_expected, MAX_TIMEOUT, NULL, 0); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } if (ret <= 0) { if (ret == URG_INVALID_RESPONSE) { // 異常なエコーバックのときは、距離データ受信中とみなして // データを読み飛ばす ignore_receive_data_with_qt(urg, MAX_TIMEOUT); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } else { // 応答がないときは、ボーレートを変更して、再度接続を行う ignore_receive_data_with_qt(urg, MAX_TIMEOUT); continue; } } else if (!strcmp("00P", receive_buffer)) { // センサとホストのボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } return set_errno_and_return(urg, URG_NOT_DETECT_BAUDRATE_ERROR); } // PP コマンドの応答を urg_t に格納する static int receive_parameter(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * 9, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; int pp_expected[] = { 0, EXPECTED_END }; unsigned short received_bits = 0x0000; char *p; int i; int ret = scip_response(urg, "PP\n", pp_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); if (ret < 0) { return ret; } else if (ret < PP_RESPONSE_LINES) { ignore_receive_data_with_qt(urg, MAX_TIMEOUT); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } p = receive_buffer; for (i = 0; i < (ret - 1); ++i) { if (!strncmp(p, "DMIN:", 5)) { urg->min_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0001; } else if (!strncmp(p, "DMAX:", 5)) { urg->max_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0002; } else if (!strncmp(p, "ARES:", 5)) { urg->area_resolution = strtol(p + 5, NULL, 10); received_bits |= 0x0004; } else if (!strncmp(p, "AMIN:", 5)) { urg->first_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0008; } else if (!strncmp(p, "AMAX:", 5)) { urg->last_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0010; } else if (!strncmp(p, "AFRT:", 5)) { urg->front_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0020; } else if (!strncmp(p, "SCAN:", 5)) { int rpm = strtol(p + 5, NULL, 10); // タイムアウト時間は、計測周期の 16 倍程度の値にする urg->scan_usec = 1000 * 1000 * 60 / rpm; urg->timeout = urg->scan_usec >> (10 - 4); received_bits |= 0x0040; } p += strlen(p) + 1; } // 全てのパラメータを受信したか確認 if (received_bits != 0x007f) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } urg_set_scanning_parameter(urg, urg->first_data_index - urg->front_data_index, urg->last_data_index - urg->front_data_index, 1); return set_errno_and_return(urg, URG_NO_ERROR); } //! SCIP 文字列のデコード long urg_scip_decode(const char data[], int size) { const char* p = data; const char* last_p = p + size; int value = 0; while (p < last_p) { value <<= 6; value &= ~0x3f; value |= *p++ - 0x30; } return value; } static int parse_parameter(const char *parameter, int size) { char buffer[5]; memcpy(buffer, parameter, size); buffer[size] = '\0'; return strtol(buffer, NULL, 10); } static urg_measurement_type_t parse_distance_parameter(urg_t *urg, const char echoback[]) { urg_measurement_type_t ret_type = URG_UNKNOWN; urg->received_range_data_byte = URG_COMMUNICATION_3_BYTE; if (echoback[1] == 'S') { urg->received_range_data_byte = URG_COMMUNICATION_2_BYTE; ret_type = URG_DISTANCE; } else if (echoback[1] == 'D') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO; } } else if (echoback[1] == 'E') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE_INTENSITY; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO_INTENSITY; } } else { return URG_UNKNOWN; } // パラメータの格納 urg->received_first_index = parse_parameter(&echoback[2], 4); urg->received_last_index = parse_parameter(&echoback[6], 4); urg->received_skip_step = parse_parameter(&echoback[10], 2); return ret_type; } static urg_measurement_type_t parse_distance_echoback(urg_t *urg, const char echoback[]) { size_t line_length; urg_measurement_type_t ret_type = URG_UNKNOWN; if (!strcmp("QT", echoback)) { return URG_STOP; } line_length = strlen(echoback); if ((line_length == 12) && ((echoback[0] == 'G') || (echoback[0] == 'H'))) { ret_type = parse_distance_parameter(urg, echoback); } else if ((line_length == 15) && ((echoback[0] == 'M') || (echoback[0] == 'N'))) { ret_type = parse_distance_parameter(urg, echoback); } return ret_type; } static int receive_length_data(urg_t *urg, long length[], unsigned short intensity[], urg_measurement_type_t type, char buffer[]) { int n; int step_filled = 0; int line_filled = 0; int multiecho_index = 0; int each_size = (urg->received_range_data_byte == URG_COMMUNICATION_2_BYTE) ? 2 : 3; int data_size = each_size; int is_intensity = URG_FALSE; int is_multiecho = URG_FALSE; int multiecho_max_size = 1; if ((type == URG_DISTANCE_INTENSITY) || (type == URG_MULTIECHO_INTENSITY)) { data_size *= 2; is_intensity = URG_TRUE; } if ((type == URG_MULTIECHO) || (type == URG_MULTIECHO_INTENSITY)) { is_multiecho = URG_TRUE; multiecho_max_size = URG_MAX_ECHO; } do { char *p = buffer; char *last_p; n = connection_readline(&urg->connection, &buffer[line_filled], BUFFER_SIZE - line_filled, urg->timeout); if (n > 0) { // チェックサムの評価 if (buffer[line_filled + n - 1] != scip_checksum(&buffer[line_filled], n - 1)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } } if (n > 0) { line_filled += n - 1; } last_p = p + line_filled; while ((last_p - p) >= data_size) { int index; if (*p == '&') { // 先頭文字が '&' だったときは、マルチエコーのデータとみなす if ((last_p - (p + 1)) < data_size) { // '&' を除いて、data_size 分データが無ければ抜ける break; } --step_filled; ++multiecho_index; ++p; --line_filled; } else { // 次のデータ multiecho_index = 0; } index = (step_filled * multiecho_max_size) + multiecho_index; if (step_filled > (urg->received_last_index - urg->received_first_index)) { // データが多過ぎる場合は、残りのデータを無視して戻る ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (is_multiecho && (multiecho_index == 0)) { // マルチエコーのデータ格納先をダミーデータで埋める int i; if (length) { for (i = 1; i < multiecho_max_size; ++i) { length[index + i] = 0; } } if (intensity) { for (i = 1; i < multiecho_max_size; ++i) { intensity[index + i] = 0; } } } // 距離データの格納 if (length) { length[index] = urg_scip_decode(p, 3); } p += 3; // 強度データの格納 if (is_intensity) { if (intensity) { intensity[index] = (unsigned short)urg_scip_decode(p, 3); } p += 3; } ++step_filled; line_filled -= data_size; } // 次に処理する文字を退避 memmove(buffer, p, line_filled); } while (n > 0); return step_filled; } //! 距離データの取得 static int receive_data(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { urg_measurement_type_t type; char buffer[BUFFER_SIZE]; int ret = 0; int n; int extended_timeout = urg->timeout + 2 * (urg->scan_usec * (urg->scanning_skip_scan) / 1000); // エコーバックの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, extended_timeout); if (n <= 0) { return set_errno_and_return(urg, URG_NO_RESPONSE); } // エコーバックの解析 type = parse_distance_echoback(urg, buffer); // 応答の取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 3) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } if (buffer[n - 1] != scip_checksum(buffer, n - 1)) { // チェックサムの評価 ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } if (type == URG_STOP) { // QT 応答の場合には、最後の改行を読み捨て、正常応答として処理する n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n == 0) { return 0; } else { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } if (urg->specified_scan_times != 1) { if (!strncmp(buffer, "00", 2)) { // "00" 応答の場合は、エコーバック応答とみなし、 // 最後の空行を読み捨て、次からのデータを返す n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 0) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } } } if (((urg->specified_scan_times == 1) && (strncmp(buffer, "00", 2))) || ((urg->specified_scan_times != 1) && (strncmp(buffer, "99", 2)))) { if (urg->error_handler) { type = urg->error_handler(buffer, urg); } if (type == URG_UNKNOWN) { // Gx, Hx のときは 00P が返されたときがデータ // Mx, Nx のときは 99b が返されたときがデータ ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } // タイムスタンプの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n > 0) { if (time_stamp) { *time_stamp = urg_scip_decode(buffer, 4); } if (system_time_stamp) { urg_get_walltime(system_time_stamp); } } // データの取得 switch (type) { case URG_DISTANCE: case URG_MULTIECHO: ret = receive_length_data(urg, data, NULL, type, buffer); break; case URG_DISTANCE_INTENSITY: case URG_MULTIECHO_INTENSITY: ret = receive_length_data(urg, data, intensity, type, buffer); break; case URG_STOP: case URG_UNKNOWN: ret = 0; break; } // specified_scan_times == 1 のときは Gx 系コマンドが使われるため // データを明示的に停止しなくてよい if ((urg->specified_scan_times > 1) && (urg->scanning_remain_times > 0)) { if (--urg->scanning_remain_times <= 0) { // データの停止のみを行う urg_stop_measurement(urg); } } return ret; } int urg_open(urg_t *urg, urg_connection_type_t connection_type, const char *device_or_address, long baudrate_or_port) { int ret; long baudrate = baudrate_or_port; urg->is_active = URG_FALSE; urg->is_sending = URG_TRUE; urg->last_errno = URG_NOT_CONNECTED; urg->timeout = MAX_TIMEOUT; urg->scanning_skip_scan = 0; urg->error_handler = NULL; // デバイスへの接続 ret = connection_open(&urg->connection, connection_type, device_or_address, baudrate_or_port); if (ret < 0) { switch (connection_type) { case URG_SERIAL: urg->last_errno = URG_SERIAL_OPEN_ERROR; break; case URG_ETHERNET: urg->last_errno = URG_ETHERNET_OPEN_ERROR; break; default: urg->last_errno = URG_INVALID_RESPONSE; break; } return urg->last_errno; } // 指定したボーレートで URG と通信できるように調整 if (connection_type == URG_ETHERNET) { // Ethernet のときは仮の通信速度を指定しておく baudrate = 115200; } if (connect_urg_device(urg, baudrate) != URG_NO_ERROR) { return set_errno_and_return(urg, ret); } urg->is_sending = URG_FALSE; // 変数の初期化 urg->last_errno = URG_NO_ERROR; urg->range_data_byte = URG_COMMUNICATION_3_BYTE; urg->specified_scan_times = 0; urg->scanning_remain_times = 0; urg->is_laser_on = URG_FALSE; // パラメータ情報を取得 ret = receive_parameter(urg); if (ret == URG_NO_ERROR) { urg->is_active = URG_TRUE; } return ret; } void urg_close(urg_t *urg) { if (urg->is_active) { ignore_receive_data_with_qt(urg, urg->timeout); } connection_close(&urg->connection); urg->is_active = URG_FALSE; } void urg_set_timeout_msec(urg_t *urg, int msec) { urg->timeout = msec; } int urg_start_time_stamp_mode(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM0 を発行する n = scip_response(urg, "TM0\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } long urg_time_stamp(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; char buffer[BUFFER_SIZE]; char *p; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } ret = scip_response(urg, "TM1\n", expected, urg->timeout, buffer, BUFFER_SIZE); if (ret < 0) { return ret; } // buffer からタイムスタンプを取得し、デコードして返す if (strcmp(buffer, "00P")) { // 最初の応答が "00P" でなければ戻る return set_errno_and_return(urg, URG_RECEIVE_ERROR); } p = buffer + 4; if (strlen(p) != 5) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (p[5] == scip_checksum(p, 4)) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } return urg_scip_decode(p, 4); } int urg_stop_time_stamp_mode(urg_t *urg) { int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM2 を発行する n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } static int send_distance_command(urg_t *urg, int scan_times, int skip_scan, char single_scan_ch, char continuous_scan_ch, char scan_type_ch) { char buffer[BUFFER_SIZE]; int write_size = 0; int front_index = urg->front_data_index; int n; urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times; urg->scanning_remain_times = urg->specified_scan_times; urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan; if (scan_times >= 100) { // 計測回数が 99 を越える場合は、無限回のスキャンを行う urg->specified_scan_times = 0; } if (urg->scanning_remain_times == 1) { // レーザ発光を指示 urg_laser_on(urg); write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n", single_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step); } else { write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n", continuous_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step, skip_scan, urg->specified_scan_times); urg->is_sending = URG_TRUE; } n = connection_write(&urg->connection, buffer, write_size); if (n != write_size) { return set_errno_and_return(urg, URG_SEND_ERROR); } return 0; } int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan) { char range_byte_ch; int ret = 0; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((skip_scan < 0) || (skip_scan > 9)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // !!! Mx 系, Nx 系の計測中のときは、QT を発行してから // !!! 計測開始コマンドを送信するようにする // !!! ただし、MD 計測中に MD を発行するように、同じコマンドの場合は // !!! Mx 系, Nx 系の計測は上書きすることができるようにする // 指定されたタイプのパケットを生成し、送信する switch (type) { case URG_DISTANCE: range_byte_ch = (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D'; ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', range_byte_ch); break; case URG_DISTANCE_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', 'E'); break; case URG_MULTIECHO: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'D'); break; case URG_MULTIECHO_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'E'); break; case URG_STOP: case URG_UNKNOWN: default: ignore_receive_data_with_qt(urg, urg->timeout); urg->last_errno = URG_INVALID_PARAMETER; ret = urg->last_errno; break; } return ret; } int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, NULL, time_stamp, system_time_stamp); } int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp); } int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp); } int urg_stop_measurement(urg_t *urg) { enum { MAX_READ_TIMES = 3 }; int ret = URG_INVALID_RESPONSE; int n; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // QT を発行する n = connection_write(&urg->connection, "QT\n", 3); if (n != 3) { return set_errno_and_return(urg, URG_SEND_ERROR); } for (i = 0; i < MAX_READ_TIMES; ++i) { // QT の応答が返されるまで、距離データを読み捨てる ret = receive_data(urg, NULL, NULL, NULL, NULL); if (ret == URG_NO_ERROR) { // 正常応答 urg->is_laser_on = URG_FALSE; urg->is_sending = URG_FALSE; return set_errno_and_return(urg, URG_NO_ERROR); } } return ret; } int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step) { // 設定の範囲外を指定したときは、エラーを返す if (((skip_step < 0) || (skip_step >= 100)) || (first_step > last_step) || (first_step < -urg->front_data_index) || (last_step > (urg->last_data_index - urg->front_data_index))) { return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR); } urg->scanning_first_step = first_step; urg->scanning_last_step = last_step; urg->scanning_skip_step = skip_step; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_set_connection_data_size(urg_t *urg, urg_range_data_byte_t data_byte) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((data_byte != URG_COMMUNICATION_3_BYTE) || (data_byte != URG_COMMUNICATION_2_BYTE)) { return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR); } urg->range_data_byte = data_byte; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_laser_on(urg_t *urg) { int expected[] = { 0, 2, EXPECTED_END }; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if (urg->is_laser_on != URG_FALSE) { // 既にレーザが発光しているときは、コマンドを送信しないようにする urg->last_errno = 0; return urg->last_errno; } ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0); if (ret >= 0) { urg->is_laser_on = URG_TRUE; ret = 0; } return ret; } int urg_laser_off(urg_t *urg) { return urg_stop_measurement(urg); } int urg_reboot(urg_t *urg) { int expected[] = { 0, 1, EXPECTED_END }; int ret; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // ２回目の RB 送信後、接続を切断する for (i = 0; i < 2; ++i) { ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0); if (ret < 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } urg->is_active = URG_FALSE; urg_close(urg); urg->last_errno = 0; return urg->last_errno; } void urg_sleep(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = 4 }; int sl_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE]; if (urg_stop_measurement(urg) != URG_NO_ERROR) { return; } scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); } void urg_wakeup(urg_t *urg) { urg_stop_measurement(urg); } int urg_is_stable(urg_t *urg) { const char *stat = urg_sensor_status(urg); return strncmp("Stable", stat, 6) ? 0 : 1; } static char *copy_token(char *dest, char *receive_buffer, const char *start_str, const char *end_ch, int lines) { size_t start_str_len = strlen(start_str); size_t end_ch_len = strlen(end_ch); int i; size_t j; for (j = 0; j < end_ch_len; ++j) { const char *p = receive_buffer; for (i = 0; i < lines; ++i) { if (!strncmp(p, start_str, start_str_len)) { char *last_p = strchr(p + start_str_len, end_ch[j]); if (last_p) { *last_p = '\0'; memcpy(dest, p + start_str_len, last_p - (p + start_str_len) + 1); return dest; } } p += strlen(p) + 1; } } return NULL; } static const char *receive_command_response(urg_t *urg, char *buffer, int buffer_size, const char* command, int response_lines) { const int vv_expected[] = { 0, EXPECTED_END }; int ret; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = scip_response(urg, command, vv_expected, urg->timeout, buffer, buffer_size); if (ret < response_lines) { return RECEIVE_ERROR_MESSAGE; } return NULL; } const char *urg_sensor_product_type(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROD:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_serial_id(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "SERI:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_vendor(urg_t *urg){ enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "VEND:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", "(", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_date(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } // Get the firmware version and append a '(', this will be what's before the date char firmware_version[50]; strcpy(firmware_version, urg_sensor_firmware_version(urg)); strcat(firmware_version, "("); ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } // Strip out the actual date from between the '(' and ')' char *date; p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", ";", VV_RESPONSE_LINES); date = copy_token(urg->return_buffer, p, firmware_version, ")", 1); return (date) ? date : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_protocol_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROT:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_status(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "STAT:", ";", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_state(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "MESM:", " (", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } void urg_set_error_handler(urg_t *urg, urg_error_handler handler) { urg->error_handler = handler; }
106  if (strncmp(buffer, command, write_size - 1)) {
107  return set_errno_and_return(urg, URG_INVALID_RESPONSE);
108  }
109  } else if (n > 0) {
110  // エコーバック以外の行のチェックサムを評価する char checksum = buffer[n - 1]; if ((checksum != scip_checksum(buffer, n - 1)) && (checksum != scip_checksum(buffer, n - 2))) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } } // ステータス応答を評価して、戻り値を決定する if (line_number == 1) { if (n == 1) { // SCIP 1.1 応答の場合は、正常応答とみなす ret = 0; } else if (n != 3) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { int i; int actual_ret = strtol(buffer, NULL, 10); for (i = 0; expected_ret[i] != EXPECTED_END; ++i) { if (expected_ret[i] == actual_ret) { ret = 0; break; } } } } ++line_number; } while (n > 0); return (ret < 0) ? ret : (line_number - 1); } static void ignore_receive_data(urg_t *urg, int timeout) { char buffer[BUFFER_SIZE]; int n; if (urg->is_sending == URG_FALSE) { return; } do { n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, timeout); } while (n >= 0); urg->is_sending = URG_FALSE; } static void ignore_receive_data_with_qt(urg_t *urg, int timeout) { if ((urg->is_sending == URG_FALSE) && (urg->is_laser_on == URG_FALSE)) { return; } connection_write(&urg->connection, "QT\n", 3); urg->is_sending = URG_TRUE; urg->is_laser_on = URG_FALSE; ignore_receive_data(urg, timeout); } static int change_sensor_baudrate(urg_t *urg, long current_baudrate, long next_baudrate) { enum { SS_COMMAND_SIZE = 10 }; char buffer[SS_COMMAND_SIZE]; int ss_expected[] = { 0, 3, 4, EXPECTED_END }; int ret; if (current_baudrate == next_baudrate) { // 現在のボーレートと設定するボーレートが一緒ならば、戻る return set_errno_and_return(urg, URG_NO_ERROR); } // "SS" コマンドでボーレートを変更する snprintf(buffer, SS_COMMAND_SIZE, "SS%06ld\n", next_baudrate); ret = scip_response(urg, buffer, ss_expected, urg->timeout, NULL, 0); // 0F 応答のときは Ethernet 用のセンサとみなし、正常応答を返す if (ret == -15) { return set_errno_and_return(urg, URG_NO_ERROR); } if (ret <= 0) { return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // 正常応答ならば、ホスト側のボーレートを変更する ret = connection_set_baudrate(&urg->connection, next_baudrate); // センサ側の設定反映を待つために少しだけ待機する ignore_receive_data(urg, MAX_TIMEOUT); return set_errno_and_return(urg, ret); } // ボーレートを変更しながら接続する static int connect_urg_device(urg_t *urg, long baudrate) { long try_baudrate[] = { 19200, 38400, 115200 }; int try_times = sizeof(try_baudrate) / sizeof(try_baudrate[0]); int i; // 指示されたボーレートから接続する for (i = 0; i < try_times; ++i) { if (try_baudrate[i] == baudrate) { try_baudrate[i] = try_baudrate[0]; try_baudrate[0] = baudrate; break; } } for (i = 0; i < try_times; ++i) { enum { RECEIVE_BUFFER_SIZE = 4 }; int qt_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE + 1]; int ret; connection_set_baudrate(&urg->connection, try_baudrate[i]); // QT を送信し、応答が返されるかでボーレートが一致しているかを確認する ret = scip_response(urg, "QT\n", qt_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); if (ret > 0) { if (!strcmp(receive_buffer, "E")) { int scip20_expected[] = { 0, EXPECTED_END }; // QT 応答の最後の改行を読み飛ばす ignore_receive_data(urg, MAX_TIMEOUT); // "E" が返された場合は、SCIP 1.1 とみなし "SCIP2.0" を送信する ret = scip_response(urg, "SCIP2.0\n", scip20_expected, MAX_TIMEOUT, NULL, 0); // SCIP2.0 応答の最後の改行を読み飛ばす ignore_receive_data(urg, MAX_TIMEOUT); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } else if (!strcmp(receive_buffer, "0Ee")) { int tm2_expected[] = { 0, EXPECTED_END }; // "0Ee" が返された場合は、TM モードとみなし "TM2" を送信する scip_response(urg, "TM2\n", tm2_expected, MAX_TIMEOUT, NULL, 0); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } if (ret <= 0) { if (ret == URG_INVALID_RESPONSE) { // 異常なエコーバックのときは、距離データ受信中とみなして // データを読み飛ばす ignore_receive_data_with_qt(urg, MAX_TIMEOUT); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } else { // 応答がないときは、ボーレートを変更して、再度接続を行う ignore_receive_data_with_qt(urg, MAX_TIMEOUT); continue; } } else if (!strcmp("00P", receive_buffer)) { // センサとホストのボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } return set_errno_and_return(urg, URG_NOT_DETECT_BAUDRATE_ERROR); } // PP コマンドの応答を urg_t に格納する static int receive_parameter(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * 9, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; int pp_expected[] = { 0, EXPECTED_END }; unsigned short received_bits = 0x0000; char *p; int i; int ret = scip_response(urg, "PP\n", pp_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); if (ret < 0) { return ret; } else if (ret < PP_RESPONSE_LINES) { ignore_receive_data_with_qt(urg, MAX_TIMEOUT); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } p = receive_buffer; for (i = 0; i < (ret - 1); ++i) { if (!strncmp(p, "DMIN:", 5)) { urg->min_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0001; } else if (!strncmp(p, "DMAX:", 5)) { urg->max_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0002; } else if (!strncmp(p, "ARES:", 5)) { urg->area_resolution = strtol(p + 5, NULL, 10); received_bits |= 0x0004; } else if (!strncmp(p, "AMIN:", 5)) { urg->first_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0008; } else if (!strncmp(p, "AMAX:", 5)) { urg->last_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0010; } else if (!strncmp(p, "AFRT:", 5)) { urg->front_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0020; } else if (!strncmp(p, "SCAN:", 5)) { int rpm = strtol(p + 5, NULL, 10); // タイムアウト時間は、計測周期の 16 倍程度の値にする urg->scan_usec = 1000 * 1000 * 60 / rpm; urg->timeout = urg->scan_usec >> (10 - 4); received_bits |= 0x0040; } p += strlen(p) + 1; } // 全てのパラメータを受信したか確認 if (received_bits != 0x007f) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } urg_set_scanning_parameter(urg, urg->first_data_index - urg->front_data_index, urg->last_data_index - urg->front_data_index, 1); return set_errno_and_return(urg, URG_NO_ERROR); } //! SCIP 文字列のデコード long urg_scip_decode(const char data[], int size) { const char* p = data; const char* last_p = p + size; int value = 0; while (p < last_p) { value <<= 6; value &= ~0x3f; value |= *p++ - 0x30; } return value; } static int parse_parameter(const char *parameter, int size) { char buffer[5]; memcpy(buffer, parameter, size); buffer[size] = '\0'; return strtol(buffer, NULL, 10); } static urg_measurement_type_t parse_distance_parameter(urg_t *urg, const char echoback[]) { urg_measurement_type_t ret_type = URG_UNKNOWN; urg->received_range_data_byte = URG_COMMUNICATION_3_BYTE; if (echoback[1] == 'S') { urg->received_range_data_byte = URG_COMMUNICATION_2_BYTE; ret_type = URG_DISTANCE; } else if (echoback[1] == 'D') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO; } } else if (echoback[1] == 'E') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE_INTENSITY; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO_INTENSITY; } } else { return URG_UNKNOWN; } // パラメータの格納 urg->received_first_index = parse_parameter(&echoback[2], 4); urg->received_last_index = parse_parameter(&echoback[6], 4); urg->received_skip_step = parse_parameter(&echoback[10], 2); return ret_type; } static urg_measurement_type_t parse_distance_echoback(urg_t *urg, const char echoback[]) { size_t line_length; urg_measurement_type_t ret_type = URG_UNKNOWN; if (!strcmp("QT", echoback)) { return URG_STOP; } line_length = strlen(echoback); if ((line_length == 12) && ((echoback[0] == 'G') || (echoback[0] == 'H'))) { ret_type = parse_distance_parameter(urg, echoback); } else if ((line_length == 15) && ((echoback[0] == 'M') || (echoback[0] == 'N'))) { ret_type = parse_distance_parameter(urg, echoback); } return ret_type; } static int receive_length_data(urg_t *urg, long length[], unsigned short intensity[], urg_measurement_type_t type, char buffer[]) { int n; int step_filled = 0; int line_filled = 0; int multiecho_index = 0; int each_size = (urg->received_range_data_byte == URG_COMMUNICATION_2_BYTE) ? 2 : 3; int data_size = each_size; int is_intensity = URG_FALSE; int is_multiecho = URG_FALSE; int multiecho_max_size = 1; if ((type == URG_DISTANCE_INTENSITY) || (type == URG_MULTIECHO_INTENSITY)) { data_size *= 2; is_intensity = URG_TRUE; } if ((type == URG_MULTIECHO) || (type == URG_MULTIECHO_INTENSITY)) { is_multiecho = URG_TRUE; multiecho_max_size = URG_MAX_ECHO; } do { char *p = buffer; char *last_p; n = connection_readline(&urg->connection, &buffer[line_filled], BUFFER_SIZE - line_filled, urg->timeout); if (n > 0) { // チェックサムの評価 if (buffer[line_filled + n - 1] != scip_checksum(&buffer[line_filled], n - 1)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } } if (n > 0) { line_filled += n - 1; } last_p = p + line_filled; while ((last_p - p) >= data_size) { int index; if (*p == '&') { // 先頭文字が '&' だったときは、マルチエコーのデータとみなす if ((last_p - (p + 1)) < data_size) { // '&' を除いて、data_size 分データが無ければ抜ける break; } --step_filled; ++multiecho_index; ++p; --line_filled; } else { // 次のデータ multiecho_index = 0; } index = (step_filled * multiecho_max_size) + multiecho_index; if (step_filled > (urg->received_last_index - urg->received_first_index)) { // データが多過ぎる場合は、残りのデータを無視して戻る ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (is_multiecho && (multiecho_index == 0)) { // マルチエコーのデータ格納先をダミーデータで埋める int i; if (length) { for (i = 1; i < multiecho_max_size; ++i) { length[index + i] = 0; } } if (intensity) { for (i = 1; i < multiecho_max_size; ++i) { intensity[index + i] = 0; } } } // 距離データの格納 if (length) { length[index] = urg_scip_decode(p, 3); } p += 3; // 強度データの格納 if (is_intensity) { if (intensity) { intensity[index] = (unsigned short)urg_scip_decode(p, 3); } p += 3; } ++step_filled; line_filled -= data_size; } // 次に処理する文字を退避 memmove(buffer, p, line_filled); } while (n > 0); return step_filled; } //! 距離データの取得 static int receive_data(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { urg_measurement_type_t type; char buffer[BUFFER_SIZE]; int ret = 0; int n; int extended_timeout = urg->timeout + 2 * (urg->scan_usec * (urg->scanning_skip_scan) / 1000); // エコーバックの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, extended_timeout); if (n <= 0) { return set_errno_and_return(urg, URG_NO_RESPONSE); } // エコーバックの解析 type = parse_distance_echoback(urg, buffer); // 応答の取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 3) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } if (buffer[n - 1] != scip_checksum(buffer, n - 1)) { // チェックサムの評価 ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } if (type == URG_STOP) { // QT 応答の場合には、最後の改行を読み捨て、正常応答として処理する n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n == 0) { return 0; } else { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } if (urg->specified_scan_times != 1) { if (!strncmp(buffer, "00", 2)) { // "00" 応答の場合は、エコーバック応答とみなし、 // 最後の空行を読み捨て、次からのデータを返す n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 0) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } } } if (((urg->specified_scan_times == 1) && (strncmp(buffer, "00", 2))) || ((urg->specified_scan_times != 1) && (strncmp(buffer, "99", 2)))) { if (urg->error_handler) { type = urg->error_handler(buffer, urg); } if (type == URG_UNKNOWN) { // Gx, Hx のときは 00P が返されたときがデータ // Mx, Nx のときは 99b が返されたときがデータ ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } // タイムスタンプの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n > 0) { if (time_stamp) { *time_stamp = urg_scip_decode(buffer, 4); } if (system_time_stamp) { urg_get_walltime(system_time_stamp); } } // データの取得 switch (type) { case URG_DISTANCE: case URG_MULTIECHO: ret = receive_length_data(urg, data, NULL, type, buffer); break; case URG_DISTANCE_INTENSITY: case URG_MULTIECHO_INTENSITY: ret = receive_length_data(urg, data, intensity, type, buffer); break; case URG_STOP: case URG_UNKNOWN: ret = 0; break; } // specified_scan_times == 1 のときは Gx 系コマンドが使われるため // データを明示的に停止しなくてよい if ((urg->specified_scan_times > 1) && (urg->scanning_remain_times > 0)) { if (--urg->scanning_remain_times <= 0) { // データの停止のみを行う urg_stop_measurement(urg); } } return ret; } int urg_open(urg_t *urg, urg_connection_type_t connection_type, const char *device_or_address, long baudrate_or_port) { int ret; long baudrate = baudrate_or_port; urg->is_active = URG_FALSE; urg->is_sending = URG_TRUE; urg->last_errno = URG_NOT_CONNECTED; urg->timeout = MAX_TIMEOUT; urg->scanning_skip_scan = 0; urg->error_handler = NULL; // デバイスへの接続 ret = connection_open(&urg->connection, connection_type, device_or_address, baudrate_or_port); if (ret < 0) { switch (connection_type) { case URG_SERIAL: urg->last_errno = URG_SERIAL_OPEN_ERROR; break; case URG_ETHERNET: urg->last_errno = URG_ETHERNET_OPEN_ERROR; break; default: urg->last_errno = URG_INVALID_RESPONSE; break; } return urg->last_errno; } // 指定したボーレートで URG と通信できるように調整 if (connection_type == URG_ETHERNET) { // Ethernet のときは仮の通信速度を指定しておく baudrate = 115200; } if (connect_urg_device(urg, baudrate) != URG_NO_ERROR) { return set_errno_and_return(urg, ret); } urg->is_sending = URG_FALSE; // 変数の初期化 urg->last_errno = URG_NO_ERROR; urg->range_data_byte = URG_COMMUNICATION_3_BYTE; urg->specified_scan_times = 0; urg->scanning_remain_times = 0; urg->is_laser_on = URG_FALSE; // パラメータ情報を取得 ret = receive_parameter(urg); if (ret == URG_NO_ERROR) { urg->is_active = URG_TRUE; } return ret; } void urg_close(urg_t *urg) { if (urg->is_active) { ignore_receive_data_with_qt(urg, urg->timeout); } connection_close(&urg->connection); urg->is_active = URG_FALSE; } void urg_set_timeout_msec(urg_t *urg, int msec) { urg->timeout = msec; } int urg_start_time_stamp_mode(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM0 を発行する n = scip_response(urg, "TM0\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } long urg_time_stamp(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; char buffer[BUFFER_SIZE]; char *p; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } ret = scip_response(urg, "TM1\n", expected, urg->timeout, buffer, BUFFER_SIZE); if (ret < 0) { return ret; } // buffer からタイムスタンプを取得し、デコードして返す if (strcmp(buffer, "00P")) { // 最初の応答が "00P" でなければ戻る return set_errno_and_return(urg, URG_RECEIVE_ERROR); } p = buffer + 4; if (strlen(p) != 5) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (p[5] == scip_checksum(p, 4)) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } return urg_scip_decode(p, 4); } int urg_stop_time_stamp_mode(urg_t *urg) { int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM2 を発行する n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } static int send_distance_command(urg_t *urg, int scan_times, int skip_scan, char single_scan_ch, char continuous_scan_ch, char scan_type_ch) { char buffer[BUFFER_SIZE]; int write_size = 0; int front_index = urg->front_data_index; int n; urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times; urg->scanning_remain_times = urg->specified_scan_times; urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan; if (scan_times >= 100) { // 計測回数が 99 を越える場合は、無限回のスキャンを行う urg->specified_scan_times = 0; } if (urg->scanning_remain_times == 1) { // レーザ発光を指示 urg_laser_on(urg); write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n", single_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step); } else { write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n", continuous_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step, skip_scan, urg->specified_scan_times); urg->is_sending = URG_TRUE; } n = connection_write(&urg->connection, buffer, write_size); if (n != write_size) { return set_errno_and_return(urg, URG_SEND_ERROR); } return 0; } int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan) { char range_byte_ch; int ret = 0; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((skip_scan < 0) || (skip_scan > 9)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // !!! Mx 系, Nx 系の計測中のときは、QT を発行してから // !!! 計測開始コマンドを送信するようにする // !!! ただし、MD 計測中に MD を発行するように、同じコマンドの場合は // !!! Mx 系, Nx 系の計測は上書きすることができるようにする // 指定されたタイプのパケットを生成し、送信する switch (type) { case URG_DISTANCE: range_byte_ch = (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D'; ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', range_byte_ch); break; case URG_DISTANCE_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', 'E'); break; case URG_MULTIECHO: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'D'); break; case URG_MULTIECHO_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'E'); break; case URG_STOP: case URG_UNKNOWN: default: ignore_receive_data_with_qt(urg, urg->timeout); urg->last_errno = URG_INVALID_PARAMETER; ret = urg->last_errno; break; } return ret; } int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, NULL, time_stamp, system_time_stamp); } int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp); } int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp); } int urg_stop_measurement(urg_t *urg) { enum { MAX_READ_TIMES = 3 }; int ret = URG_INVALID_RESPONSE; int n; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // QT を発行する n = connection_write(&urg->connection, "QT\n", 3); if (n != 3) { return set_errno_and_return(urg, URG_SEND_ERROR); } for (i = 0; i < MAX_READ_TIMES; ++i) { // QT の応答が返されるまで、距離データを読み捨てる ret = receive_data(urg, NULL, NULL, NULL, NULL); if (ret == URG_NO_ERROR) { // 正常応答 urg->is_laser_on = URG_FALSE; urg->is_sending = URG_FALSE; return set_errno_and_return(urg, URG_NO_ERROR); } } return ret; } int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step) { // 設定の範囲外を指定したときは、エラーを返す if (((skip_step < 0) || (skip_step >= 100)) || (first_step > last_step) || (first_step < -urg->front_data_index) || (last_step > (urg->last_data_index - urg->front_data_index))) { return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR); } urg->scanning_first_step = first_step; urg->scanning_last_step = last_step; urg->scanning_skip_step = skip_step; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_set_connection_data_size(urg_t *urg, urg_range_data_byte_t data_byte) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((data_byte != URG_COMMUNICATION_3_BYTE) || (data_byte != URG_COMMUNICATION_2_BYTE)) { return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR); } urg->range_data_byte = data_byte; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_laser_on(urg_t *urg) { int expected[] = { 0, 2, EXPECTED_END }; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if (urg->is_laser_on != URG_FALSE) { // 既にレーザが発光しているときは、コマンドを送信しないようにする urg->last_errno = 0; return urg->last_errno; } ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0); if (ret >= 0) { urg->is_laser_on = URG_TRUE; ret = 0; } return ret; } int urg_laser_off(urg_t *urg) { return urg_stop_measurement(urg); } int urg_reboot(urg_t *urg) { int expected[] = { 0, 1, EXPECTED_END }; int ret; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // ２回目の RB 送信後、接続を切断する for (i = 0; i < 2; ++i) { ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0); if (ret < 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } urg->is_active = URG_FALSE; urg_close(urg); urg->last_errno = 0; return urg->last_errno; } void urg_sleep(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = 4 }; int sl_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE]; if (urg_stop_measurement(urg) != URG_NO_ERROR) { return; } scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); } void urg_wakeup(urg_t *urg) { urg_stop_measurement(urg); } int urg_is_stable(urg_t *urg) { const char *stat = urg_sensor_status(urg); return strncmp("Stable", stat, 6) ? 0 : 1; } static char *copy_token(char *dest, char *receive_buffer, const char *start_str, const char *end_ch, int lines) { size_t start_str_len = strlen(start_str); size_t end_ch_len = strlen(end_ch); int i; size_t j; for (j = 0; j < end_ch_len; ++j) { const char *p = receive_buffer; for (i = 0; i < lines; ++i) { if (!strncmp(p, start_str, start_str_len)) { char *last_p = strchr(p + start_str_len, end_ch[j]); if (last_p) { *last_p = '\0'; memcpy(dest, p + start_str_len, last_p - (p + start_str_len) + 1); return dest; } } p += strlen(p) + 1; } } return NULL; } static const char *receive_command_response(urg_t *urg, char *buffer, int buffer_size, const char* command, int response_lines) { const int vv_expected[] = { 0, EXPECTED_END }; int ret; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = scip_response(urg, command, vv_expected, urg->timeout, buffer, buffer_size); if (ret < response_lines) { return RECEIVE_ERROR_MESSAGE; } return NULL; } const char *urg_sensor_product_type(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROD:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_serial_id(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "SERI:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_vendor(urg_t *urg){ enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "VEND:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", "(", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_date(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } // Get the firmware version and append a '(', this will be what's before the date char firmware_version[50]; strcpy(firmware_version, urg_sensor_firmware_version(urg)); strcat(firmware_version, "("); ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } // Strip out the actual date from between the '(' and ')' char *date; p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", ";", VV_RESPONSE_LINES); date = copy_token(urg->return_buffer, p, firmware_version, ")", 1); return (date) ? date : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_protocol_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROT:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_status(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "STAT:", ";", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_state(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "MESM:", " (", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } void urg_set_error_handler(urg_t *urg, urg_error_handler handler) { urg->error_handler = handler; }
111  char checksum = buffer[n - 1];
112  if ((checksum != scip_checksum(buffer, n - 1)) &&
113  (checksum != scip_checksum(buffer, n - 2))) {
114  return set_errno_and_return(urg, URG_CHECKSUM_ERROR);
115  }
116  }
117 
118  // ステータス応答を評価して、戻り値を決定する if (line_number == 1) { if (n == 1) { // SCIP 1.1 応答の場合は、正常応答とみなす ret = 0; } else if (n != 3) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { int i; int actual_ret = strtol(buffer, NULL, 10); for (i = 0; expected_ret[i] != EXPECTED_END; ++i) { if (expected_ret[i] == actual_ret) { ret = 0; break; } } } } ++line_number; } while (n > 0); return (ret < 0) ? ret : (line_number - 1); } static void ignore_receive_data(urg_t *urg, int timeout) { char buffer[BUFFER_SIZE]; int n; if (urg->is_sending == URG_FALSE) { return; } do { n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, timeout); } while (n >= 0); urg->is_sending = URG_FALSE; } static void ignore_receive_data_with_qt(urg_t *urg, int timeout) { if ((urg->is_sending == URG_FALSE) && (urg->is_laser_on == URG_FALSE)) { return; } connection_write(&urg->connection, "QT\n", 3); urg->is_sending = URG_TRUE; urg->is_laser_on = URG_FALSE; ignore_receive_data(urg, timeout); } static int change_sensor_baudrate(urg_t *urg, long current_baudrate, long next_baudrate) { enum { SS_COMMAND_SIZE = 10 }; char buffer[SS_COMMAND_SIZE]; int ss_expected[] = { 0, 3, 4, EXPECTED_END }; int ret; if (current_baudrate == next_baudrate) { // 現在のボーレートと設定するボーレートが一緒ならば、戻る return set_errno_and_return(urg, URG_NO_ERROR); } // "SS" コマンドでボーレートを変更する snprintf(buffer, SS_COMMAND_SIZE, "SS%06ld\n", next_baudrate); ret = scip_response(urg, buffer, ss_expected, urg->timeout, NULL, 0); // 0F 応答のときは Ethernet 用のセンサとみなし、正常応答を返す if (ret == -15) { return set_errno_and_return(urg, URG_NO_ERROR); } if (ret <= 0) { return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // 正常応答ならば、ホスト側のボーレートを変更する ret = connection_set_baudrate(&urg->connection, next_baudrate); // センサ側の設定反映を待つために少しだけ待機する ignore_receive_data(urg, MAX_TIMEOUT); return set_errno_and_return(urg, ret); } // ボーレートを変更しながら接続する static int connect_urg_device(urg_t *urg, long baudrate) { long try_baudrate[] = { 19200, 38400, 115200 }; int try_times = sizeof(try_baudrate) / sizeof(try_baudrate[0]); int i; // 指示されたボーレートから接続する for (i = 0; i < try_times; ++i) { if (try_baudrate[i] == baudrate) { try_baudrate[i] = try_baudrate[0]; try_baudrate[0] = baudrate; break; } } for (i = 0; i < try_times; ++i) { enum { RECEIVE_BUFFER_SIZE = 4 }; int qt_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE + 1]; int ret; connection_set_baudrate(&urg->connection, try_baudrate[i]); // QT を送信し、応答が返されるかでボーレートが一致しているかを確認する ret = scip_response(urg, "QT\n", qt_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); if (ret > 0) { if (!strcmp(receive_buffer, "E")) { int scip20_expected[] = { 0, EXPECTED_END }; // QT 応答の最後の改行を読み飛ばす ignore_receive_data(urg, MAX_TIMEOUT); // "E" が返された場合は、SCIP 1.1 とみなし "SCIP2.0" を送信する ret = scip_response(urg, "SCIP2.0\n", scip20_expected, MAX_TIMEOUT, NULL, 0); // SCIP2.0 応答の最後の改行を読み飛ばす ignore_receive_data(urg, MAX_TIMEOUT); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } else if (!strcmp(receive_buffer, "0Ee")) { int tm2_expected[] = { 0, EXPECTED_END }; // "0Ee" が返された場合は、TM モードとみなし "TM2" を送信する scip_response(urg, "TM2\n", tm2_expected, MAX_TIMEOUT, NULL, 0); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } if (ret <= 0) { if (ret == URG_INVALID_RESPONSE) { // 異常なエコーバックのときは、距離データ受信中とみなして // データを読み飛ばす ignore_receive_data_with_qt(urg, MAX_TIMEOUT); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } else { // 応答がないときは、ボーレートを変更して、再度接続を行う ignore_receive_data_with_qt(urg, MAX_TIMEOUT); continue; } } else if (!strcmp("00P", receive_buffer)) { // センサとホストのボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } return set_errno_and_return(urg, URG_NOT_DETECT_BAUDRATE_ERROR); } // PP コマンドの応答を urg_t に格納する static int receive_parameter(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * 9, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; int pp_expected[] = { 0, EXPECTED_END }; unsigned short received_bits = 0x0000; char *p; int i; int ret = scip_response(urg, "PP\n", pp_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); if (ret < 0) { return ret; } else if (ret < PP_RESPONSE_LINES) { ignore_receive_data_with_qt(urg, MAX_TIMEOUT); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } p = receive_buffer; for (i = 0; i < (ret - 1); ++i) { if (!strncmp(p, "DMIN:", 5)) { urg->min_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0001; } else if (!strncmp(p, "DMAX:", 5)) { urg->max_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0002; } else if (!strncmp(p, "ARES:", 5)) { urg->area_resolution = strtol(p + 5, NULL, 10); received_bits |= 0x0004; } else if (!strncmp(p, "AMIN:", 5)) { urg->first_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0008; } else if (!strncmp(p, "AMAX:", 5)) { urg->last_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0010; } else if (!strncmp(p, "AFRT:", 5)) { urg->front_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0020; } else if (!strncmp(p, "SCAN:", 5)) { int rpm = strtol(p + 5, NULL, 10); // タイムアウト時間は、計測周期の 16 倍程度の値にする urg->scan_usec = 1000 * 1000 * 60 / rpm; urg->timeout = urg->scan_usec >> (10 - 4); received_bits |= 0x0040; } p += strlen(p) + 1; } // 全てのパラメータを受信したか確認 if (received_bits != 0x007f) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } urg_set_scanning_parameter(urg, urg->first_data_index - urg->front_data_index, urg->last_data_index - urg->front_data_index, 1); return set_errno_and_return(urg, URG_NO_ERROR); } //! SCIP 文字列のデコード long urg_scip_decode(const char data[], int size) { const char* p = data; const char* last_p = p + size; int value = 0; while (p < last_p) { value <<= 6; value &= ~0x3f; value |= *p++ - 0x30; } return value; } static int parse_parameter(const char *parameter, int size) { char buffer[5]; memcpy(buffer, parameter, size); buffer[size] = '\0'; return strtol(buffer, NULL, 10); } static urg_measurement_type_t parse_distance_parameter(urg_t *urg, const char echoback[]) { urg_measurement_type_t ret_type = URG_UNKNOWN; urg->received_range_data_byte = URG_COMMUNICATION_3_BYTE; if (echoback[1] == 'S') { urg->received_range_data_byte = URG_COMMUNICATION_2_BYTE; ret_type = URG_DISTANCE; } else if (echoback[1] == 'D') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO; } } else if (echoback[1] == 'E') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE_INTENSITY; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO_INTENSITY; } } else { return URG_UNKNOWN; } // パラメータの格納 urg->received_first_index = parse_parameter(&echoback[2], 4); urg->received_last_index = parse_parameter(&echoback[6], 4); urg->received_skip_step = parse_parameter(&echoback[10], 2); return ret_type; } static urg_measurement_type_t parse_distance_echoback(urg_t *urg, const char echoback[]) { size_t line_length; urg_measurement_type_t ret_type = URG_UNKNOWN; if (!strcmp("QT", echoback)) { return URG_STOP; } line_length = strlen(echoback); if ((line_length == 12) && ((echoback[0] == 'G') || (echoback[0] == 'H'))) { ret_type = parse_distance_parameter(urg, echoback); } else if ((line_length == 15) && ((echoback[0] == 'M') || (echoback[0] == 'N'))) { ret_type = parse_distance_parameter(urg, echoback); } return ret_type; } static int receive_length_data(urg_t *urg, long length[], unsigned short intensity[], urg_measurement_type_t type, char buffer[]) { int n; int step_filled = 0; int line_filled = 0; int multiecho_index = 0; int each_size = (urg->received_range_data_byte == URG_COMMUNICATION_2_BYTE) ? 2 : 3; int data_size = each_size; int is_intensity = URG_FALSE; int is_multiecho = URG_FALSE; int multiecho_max_size = 1; if ((type == URG_DISTANCE_INTENSITY) || (type == URG_MULTIECHO_INTENSITY)) { data_size *= 2; is_intensity = URG_TRUE; } if ((type == URG_MULTIECHO) || (type == URG_MULTIECHO_INTENSITY)) { is_multiecho = URG_TRUE; multiecho_max_size = URG_MAX_ECHO; } do { char *p = buffer; char *last_p; n = connection_readline(&urg->connection, &buffer[line_filled], BUFFER_SIZE - line_filled, urg->timeout); if (n > 0) { // チェックサムの評価 if (buffer[line_filled + n - 1] != scip_checksum(&buffer[line_filled], n - 1)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } } if (n > 0) { line_filled += n - 1; } last_p = p + line_filled; while ((last_p - p) >= data_size) { int index; if (*p == '&') { // 先頭文字が '&' だったときは、マルチエコーのデータとみなす if ((last_p - (p + 1)) < data_size) { // '&' を除いて、data_size 分データが無ければ抜ける break; } --step_filled; ++multiecho_index; ++p; --line_filled; } else { // 次のデータ multiecho_index = 0; } index = (step_filled * multiecho_max_size) + multiecho_index; if (step_filled > (urg->received_last_index - urg->received_first_index)) { // データが多過ぎる場合は、残りのデータを無視して戻る ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (is_multiecho && (multiecho_index == 0)) { // マルチエコーのデータ格納先をダミーデータで埋める int i; if (length) { for (i = 1; i < multiecho_max_size; ++i) { length[index + i] = 0; } } if (intensity) { for (i = 1; i < multiecho_max_size; ++i) { intensity[index + i] = 0; } } } // 距離データの格納 if (length) { length[index] = urg_scip_decode(p, 3); } p += 3; // 強度データの格納 if (is_intensity) { if (intensity) { intensity[index] = (unsigned short)urg_scip_decode(p, 3); } p += 3; } ++step_filled; line_filled -= data_size; } // 次に処理する文字を退避 memmove(buffer, p, line_filled); } while (n > 0); return step_filled; } //! 距離データの取得 static int receive_data(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { urg_measurement_type_t type; char buffer[BUFFER_SIZE]; int ret = 0; int n; int extended_timeout = urg->timeout + 2 * (urg->scan_usec * (urg->scanning_skip_scan) / 1000); // エコーバックの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, extended_timeout); if (n <= 0) { return set_errno_and_return(urg, URG_NO_RESPONSE); } // エコーバックの解析 type = parse_distance_echoback(urg, buffer); // 応答の取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 3) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } if (buffer[n - 1] != scip_checksum(buffer, n - 1)) { // チェックサムの評価 ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } if (type == URG_STOP) { // QT 応答の場合には、最後の改行を読み捨て、正常応答として処理する n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n == 0) { return 0; } else { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } if (urg->specified_scan_times != 1) { if (!strncmp(buffer, "00", 2)) { // "00" 応答の場合は、エコーバック応答とみなし、 // 最後の空行を読み捨て、次からのデータを返す n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 0) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } } } if (((urg->specified_scan_times == 1) && (strncmp(buffer, "00", 2))) || ((urg->specified_scan_times != 1) && (strncmp(buffer, "99", 2)))) { if (urg->error_handler) { type = urg->error_handler(buffer, urg); } if (type == URG_UNKNOWN) { // Gx, Hx のときは 00P が返されたときがデータ // Mx, Nx のときは 99b が返されたときがデータ ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } // タイムスタンプの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n > 0) { if (time_stamp) { *time_stamp = urg_scip_decode(buffer, 4); } if (system_time_stamp) { urg_get_walltime(system_time_stamp); } } // データの取得 switch (type) { case URG_DISTANCE: case URG_MULTIECHO: ret = receive_length_data(urg, data, NULL, type, buffer); break; case URG_DISTANCE_INTENSITY: case URG_MULTIECHO_INTENSITY: ret = receive_length_data(urg, data, intensity, type, buffer); break; case URG_STOP: case URG_UNKNOWN: ret = 0; break; } // specified_scan_times == 1 のときは Gx 系コマンドが使われるため // データを明示的に停止しなくてよい if ((urg->specified_scan_times > 1) && (urg->scanning_remain_times > 0)) { if (--urg->scanning_remain_times <= 0) { // データの停止のみを行う urg_stop_measurement(urg); } } return ret; } int urg_open(urg_t *urg, urg_connection_type_t connection_type, const char *device_or_address, long baudrate_or_port) { int ret; long baudrate = baudrate_or_port; urg->is_active = URG_FALSE; urg->is_sending = URG_TRUE; urg->last_errno = URG_NOT_CONNECTED; urg->timeout = MAX_TIMEOUT; urg->scanning_skip_scan = 0; urg->error_handler = NULL; // デバイスへの接続 ret = connection_open(&urg->connection, connection_type, device_or_address, baudrate_or_port); if (ret < 0) { switch (connection_type) { case URG_SERIAL: urg->last_errno = URG_SERIAL_OPEN_ERROR; break; case URG_ETHERNET: urg->last_errno = URG_ETHERNET_OPEN_ERROR; break; default: urg->last_errno = URG_INVALID_RESPONSE; break; } return urg->last_errno; } // 指定したボーレートで URG と通信できるように調整 if (connection_type == URG_ETHERNET) { // Ethernet のときは仮の通信速度を指定しておく baudrate = 115200; } if (connect_urg_device(urg, baudrate) != URG_NO_ERROR) { return set_errno_and_return(urg, ret); } urg->is_sending = URG_FALSE; // 変数の初期化 urg->last_errno = URG_NO_ERROR; urg->range_data_byte = URG_COMMUNICATION_3_BYTE; urg->specified_scan_times = 0; urg->scanning_remain_times = 0; urg->is_laser_on = URG_FALSE; // パラメータ情報を取得 ret = receive_parameter(urg); if (ret == URG_NO_ERROR) { urg->is_active = URG_TRUE; } return ret; } void urg_close(urg_t *urg) { if (urg->is_active) { ignore_receive_data_with_qt(urg, urg->timeout); } connection_close(&urg->connection); urg->is_active = URG_FALSE; } void urg_set_timeout_msec(urg_t *urg, int msec) { urg->timeout = msec; } int urg_start_time_stamp_mode(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM0 を発行する n = scip_response(urg, "TM0\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } long urg_time_stamp(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; char buffer[BUFFER_SIZE]; char *p; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } ret = scip_response(urg, "TM1\n", expected, urg->timeout, buffer, BUFFER_SIZE); if (ret < 0) { return ret; } // buffer からタイムスタンプを取得し、デコードして返す if (strcmp(buffer, "00P")) { // 最初の応答が "00P" でなければ戻る return set_errno_and_return(urg, URG_RECEIVE_ERROR); } p = buffer + 4; if (strlen(p) != 5) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (p[5] == scip_checksum(p, 4)) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } return urg_scip_decode(p, 4); } int urg_stop_time_stamp_mode(urg_t *urg) { int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM2 を発行する n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } static int send_distance_command(urg_t *urg, int scan_times, int skip_scan, char single_scan_ch, char continuous_scan_ch, char scan_type_ch) { char buffer[BUFFER_SIZE]; int write_size = 0; int front_index = urg->front_data_index; int n; urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times; urg->scanning_remain_times = urg->specified_scan_times; urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan; if (scan_times >= 100) { // 計測回数が 99 を越える場合は、無限回のスキャンを行う urg->specified_scan_times = 0; } if (urg->scanning_remain_times == 1) { // レーザ発光を指示 urg_laser_on(urg); write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n", single_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step); } else { write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n", continuous_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step, skip_scan, urg->specified_scan_times); urg->is_sending = URG_TRUE; } n = connection_write(&urg->connection, buffer, write_size); if (n != write_size) { return set_errno_and_return(urg, URG_SEND_ERROR); } return 0; } int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan) { char range_byte_ch; int ret = 0; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((skip_scan < 0) || (skip_scan > 9)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // !!! Mx 系, Nx 系の計測中のときは、QT を発行してから // !!! 計測開始コマンドを送信するようにする // !!! ただし、MD 計測中に MD を発行するように、同じコマンドの場合は // !!! Mx 系, Nx 系の計測は上書きすることができるようにする // 指定されたタイプのパケットを生成し、送信する switch (type) { case URG_DISTANCE: range_byte_ch = (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D'; ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', range_byte_ch); break; case URG_DISTANCE_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', 'E'); break; case URG_MULTIECHO: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'D'); break; case URG_MULTIECHO_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'E'); break; case URG_STOP: case URG_UNKNOWN: default: ignore_receive_data_with_qt(urg, urg->timeout); urg->last_errno = URG_INVALID_PARAMETER; ret = urg->last_errno; break; } return ret; } int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, NULL, time_stamp, system_time_stamp); } int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp); } int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp); } int urg_stop_measurement(urg_t *urg) { enum { MAX_READ_TIMES = 3 }; int ret = URG_INVALID_RESPONSE; int n; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // QT を発行する n = connection_write(&urg->connection, "QT\n", 3); if (n != 3) { return set_errno_and_return(urg, URG_SEND_ERROR); } for (i = 0; i < MAX_READ_TIMES; ++i) { // QT の応答が返されるまで、距離データを読み捨てる ret = receive_data(urg, NULL, NULL, NULL, NULL); if (ret == URG_NO_ERROR) { // 正常応答 urg->is_laser_on = URG_FALSE; urg->is_sending = URG_FALSE; return set_errno_and_return(urg, URG_NO_ERROR); } } return ret; } int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step) { // 設定の範囲外を指定したときは、エラーを返す if (((skip_step < 0) || (skip_step >= 100)) || (first_step > last_step) || (first_step < -urg->front_data_index) || (last_step > (urg->last_data_index - urg->front_data_index))) { return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR); } urg->scanning_first_step = first_step; urg->scanning_last_step = last_step; urg->scanning_skip_step = skip_step; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_set_connection_data_size(urg_t *urg, urg_range_data_byte_t data_byte) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((data_byte != URG_COMMUNICATION_3_BYTE) || (data_byte != URG_COMMUNICATION_2_BYTE)) { return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR); } urg->range_data_byte = data_byte; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_laser_on(urg_t *urg) { int expected[] = { 0, 2, EXPECTED_END }; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if (urg->is_laser_on != URG_FALSE) { // 既にレーザが発光しているときは、コマンドを送信しないようにする urg->last_errno = 0; return urg->last_errno; } ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0); if (ret >= 0) { urg->is_laser_on = URG_TRUE; ret = 0; } return ret; } int urg_laser_off(urg_t *urg) { return urg_stop_measurement(urg); } int urg_reboot(urg_t *urg) { int expected[] = { 0, 1, EXPECTED_END }; int ret; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // ２回目の RB 送信後、接続を切断する for (i = 0; i < 2; ++i) { ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0); if (ret < 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } urg->is_active = URG_FALSE; urg_close(urg); urg->last_errno = 0; return urg->last_errno; } void urg_sleep(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = 4 }; int sl_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE]; if (urg_stop_measurement(urg) != URG_NO_ERROR) { return; } scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); } void urg_wakeup(urg_t *urg) { urg_stop_measurement(urg); } int urg_is_stable(urg_t *urg) { const char *stat = urg_sensor_status(urg); return strncmp("Stable", stat, 6) ? 0 : 1; } static char *copy_token(char *dest, char *receive_buffer, const char *start_str, const char *end_ch, int lines) { size_t start_str_len = strlen(start_str); size_t end_ch_len = strlen(end_ch); int i; size_t j; for (j = 0; j < end_ch_len; ++j) { const char *p = receive_buffer; for (i = 0; i < lines; ++i) { if (!strncmp(p, start_str, start_str_len)) { char *last_p = strchr(p + start_str_len, end_ch[j]); if (last_p) { *last_p = '\0'; memcpy(dest, p + start_str_len, last_p - (p + start_str_len) + 1); return dest; } } p += strlen(p) + 1; } } return NULL; } static const char *receive_command_response(urg_t *urg, char *buffer, int buffer_size, const char* command, int response_lines) { const int vv_expected[] = { 0, EXPECTED_END }; int ret; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = scip_response(urg, command, vv_expected, urg->timeout, buffer, buffer_size); if (ret < response_lines) { return RECEIVE_ERROR_MESSAGE; } return NULL; } const char *urg_sensor_product_type(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROD:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_serial_id(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "SERI:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_vendor(urg_t *urg){ enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "VEND:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", "(", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_date(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } // Get the firmware version and append a '(', this will be what's before the date char firmware_version[50]; strcpy(firmware_version, urg_sensor_firmware_version(urg)); strcat(firmware_version, "("); ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } // Strip out the actual date from between the '(' and ')' char *date; p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", ";", VV_RESPONSE_LINES); date = copy_token(urg->return_buffer, p, firmware_version, ")", 1); return (date) ? date : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_protocol_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROT:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_status(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "STAT:", ";", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_state(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "MESM:", " (", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } void urg_set_error_handler(urg_t *urg, urg_error_handler handler) { urg->error_handler = handler; }
119  if (line_number == 1) {
120  if (n == 1) {
121  // SCIP 1.1 応答の場合は、正常応答とみなす
122  ret = 0;
123 
124  } else if (n != 3) {
125  return set_errno_and_return(urg, URG_INVALID_RESPONSE);
126 
127  } else {
128  int i;
129  int actual_ret = strtol(buffer, NULL, 10);
130  for (i = 0; expected_ret[i] != EXPECTED_END; ++i) {
131  if (expected_ret[i] == actual_ret) {
132  ret = 0;
133  break;
134  }
135  }
136  }
137  }
138 
139  ++line_number;
140  } while (n > 0);
141 
142  return (ret < 0) ? ret : (line_number - 1);
143 }
144 
145 
146 static void ignore_receive_data(urg_t *urg, int timeout)
147 {
148  char buffer[BUFFER_SIZE];
149  int n;
150 
151  if (urg->is_sending == URG_FALSE) {
152  return;
153  }
154 
155  do {
156  n = connection_readline(&urg->connection,
157  buffer, BUFFER_SIZE, timeout);
158  } while (n >= 0);
159 
160  urg->is_sending = URG_FALSE;
161 }
162 
163 
164 static void ignore_receive_data_with_qt(urg_t *urg, int timeout)
165 {
166  if ((urg->is_sending == URG_FALSE) && (urg->is_laser_on == URG_FALSE)) {
167  return;
168  }
169 
170  connection_write(&urg->connection, "QT\n", 3);
171  urg->is_sending = URG_TRUE;
172  urg->is_laser_on = URG_FALSE;
173  ignore_receive_data(urg, timeout);
174 }
175 
176 
177 static int change_sensor_baudrate(urg_t *urg,
178  long current_baudrate, long next_baudrate)
179 {
180  enum { SS_COMMAND_SIZE = 10 };
181  char buffer[SS_COMMAND_SIZE];
182  int ss_expected[] = { 0, 3, 4, EXPECTED_END };
183  int ret;
184 
185  if (current_baudrate == next_baudrate) {
186  // 現在のボーレートと設定するボーレートが一緒ならば、戻る return set_errno_and_return(urg, URG_NO_ERROR); } // "SS" コマンドでボーレートを変更する snprintf(buffer, SS_COMMAND_SIZE, "SS%06ld\n", next_baudrate); ret = scip_response(urg, buffer, ss_expected, urg->timeout, NULL, 0); // 0F 応答のときは Ethernet 用のセンサとみなし、正常応答を返す if (ret == -15) { return set_errno_and_return(urg, URG_NO_ERROR); } if (ret <= 0) { return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // 正常応答ならば、ホスト側のボーレートを変更する ret = connection_set_baudrate(&urg->connection, next_baudrate); // センサ側の設定反映を待つために少しだけ待機する ignore_receive_data(urg, MAX_TIMEOUT); return set_errno_and_return(urg, ret); } // ボーレートを変更しながら接続する static int connect_urg_device(urg_t *urg, long baudrate) { long try_baudrate[] = { 19200, 38400, 115200 }; int try_times = sizeof(try_baudrate) / sizeof(try_baudrate[0]); int i; // 指示されたボーレートから接続する for (i = 0; i < try_times; ++i) { if (try_baudrate[i] == baudrate) { try_baudrate[i] = try_baudrate[0]; try_baudrate[0] = baudrate; break; } } for (i = 0; i < try_times; ++i) { enum { RECEIVE_BUFFER_SIZE = 4 }; int qt_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE + 1]; int ret; connection_set_baudrate(&urg->connection, try_baudrate[i]); // QT を送信し、応答が返されるかでボーレートが一致しているかを確認する ret = scip_response(urg, "QT\n", qt_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); if (ret > 0) { if (!strcmp(receive_buffer, "E")) { int scip20_expected[] = { 0, EXPECTED_END }; // QT 応答の最後の改行を読み飛ばす ignore_receive_data(urg, MAX_TIMEOUT); // "E" が返された場合は、SCIP 1.1 とみなし "SCIP2.0" を送信する ret = scip_response(urg, "SCIP2.0\n", scip20_expected, MAX_TIMEOUT, NULL, 0); // SCIP2.0 応答の最後の改行を読み飛ばす ignore_receive_data(urg, MAX_TIMEOUT); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } else if (!strcmp(receive_buffer, "0Ee")) { int tm2_expected[] = { 0, EXPECTED_END }; // "0Ee" が返された場合は、TM モードとみなし "TM2" を送信する scip_response(urg, "TM2\n", tm2_expected, MAX_TIMEOUT, NULL, 0); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } if (ret <= 0) { if (ret == URG_INVALID_RESPONSE) { // 異常なエコーバックのときは、距離データ受信中とみなして // データを読み飛ばす ignore_receive_data_with_qt(urg, MAX_TIMEOUT); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } else { // 応答がないときは、ボーレートを変更して、再度接続を行う ignore_receive_data_with_qt(urg, MAX_TIMEOUT); continue; } } else if (!strcmp("00P", receive_buffer)) { // センサとホストのボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } return set_errno_and_return(urg, URG_NOT_DETECT_BAUDRATE_ERROR); } // PP コマンドの応答を urg_t に格納する static int receive_parameter(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * 9, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; int pp_expected[] = { 0, EXPECTED_END }; unsigned short received_bits = 0x0000; char *p; int i; int ret = scip_response(urg, "PP\n", pp_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); if (ret < 0) { return ret; } else if (ret < PP_RESPONSE_LINES) { ignore_receive_data_with_qt(urg, MAX_TIMEOUT); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } p = receive_buffer; for (i = 0; i < (ret - 1); ++i) { if (!strncmp(p, "DMIN:", 5)) { urg->min_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0001; } else if (!strncmp(p, "DMAX:", 5)) { urg->max_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0002; } else if (!strncmp(p, "ARES:", 5)) { urg->area_resolution = strtol(p + 5, NULL, 10); received_bits |= 0x0004; } else if (!strncmp(p, "AMIN:", 5)) { urg->first_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0008; } else if (!strncmp(p, "AMAX:", 5)) { urg->last_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0010; } else if (!strncmp(p, "AFRT:", 5)) { urg->front_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0020; } else if (!strncmp(p, "SCAN:", 5)) { int rpm = strtol(p + 5, NULL, 10); // タイムアウト時間は、計測周期の 16 倍程度の値にする urg->scan_usec = 1000 * 1000 * 60 / rpm; urg->timeout = urg->scan_usec >> (10 - 4); received_bits |= 0x0040; } p += strlen(p) + 1; } // 全てのパラメータを受信したか確認 if (received_bits != 0x007f) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } urg_set_scanning_parameter(urg, urg->first_data_index - urg->front_data_index, urg->last_data_index - urg->front_data_index, 1); return set_errno_and_return(urg, URG_NO_ERROR); } //! SCIP 文字列のデコード long urg_scip_decode(const char data[], int size) { const char* p = data; const char* last_p = p + size; int value = 0; while (p < last_p) { value <<= 6; value &= ~0x3f; value |= *p++ - 0x30; } return value; } static int parse_parameter(const char *parameter, int size) { char buffer[5]; memcpy(buffer, parameter, size); buffer[size] = '\0'; return strtol(buffer, NULL, 10); } static urg_measurement_type_t parse_distance_parameter(urg_t *urg, const char echoback[]) { urg_measurement_type_t ret_type = URG_UNKNOWN; urg->received_range_data_byte = URG_COMMUNICATION_3_BYTE; if (echoback[1] == 'S') { urg->received_range_data_byte = URG_COMMUNICATION_2_BYTE; ret_type = URG_DISTANCE; } else if (echoback[1] == 'D') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO; } } else if (echoback[1] == 'E') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE_INTENSITY; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO_INTENSITY; } } else { return URG_UNKNOWN; } // パラメータの格納 urg->received_first_index = parse_parameter(&echoback[2], 4); urg->received_last_index = parse_parameter(&echoback[6], 4); urg->received_skip_step = parse_parameter(&echoback[10], 2); return ret_type; } static urg_measurement_type_t parse_distance_echoback(urg_t *urg, const char echoback[]) { size_t line_length; urg_measurement_type_t ret_type = URG_UNKNOWN; if (!strcmp("QT", echoback)) { return URG_STOP; } line_length = strlen(echoback); if ((line_length == 12) && ((echoback[0] == 'G') || (echoback[0] == 'H'))) { ret_type = parse_distance_parameter(urg, echoback); } else if ((line_length == 15) && ((echoback[0] == 'M') || (echoback[0] == 'N'))) { ret_type = parse_distance_parameter(urg, echoback); } return ret_type; } static int receive_length_data(urg_t *urg, long length[], unsigned short intensity[], urg_measurement_type_t type, char buffer[]) { int n; int step_filled = 0; int line_filled = 0; int multiecho_index = 0; int each_size = (urg->received_range_data_byte == URG_COMMUNICATION_2_BYTE) ? 2 : 3; int data_size = each_size; int is_intensity = URG_FALSE; int is_multiecho = URG_FALSE; int multiecho_max_size = 1; if ((type == URG_DISTANCE_INTENSITY) || (type == URG_MULTIECHO_INTENSITY)) { data_size *= 2; is_intensity = URG_TRUE; } if ((type == URG_MULTIECHO) || (type == URG_MULTIECHO_INTENSITY)) { is_multiecho = URG_TRUE; multiecho_max_size = URG_MAX_ECHO; } do { char *p = buffer; char *last_p; n = connection_readline(&urg->connection, &buffer[line_filled], BUFFER_SIZE - line_filled, urg->timeout); if (n > 0) { // チェックサムの評価 if (buffer[line_filled + n - 1] != scip_checksum(&buffer[line_filled], n - 1)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } } if (n > 0) { line_filled += n - 1; } last_p = p + line_filled; while ((last_p - p) >= data_size) { int index; if (*p == '&') { // 先頭文字が '&' だったときは、マルチエコーのデータとみなす if ((last_p - (p + 1)) < data_size) { // '&' を除いて、data_size 分データが無ければ抜ける break; } --step_filled; ++multiecho_index; ++p; --line_filled; } else { // 次のデータ multiecho_index = 0; } index = (step_filled * multiecho_max_size) + multiecho_index; if (step_filled > (urg->received_last_index - urg->received_first_index)) { // データが多過ぎる場合は、残りのデータを無視して戻る ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (is_multiecho && (multiecho_index == 0)) { // マルチエコーのデータ格納先をダミーデータで埋める int i; if (length) { for (i = 1; i < multiecho_max_size; ++i) { length[index + i] = 0; } } if (intensity) { for (i = 1; i < multiecho_max_size; ++i) { intensity[index + i] = 0; } } } // 距離データの格納 if (length) { length[index] = urg_scip_decode(p, 3); } p += 3; // 強度データの格納 if (is_intensity) { if (intensity) { intensity[index] = (unsigned short)urg_scip_decode(p, 3); } p += 3; } ++step_filled; line_filled -= data_size; } // 次に処理する文字を退避 memmove(buffer, p, line_filled); } while (n > 0); return step_filled; } //! 距離データの取得 static int receive_data(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { urg_measurement_type_t type; char buffer[BUFFER_SIZE]; int ret = 0; int n; int extended_timeout = urg->timeout + 2 * (urg->scan_usec * (urg->scanning_skip_scan) / 1000); // エコーバックの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, extended_timeout); if (n <= 0) { return set_errno_and_return(urg, URG_NO_RESPONSE); } // エコーバックの解析 type = parse_distance_echoback(urg, buffer); // 応答の取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 3) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } if (buffer[n - 1] != scip_checksum(buffer, n - 1)) { // チェックサムの評価 ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } if (type == URG_STOP) { // QT 応答の場合には、最後の改行を読み捨て、正常応答として処理する n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n == 0) { return 0; } else { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } if (urg->specified_scan_times != 1) { if (!strncmp(buffer, "00", 2)) { // "00" 応答の場合は、エコーバック応答とみなし、 // 最後の空行を読み捨て、次からのデータを返す n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 0) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } } } if (((urg->specified_scan_times == 1) && (strncmp(buffer, "00", 2))) || ((urg->specified_scan_times != 1) && (strncmp(buffer, "99", 2)))) { if (urg->error_handler) { type = urg->error_handler(buffer, urg); } if (type == URG_UNKNOWN) { // Gx, Hx のときは 00P が返されたときがデータ // Mx, Nx のときは 99b が返されたときがデータ ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } // タイムスタンプの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n > 0) { if (time_stamp) { *time_stamp = urg_scip_decode(buffer, 4); } if (system_time_stamp) { urg_get_walltime(system_time_stamp); } } // データの取得 switch (type) { case URG_DISTANCE: case URG_MULTIECHO: ret = receive_length_data(urg, data, NULL, type, buffer); break; case URG_DISTANCE_INTENSITY: case URG_MULTIECHO_INTENSITY: ret = receive_length_data(urg, data, intensity, type, buffer); break; case URG_STOP: case URG_UNKNOWN: ret = 0; break; } // specified_scan_times == 1 のときは Gx 系コマンドが使われるため // データを明示的に停止しなくてよい if ((urg->specified_scan_times > 1) && (urg->scanning_remain_times > 0)) { if (--urg->scanning_remain_times <= 0) { // データの停止のみを行う urg_stop_measurement(urg); } } return ret; } int urg_open(urg_t *urg, urg_connection_type_t connection_type, const char *device_or_address, long baudrate_or_port) { int ret; long baudrate = baudrate_or_port; urg->is_active = URG_FALSE; urg->is_sending = URG_TRUE; urg->last_errno = URG_NOT_CONNECTED; urg->timeout = MAX_TIMEOUT; urg->scanning_skip_scan = 0; urg->error_handler = NULL; // デバイスへの接続 ret = connection_open(&urg->connection, connection_type, device_or_address, baudrate_or_port); if (ret < 0) { switch (connection_type) { case URG_SERIAL: urg->last_errno = URG_SERIAL_OPEN_ERROR; break; case URG_ETHERNET: urg->last_errno = URG_ETHERNET_OPEN_ERROR; break; default: urg->last_errno = URG_INVALID_RESPONSE; break; } return urg->last_errno; } // 指定したボーレートで URG と通信できるように調整 if (connection_type == URG_ETHERNET) { // Ethernet のときは仮の通信速度を指定しておく baudrate = 115200; } if (connect_urg_device(urg, baudrate) != URG_NO_ERROR) { return set_errno_and_return(urg, ret); } urg->is_sending = URG_FALSE; // 変数の初期化 urg->last_errno = URG_NO_ERROR; urg->range_data_byte = URG_COMMUNICATION_3_BYTE; urg->specified_scan_times = 0; urg->scanning_remain_times = 0; urg->is_laser_on = URG_FALSE; // パラメータ情報を取得 ret = receive_parameter(urg); if (ret == URG_NO_ERROR) { urg->is_active = URG_TRUE; } return ret; } void urg_close(urg_t *urg) { if (urg->is_active) { ignore_receive_data_with_qt(urg, urg->timeout); } connection_close(&urg->connection); urg->is_active = URG_FALSE; } void urg_set_timeout_msec(urg_t *urg, int msec) { urg->timeout = msec; } int urg_start_time_stamp_mode(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM0 を発行する n = scip_response(urg, "TM0\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } long urg_time_stamp(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; char buffer[BUFFER_SIZE]; char *p; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } ret = scip_response(urg, "TM1\n", expected, urg->timeout, buffer, BUFFER_SIZE); if (ret < 0) { return ret; } // buffer からタイムスタンプを取得し、デコードして返す if (strcmp(buffer, "00P")) { // 最初の応答が "00P" でなければ戻る return set_errno_and_return(urg, URG_RECEIVE_ERROR); } p = buffer + 4; if (strlen(p) != 5) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (p[5] == scip_checksum(p, 4)) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } return urg_scip_decode(p, 4); } int urg_stop_time_stamp_mode(urg_t *urg) { int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM2 を発行する n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } static int send_distance_command(urg_t *urg, int scan_times, int skip_scan, char single_scan_ch, char continuous_scan_ch, char scan_type_ch) { char buffer[BUFFER_SIZE]; int write_size = 0; int front_index = urg->front_data_index; int n; urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times; urg->scanning_remain_times = urg->specified_scan_times; urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan; if (scan_times >= 100) { // 計測回数が 99 を越える場合は、無限回のスキャンを行う urg->specified_scan_times = 0; } if (urg->scanning_remain_times == 1) { // レーザ発光を指示 urg_laser_on(urg); write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n", single_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step); } else { write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n", continuous_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step, skip_scan, urg->specified_scan_times); urg->is_sending = URG_TRUE; } n = connection_write(&urg->connection, buffer, write_size); if (n != write_size) { return set_errno_and_return(urg, URG_SEND_ERROR); } return 0; } int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan) { char range_byte_ch; int ret = 0; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((skip_scan < 0) || (skip_scan > 9)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // !!! Mx 系, Nx 系の計測中のときは、QT を発行してから // !!! 計測開始コマンドを送信するようにする // !!! ただし、MD 計測中に MD を発行するように、同じコマンドの場合は // !!! Mx 系, Nx 系の計測は上書きすることができるようにする // 指定されたタイプのパケットを生成し、送信する switch (type) { case URG_DISTANCE: range_byte_ch = (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D'; ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', range_byte_ch); break; case URG_DISTANCE_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', 'E'); break; case URG_MULTIECHO: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'D'); break; case URG_MULTIECHO_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'E'); break; case URG_STOP: case URG_UNKNOWN: default: ignore_receive_data_with_qt(urg, urg->timeout); urg->last_errno = URG_INVALID_PARAMETER; ret = urg->last_errno; break; } return ret; } int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, NULL, time_stamp, system_time_stamp); } int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp); } int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp); } int urg_stop_measurement(urg_t *urg) { enum { MAX_READ_TIMES = 3 }; int ret = URG_INVALID_RESPONSE; int n; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // QT を発行する n = connection_write(&urg->connection, "QT\n", 3); if (n != 3) { return set_errno_and_return(urg, URG_SEND_ERROR); } for (i = 0; i < MAX_READ_TIMES; ++i) { // QT の応答が返されるまで、距離データを読み捨てる ret = receive_data(urg, NULL, NULL, NULL, NULL); if (ret == URG_NO_ERROR) { // 正常応答 urg->is_laser_on = URG_FALSE; urg->is_sending = URG_FALSE; return set_errno_and_return(urg, URG_NO_ERROR); } } return ret; } int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step) { // 設定の範囲外を指定したときは、エラーを返す if (((skip_step < 0) || (skip_step >= 100)) || (first_step > last_step) || (first_step < -urg->front_data_index) || (last_step > (urg->last_data_index - urg->front_data_index))) { return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR); } urg->scanning_first_step = first_step; urg->scanning_last_step = last_step; urg->scanning_skip_step = skip_step; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_set_connection_data_size(urg_t *urg, urg_range_data_byte_t data_byte) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((data_byte != URG_COMMUNICATION_3_BYTE) || (data_byte != URG_COMMUNICATION_2_BYTE)) { return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR); } urg->range_data_byte = data_byte; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_laser_on(urg_t *urg) { int expected[] = { 0, 2, EXPECTED_END }; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if (urg->is_laser_on != URG_FALSE) { // 既にレーザが発光しているときは、コマンドを送信しないようにする urg->last_errno = 0; return urg->last_errno; } ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0); if (ret >= 0) { urg->is_laser_on = URG_TRUE; ret = 0; } return ret; } int urg_laser_off(urg_t *urg) { return urg_stop_measurement(urg); } int urg_reboot(urg_t *urg) { int expected[] = { 0, 1, EXPECTED_END }; int ret; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // ２回目の RB 送信後、接続を切断する for (i = 0; i < 2; ++i) { ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0); if (ret < 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } urg->is_active = URG_FALSE; urg_close(urg); urg->last_errno = 0; return urg->last_errno; } void urg_sleep(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = 4 }; int sl_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE]; if (urg_stop_measurement(urg) != URG_NO_ERROR) { return; } scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); } void urg_wakeup(urg_t *urg) { urg_stop_measurement(urg); } int urg_is_stable(urg_t *urg) { const char *stat = urg_sensor_status(urg); return strncmp("Stable", stat, 6) ? 0 : 1; } static char *copy_token(char *dest, char *receive_buffer, const char *start_str, const char *end_ch, int lines) { size_t start_str_len = strlen(start_str); size_t end_ch_len = strlen(end_ch); int i; size_t j; for (j = 0; j < end_ch_len; ++j) { const char *p = receive_buffer; for (i = 0; i < lines; ++i) { if (!strncmp(p, start_str, start_str_len)) { char *last_p = strchr(p + start_str_len, end_ch[j]); if (last_p) { *last_p = '\0'; memcpy(dest, p + start_str_len, last_p - (p + start_str_len) + 1); return dest; } } p += strlen(p) + 1; } } return NULL; } static const char *receive_command_response(urg_t *urg, char *buffer, int buffer_size, const char* command, int response_lines) { const int vv_expected[] = { 0, EXPECTED_END }; int ret; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = scip_response(urg, command, vv_expected, urg->timeout, buffer, buffer_size); if (ret < response_lines) { return RECEIVE_ERROR_MESSAGE; } return NULL; } const char *urg_sensor_product_type(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROD:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_serial_id(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "SERI:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_vendor(urg_t *urg){ enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "VEND:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", "(", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_date(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } // Get the firmware version and append a '(', this will be what's before the date char firmware_version[50]; strcpy(firmware_version, urg_sensor_firmware_version(urg)); strcat(firmware_version, "("); ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } // Strip out the actual date from between the '(' and ')' char *date; p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", ";", VV_RESPONSE_LINES); date = copy_token(urg->return_buffer, p, firmware_version, ")", 1); return (date) ? date : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_protocol_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROT:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_status(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "STAT:", ";", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_state(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "MESM:", " (", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } void urg_set_error_handler(urg_t *urg, urg_error_handler handler) { urg->error_handler = handler; }
187  return set_errno_and_return(urg, URG_NO_ERROR);
188  }
189 
190  // "SS" コマンドでボーレートを変更する snprintf(buffer, SS_COMMAND_SIZE, "SS%06ld\n", next_baudrate); ret = scip_response(urg, buffer, ss_expected, urg->timeout, NULL, 0); // 0F 応答のときは Ethernet 用のセンサとみなし、正常応答を返す if (ret == -15) { return set_errno_and_return(urg, URG_NO_ERROR); } if (ret <= 0) { return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // 正常応答ならば、ホスト側のボーレートを変更する ret = connection_set_baudrate(&urg->connection, next_baudrate); // センサ側の設定反映を待つために少しだけ待機する ignore_receive_data(urg, MAX_TIMEOUT); return set_errno_and_return(urg, ret); } // ボーレートを変更しながら接続する static int connect_urg_device(urg_t *urg, long baudrate) { long try_baudrate[] = { 19200, 38400, 115200 }; int try_times = sizeof(try_baudrate) / sizeof(try_baudrate[0]); int i; // 指示されたボーレートから接続する for (i = 0; i < try_times; ++i) { if (try_baudrate[i] == baudrate) { try_baudrate[i] = try_baudrate[0]; try_baudrate[0] = baudrate; break; } } for (i = 0; i < try_times; ++i) { enum { RECEIVE_BUFFER_SIZE = 4 }; int qt_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE + 1]; int ret; connection_set_baudrate(&urg->connection, try_baudrate[i]); // QT を送信し、応答が返されるかでボーレートが一致しているかを確認する ret = scip_response(urg, "QT\n", qt_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); if (ret > 0) { if (!strcmp(receive_buffer, "E")) { int scip20_expected[] = { 0, EXPECTED_END }; // QT 応答の最後の改行を読み飛ばす ignore_receive_data(urg, MAX_TIMEOUT); // "E" が返された場合は、SCIP 1.1 とみなし "SCIP2.0" を送信する ret = scip_response(urg, "SCIP2.0\n", scip20_expected, MAX_TIMEOUT, NULL, 0); // SCIP2.0 応答の最後の改行を読み飛ばす ignore_receive_data(urg, MAX_TIMEOUT); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } else if (!strcmp(receive_buffer, "0Ee")) { int tm2_expected[] = { 0, EXPECTED_END }; // "0Ee" が返された場合は、TM モードとみなし "TM2" を送信する scip_response(urg, "TM2\n", tm2_expected, MAX_TIMEOUT, NULL, 0); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } if (ret <= 0) { if (ret == URG_INVALID_RESPONSE) { // 異常なエコーバックのときは、距離データ受信中とみなして // データを読み飛ばす ignore_receive_data_with_qt(urg, MAX_TIMEOUT); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } else { // 応答がないときは、ボーレートを変更して、再度接続を行う ignore_receive_data_with_qt(urg, MAX_TIMEOUT); continue; } } else if (!strcmp("00P", receive_buffer)) { // センサとホストのボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } return set_errno_and_return(urg, URG_NOT_DETECT_BAUDRATE_ERROR); } // PP コマンドの応答を urg_t に格納する static int receive_parameter(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * 9, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; int pp_expected[] = { 0, EXPECTED_END }; unsigned short received_bits = 0x0000; char *p; int i; int ret = scip_response(urg, "PP\n", pp_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); if (ret < 0) { return ret; } else if (ret < PP_RESPONSE_LINES) { ignore_receive_data_with_qt(urg, MAX_TIMEOUT); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } p = receive_buffer; for (i = 0; i < (ret - 1); ++i) { if (!strncmp(p, "DMIN:", 5)) { urg->min_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0001; } else if (!strncmp(p, "DMAX:", 5)) { urg->max_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0002; } else if (!strncmp(p, "ARES:", 5)) { urg->area_resolution = strtol(p + 5, NULL, 10); received_bits |= 0x0004; } else if (!strncmp(p, "AMIN:", 5)) { urg->first_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0008; } else if (!strncmp(p, "AMAX:", 5)) { urg->last_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0010; } else if (!strncmp(p, "AFRT:", 5)) { urg->front_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0020; } else if (!strncmp(p, "SCAN:", 5)) { int rpm = strtol(p + 5, NULL, 10); // タイムアウト時間は、計測周期の 16 倍程度の値にする urg->scan_usec = 1000 * 1000 * 60 / rpm; urg->timeout = urg->scan_usec >> (10 - 4); received_bits |= 0x0040; } p += strlen(p) + 1; } // 全てのパラメータを受信したか確認 if (received_bits != 0x007f) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } urg_set_scanning_parameter(urg, urg->first_data_index - urg->front_data_index, urg->last_data_index - urg->front_data_index, 1); return set_errno_and_return(urg, URG_NO_ERROR); } //! SCIP 文字列のデコード long urg_scip_decode(const char data[], int size) { const char* p = data; const char* last_p = p + size; int value = 0; while (p < last_p) { value <<= 6; value &= ~0x3f; value |= *p++ - 0x30; } return value; } static int parse_parameter(const char *parameter, int size) { char buffer[5]; memcpy(buffer, parameter, size); buffer[size] = '\0'; return strtol(buffer, NULL, 10); } static urg_measurement_type_t parse_distance_parameter(urg_t *urg, const char echoback[]) { urg_measurement_type_t ret_type = URG_UNKNOWN; urg->received_range_data_byte = URG_COMMUNICATION_3_BYTE; if (echoback[1] == 'S') { urg->received_range_data_byte = URG_COMMUNICATION_2_BYTE; ret_type = URG_DISTANCE; } else if (echoback[1] == 'D') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO; } } else if (echoback[1] == 'E') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE_INTENSITY; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO_INTENSITY; } } else { return URG_UNKNOWN; } // パラメータの格納 urg->received_first_index = parse_parameter(&echoback[2], 4); urg->received_last_index = parse_parameter(&echoback[6], 4); urg->received_skip_step = parse_parameter(&echoback[10], 2); return ret_type; } static urg_measurement_type_t parse_distance_echoback(urg_t *urg, const char echoback[]) { size_t line_length; urg_measurement_type_t ret_type = URG_UNKNOWN; if (!strcmp("QT", echoback)) { return URG_STOP; } line_length = strlen(echoback); if ((line_length == 12) && ((echoback[0] == 'G') || (echoback[0] == 'H'))) { ret_type = parse_distance_parameter(urg, echoback); } else if ((line_length == 15) && ((echoback[0] == 'M') || (echoback[0] == 'N'))) { ret_type = parse_distance_parameter(urg, echoback); } return ret_type; } static int receive_length_data(urg_t *urg, long length[], unsigned short intensity[], urg_measurement_type_t type, char buffer[]) { int n; int step_filled = 0; int line_filled = 0; int multiecho_index = 0; int each_size = (urg->received_range_data_byte == URG_COMMUNICATION_2_BYTE) ? 2 : 3; int data_size = each_size; int is_intensity = URG_FALSE; int is_multiecho = URG_FALSE; int multiecho_max_size = 1; if ((type == URG_DISTANCE_INTENSITY) || (type == URG_MULTIECHO_INTENSITY)) { data_size *= 2; is_intensity = URG_TRUE; } if ((type == URG_MULTIECHO) || (type == URG_MULTIECHO_INTENSITY)) { is_multiecho = URG_TRUE; multiecho_max_size = URG_MAX_ECHO; } do { char *p = buffer; char *last_p; n = connection_readline(&urg->connection, &buffer[line_filled], BUFFER_SIZE - line_filled, urg->timeout); if (n > 0) { // チェックサムの評価 if (buffer[line_filled + n - 1] != scip_checksum(&buffer[line_filled], n - 1)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } } if (n > 0) { line_filled += n - 1; } last_p = p + line_filled; while ((last_p - p) >= data_size) { int index; if (*p == '&') { // 先頭文字が '&' だったときは、マルチエコーのデータとみなす if ((last_p - (p + 1)) < data_size) { // '&' を除いて、data_size 分データが無ければ抜ける break; } --step_filled; ++multiecho_index; ++p; --line_filled; } else { // 次のデータ multiecho_index = 0; } index = (step_filled * multiecho_max_size) + multiecho_index; if (step_filled > (urg->received_last_index - urg->received_first_index)) { // データが多過ぎる場合は、残りのデータを無視して戻る ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (is_multiecho && (multiecho_index == 0)) { // マルチエコーのデータ格納先をダミーデータで埋める int i; if (length) { for (i = 1; i < multiecho_max_size; ++i) { length[index + i] = 0; } } if (intensity) { for (i = 1; i < multiecho_max_size; ++i) { intensity[index + i] = 0; } } } // 距離データの格納 if (length) { length[index] = urg_scip_decode(p, 3); } p += 3; // 強度データの格納 if (is_intensity) { if (intensity) { intensity[index] = (unsigned short)urg_scip_decode(p, 3); } p += 3; } ++step_filled; line_filled -= data_size; } // 次に処理する文字を退避 memmove(buffer, p, line_filled); } while (n > 0); return step_filled; } //! 距離データの取得 static int receive_data(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { urg_measurement_type_t type; char buffer[BUFFER_SIZE]; int ret = 0; int n; int extended_timeout = urg->timeout + 2 * (urg->scan_usec * (urg->scanning_skip_scan) / 1000); // エコーバックの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, extended_timeout); if (n <= 0) { return set_errno_and_return(urg, URG_NO_RESPONSE); } // エコーバックの解析 type = parse_distance_echoback(urg, buffer); // 応答の取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 3) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } if (buffer[n - 1] != scip_checksum(buffer, n - 1)) { // チェックサムの評価 ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } if (type == URG_STOP) { // QT 応答の場合には、最後の改行を読み捨て、正常応答として処理する n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n == 0) { return 0; } else { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } if (urg->specified_scan_times != 1) { if (!strncmp(buffer, "00", 2)) { // "00" 応答の場合は、エコーバック応答とみなし、 // 最後の空行を読み捨て、次からのデータを返す n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 0) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } } } if (((urg->specified_scan_times == 1) && (strncmp(buffer, "00", 2))) || ((urg->specified_scan_times != 1) && (strncmp(buffer, "99", 2)))) { if (urg->error_handler) { type = urg->error_handler(buffer, urg); } if (type == URG_UNKNOWN) { // Gx, Hx のときは 00P が返されたときがデータ // Mx, Nx のときは 99b が返されたときがデータ ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } // タイムスタンプの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n > 0) { if (time_stamp) { *time_stamp = urg_scip_decode(buffer, 4); } if (system_time_stamp) { urg_get_walltime(system_time_stamp); } } // データの取得 switch (type) { case URG_DISTANCE: case URG_MULTIECHO: ret = receive_length_data(urg, data, NULL, type, buffer); break; case URG_DISTANCE_INTENSITY: case URG_MULTIECHO_INTENSITY: ret = receive_length_data(urg, data, intensity, type, buffer); break; case URG_STOP: case URG_UNKNOWN: ret = 0; break; } // specified_scan_times == 1 のときは Gx 系コマンドが使われるため // データを明示的に停止しなくてよい if ((urg->specified_scan_times > 1) && (urg->scanning_remain_times > 0)) { if (--urg->scanning_remain_times <= 0) { // データの停止のみを行う urg_stop_measurement(urg); } } return ret; } int urg_open(urg_t *urg, urg_connection_type_t connection_type, const char *device_or_address, long baudrate_or_port) { int ret; long baudrate = baudrate_or_port; urg->is_active = URG_FALSE; urg->is_sending = URG_TRUE; urg->last_errno = URG_NOT_CONNECTED; urg->timeout = MAX_TIMEOUT; urg->scanning_skip_scan = 0; urg->error_handler = NULL; // デバイスへの接続 ret = connection_open(&urg->connection, connection_type, device_or_address, baudrate_or_port); if (ret < 0) { switch (connection_type) { case URG_SERIAL: urg->last_errno = URG_SERIAL_OPEN_ERROR; break; case URG_ETHERNET: urg->last_errno = URG_ETHERNET_OPEN_ERROR; break; default: urg->last_errno = URG_INVALID_RESPONSE; break; } return urg->last_errno; } // 指定したボーレートで URG と通信できるように調整 if (connection_type == URG_ETHERNET) { // Ethernet のときは仮の通信速度を指定しておく baudrate = 115200; } if (connect_urg_device(urg, baudrate) != URG_NO_ERROR) { return set_errno_and_return(urg, ret); } urg->is_sending = URG_FALSE; // 変数の初期化 urg->last_errno = URG_NO_ERROR; urg->range_data_byte = URG_COMMUNICATION_3_BYTE; urg->specified_scan_times = 0; urg->scanning_remain_times = 0; urg->is_laser_on = URG_FALSE; // パラメータ情報を取得 ret = receive_parameter(urg); if (ret == URG_NO_ERROR) { urg->is_active = URG_TRUE; } return ret; } void urg_close(urg_t *urg) { if (urg->is_active) { ignore_receive_data_with_qt(urg, urg->timeout); } connection_close(&urg->connection); urg->is_active = URG_FALSE; } void urg_set_timeout_msec(urg_t *urg, int msec) { urg->timeout = msec; } int urg_start_time_stamp_mode(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM0 を発行する n = scip_response(urg, "TM0\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } long urg_time_stamp(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; char buffer[BUFFER_SIZE]; char *p; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } ret = scip_response(urg, "TM1\n", expected, urg->timeout, buffer, BUFFER_SIZE); if (ret < 0) { return ret; } // buffer からタイムスタンプを取得し、デコードして返す if (strcmp(buffer, "00P")) { // 最初の応答が "00P" でなければ戻る return set_errno_and_return(urg, URG_RECEIVE_ERROR); } p = buffer + 4; if (strlen(p) != 5) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (p[5] == scip_checksum(p, 4)) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } return urg_scip_decode(p, 4); } int urg_stop_time_stamp_mode(urg_t *urg) { int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM2 を発行する n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } static int send_distance_command(urg_t *urg, int scan_times, int skip_scan, char single_scan_ch, char continuous_scan_ch, char scan_type_ch) { char buffer[BUFFER_SIZE]; int write_size = 0; int front_index = urg->front_data_index; int n; urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times; urg->scanning_remain_times = urg->specified_scan_times; urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan; if (scan_times >= 100) { // 計測回数が 99 を越える場合は、無限回のスキャンを行う urg->specified_scan_times = 0; } if (urg->scanning_remain_times == 1) { // レーザ発光を指示 urg_laser_on(urg); write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n", single_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step); } else { write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n", continuous_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step, skip_scan, urg->specified_scan_times); urg->is_sending = URG_TRUE; } n = connection_write(&urg->connection, buffer, write_size); if (n != write_size) { return set_errno_and_return(urg, URG_SEND_ERROR); } return 0; } int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan) { char range_byte_ch; int ret = 0; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((skip_scan < 0) || (skip_scan > 9)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // !!! Mx 系, Nx 系の計測中のときは、QT を発行してから // !!! 計測開始コマンドを送信するようにする // !!! ただし、MD 計測中に MD を発行するように、同じコマンドの場合は // !!! Mx 系, Nx 系の計測は上書きすることができるようにする // 指定されたタイプのパケットを生成し、送信する switch (type) { case URG_DISTANCE: range_byte_ch = (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D'; ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', range_byte_ch); break; case URG_DISTANCE_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', 'E'); break; case URG_MULTIECHO: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'D'); break; case URG_MULTIECHO_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'E'); break; case URG_STOP: case URG_UNKNOWN: default: ignore_receive_data_with_qt(urg, urg->timeout); urg->last_errno = URG_INVALID_PARAMETER; ret = urg->last_errno; break; } return ret; } int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, NULL, time_stamp, system_time_stamp); } int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp); } int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp); } int urg_stop_measurement(urg_t *urg) { enum { MAX_READ_TIMES = 3 }; int ret = URG_INVALID_RESPONSE; int n; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // QT を発行する n = connection_write(&urg->connection, "QT\n", 3); if (n != 3) { return set_errno_and_return(urg, URG_SEND_ERROR); } for (i = 0; i < MAX_READ_TIMES; ++i) { // QT の応答が返されるまで、距離データを読み捨てる ret = receive_data(urg, NULL, NULL, NULL, NULL); if (ret == URG_NO_ERROR) { // 正常応答 urg->is_laser_on = URG_FALSE; urg->is_sending = URG_FALSE; return set_errno_and_return(urg, URG_NO_ERROR); } } return ret; } int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step) { // 設定の範囲外を指定したときは、エラーを返す if (((skip_step < 0) || (skip_step >= 100)) || (first_step > last_step) || (first_step < -urg->front_data_index) || (last_step > (urg->last_data_index - urg->front_data_index))) { return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR); } urg->scanning_first_step = first_step; urg->scanning_last_step = last_step; urg->scanning_skip_step = skip_step; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_set_connection_data_size(urg_t *urg, urg_range_data_byte_t data_byte) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((data_byte != URG_COMMUNICATION_3_BYTE) || (data_byte != URG_COMMUNICATION_2_BYTE)) { return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR); } urg->range_data_byte = data_byte; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_laser_on(urg_t *urg) { int expected[] = { 0, 2, EXPECTED_END }; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if (urg->is_laser_on != URG_FALSE) { // 既にレーザが発光しているときは、コマンドを送信しないようにする urg->last_errno = 0; return urg->last_errno; } ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0); if (ret >= 0) { urg->is_laser_on = URG_TRUE; ret = 0; } return ret; } int urg_laser_off(urg_t *urg) { return urg_stop_measurement(urg); } int urg_reboot(urg_t *urg) { int expected[] = { 0, 1, EXPECTED_END }; int ret; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // ２回目の RB 送信後、接続を切断する for (i = 0; i < 2; ++i) { ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0); if (ret < 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } urg->is_active = URG_FALSE; urg_close(urg); urg->last_errno = 0; return urg->last_errno; } void urg_sleep(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = 4 }; int sl_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE]; if (urg_stop_measurement(urg) != URG_NO_ERROR) { return; } scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); } void urg_wakeup(urg_t *urg) { urg_stop_measurement(urg); } int urg_is_stable(urg_t *urg) { const char *stat = urg_sensor_status(urg); return strncmp("Stable", stat, 6) ? 0 : 1; } static char *copy_token(char *dest, char *receive_buffer, const char *start_str, const char *end_ch, int lines) { size_t start_str_len = strlen(start_str); size_t end_ch_len = strlen(end_ch); int i; size_t j; for (j = 0; j < end_ch_len; ++j) { const char *p = receive_buffer; for (i = 0; i < lines; ++i) { if (!strncmp(p, start_str, start_str_len)) { char *last_p = strchr(p + start_str_len, end_ch[j]); if (last_p) { *last_p = '\0'; memcpy(dest, p + start_str_len, last_p - (p + start_str_len) + 1); return dest; } } p += strlen(p) + 1; } } return NULL; } static const char *receive_command_response(urg_t *urg, char *buffer, int buffer_size, const char* command, int response_lines) { const int vv_expected[] = { 0, EXPECTED_END }; int ret; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = scip_response(urg, command, vv_expected, urg->timeout, buffer, buffer_size); if (ret < response_lines) { return RECEIVE_ERROR_MESSAGE; } return NULL; } const char *urg_sensor_product_type(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROD:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_serial_id(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "SERI:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_vendor(urg_t *urg){ enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "VEND:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", "(", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_date(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } // Get the firmware version and append a '(', this will be what's before the date char firmware_version[50]; strcpy(firmware_version, urg_sensor_firmware_version(urg)); strcat(firmware_version, "("); ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } // Strip out the actual date from between the '(' and ')' char *date; p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", ";", VV_RESPONSE_LINES); date = copy_token(urg->return_buffer, p, firmware_version, ")", 1); return (date) ? date : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_protocol_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROT:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_status(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "STAT:", ";", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_state(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "MESM:", " (", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } void urg_set_error_handler(urg_t *urg, urg_error_handler handler) { urg->error_handler = handler; }
191  snprintf(buffer, SS_COMMAND_SIZE, "SS%06ld\n", next_baudrate);
192  ret = scip_response(urg, buffer, ss_expected, urg->timeout, NULL, 0);
193 
194  // 0F 応答のときは Ethernet 用のセンサとみなし、正常応答を返す
195  if (ret == -15) {
196  return set_errno_and_return(urg, URG_NO_ERROR);
197  }
198  if (ret <= 0) {
199  return set_errno_and_return(urg, URG_INVALID_PARAMETER);
200  }
201 
202  // 正常応答ならば、ホスト側のボーレートを変更する ret = connection_set_baudrate(&urg->connection, next_baudrate); // センサ側の設定反映を待つために少しだけ待機する ignore_receive_data(urg, MAX_TIMEOUT); return set_errno_and_return(urg, ret); } // ボーレートを変更しながら接続する static int connect_urg_device(urg_t *urg, long baudrate) { long try_baudrate[] = { 19200, 38400, 115200 }; int try_times = sizeof(try_baudrate) / sizeof(try_baudrate[0]); int i; // 指示されたボーレートから接続する for (i = 0; i < try_times; ++i) { if (try_baudrate[i] == baudrate) { try_baudrate[i] = try_baudrate[0]; try_baudrate[0] = baudrate; break; } } for (i = 0; i < try_times; ++i) { enum { RECEIVE_BUFFER_SIZE = 4 }; int qt_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE + 1]; int ret; connection_set_baudrate(&urg->connection, try_baudrate[i]); // QT を送信し、応答が返されるかでボーレートが一致しているかを確認する ret = scip_response(urg, "QT\n", qt_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); if (ret > 0) { if (!strcmp(receive_buffer, "E")) { int scip20_expected[] = { 0, EXPECTED_END }; // QT 応答の最後の改行を読み飛ばす ignore_receive_data(urg, MAX_TIMEOUT); // "E" が返された場合は、SCIP 1.1 とみなし "SCIP2.0" を送信する ret = scip_response(urg, "SCIP2.0\n", scip20_expected, MAX_TIMEOUT, NULL, 0); // SCIP2.0 応答の最後の改行を読み飛ばす ignore_receive_data(urg, MAX_TIMEOUT); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } else if (!strcmp(receive_buffer, "0Ee")) { int tm2_expected[] = { 0, EXPECTED_END }; // "0Ee" が返された場合は、TM モードとみなし "TM2" を送信する scip_response(urg, "TM2\n", tm2_expected, MAX_TIMEOUT, NULL, 0); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } if (ret <= 0) { if (ret == URG_INVALID_RESPONSE) { // 異常なエコーバックのときは、距離データ受信中とみなして // データを読み飛ばす ignore_receive_data_with_qt(urg, MAX_TIMEOUT); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } else { // 応答がないときは、ボーレートを変更して、再度接続を行う ignore_receive_data_with_qt(urg, MAX_TIMEOUT); continue; } } else if (!strcmp("00P", receive_buffer)) { // センサとホストのボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } return set_errno_and_return(urg, URG_NOT_DETECT_BAUDRATE_ERROR); } // PP コマンドの応答を urg_t に格納する static int receive_parameter(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * 9, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; int pp_expected[] = { 0, EXPECTED_END }; unsigned short received_bits = 0x0000; char *p; int i; int ret = scip_response(urg, "PP\n", pp_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); if (ret < 0) { return ret; } else if (ret < PP_RESPONSE_LINES) { ignore_receive_data_with_qt(urg, MAX_TIMEOUT); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } p = receive_buffer; for (i = 0; i < (ret - 1); ++i) { if (!strncmp(p, "DMIN:", 5)) { urg->min_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0001; } else if (!strncmp(p, "DMAX:", 5)) { urg->max_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0002; } else if (!strncmp(p, "ARES:", 5)) { urg->area_resolution = strtol(p + 5, NULL, 10); received_bits |= 0x0004; } else if (!strncmp(p, "AMIN:", 5)) { urg->first_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0008; } else if (!strncmp(p, "AMAX:", 5)) { urg->last_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0010; } else if (!strncmp(p, "AFRT:", 5)) { urg->front_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0020; } else if (!strncmp(p, "SCAN:", 5)) { int rpm = strtol(p + 5, NULL, 10); // タイムアウト時間は、計測周期の 16 倍程度の値にする urg->scan_usec = 1000 * 1000 * 60 / rpm; urg->timeout = urg->scan_usec >> (10 - 4); received_bits |= 0x0040; } p += strlen(p) + 1; } // 全てのパラメータを受信したか確認 if (received_bits != 0x007f) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } urg_set_scanning_parameter(urg, urg->first_data_index - urg->front_data_index, urg->last_data_index - urg->front_data_index, 1); return set_errno_and_return(urg, URG_NO_ERROR); } //! SCIP 文字列のデコード long urg_scip_decode(const char data[], int size) { const char* p = data; const char* last_p = p + size; int value = 0; while (p < last_p) { value <<= 6; value &= ~0x3f; value |= *p++ - 0x30; } return value; } static int parse_parameter(const char *parameter, int size) { char buffer[5]; memcpy(buffer, parameter, size); buffer[size] = '\0'; return strtol(buffer, NULL, 10); } static urg_measurement_type_t parse_distance_parameter(urg_t *urg, const char echoback[]) { urg_measurement_type_t ret_type = URG_UNKNOWN; urg->received_range_data_byte = URG_COMMUNICATION_3_BYTE; if (echoback[1] == 'S') { urg->received_range_data_byte = URG_COMMUNICATION_2_BYTE; ret_type = URG_DISTANCE; } else if (echoback[1] == 'D') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO; } } else if (echoback[1] == 'E') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE_INTENSITY; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO_INTENSITY; } } else { return URG_UNKNOWN; } // パラメータの格納 urg->received_first_index = parse_parameter(&echoback[2], 4); urg->received_last_index = parse_parameter(&echoback[6], 4); urg->received_skip_step = parse_parameter(&echoback[10], 2); return ret_type; } static urg_measurement_type_t parse_distance_echoback(urg_t *urg, const char echoback[]) { size_t line_length; urg_measurement_type_t ret_type = URG_UNKNOWN; if (!strcmp("QT", echoback)) { return URG_STOP; } line_length = strlen(echoback); if ((line_length == 12) && ((echoback[0] == 'G') || (echoback[0] == 'H'))) { ret_type = parse_distance_parameter(urg, echoback); } else if ((line_length == 15) && ((echoback[0] == 'M') || (echoback[0] == 'N'))) { ret_type = parse_distance_parameter(urg, echoback); } return ret_type; } static int receive_length_data(urg_t *urg, long length[], unsigned short intensity[], urg_measurement_type_t type, char buffer[]) { int n; int step_filled = 0; int line_filled = 0; int multiecho_index = 0; int each_size = (urg->received_range_data_byte == URG_COMMUNICATION_2_BYTE) ? 2 : 3; int data_size = each_size; int is_intensity = URG_FALSE; int is_multiecho = URG_FALSE; int multiecho_max_size = 1; if ((type == URG_DISTANCE_INTENSITY) || (type == URG_MULTIECHO_INTENSITY)) { data_size *= 2; is_intensity = URG_TRUE; } if ((type == URG_MULTIECHO) || (type == URG_MULTIECHO_INTENSITY)) { is_multiecho = URG_TRUE; multiecho_max_size = URG_MAX_ECHO; } do { char *p = buffer; char *last_p; n = connection_readline(&urg->connection, &buffer[line_filled], BUFFER_SIZE - line_filled, urg->timeout); if (n > 0) { // チェックサムの評価 if (buffer[line_filled + n - 1] != scip_checksum(&buffer[line_filled], n - 1)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } } if (n > 0) { line_filled += n - 1; } last_p = p + line_filled; while ((last_p - p) >= data_size) { int index; if (*p == '&') { // 先頭文字が '&' だったときは、マルチエコーのデータとみなす if ((last_p - (p + 1)) < data_size) { // '&' を除いて、data_size 分データが無ければ抜ける break; } --step_filled; ++multiecho_index; ++p; --line_filled; } else { // 次のデータ multiecho_index = 0; } index = (step_filled * multiecho_max_size) + multiecho_index; if (step_filled > (urg->received_last_index - urg->received_first_index)) { // データが多過ぎる場合は、残りのデータを無視して戻る ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (is_multiecho && (multiecho_index == 0)) { // マルチエコーのデータ格納先をダミーデータで埋める int i; if (length) { for (i = 1; i < multiecho_max_size; ++i) { length[index + i] = 0; } } if (intensity) { for (i = 1; i < multiecho_max_size; ++i) { intensity[index + i] = 0; } } } // 距離データの格納 if (length) { length[index] = urg_scip_decode(p, 3); } p += 3; // 強度データの格納 if (is_intensity) { if (intensity) { intensity[index] = (unsigned short)urg_scip_decode(p, 3); } p += 3; } ++step_filled; line_filled -= data_size; } // 次に処理する文字を退避 memmove(buffer, p, line_filled); } while (n > 0); return step_filled; } //! 距離データの取得 static int receive_data(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { urg_measurement_type_t type; char buffer[BUFFER_SIZE]; int ret = 0; int n; int extended_timeout = urg->timeout + 2 * (urg->scan_usec * (urg->scanning_skip_scan) / 1000); // エコーバックの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, extended_timeout); if (n <= 0) { return set_errno_and_return(urg, URG_NO_RESPONSE); } // エコーバックの解析 type = parse_distance_echoback(urg, buffer); // 応答の取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 3) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } if (buffer[n - 1] != scip_checksum(buffer, n - 1)) { // チェックサムの評価 ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } if (type == URG_STOP) { // QT 応答の場合には、最後の改行を読み捨て、正常応答として処理する n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n == 0) { return 0; } else { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } if (urg->specified_scan_times != 1) { if (!strncmp(buffer, "00", 2)) { // "00" 応答の場合は、エコーバック応答とみなし、 // 最後の空行を読み捨て、次からのデータを返す n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 0) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } } } if (((urg->specified_scan_times == 1) && (strncmp(buffer, "00", 2))) || ((urg->specified_scan_times != 1) && (strncmp(buffer, "99", 2)))) { if (urg->error_handler) { type = urg->error_handler(buffer, urg); } if (type == URG_UNKNOWN) { // Gx, Hx のときは 00P が返されたときがデータ // Mx, Nx のときは 99b が返されたときがデータ ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } // タイムスタンプの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n > 0) { if (time_stamp) { *time_stamp = urg_scip_decode(buffer, 4); } if (system_time_stamp) { urg_get_walltime(system_time_stamp); } } // データの取得 switch (type) { case URG_DISTANCE: case URG_MULTIECHO: ret = receive_length_data(urg, data, NULL, type, buffer); break; case URG_DISTANCE_INTENSITY: case URG_MULTIECHO_INTENSITY: ret = receive_length_data(urg, data, intensity, type, buffer); break; case URG_STOP: case URG_UNKNOWN: ret = 0; break; } // specified_scan_times == 1 のときは Gx 系コマンドが使われるため // データを明示的に停止しなくてよい if ((urg->specified_scan_times > 1) && (urg->scanning_remain_times > 0)) { if (--urg->scanning_remain_times <= 0) { // データの停止のみを行う urg_stop_measurement(urg); } } return ret; } int urg_open(urg_t *urg, urg_connection_type_t connection_type, const char *device_or_address, long baudrate_or_port) { int ret; long baudrate = baudrate_or_port; urg->is_active = URG_FALSE; urg->is_sending = URG_TRUE; urg->last_errno = URG_NOT_CONNECTED; urg->timeout = MAX_TIMEOUT; urg->scanning_skip_scan = 0; urg->error_handler = NULL; // デバイスへの接続 ret = connection_open(&urg->connection, connection_type, device_or_address, baudrate_or_port); if (ret < 0) { switch (connection_type) { case URG_SERIAL: urg->last_errno = URG_SERIAL_OPEN_ERROR; break; case URG_ETHERNET: urg->last_errno = URG_ETHERNET_OPEN_ERROR; break; default: urg->last_errno = URG_INVALID_RESPONSE; break; } return urg->last_errno; } // 指定したボーレートで URG と通信できるように調整 if (connection_type == URG_ETHERNET) { // Ethernet のときは仮の通信速度を指定しておく baudrate = 115200; } if (connect_urg_device(urg, baudrate) != URG_NO_ERROR) { return set_errno_and_return(urg, ret); } urg->is_sending = URG_FALSE; // 変数の初期化 urg->last_errno = URG_NO_ERROR; urg->range_data_byte = URG_COMMUNICATION_3_BYTE; urg->specified_scan_times = 0; urg->scanning_remain_times = 0; urg->is_laser_on = URG_FALSE; // パラメータ情報を取得 ret = receive_parameter(urg); if (ret == URG_NO_ERROR) { urg->is_active = URG_TRUE; } return ret; } void urg_close(urg_t *urg) { if (urg->is_active) { ignore_receive_data_with_qt(urg, urg->timeout); } connection_close(&urg->connection); urg->is_active = URG_FALSE; } void urg_set_timeout_msec(urg_t *urg, int msec) { urg->timeout = msec; } int urg_start_time_stamp_mode(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM0 を発行する n = scip_response(urg, "TM0\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } long urg_time_stamp(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; char buffer[BUFFER_SIZE]; char *p; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } ret = scip_response(urg, "TM1\n", expected, urg->timeout, buffer, BUFFER_SIZE); if (ret < 0) { return ret; } // buffer からタイムスタンプを取得し、デコードして返す if (strcmp(buffer, "00P")) { // 最初の応答が "00P" でなければ戻る return set_errno_and_return(urg, URG_RECEIVE_ERROR); } p = buffer + 4; if (strlen(p) != 5) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (p[5] == scip_checksum(p, 4)) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } return urg_scip_decode(p, 4); } int urg_stop_time_stamp_mode(urg_t *urg) { int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM2 を発行する n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } static int send_distance_command(urg_t *urg, int scan_times, int skip_scan, char single_scan_ch, char continuous_scan_ch, char scan_type_ch) { char buffer[BUFFER_SIZE]; int write_size = 0; int front_index = urg->front_data_index; int n; urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times; urg->scanning_remain_times = urg->specified_scan_times; urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan; if (scan_times >= 100) { // 計測回数が 99 を越える場合は、無限回のスキャンを行う urg->specified_scan_times = 0; } if (urg->scanning_remain_times == 1) { // レーザ発光を指示 urg_laser_on(urg); write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n", single_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step); } else { write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n", continuous_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step, skip_scan, urg->specified_scan_times); urg->is_sending = URG_TRUE; } n = connection_write(&urg->connection, buffer, write_size); if (n != write_size) { return set_errno_and_return(urg, URG_SEND_ERROR); } return 0; } int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan) { char range_byte_ch; int ret = 0; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((skip_scan < 0) || (skip_scan > 9)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // !!! Mx 系, Nx 系の計測中のときは、QT を発行してから // !!! 計測開始コマンドを送信するようにする // !!! ただし、MD 計測中に MD を発行するように、同じコマンドの場合は // !!! Mx 系, Nx 系の計測は上書きすることができるようにする // 指定されたタイプのパケットを生成し、送信する switch (type) { case URG_DISTANCE: range_byte_ch = (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D'; ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', range_byte_ch); break; case URG_DISTANCE_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', 'E'); break; case URG_MULTIECHO: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'D'); break; case URG_MULTIECHO_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'E'); break; case URG_STOP: case URG_UNKNOWN: default: ignore_receive_data_with_qt(urg, urg->timeout); urg->last_errno = URG_INVALID_PARAMETER; ret = urg->last_errno; break; } return ret; } int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, NULL, time_stamp, system_time_stamp); } int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp); } int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp); } int urg_stop_measurement(urg_t *urg) { enum { MAX_READ_TIMES = 3 }; int ret = URG_INVALID_RESPONSE; int n; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // QT を発行する n = connection_write(&urg->connection, "QT\n", 3); if (n != 3) { return set_errno_and_return(urg, URG_SEND_ERROR); } for (i = 0; i < MAX_READ_TIMES; ++i) { // QT の応答が返されるまで、距離データを読み捨てる ret = receive_data(urg, NULL, NULL, NULL, NULL); if (ret == URG_NO_ERROR) { // 正常応答 urg->is_laser_on = URG_FALSE; urg->is_sending = URG_FALSE; return set_errno_and_return(urg, URG_NO_ERROR); } } return ret; } int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step) { // 設定の範囲外を指定したときは、エラーを返す if (((skip_step < 0) || (skip_step >= 100)) || (first_step > last_step) || (first_step < -urg->front_data_index) || (last_step > (urg->last_data_index - urg->front_data_index))) { return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR); } urg->scanning_first_step = first_step; urg->scanning_last_step = last_step; urg->scanning_skip_step = skip_step; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_set_connection_data_size(urg_t *urg, urg_range_data_byte_t data_byte) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((data_byte != URG_COMMUNICATION_3_BYTE) || (data_byte != URG_COMMUNICATION_2_BYTE)) { return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR); } urg->range_data_byte = data_byte; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_laser_on(urg_t *urg) { int expected[] = { 0, 2, EXPECTED_END }; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if (urg->is_laser_on != URG_FALSE) { // 既にレーザが発光しているときは、コマンドを送信しないようにする urg->last_errno = 0; return urg->last_errno; } ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0); if (ret >= 0) { urg->is_laser_on = URG_TRUE; ret = 0; } return ret; } int urg_laser_off(urg_t *urg) { return urg_stop_measurement(urg); } int urg_reboot(urg_t *urg) { int expected[] = { 0, 1, EXPECTED_END }; int ret; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // ２回目の RB 送信後、接続を切断する for (i = 0; i < 2; ++i) { ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0); if (ret < 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } urg->is_active = URG_FALSE; urg_close(urg); urg->last_errno = 0; return urg->last_errno; } void urg_sleep(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = 4 }; int sl_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE]; if (urg_stop_measurement(urg) != URG_NO_ERROR) { return; } scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); } void urg_wakeup(urg_t *urg) { urg_stop_measurement(urg); } int urg_is_stable(urg_t *urg) { const char *stat = urg_sensor_status(urg); return strncmp("Stable", stat, 6) ? 0 : 1; } static char *copy_token(char *dest, char *receive_buffer, const char *start_str, const char *end_ch, int lines) { size_t start_str_len = strlen(start_str); size_t end_ch_len = strlen(end_ch); int i; size_t j; for (j = 0; j < end_ch_len; ++j) { const char *p = receive_buffer; for (i = 0; i < lines; ++i) { if (!strncmp(p, start_str, start_str_len)) { char *last_p = strchr(p + start_str_len, end_ch[j]); if (last_p) { *last_p = '\0'; memcpy(dest, p + start_str_len, last_p - (p + start_str_len) + 1); return dest; } } p += strlen(p) + 1; } } return NULL; } static const char *receive_command_response(urg_t *urg, char *buffer, int buffer_size, const char* command, int response_lines) { const int vv_expected[] = { 0, EXPECTED_END }; int ret; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = scip_response(urg, command, vv_expected, urg->timeout, buffer, buffer_size); if (ret < response_lines) { return RECEIVE_ERROR_MESSAGE; } return NULL; } const char *urg_sensor_product_type(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROD:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_serial_id(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "SERI:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_vendor(urg_t *urg){ enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "VEND:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", "(", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_date(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } // Get the firmware version and append a '(', this will be what's before the date char firmware_version[50]; strcpy(firmware_version, urg_sensor_firmware_version(urg)); strcat(firmware_version, "("); ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } // Strip out the actual date from between the '(' and ')' char *date; p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", ";", VV_RESPONSE_LINES); date = copy_token(urg->return_buffer, p, firmware_version, ")", 1); return (date) ? date : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_protocol_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROT:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_status(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "STAT:", ";", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_state(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "MESM:", " (", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } void urg_set_error_handler(urg_t *urg, urg_error_handler handler) { urg->error_handler = handler; }
203  ret = connection_set_baudrate(&urg->connection, next_baudrate);
204 
205  // センサ側の設定反映を待つために少しだけ待機する ignore_receive_data(urg, MAX_TIMEOUT); return set_errno_and_return(urg, ret); } // ボーレートを変更しながら接続する static int connect_urg_device(urg_t *urg, long baudrate) { long try_baudrate[] = { 19200, 38400, 115200 }; int try_times = sizeof(try_baudrate) / sizeof(try_baudrate[0]); int i; // 指示されたボーレートから接続する for (i = 0; i < try_times; ++i) { if (try_baudrate[i] == baudrate) { try_baudrate[i] = try_baudrate[0]; try_baudrate[0] = baudrate; break; } } for (i = 0; i < try_times; ++i) { enum { RECEIVE_BUFFER_SIZE = 4 }; int qt_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE + 1]; int ret; connection_set_baudrate(&urg->connection, try_baudrate[i]); // QT を送信し、応答が返されるかでボーレートが一致しているかを確認する ret = scip_response(urg, "QT\n", qt_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); if (ret > 0) { if (!strcmp(receive_buffer, "E")) { int scip20_expected[] = { 0, EXPECTED_END }; // QT 応答の最後の改行を読み飛ばす ignore_receive_data(urg, MAX_TIMEOUT); // "E" が返された場合は、SCIP 1.1 とみなし "SCIP2.0" を送信する ret = scip_response(urg, "SCIP2.0\n", scip20_expected, MAX_TIMEOUT, NULL, 0); // SCIP2.0 応答の最後の改行を読み飛ばす ignore_receive_data(urg, MAX_TIMEOUT); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } else if (!strcmp(receive_buffer, "0Ee")) { int tm2_expected[] = { 0, EXPECTED_END }; // "0Ee" が返された場合は、TM モードとみなし "TM2" を送信する scip_response(urg, "TM2\n", tm2_expected, MAX_TIMEOUT, NULL, 0); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } if (ret <= 0) { if (ret == URG_INVALID_RESPONSE) { // 異常なエコーバックのときは、距離データ受信中とみなして // データを読み飛ばす ignore_receive_data_with_qt(urg, MAX_TIMEOUT); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } else { // 応答がないときは、ボーレートを変更して、再度接続を行う ignore_receive_data_with_qt(urg, MAX_TIMEOUT); continue; } } else if (!strcmp("00P", receive_buffer)) { // センサとホストのボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } return set_errno_and_return(urg, URG_NOT_DETECT_BAUDRATE_ERROR); } // PP コマンドの応答を urg_t に格納する static int receive_parameter(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * 9, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; int pp_expected[] = { 0, EXPECTED_END }; unsigned short received_bits = 0x0000; char *p; int i; int ret = scip_response(urg, "PP\n", pp_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); if (ret < 0) { return ret; } else if (ret < PP_RESPONSE_LINES) { ignore_receive_data_with_qt(urg, MAX_TIMEOUT); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } p = receive_buffer; for (i = 0; i < (ret - 1); ++i) { if (!strncmp(p, "DMIN:", 5)) { urg->min_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0001; } else if (!strncmp(p, "DMAX:", 5)) { urg->max_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0002; } else if (!strncmp(p, "ARES:", 5)) { urg->area_resolution = strtol(p + 5, NULL, 10); received_bits |= 0x0004; } else if (!strncmp(p, "AMIN:", 5)) { urg->first_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0008; } else if (!strncmp(p, "AMAX:", 5)) { urg->last_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0010; } else if (!strncmp(p, "AFRT:", 5)) { urg->front_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0020; } else if (!strncmp(p, "SCAN:", 5)) { int rpm = strtol(p + 5, NULL, 10); // タイムアウト時間は、計測周期の 16 倍程度の値にする urg->scan_usec = 1000 * 1000 * 60 / rpm; urg->timeout = urg->scan_usec >> (10 - 4); received_bits |= 0x0040; } p += strlen(p) + 1; } // 全てのパラメータを受信したか確認 if (received_bits != 0x007f) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } urg_set_scanning_parameter(urg, urg->first_data_index - urg->front_data_index, urg->last_data_index - urg->front_data_index, 1); return set_errno_and_return(urg, URG_NO_ERROR); } //! SCIP 文字列のデコード long urg_scip_decode(const char data[], int size) { const char* p = data; const char* last_p = p + size; int value = 0; while (p < last_p) { value <<= 6; value &= ~0x3f; value |= *p++ - 0x30; } return value; } static int parse_parameter(const char *parameter, int size) { char buffer[5]; memcpy(buffer, parameter, size); buffer[size] = '\0'; return strtol(buffer, NULL, 10); } static urg_measurement_type_t parse_distance_parameter(urg_t *urg, const char echoback[]) { urg_measurement_type_t ret_type = URG_UNKNOWN; urg->received_range_data_byte = URG_COMMUNICATION_3_BYTE; if (echoback[1] == 'S') { urg->received_range_data_byte = URG_COMMUNICATION_2_BYTE; ret_type = URG_DISTANCE; } else if (echoback[1] == 'D') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO; } } else if (echoback[1] == 'E') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE_INTENSITY; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO_INTENSITY; } } else { return URG_UNKNOWN; } // パラメータの格納 urg->received_first_index = parse_parameter(&echoback[2], 4); urg->received_last_index = parse_parameter(&echoback[6], 4); urg->received_skip_step = parse_parameter(&echoback[10], 2); return ret_type; } static urg_measurement_type_t parse_distance_echoback(urg_t *urg, const char echoback[]) { size_t line_length; urg_measurement_type_t ret_type = URG_UNKNOWN; if (!strcmp("QT", echoback)) { return URG_STOP; } line_length = strlen(echoback); if ((line_length == 12) && ((echoback[0] == 'G') || (echoback[0] == 'H'))) { ret_type = parse_distance_parameter(urg, echoback); } else if ((line_length == 15) && ((echoback[0] == 'M') || (echoback[0] == 'N'))) { ret_type = parse_distance_parameter(urg, echoback); } return ret_type; } static int receive_length_data(urg_t *urg, long length[], unsigned short intensity[], urg_measurement_type_t type, char buffer[]) { int n; int step_filled = 0; int line_filled = 0; int multiecho_index = 0; int each_size = (urg->received_range_data_byte == URG_COMMUNICATION_2_BYTE) ? 2 : 3; int data_size = each_size; int is_intensity = URG_FALSE; int is_multiecho = URG_FALSE; int multiecho_max_size = 1; if ((type == URG_DISTANCE_INTENSITY) || (type == URG_MULTIECHO_INTENSITY)) { data_size *= 2; is_intensity = URG_TRUE; } if ((type == URG_MULTIECHO) || (type == URG_MULTIECHO_INTENSITY)) { is_multiecho = URG_TRUE; multiecho_max_size = URG_MAX_ECHO; } do { char *p = buffer; char *last_p; n = connection_readline(&urg->connection, &buffer[line_filled], BUFFER_SIZE - line_filled, urg->timeout); if (n > 0) { // チェックサムの評価 if (buffer[line_filled + n - 1] != scip_checksum(&buffer[line_filled], n - 1)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } } if (n > 0) { line_filled += n - 1; } last_p = p + line_filled; while ((last_p - p) >= data_size) { int index; if (*p == '&') { // 先頭文字が '&' だったときは、マルチエコーのデータとみなす if ((last_p - (p + 1)) < data_size) { // '&' を除いて、data_size 分データが無ければ抜ける break; } --step_filled; ++multiecho_index; ++p; --line_filled; } else { // 次のデータ multiecho_index = 0; } index = (step_filled * multiecho_max_size) + multiecho_index; if (step_filled > (urg->received_last_index - urg->received_first_index)) { // データが多過ぎる場合は、残りのデータを無視して戻る ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (is_multiecho && (multiecho_index == 0)) { // マルチエコーのデータ格納先をダミーデータで埋める int i; if (length) { for (i = 1; i < multiecho_max_size; ++i) { length[index + i] = 0; } } if (intensity) { for (i = 1; i < multiecho_max_size; ++i) { intensity[index + i] = 0; } } } // 距離データの格納 if (length) { length[index] = urg_scip_decode(p, 3); } p += 3; // 強度データの格納 if (is_intensity) { if (intensity) { intensity[index] = (unsigned short)urg_scip_decode(p, 3); } p += 3; } ++step_filled; line_filled -= data_size; } // 次に処理する文字を退避 memmove(buffer, p, line_filled); } while (n > 0); return step_filled; } //! 距離データの取得 static int receive_data(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { urg_measurement_type_t type; char buffer[BUFFER_SIZE]; int ret = 0; int n; int extended_timeout = urg->timeout + 2 * (urg->scan_usec * (urg->scanning_skip_scan) / 1000); // エコーバックの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, extended_timeout); if (n <= 0) { return set_errno_and_return(urg, URG_NO_RESPONSE); } // エコーバックの解析 type = parse_distance_echoback(urg, buffer); // 応答の取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 3) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } if (buffer[n - 1] != scip_checksum(buffer, n - 1)) { // チェックサムの評価 ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } if (type == URG_STOP) { // QT 応答の場合には、最後の改行を読み捨て、正常応答として処理する n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n == 0) { return 0; } else { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } if (urg->specified_scan_times != 1) { if (!strncmp(buffer, "00", 2)) { // "00" 応答の場合は、エコーバック応答とみなし、 // 最後の空行を読み捨て、次からのデータを返す n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 0) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } } } if (((urg->specified_scan_times == 1) && (strncmp(buffer, "00", 2))) || ((urg->specified_scan_times != 1) && (strncmp(buffer, "99", 2)))) { if (urg->error_handler) { type = urg->error_handler(buffer, urg); } if (type == URG_UNKNOWN) { // Gx, Hx のときは 00P が返されたときがデータ // Mx, Nx のときは 99b が返されたときがデータ ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } // タイムスタンプの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n > 0) { if (time_stamp) { *time_stamp = urg_scip_decode(buffer, 4); } if (system_time_stamp) { urg_get_walltime(system_time_stamp); } } // データの取得 switch (type) { case URG_DISTANCE: case URG_MULTIECHO: ret = receive_length_data(urg, data, NULL, type, buffer); break; case URG_DISTANCE_INTENSITY: case URG_MULTIECHO_INTENSITY: ret = receive_length_data(urg, data, intensity, type, buffer); break; case URG_STOP: case URG_UNKNOWN: ret = 0; break; } // specified_scan_times == 1 のときは Gx 系コマンドが使われるため // データを明示的に停止しなくてよい if ((urg->specified_scan_times > 1) && (urg->scanning_remain_times > 0)) { if (--urg->scanning_remain_times <= 0) { // データの停止のみを行う urg_stop_measurement(urg); } } return ret; } int urg_open(urg_t *urg, urg_connection_type_t connection_type, const char *device_or_address, long baudrate_or_port) { int ret; long baudrate = baudrate_or_port; urg->is_active = URG_FALSE; urg->is_sending = URG_TRUE; urg->last_errno = URG_NOT_CONNECTED; urg->timeout = MAX_TIMEOUT; urg->scanning_skip_scan = 0; urg->error_handler = NULL; // デバイスへの接続 ret = connection_open(&urg->connection, connection_type, device_or_address, baudrate_or_port); if (ret < 0) { switch (connection_type) { case URG_SERIAL: urg->last_errno = URG_SERIAL_OPEN_ERROR; break; case URG_ETHERNET: urg->last_errno = URG_ETHERNET_OPEN_ERROR; break; default: urg->last_errno = URG_INVALID_RESPONSE; break; } return urg->last_errno; } // 指定したボーレートで URG と通信できるように調整 if (connection_type == URG_ETHERNET) { // Ethernet のときは仮の通信速度を指定しておく baudrate = 115200; } if (connect_urg_device(urg, baudrate) != URG_NO_ERROR) { return set_errno_and_return(urg, ret); } urg->is_sending = URG_FALSE; // 変数の初期化 urg->last_errno = URG_NO_ERROR; urg->range_data_byte = URG_COMMUNICATION_3_BYTE; urg->specified_scan_times = 0; urg->scanning_remain_times = 0; urg->is_laser_on = URG_FALSE; // パラメータ情報を取得 ret = receive_parameter(urg); if (ret == URG_NO_ERROR) { urg->is_active = URG_TRUE; } return ret; } void urg_close(urg_t *urg) { if (urg->is_active) { ignore_receive_data_with_qt(urg, urg->timeout); } connection_close(&urg->connection); urg->is_active = URG_FALSE; } void urg_set_timeout_msec(urg_t *urg, int msec) { urg->timeout = msec; } int urg_start_time_stamp_mode(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM0 を発行する n = scip_response(urg, "TM0\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } long urg_time_stamp(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; char buffer[BUFFER_SIZE]; char *p; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } ret = scip_response(urg, "TM1\n", expected, urg->timeout, buffer, BUFFER_SIZE); if (ret < 0) { return ret; } // buffer からタイムスタンプを取得し、デコードして返す if (strcmp(buffer, "00P")) { // 最初の応答が "00P" でなければ戻る return set_errno_and_return(urg, URG_RECEIVE_ERROR); } p = buffer + 4; if (strlen(p) != 5) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (p[5] == scip_checksum(p, 4)) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } return urg_scip_decode(p, 4); } int urg_stop_time_stamp_mode(urg_t *urg) { int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM2 を発行する n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } static int send_distance_command(urg_t *urg, int scan_times, int skip_scan, char single_scan_ch, char continuous_scan_ch, char scan_type_ch) { char buffer[BUFFER_SIZE]; int write_size = 0; int front_index = urg->front_data_index; int n; urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times; urg->scanning_remain_times = urg->specified_scan_times; urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan; if (scan_times >= 100) { // 計測回数が 99 を越える場合は、無限回のスキャンを行う urg->specified_scan_times = 0; } if (urg->scanning_remain_times == 1) { // レーザ発光を指示 urg_laser_on(urg); write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n", single_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step); } else { write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n", continuous_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step, skip_scan, urg->specified_scan_times); urg->is_sending = URG_TRUE; } n = connection_write(&urg->connection, buffer, write_size); if (n != write_size) { return set_errno_and_return(urg, URG_SEND_ERROR); } return 0; } int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan) { char range_byte_ch; int ret = 0; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((skip_scan < 0) || (skip_scan > 9)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // !!! Mx 系, Nx 系の計測中のときは、QT を発行してから // !!! 計測開始コマンドを送信するようにする // !!! ただし、MD 計測中に MD を発行するように、同じコマンドの場合は // !!! Mx 系, Nx 系の計測は上書きすることができるようにする // 指定されたタイプのパケットを生成し、送信する switch (type) { case URG_DISTANCE: range_byte_ch = (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D'; ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', range_byte_ch); break; case URG_DISTANCE_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', 'E'); break; case URG_MULTIECHO: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'D'); break; case URG_MULTIECHO_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'E'); break; case URG_STOP: case URG_UNKNOWN: default: ignore_receive_data_with_qt(urg, urg->timeout); urg->last_errno = URG_INVALID_PARAMETER; ret = urg->last_errno; break; } return ret; } int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, NULL, time_stamp, system_time_stamp); } int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp); } int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp); } int urg_stop_measurement(urg_t *urg) { enum { MAX_READ_TIMES = 3 }; int ret = URG_INVALID_RESPONSE; int n; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // QT を発行する n = connection_write(&urg->connection, "QT\n", 3); if (n != 3) { return set_errno_and_return(urg, URG_SEND_ERROR); } for (i = 0; i < MAX_READ_TIMES; ++i) { // QT の応答が返されるまで、距離データを読み捨てる ret = receive_data(urg, NULL, NULL, NULL, NULL); if (ret == URG_NO_ERROR) { // 正常応答 urg->is_laser_on = URG_FALSE; urg->is_sending = URG_FALSE; return set_errno_and_return(urg, URG_NO_ERROR); } } return ret; } int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step) { // 設定の範囲外を指定したときは、エラーを返す if (((skip_step < 0) || (skip_step >= 100)) || (first_step > last_step) || (first_step < -urg->front_data_index) || (last_step > (urg->last_data_index - urg->front_data_index))) { return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR); } urg->scanning_first_step = first_step; urg->scanning_last_step = last_step; urg->scanning_skip_step = skip_step; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_set_connection_data_size(urg_t *urg, urg_range_data_byte_t data_byte) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((data_byte != URG_COMMUNICATION_3_BYTE) || (data_byte != URG_COMMUNICATION_2_BYTE)) { return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR); } urg->range_data_byte = data_byte; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_laser_on(urg_t *urg) { int expected[] = { 0, 2, EXPECTED_END }; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if (urg->is_laser_on != URG_FALSE) { // 既にレーザが発光しているときは、コマンドを送信しないようにする urg->last_errno = 0; return urg->last_errno; } ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0); if (ret >= 0) { urg->is_laser_on = URG_TRUE; ret = 0; } return ret; } int urg_laser_off(urg_t *urg) { return urg_stop_measurement(urg); } int urg_reboot(urg_t *urg) { int expected[] = { 0, 1, EXPECTED_END }; int ret; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // ２回目の RB 送信後、接続を切断する for (i = 0; i < 2; ++i) { ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0); if (ret < 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } urg->is_active = URG_FALSE; urg_close(urg); urg->last_errno = 0; return urg->last_errno; } void urg_sleep(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = 4 }; int sl_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE]; if (urg_stop_measurement(urg) != URG_NO_ERROR) { return; } scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); } void urg_wakeup(urg_t *urg) { urg_stop_measurement(urg); } int urg_is_stable(urg_t *urg) { const char *stat = urg_sensor_status(urg); return strncmp("Stable", stat, 6) ? 0 : 1; } static char *copy_token(char *dest, char *receive_buffer, const char *start_str, const char *end_ch, int lines) { size_t start_str_len = strlen(start_str); size_t end_ch_len = strlen(end_ch); int i; size_t j; for (j = 0; j < end_ch_len; ++j) { const char *p = receive_buffer; for (i = 0; i < lines; ++i) { if (!strncmp(p, start_str, start_str_len)) { char *last_p = strchr(p + start_str_len, end_ch[j]); if (last_p) { *last_p = '\0'; memcpy(dest, p + start_str_len, last_p - (p + start_str_len) + 1); return dest; } } p += strlen(p) + 1; } } return NULL; } static const char *receive_command_response(urg_t *urg, char *buffer, int buffer_size, const char* command, int response_lines) { const int vv_expected[] = { 0, EXPECTED_END }; int ret; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = scip_response(urg, command, vv_expected, urg->timeout, buffer, buffer_size); if (ret < response_lines) { return RECEIVE_ERROR_MESSAGE; } return NULL; } const char *urg_sensor_product_type(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROD:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_serial_id(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "SERI:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_vendor(urg_t *urg){ enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "VEND:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", "(", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_date(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } // Get the firmware version and append a '(', this will be what's before the date char firmware_version[50]; strcpy(firmware_version, urg_sensor_firmware_version(urg)); strcat(firmware_version, "("); ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } // Strip out the actual date from between the '(' and ')' char *date; p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", ";", VV_RESPONSE_LINES); date = copy_token(urg->return_buffer, p, firmware_version, ")", 1); return (date) ? date : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_protocol_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROT:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_status(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "STAT:", ";", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_state(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "MESM:", " (", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } void urg_set_error_handler(urg_t *urg, urg_error_handler handler) { urg->error_handler = handler; }
206  ignore_receive_data(urg, MAX_TIMEOUT);
207 
208  return set_errno_and_return(urg, ret);
209 }
210 
211 
212 // ボーレートを変更しながら接続するstatic int connect_urg_device(urg_t *urg, long baudrate) { long try_baudrate[] = { 19200, 38400, 115200 }; int try_times = sizeof(try_baudrate) / sizeof(try_baudrate[0]); int i; // 指示されたボーレートから接続する for (i = 0; i < try_times; ++i) { if (try_baudrate[i] == baudrate) { try_baudrate[i] = try_baudrate[0]; try_baudrate[0] = baudrate; break; } } for (i = 0; i < try_times; ++i) { enum { RECEIVE_BUFFER_SIZE = 4 }; int qt_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE + 1]; int ret; connection_set_baudrate(&urg->connection, try_baudrate[i]); // QT を送信し、応答が返されるかでボーレートが一致しているかを確認する ret = scip_response(urg, "QT\n", qt_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); if (ret > 0) { if (!strcmp(receive_buffer, "E")) { int scip20_expected[] = { 0, EXPECTED_END }; // QT 応答の最後の改行を読み飛ばす ignore_receive_data(urg, MAX_TIMEOUT); // "E" が返された場合は、SCIP 1.1 とみなし "SCIP2.0" を送信する ret = scip_response(urg, "SCIP2.0\n", scip20_expected, MAX_TIMEOUT, NULL, 0); // SCIP2.0 応答の最後の改行を読み飛ばす ignore_receive_data(urg, MAX_TIMEOUT); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } else if (!strcmp(receive_buffer, "0Ee")) { int tm2_expected[] = { 0, EXPECTED_END }; // "0Ee" が返された場合は、TM モードとみなし "TM2" を送信する scip_response(urg, "TM2\n", tm2_expected, MAX_TIMEOUT, NULL, 0); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } if (ret <= 0) { if (ret == URG_INVALID_RESPONSE) { // 異常なエコーバックのときは、距離データ受信中とみなして // データを読み飛ばす ignore_receive_data_with_qt(urg, MAX_TIMEOUT); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } else { // 応答がないときは、ボーレートを変更して、再度接続を行う ignore_receive_data_with_qt(urg, MAX_TIMEOUT); continue; } } else if (!strcmp("00P", receive_buffer)) { // センサとホストのボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } return set_errno_and_return(urg, URG_NOT_DETECT_BAUDRATE_ERROR); } // PP コマンドの応答を urg_t に格納する static int receive_parameter(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * 9, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; int pp_expected[] = { 0, EXPECTED_END }; unsigned short received_bits = 0x0000; char *p; int i; int ret = scip_response(urg, "PP\n", pp_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); if (ret < 0) { return ret; } else if (ret < PP_RESPONSE_LINES) { ignore_receive_data_with_qt(urg, MAX_TIMEOUT); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } p = receive_buffer; for (i = 0; i < (ret - 1); ++i) { if (!strncmp(p, "DMIN:", 5)) { urg->min_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0001; } else if (!strncmp(p, "DMAX:", 5)) { urg->max_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0002; } else if (!strncmp(p, "ARES:", 5)) { urg->area_resolution = strtol(p + 5, NULL, 10); received_bits |= 0x0004; } else if (!strncmp(p, "AMIN:", 5)) { urg->first_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0008; } else if (!strncmp(p, "AMAX:", 5)) { urg->last_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0010; } else if (!strncmp(p, "AFRT:", 5)) { urg->front_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0020; } else if (!strncmp(p, "SCAN:", 5)) { int rpm = strtol(p + 5, NULL, 10); // タイムアウト時間は、計測周期の 16 倍程度の値にする urg->scan_usec = 1000 * 1000 * 60 / rpm; urg->timeout = urg->scan_usec >> (10 - 4); received_bits |= 0x0040; } p += strlen(p) + 1; } // 全てのパラメータを受信したか確認 if (received_bits != 0x007f) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } urg_set_scanning_parameter(urg, urg->first_data_index - urg->front_data_index, urg->last_data_index - urg->front_data_index, 1); return set_errno_and_return(urg, URG_NO_ERROR); } //! SCIP 文字列のデコード long urg_scip_decode(const char data[], int size) { const char* p = data; const char* last_p = p + size; int value = 0; while (p < last_p) { value <<= 6; value &= ~0x3f; value |= *p++ - 0x30; } return value; } static int parse_parameter(const char *parameter, int size) { char buffer[5]; memcpy(buffer, parameter, size); buffer[size] = '\0'; return strtol(buffer, NULL, 10); } static urg_measurement_type_t parse_distance_parameter(urg_t *urg, const char echoback[]) { urg_measurement_type_t ret_type = URG_UNKNOWN; urg->received_range_data_byte = URG_COMMUNICATION_3_BYTE; if (echoback[1] == 'S') { urg->received_range_data_byte = URG_COMMUNICATION_2_BYTE; ret_type = URG_DISTANCE; } else if (echoback[1] == 'D') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO; } } else if (echoback[1] == 'E') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE_INTENSITY; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO_INTENSITY; } } else { return URG_UNKNOWN; } // パラメータの格納 urg->received_first_index = parse_parameter(&echoback[2], 4); urg->received_last_index = parse_parameter(&echoback[6], 4); urg->received_skip_step = parse_parameter(&echoback[10], 2); return ret_type; } static urg_measurement_type_t parse_distance_echoback(urg_t *urg, const char echoback[]) { size_t line_length; urg_measurement_type_t ret_type = URG_UNKNOWN; if (!strcmp("QT", echoback)) { return URG_STOP; } line_length = strlen(echoback); if ((line_length == 12) && ((echoback[0] == 'G') || (echoback[0] == 'H'))) { ret_type = parse_distance_parameter(urg, echoback); } else if ((line_length == 15) && ((echoback[0] == 'M') || (echoback[0] == 'N'))) { ret_type = parse_distance_parameter(urg, echoback); } return ret_type; } static int receive_length_data(urg_t *urg, long length[], unsigned short intensity[], urg_measurement_type_t type, char buffer[]) { int n; int step_filled = 0; int line_filled = 0; int multiecho_index = 0; int each_size = (urg->received_range_data_byte == URG_COMMUNICATION_2_BYTE) ? 2 : 3; int data_size = each_size; int is_intensity = URG_FALSE; int is_multiecho = URG_FALSE; int multiecho_max_size = 1; if ((type == URG_DISTANCE_INTENSITY) || (type == URG_MULTIECHO_INTENSITY)) { data_size *= 2; is_intensity = URG_TRUE; } if ((type == URG_MULTIECHO) || (type == URG_MULTIECHO_INTENSITY)) { is_multiecho = URG_TRUE; multiecho_max_size = URG_MAX_ECHO; } do { char *p = buffer; char *last_p; n = connection_readline(&urg->connection, &buffer[line_filled], BUFFER_SIZE - line_filled, urg->timeout); if (n > 0) { // チェックサムの評価 if (buffer[line_filled + n - 1] != scip_checksum(&buffer[line_filled], n - 1)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } } if (n > 0) { line_filled += n - 1; } last_p = p + line_filled; while ((last_p - p) >= data_size) { int index; if (*p == '&') { // 先頭文字が '&' だったときは、マルチエコーのデータとみなす if ((last_p - (p + 1)) < data_size) { // '&' を除いて、data_size 分データが無ければ抜ける break; } --step_filled; ++multiecho_index; ++p; --line_filled; } else { // 次のデータ multiecho_index = 0; } index = (step_filled * multiecho_max_size) + multiecho_index; if (step_filled > (urg->received_last_index - urg->received_first_index)) { // データが多過ぎる場合は、残りのデータを無視して戻る ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (is_multiecho && (multiecho_index == 0)) { // マルチエコーのデータ格納先をダミーデータで埋める int i; if (length) { for (i = 1; i < multiecho_max_size; ++i) { length[index + i] = 0; } } if (intensity) { for (i = 1; i < multiecho_max_size; ++i) { intensity[index + i] = 0; } } } // 距離データの格納 if (length) { length[index] = urg_scip_decode(p, 3); } p += 3; // 強度データの格納 if (is_intensity) { if (intensity) { intensity[index] = (unsigned short)urg_scip_decode(p, 3); } p += 3; } ++step_filled; line_filled -= data_size; } // 次に処理する文字を退避 memmove(buffer, p, line_filled); } while (n > 0); return step_filled; } //! 距離データの取得 static int receive_data(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { urg_measurement_type_t type; char buffer[BUFFER_SIZE]; int ret = 0; int n; int extended_timeout = urg->timeout + 2 * (urg->scan_usec * (urg->scanning_skip_scan) / 1000); // エコーバックの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, extended_timeout); if (n <= 0) { return set_errno_and_return(urg, URG_NO_RESPONSE); } // エコーバックの解析 type = parse_distance_echoback(urg, buffer); // 応答の取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 3) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } if (buffer[n - 1] != scip_checksum(buffer, n - 1)) { // チェックサムの評価 ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } if (type == URG_STOP) { // QT 応答の場合には、最後の改行を読み捨て、正常応答として処理する n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n == 0) { return 0; } else { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } if (urg->specified_scan_times != 1) { if (!strncmp(buffer, "00", 2)) { // "00" 応答の場合は、エコーバック応答とみなし、 // 最後の空行を読み捨て、次からのデータを返す n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 0) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } } } if (((urg->specified_scan_times == 1) && (strncmp(buffer, "00", 2))) || ((urg->specified_scan_times != 1) && (strncmp(buffer, "99", 2)))) { if (urg->error_handler) { type = urg->error_handler(buffer, urg); } if (type == URG_UNKNOWN) { // Gx, Hx のときは 00P が返されたときがデータ // Mx, Nx のときは 99b が返されたときがデータ ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } // タイムスタンプの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n > 0) { if (time_stamp) { *time_stamp = urg_scip_decode(buffer, 4); } if (system_time_stamp) { urg_get_walltime(system_time_stamp); } } // データの取得 switch (type) { case URG_DISTANCE: case URG_MULTIECHO: ret = receive_length_data(urg, data, NULL, type, buffer); break; case URG_DISTANCE_INTENSITY: case URG_MULTIECHO_INTENSITY: ret = receive_length_data(urg, data, intensity, type, buffer); break; case URG_STOP: case URG_UNKNOWN: ret = 0; break; } // specified_scan_times == 1 のときは Gx 系コマンドが使われるため // データを明示的に停止しなくてよい if ((urg->specified_scan_times > 1) && (urg->scanning_remain_times > 0)) { if (--urg->scanning_remain_times <= 0) { // データの停止のみを行う urg_stop_measurement(urg); } } return ret; } int urg_open(urg_t *urg, urg_connection_type_t connection_type, const char *device_or_address, long baudrate_or_port) { int ret; long baudrate = baudrate_or_port; urg->is_active = URG_FALSE; urg->is_sending = URG_TRUE; urg->last_errno = URG_NOT_CONNECTED; urg->timeout = MAX_TIMEOUT; urg->scanning_skip_scan = 0; urg->error_handler = NULL; // デバイスへの接続 ret = connection_open(&urg->connection, connection_type, device_or_address, baudrate_or_port); if (ret < 0) { switch (connection_type) { case URG_SERIAL: urg->last_errno = URG_SERIAL_OPEN_ERROR; break; case URG_ETHERNET: urg->last_errno = URG_ETHERNET_OPEN_ERROR; break; default: urg->last_errno = URG_INVALID_RESPONSE; break; } return urg->last_errno; } // 指定したボーレートで URG と通信できるように調整 if (connection_type == URG_ETHERNET) { // Ethernet のときは仮の通信速度を指定しておく baudrate = 115200; } if (connect_urg_device(urg, baudrate) != URG_NO_ERROR) { return set_errno_and_return(urg, ret); } urg->is_sending = URG_FALSE; // 変数の初期化 urg->last_errno = URG_NO_ERROR; urg->range_data_byte = URG_COMMUNICATION_3_BYTE; urg->specified_scan_times = 0; urg->scanning_remain_times = 0; urg->is_laser_on = URG_FALSE; // パラメータ情報を取得 ret = receive_parameter(urg); if (ret == URG_NO_ERROR) { urg->is_active = URG_TRUE; } return ret; } void urg_close(urg_t *urg) { if (urg->is_active) { ignore_receive_data_with_qt(urg, urg->timeout); } connection_close(&urg->connection); urg->is_active = URG_FALSE; } void urg_set_timeout_msec(urg_t *urg, int msec) { urg->timeout = msec; } int urg_start_time_stamp_mode(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM0 を発行する n = scip_response(urg, "TM0\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } long urg_time_stamp(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; char buffer[BUFFER_SIZE]; char *p; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } ret = scip_response(urg, "TM1\n", expected, urg->timeout, buffer, BUFFER_SIZE); if (ret < 0) { return ret; } // buffer からタイムスタンプを取得し、デコードして返す if (strcmp(buffer, "00P")) { // 最初の応答が "00P" でなければ戻る return set_errno_and_return(urg, URG_RECEIVE_ERROR); } p = buffer + 4; if (strlen(p) != 5) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (p[5] == scip_checksum(p, 4)) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } return urg_scip_decode(p, 4); } int urg_stop_time_stamp_mode(urg_t *urg) { int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM2 を発行する n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } static int send_distance_command(urg_t *urg, int scan_times, int skip_scan, char single_scan_ch, char continuous_scan_ch, char scan_type_ch) { char buffer[BUFFER_SIZE]; int write_size = 0; int front_index = urg->front_data_index; int n; urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times; urg->scanning_remain_times = urg->specified_scan_times; urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan; if (scan_times >= 100) { // 計測回数が 99 を越える場合は、無限回のスキャンを行う urg->specified_scan_times = 0; } if (urg->scanning_remain_times == 1) { // レーザ発光を指示 urg_laser_on(urg); write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n", single_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step); } else { write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n", continuous_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step, skip_scan, urg->specified_scan_times); urg->is_sending = URG_TRUE; } n = connection_write(&urg->connection, buffer, write_size); if (n != write_size) { return set_errno_and_return(urg, URG_SEND_ERROR); } return 0; } int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan) { char range_byte_ch; int ret = 0; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((skip_scan < 0) || (skip_scan > 9)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // !!! Mx 系, Nx 系の計測中のときは、QT を発行してから // !!! 計測開始コマンドを送信するようにする // !!! ただし、MD 計測中に MD を発行するように、同じコマンドの場合は // !!! Mx 系, Nx 系の計測は上書きすることができるようにする // 指定されたタイプのパケットを生成し、送信する switch (type) { case URG_DISTANCE: range_byte_ch = (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D'; ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', range_byte_ch); break; case URG_DISTANCE_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', 'E'); break; case URG_MULTIECHO: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'D'); break; case URG_MULTIECHO_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'E'); break; case URG_STOP: case URG_UNKNOWN: default: ignore_receive_data_with_qt(urg, urg->timeout); urg->last_errno = URG_INVALID_PARAMETER; ret = urg->last_errno; break; } return ret; } int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, NULL, time_stamp, system_time_stamp); } int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp); } int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp); } int urg_stop_measurement(urg_t *urg) { enum { MAX_READ_TIMES = 3 }; int ret = URG_INVALID_RESPONSE; int n; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // QT を発行する n = connection_write(&urg->connection, "QT\n", 3); if (n != 3) { return set_errno_and_return(urg, URG_SEND_ERROR); } for (i = 0; i < MAX_READ_TIMES; ++i) { // QT の応答が返されるまで、距離データを読み捨てる ret = receive_data(urg, NULL, NULL, NULL, NULL); if (ret == URG_NO_ERROR) { // 正常応答 urg->is_laser_on = URG_FALSE; urg->is_sending = URG_FALSE; return set_errno_and_return(urg, URG_NO_ERROR); } } return ret; } int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step) { // 設定の範囲外を指定したときは、エラーを返す if (((skip_step < 0) || (skip_step >= 100)) || (first_step > last_step) || (first_step < -urg->front_data_index) || (last_step > (urg->last_data_index - urg->front_data_index))) { return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR); } urg->scanning_first_step = first_step; urg->scanning_last_step = last_step; urg->scanning_skip_step = skip_step; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_set_connection_data_size(urg_t *urg, urg_range_data_byte_t data_byte) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((data_byte != URG_COMMUNICATION_3_BYTE) || (data_byte != URG_COMMUNICATION_2_BYTE)) { return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR); } urg->range_data_byte = data_byte; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_laser_on(urg_t *urg) { int expected[] = { 0, 2, EXPECTED_END }; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if (urg->is_laser_on != URG_FALSE) { // 既にレーザが発光しているときは、コマンドを送信しないようにする urg->last_errno = 0; return urg->last_errno; } ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0); if (ret >= 0) { urg->is_laser_on = URG_TRUE; ret = 0; } return ret; } int urg_laser_off(urg_t *urg) { return urg_stop_measurement(urg); } int urg_reboot(urg_t *urg) { int expected[] = { 0, 1, EXPECTED_END }; int ret; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // ２回目の RB 送信後、接続を切断する for (i = 0; i < 2; ++i) { ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0); if (ret < 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } urg->is_active = URG_FALSE; urg_close(urg); urg->last_errno = 0; return urg->last_errno; } void urg_sleep(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = 4 }; int sl_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE]; if (urg_stop_measurement(urg) != URG_NO_ERROR) { return; } scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); } void urg_wakeup(urg_t *urg) { urg_stop_measurement(urg); } int urg_is_stable(urg_t *urg) { const char *stat = urg_sensor_status(urg); return strncmp("Stable", stat, 6) ? 0 : 1; } static char *copy_token(char *dest, char *receive_buffer, const char *start_str, const char *end_ch, int lines) { size_t start_str_len = strlen(start_str); size_t end_ch_len = strlen(end_ch); int i; size_t j; for (j = 0; j < end_ch_len; ++j) { const char *p = receive_buffer; for (i = 0; i < lines; ++i) { if (!strncmp(p, start_str, start_str_len)) { char *last_p = strchr(p + start_str_len, end_ch[j]); if (last_p) { *last_p = '\0'; memcpy(dest, p + start_str_len, last_p - (p + start_str_len) + 1); return dest; } } p += strlen(p) + 1; } } return NULL; } static const char *receive_command_response(urg_t *urg, char *buffer, int buffer_size, const char* command, int response_lines) { const int vv_expected[] = { 0, EXPECTED_END }; int ret; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = scip_response(urg, command, vv_expected, urg->timeout, buffer, buffer_size); if (ret < response_lines) { return RECEIVE_ERROR_MESSAGE; } return NULL; } const char *urg_sensor_product_type(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROD:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_serial_id(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "SERI:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_vendor(urg_t *urg){ enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "VEND:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", "(", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_date(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } // Get the firmware version and append a '(', this will be what's before the date char firmware_version[50]; strcpy(firmware_version, urg_sensor_firmware_version(urg)); strcat(firmware_version, "("); ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } // Strip out the actual date from between the '(' and ')' char *date; p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", ";", VV_RESPONSE_LINES); date = copy_token(urg->return_buffer, p, firmware_version, ")", 1); return (date) ? date : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_protocol_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROT:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_status(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "STAT:", ";", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_state(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "MESM:", " (", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } void urg_set_error_handler(urg_t *urg, urg_error_handler handler) { urg->error_handler = handler; }
213 static int connect_urg_device(urg_t *urg, long baudrate)
214 {
215  long try_baudrate[] = { 19200, 38400, 115200 };
216  int try_times = sizeof(try_baudrate) / sizeof(try_baudrate[0]);
217  int i;
218 
219  // 指示されたボーレートから接続する for (i = 0; i < try_times; ++i) { if (try_baudrate[i] == baudrate) { try_baudrate[i] = try_baudrate[0]; try_baudrate[0] = baudrate; break; } } for (i = 0; i < try_times; ++i) { enum { RECEIVE_BUFFER_SIZE = 4 }; int qt_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE + 1]; int ret; connection_set_baudrate(&urg->connection, try_baudrate[i]); // QT を送信し、応答が返されるかでボーレートが一致しているかを確認する ret = scip_response(urg, "QT\n", qt_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); if (ret > 0) { if (!strcmp(receive_buffer, "E")) { int scip20_expected[] = { 0, EXPECTED_END }; // QT 応答の最後の改行を読み飛ばす ignore_receive_data(urg, MAX_TIMEOUT); // "E" が返された場合は、SCIP 1.1 とみなし "SCIP2.0" を送信する ret = scip_response(urg, "SCIP2.0\n", scip20_expected, MAX_TIMEOUT, NULL, 0); // SCIP2.0 応答の最後の改行を読み飛ばす ignore_receive_data(urg, MAX_TIMEOUT); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } else if (!strcmp(receive_buffer, "0Ee")) { int tm2_expected[] = { 0, EXPECTED_END }; // "0Ee" が返された場合は、TM モードとみなし "TM2" を送信する scip_response(urg, "TM2\n", tm2_expected, MAX_TIMEOUT, NULL, 0); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } if (ret <= 0) { if (ret == URG_INVALID_RESPONSE) { // 異常なエコーバックのときは、距離データ受信中とみなして // データを読み飛ばす ignore_receive_data_with_qt(urg, MAX_TIMEOUT); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } else { // 応答がないときは、ボーレートを変更して、再度接続を行う ignore_receive_data_with_qt(urg, MAX_TIMEOUT); continue; } } else if (!strcmp("00P", receive_buffer)) { // センサとホストのボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } return set_errno_and_return(urg, URG_NOT_DETECT_BAUDRATE_ERROR); } // PP コマンドの応答を urg_t に格納する static int receive_parameter(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * 9, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; int pp_expected[] = { 0, EXPECTED_END }; unsigned short received_bits = 0x0000; char *p; int i; int ret = scip_response(urg, "PP\n", pp_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); if (ret < 0) { return ret; } else if (ret < PP_RESPONSE_LINES) { ignore_receive_data_with_qt(urg, MAX_TIMEOUT); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } p = receive_buffer; for (i = 0; i < (ret - 1); ++i) { if (!strncmp(p, "DMIN:", 5)) { urg->min_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0001; } else if (!strncmp(p, "DMAX:", 5)) { urg->max_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0002; } else if (!strncmp(p, "ARES:", 5)) { urg->area_resolution = strtol(p + 5, NULL, 10); received_bits |= 0x0004; } else if (!strncmp(p, "AMIN:", 5)) { urg->first_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0008; } else if (!strncmp(p, "AMAX:", 5)) { urg->last_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0010; } else if (!strncmp(p, "AFRT:", 5)) { urg->front_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0020; } else if (!strncmp(p, "SCAN:", 5)) { int rpm = strtol(p + 5, NULL, 10); // タイムアウト時間は、計測周期の 16 倍程度の値にする urg->scan_usec = 1000 * 1000 * 60 / rpm; urg->timeout = urg->scan_usec >> (10 - 4); received_bits |= 0x0040; } p += strlen(p) + 1; } // 全てのパラメータを受信したか確認 if (received_bits != 0x007f) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } urg_set_scanning_parameter(urg, urg->first_data_index - urg->front_data_index, urg->last_data_index - urg->front_data_index, 1); return set_errno_and_return(urg, URG_NO_ERROR); } //! SCIP 文字列のデコード long urg_scip_decode(const char data[], int size) { const char* p = data; const char* last_p = p + size; int value = 0; while (p < last_p) { value <<= 6; value &= ~0x3f; value |= *p++ - 0x30; } return value; } static int parse_parameter(const char *parameter, int size) { char buffer[5]; memcpy(buffer, parameter, size); buffer[size] = '\0'; return strtol(buffer, NULL, 10); } static urg_measurement_type_t parse_distance_parameter(urg_t *urg, const char echoback[]) { urg_measurement_type_t ret_type = URG_UNKNOWN; urg->received_range_data_byte = URG_COMMUNICATION_3_BYTE; if (echoback[1] == 'S') { urg->received_range_data_byte = URG_COMMUNICATION_2_BYTE; ret_type = URG_DISTANCE; } else if (echoback[1] == 'D') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO; } } else if (echoback[1] == 'E') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE_INTENSITY; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO_INTENSITY; } } else { return URG_UNKNOWN; } // パラメータの格納 urg->received_first_index = parse_parameter(&echoback[2], 4); urg->received_last_index = parse_parameter(&echoback[6], 4); urg->received_skip_step = parse_parameter(&echoback[10], 2); return ret_type; } static urg_measurement_type_t parse_distance_echoback(urg_t *urg, const char echoback[]) { size_t line_length; urg_measurement_type_t ret_type = URG_UNKNOWN; if (!strcmp("QT", echoback)) { return URG_STOP; } line_length = strlen(echoback); if ((line_length == 12) && ((echoback[0] == 'G') || (echoback[0] == 'H'))) { ret_type = parse_distance_parameter(urg, echoback); } else if ((line_length == 15) && ((echoback[0] == 'M') || (echoback[0] == 'N'))) { ret_type = parse_distance_parameter(urg, echoback); } return ret_type; } static int receive_length_data(urg_t *urg, long length[], unsigned short intensity[], urg_measurement_type_t type, char buffer[]) { int n; int step_filled = 0; int line_filled = 0; int multiecho_index = 0; int each_size = (urg->received_range_data_byte == URG_COMMUNICATION_2_BYTE) ? 2 : 3; int data_size = each_size; int is_intensity = URG_FALSE; int is_multiecho = URG_FALSE; int multiecho_max_size = 1; if ((type == URG_DISTANCE_INTENSITY) || (type == URG_MULTIECHO_INTENSITY)) { data_size *= 2; is_intensity = URG_TRUE; } if ((type == URG_MULTIECHO) || (type == URG_MULTIECHO_INTENSITY)) { is_multiecho = URG_TRUE; multiecho_max_size = URG_MAX_ECHO; } do { char *p = buffer; char *last_p; n = connection_readline(&urg->connection, &buffer[line_filled], BUFFER_SIZE - line_filled, urg->timeout); if (n > 0) { // チェックサムの評価 if (buffer[line_filled + n - 1] != scip_checksum(&buffer[line_filled], n - 1)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } } if (n > 0) { line_filled += n - 1; } last_p = p + line_filled; while ((last_p - p) >= data_size) { int index; if (*p == '&') { // 先頭文字が '&' だったときは、マルチエコーのデータとみなす if ((last_p - (p + 1)) < data_size) { // '&' を除いて、data_size 分データが無ければ抜ける break; } --step_filled; ++multiecho_index; ++p; --line_filled; } else { // 次のデータ multiecho_index = 0; } index = (step_filled * multiecho_max_size) + multiecho_index; if (step_filled > (urg->received_last_index - urg->received_first_index)) { // データが多過ぎる場合は、残りのデータを無視して戻る ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (is_multiecho && (multiecho_index == 0)) { // マルチエコーのデータ格納先をダミーデータで埋める int i; if (length) { for (i = 1; i < multiecho_max_size; ++i) { length[index + i] = 0; } } if (intensity) { for (i = 1; i < multiecho_max_size; ++i) { intensity[index + i] = 0; } } } // 距離データの格納 if (length) { length[index] = urg_scip_decode(p, 3); } p += 3; // 強度データの格納 if (is_intensity) { if (intensity) { intensity[index] = (unsigned short)urg_scip_decode(p, 3); } p += 3; } ++step_filled; line_filled -= data_size; } // 次に処理する文字を退避 memmove(buffer, p, line_filled); } while (n > 0); return step_filled; } //! 距離データの取得 static int receive_data(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { urg_measurement_type_t type; char buffer[BUFFER_SIZE]; int ret = 0; int n; int extended_timeout = urg->timeout + 2 * (urg->scan_usec * (urg->scanning_skip_scan) / 1000); // エコーバックの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, extended_timeout); if (n <= 0) { return set_errno_and_return(urg, URG_NO_RESPONSE); } // エコーバックの解析 type = parse_distance_echoback(urg, buffer); // 応答の取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 3) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } if (buffer[n - 1] != scip_checksum(buffer, n - 1)) { // チェックサムの評価 ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } if (type == URG_STOP) { // QT 応答の場合には、最後の改行を読み捨て、正常応答として処理する n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n == 0) { return 0; } else { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } if (urg->specified_scan_times != 1) { if (!strncmp(buffer, "00", 2)) { // "00" 応答の場合は、エコーバック応答とみなし、 // 最後の空行を読み捨て、次からのデータを返す n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 0) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } } } if (((urg->specified_scan_times == 1) && (strncmp(buffer, "00", 2))) || ((urg->specified_scan_times != 1) && (strncmp(buffer, "99", 2)))) { if (urg->error_handler) { type = urg->error_handler(buffer, urg); } if (type == URG_UNKNOWN) { // Gx, Hx のときは 00P が返されたときがデータ // Mx, Nx のときは 99b が返されたときがデータ ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } // タイムスタンプの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n > 0) { if (time_stamp) { *time_stamp = urg_scip_decode(buffer, 4); } if (system_time_stamp) { urg_get_walltime(system_time_stamp); } } // データの取得 switch (type) { case URG_DISTANCE: case URG_MULTIECHO: ret = receive_length_data(urg, data, NULL, type, buffer); break; case URG_DISTANCE_INTENSITY: case URG_MULTIECHO_INTENSITY: ret = receive_length_data(urg, data, intensity, type, buffer); break; case URG_STOP: case URG_UNKNOWN: ret = 0; break; } // specified_scan_times == 1 のときは Gx 系コマンドが使われるため // データを明示的に停止しなくてよい if ((urg->specified_scan_times > 1) && (urg->scanning_remain_times > 0)) { if (--urg->scanning_remain_times <= 0) { // データの停止のみを行う urg_stop_measurement(urg); } } return ret; } int urg_open(urg_t *urg, urg_connection_type_t connection_type, const char *device_or_address, long baudrate_or_port) { int ret; long baudrate = baudrate_or_port; urg->is_active = URG_FALSE; urg->is_sending = URG_TRUE; urg->last_errno = URG_NOT_CONNECTED; urg->timeout = MAX_TIMEOUT; urg->scanning_skip_scan = 0; urg->error_handler = NULL; // デバイスへの接続 ret = connection_open(&urg->connection, connection_type, device_or_address, baudrate_or_port); if (ret < 0) { switch (connection_type) { case URG_SERIAL: urg->last_errno = URG_SERIAL_OPEN_ERROR; break; case URG_ETHERNET: urg->last_errno = URG_ETHERNET_OPEN_ERROR; break; default: urg->last_errno = URG_INVALID_RESPONSE; break; } return urg->last_errno; } // 指定したボーレートで URG と通信できるように調整 if (connection_type == URG_ETHERNET) { // Ethernet のときは仮の通信速度を指定しておく baudrate = 115200; } if (connect_urg_device(urg, baudrate) != URG_NO_ERROR) { return set_errno_and_return(urg, ret); } urg->is_sending = URG_FALSE; // 変数の初期化 urg->last_errno = URG_NO_ERROR; urg->range_data_byte = URG_COMMUNICATION_3_BYTE; urg->specified_scan_times = 0; urg->scanning_remain_times = 0; urg->is_laser_on = URG_FALSE; // パラメータ情報を取得 ret = receive_parameter(urg); if (ret == URG_NO_ERROR) { urg->is_active = URG_TRUE; } return ret; } void urg_close(urg_t *urg) { if (urg->is_active) { ignore_receive_data_with_qt(urg, urg->timeout); } connection_close(&urg->connection); urg->is_active = URG_FALSE; } void urg_set_timeout_msec(urg_t *urg, int msec) { urg->timeout = msec; } int urg_start_time_stamp_mode(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM0 を発行する n = scip_response(urg, "TM0\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } long urg_time_stamp(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; char buffer[BUFFER_SIZE]; char *p; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } ret = scip_response(urg, "TM1\n", expected, urg->timeout, buffer, BUFFER_SIZE); if (ret < 0) { return ret; } // buffer からタイムスタンプを取得し、デコードして返す if (strcmp(buffer, "00P")) { // 最初の応答が "00P" でなければ戻る return set_errno_and_return(urg, URG_RECEIVE_ERROR); } p = buffer + 4; if (strlen(p) != 5) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (p[5] == scip_checksum(p, 4)) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } return urg_scip_decode(p, 4); } int urg_stop_time_stamp_mode(urg_t *urg) { int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM2 を発行する n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } static int send_distance_command(urg_t *urg, int scan_times, int skip_scan, char single_scan_ch, char continuous_scan_ch, char scan_type_ch) { char buffer[BUFFER_SIZE]; int write_size = 0; int front_index = urg->front_data_index; int n; urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times; urg->scanning_remain_times = urg->specified_scan_times; urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan; if (scan_times >= 100) { // 計測回数が 99 を越える場合は、無限回のスキャンを行う urg->specified_scan_times = 0; } if (urg->scanning_remain_times == 1) { // レーザ発光を指示 urg_laser_on(urg); write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n", single_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step); } else { write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n", continuous_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step, skip_scan, urg->specified_scan_times); urg->is_sending = URG_TRUE; } n = connection_write(&urg->connection, buffer, write_size); if (n != write_size) { return set_errno_and_return(urg, URG_SEND_ERROR); } return 0; } int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan) { char range_byte_ch; int ret = 0; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((skip_scan < 0) || (skip_scan > 9)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // !!! Mx 系, Nx 系の計測中のときは、QT を発行してから // !!! 計測開始コマンドを送信するようにする // !!! ただし、MD 計測中に MD を発行するように、同じコマンドの場合は // !!! Mx 系, Nx 系の計測は上書きすることができるようにする // 指定されたタイプのパケットを生成し、送信する switch (type) { case URG_DISTANCE: range_byte_ch = (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D'; ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', range_byte_ch); break; case URG_DISTANCE_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', 'E'); break; case URG_MULTIECHO: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'D'); break; case URG_MULTIECHO_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'E'); break; case URG_STOP: case URG_UNKNOWN: default: ignore_receive_data_with_qt(urg, urg->timeout); urg->last_errno = URG_INVALID_PARAMETER; ret = urg->last_errno; break; } return ret; } int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, NULL, time_stamp, system_time_stamp); } int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp); } int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp); } int urg_stop_measurement(urg_t *urg) { enum { MAX_READ_TIMES = 3 }; int ret = URG_INVALID_RESPONSE; int n; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // QT を発行する n = connection_write(&urg->connection, "QT\n", 3); if (n != 3) { return set_errno_and_return(urg, URG_SEND_ERROR); } for (i = 0; i < MAX_READ_TIMES; ++i) { // QT の応答が返されるまで、距離データを読み捨てる ret = receive_data(urg, NULL, NULL, NULL, NULL); if (ret == URG_NO_ERROR) { // 正常応答 urg->is_laser_on = URG_FALSE; urg->is_sending = URG_FALSE; return set_errno_and_return(urg, URG_NO_ERROR); } } return ret; } int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step) { // 設定の範囲外を指定したときは、エラーを返す if (((skip_step < 0) || (skip_step >= 100)) || (first_step > last_step) || (first_step < -urg->front_data_index) || (last_step > (urg->last_data_index - urg->front_data_index))) { return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR); } urg->scanning_first_step = first_step; urg->scanning_last_step = last_step; urg->scanning_skip_step = skip_step; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_set_connection_data_size(urg_t *urg, urg_range_data_byte_t data_byte) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((data_byte != URG_COMMUNICATION_3_BYTE) || (data_byte != URG_COMMUNICATION_2_BYTE)) { return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR); } urg->range_data_byte = data_byte; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_laser_on(urg_t *urg) { int expected[] = { 0, 2, EXPECTED_END }; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if (urg->is_laser_on != URG_FALSE) { // 既にレーザが発光しているときは、コマンドを送信しないようにする urg->last_errno = 0; return urg->last_errno; } ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0); if (ret >= 0) { urg->is_laser_on = URG_TRUE; ret = 0; } return ret; } int urg_laser_off(urg_t *urg) { return urg_stop_measurement(urg); } int urg_reboot(urg_t *urg) { int expected[] = { 0, 1, EXPECTED_END }; int ret; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // ２回目の RB 送信後、接続を切断する for (i = 0; i < 2; ++i) { ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0); if (ret < 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } urg->is_active = URG_FALSE; urg_close(urg); urg->last_errno = 0; return urg->last_errno; } void urg_sleep(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = 4 }; int sl_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE]; if (urg_stop_measurement(urg) != URG_NO_ERROR) { return; } scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); } void urg_wakeup(urg_t *urg) { urg_stop_measurement(urg); } int urg_is_stable(urg_t *urg) { const char *stat = urg_sensor_status(urg); return strncmp("Stable", stat, 6) ? 0 : 1; } static char *copy_token(char *dest, char *receive_buffer, const char *start_str, const char *end_ch, int lines) { size_t start_str_len = strlen(start_str); size_t end_ch_len = strlen(end_ch); int i; size_t j; for (j = 0; j < end_ch_len; ++j) { const char *p = receive_buffer; for (i = 0; i < lines; ++i) { if (!strncmp(p, start_str, start_str_len)) { char *last_p = strchr(p + start_str_len, end_ch[j]); if (last_p) { *last_p = '\0'; memcpy(dest, p + start_str_len, last_p - (p + start_str_len) + 1); return dest; } } p += strlen(p) + 1; } } return NULL; } static const char *receive_command_response(urg_t *urg, char *buffer, int buffer_size, const char* command, int response_lines) { const int vv_expected[] = { 0, EXPECTED_END }; int ret; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = scip_response(urg, command, vv_expected, urg->timeout, buffer, buffer_size); if (ret < response_lines) { return RECEIVE_ERROR_MESSAGE; } return NULL; } const char *urg_sensor_product_type(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROD:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_serial_id(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "SERI:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_vendor(urg_t *urg){ enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "VEND:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", "(", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_date(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } // Get the firmware version and append a '(', this will be what's before the date char firmware_version[50]; strcpy(firmware_version, urg_sensor_firmware_version(urg)); strcat(firmware_version, "("); ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } // Strip out the actual date from between the '(' and ')' char *date; p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", ";", VV_RESPONSE_LINES); date = copy_token(urg->return_buffer, p, firmware_version, ")", 1); return (date) ? date : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_protocol_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROT:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_status(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "STAT:", ";", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_state(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "MESM:", " (", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } void urg_set_error_handler(urg_t *urg, urg_error_handler handler) { urg->error_handler = handler; }
220  for (i = 0; i < try_times; ++i) {
221  if (try_baudrate[i] == baudrate) {
222  try_baudrate[i] = try_baudrate[0];
223  try_baudrate[0] = baudrate;
224  break;
225  }
226  }
227 
228  for (i = 0; i < try_times; ++i) {
229  enum { RECEIVE_BUFFER_SIZE = 4 };
230  int qt_expected[] = { 0, EXPECTED_END };
231  char receive_buffer[RECEIVE_BUFFER_SIZE + 1];
232  int ret;
233 
234  connection_set_baudrate(&urg->connection, try_baudrate[i]);
235 
236  // QT を送信し、応答が返されるかでボーレートが一致しているかを確認する ret = scip_response(urg, "QT\n", qt_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); if (ret > 0) { if (!strcmp(receive_buffer, "E")) { int scip20_expected[] = { 0, EXPECTED_END }; // QT 応答の最後の改行を読み飛ばす ignore_receive_data(urg, MAX_TIMEOUT); // "E" が返された場合は、SCIP 1.1 とみなし "SCIP2.0" を送信する ret = scip_response(urg, "SCIP2.0\n", scip20_expected, MAX_TIMEOUT, NULL, 0); // SCIP2.0 応答の最後の改行を読み飛ばす ignore_receive_data(urg, MAX_TIMEOUT); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } else if (!strcmp(receive_buffer, "0Ee")) { int tm2_expected[] = { 0, EXPECTED_END }; // "0Ee" が返された場合は、TM モードとみなし "TM2" を送信する scip_response(urg, "TM2\n", tm2_expected, MAX_TIMEOUT, NULL, 0); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } if (ret <= 0) { if (ret == URG_INVALID_RESPONSE) { // 異常なエコーバックのときは、距離データ受信中とみなして // データを読み飛ばす ignore_receive_data_with_qt(urg, MAX_TIMEOUT); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } else { // 応答がないときは、ボーレートを変更して、再度接続を行う ignore_receive_data_with_qt(urg, MAX_TIMEOUT); continue; } } else if (!strcmp("00P", receive_buffer)) { // センサとホストのボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } return set_errno_and_return(urg, URG_NOT_DETECT_BAUDRATE_ERROR); } // PP コマンドの応答を urg_t に格納する static int receive_parameter(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * 9, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; int pp_expected[] = { 0, EXPECTED_END }; unsigned short received_bits = 0x0000; char *p; int i; int ret = scip_response(urg, "PP\n", pp_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); if (ret < 0) { return ret; } else if (ret < PP_RESPONSE_LINES) { ignore_receive_data_with_qt(urg, MAX_TIMEOUT); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } p = receive_buffer; for (i = 0; i < (ret - 1); ++i) { if (!strncmp(p, "DMIN:", 5)) { urg->min_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0001; } else if (!strncmp(p, "DMAX:", 5)) { urg->max_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0002; } else if (!strncmp(p, "ARES:", 5)) { urg->area_resolution = strtol(p + 5, NULL, 10); received_bits |= 0x0004; } else if (!strncmp(p, "AMIN:", 5)) { urg->first_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0008; } else if (!strncmp(p, "AMAX:", 5)) { urg->last_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0010; } else if (!strncmp(p, "AFRT:", 5)) { urg->front_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0020; } else if (!strncmp(p, "SCAN:", 5)) { int rpm = strtol(p + 5, NULL, 10); // タイムアウト時間は、計測周期の 16 倍程度の値にする urg->scan_usec = 1000 * 1000 * 60 / rpm; urg->timeout = urg->scan_usec >> (10 - 4); received_bits |= 0x0040; } p += strlen(p) + 1; } // 全てのパラメータを受信したか確認 if (received_bits != 0x007f) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } urg_set_scanning_parameter(urg, urg->first_data_index - urg->front_data_index, urg->last_data_index - urg->front_data_index, 1); return set_errno_and_return(urg, URG_NO_ERROR); } //! SCIP 文字列のデコード long urg_scip_decode(const char data[], int size) { const char* p = data; const char* last_p = p + size; int value = 0; while (p < last_p) { value <<= 6; value &= ~0x3f; value |= *p++ - 0x30; } return value; } static int parse_parameter(const char *parameter, int size) { char buffer[5]; memcpy(buffer, parameter, size); buffer[size] = '\0'; return strtol(buffer, NULL, 10); } static urg_measurement_type_t parse_distance_parameter(urg_t *urg, const char echoback[]) { urg_measurement_type_t ret_type = URG_UNKNOWN; urg->received_range_data_byte = URG_COMMUNICATION_3_BYTE; if (echoback[1] == 'S') { urg->received_range_data_byte = URG_COMMUNICATION_2_BYTE; ret_type = URG_DISTANCE; } else if (echoback[1] == 'D') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO; } } else if (echoback[1] == 'E') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE_INTENSITY; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO_INTENSITY; } } else { return URG_UNKNOWN; } // パラメータの格納 urg->received_first_index = parse_parameter(&echoback[2], 4); urg->received_last_index = parse_parameter(&echoback[6], 4); urg->received_skip_step = parse_parameter(&echoback[10], 2); return ret_type; } static urg_measurement_type_t parse_distance_echoback(urg_t *urg, const char echoback[]) { size_t line_length; urg_measurement_type_t ret_type = URG_UNKNOWN; if (!strcmp("QT", echoback)) { return URG_STOP; } line_length = strlen(echoback); if ((line_length == 12) && ((echoback[0] == 'G') || (echoback[0] == 'H'))) { ret_type = parse_distance_parameter(urg, echoback); } else if ((line_length == 15) && ((echoback[0] == 'M') || (echoback[0] == 'N'))) { ret_type = parse_distance_parameter(urg, echoback); } return ret_type; } static int receive_length_data(urg_t *urg, long length[], unsigned short intensity[], urg_measurement_type_t type, char buffer[]) { int n; int step_filled = 0; int line_filled = 0; int multiecho_index = 0; int each_size = (urg->received_range_data_byte == URG_COMMUNICATION_2_BYTE) ? 2 : 3; int data_size = each_size; int is_intensity = URG_FALSE; int is_multiecho = URG_FALSE; int multiecho_max_size = 1; if ((type == URG_DISTANCE_INTENSITY) || (type == URG_MULTIECHO_INTENSITY)) { data_size *= 2; is_intensity = URG_TRUE; } if ((type == URG_MULTIECHO) || (type == URG_MULTIECHO_INTENSITY)) { is_multiecho = URG_TRUE; multiecho_max_size = URG_MAX_ECHO; } do { char *p = buffer; char *last_p; n = connection_readline(&urg->connection, &buffer[line_filled], BUFFER_SIZE - line_filled, urg->timeout); if (n > 0) { // チェックサムの評価 if (buffer[line_filled + n - 1] != scip_checksum(&buffer[line_filled], n - 1)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } } if (n > 0) { line_filled += n - 1; } last_p = p + line_filled; while ((last_p - p) >= data_size) { int index; if (*p == '&') { // 先頭文字が '&' だったときは、マルチエコーのデータとみなす if ((last_p - (p + 1)) < data_size) { // '&' を除いて、data_size 分データが無ければ抜ける break; } --step_filled; ++multiecho_index; ++p; --line_filled; } else { // 次のデータ multiecho_index = 0; } index = (step_filled * multiecho_max_size) + multiecho_index; if (step_filled > (urg->received_last_index - urg->received_first_index)) { // データが多過ぎる場合は、残りのデータを無視して戻る ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (is_multiecho && (multiecho_index == 0)) { // マルチエコーのデータ格納先をダミーデータで埋める int i; if (length) { for (i = 1; i < multiecho_max_size; ++i) { length[index + i] = 0; } } if (intensity) { for (i = 1; i < multiecho_max_size; ++i) { intensity[index + i] = 0; } } } // 距離データの格納 if (length) { length[index] = urg_scip_decode(p, 3); } p += 3; // 強度データの格納 if (is_intensity) { if (intensity) { intensity[index] = (unsigned short)urg_scip_decode(p, 3); } p += 3; } ++step_filled; line_filled -= data_size; } // 次に処理する文字を退避 memmove(buffer, p, line_filled); } while (n > 0); return step_filled; } //! 距離データの取得 static int receive_data(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { urg_measurement_type_t type; char buffer[BUFFER_SIZE]; int ret = 0; int n; int extended_timeout = urg->timeout + 2 * (urg->scan_usec * (urg->scanning_skip_scan) / 1000); // エコーバックの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, extended_timeout); if (n <= 0) { return set_errno_and_return(urg, URG_NO_RESPONSE); } // エコーバックの解析 type = parse_distance_echoback(urg, buffer); // 応答の取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 3) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } if (buffer[n - 1] != scip_checksum(buffer, n - 1)) { // チェックサムの評価 ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } if (type == URG_STOP) { // QT 応答の場合には、最後の改行を読み捨て、正常応答として処理する n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n == 0) { return 0; } else { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } if (urg->specified_scan_times != 1) { if (!strncmp(buffer, "00", 2)) { // "00" 応答の場合は、エコーバック応答とみなし、 // 最後の空行を読み捨て、次からのデータを返す n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 0) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } } } if (((urg->specified_scan_times == 1) && (strncmp(buffer, "00", 2))) || ((urg->specified_scan_times != 1) && (strncmp(buffer, "99", 2)))) { if (urg->error_handler) { type = urg->error_handler(buffer, urg); } if (type == URG_UNKNOWN) { // Gx, Hx のときは 00P が返されたときがデータ // Mx, Nx のときは 99b が返されたときがデータ ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } // タイムスタンプの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n > 0) { if (time_stamp) { *time_stamp = urg_scip_decode(buffer, 4); } if (system_time_stamp) { urg_get_walltime(system_time_stamp); } } // データの取得 switch (type) { case URG_DISTANCE: case URG_MULTIECHO: ret = receive_length_data(urg, data, NULL, type, buffer); break; case URG_DISTANCE_INTENSITY: case URG_MULTIECHO_INTENSITY: ret = receive_length_data(urg, data, intensity, type, buffer); break; case URG_STOP: case URG_UNKNOWN: ret = 0; break; } // specified_scan_times == 1 のときは Gx 系コマンドが使われるため // データを明示的に停止しなくてよい if ((urg->specified_scan_times > 1) && (urg->scanning_remain_times > 0)) { if (--urg->scanning_remain_times <= 0) { // データの停止のみを行う urg_stop_measurement(urg); } } return ret; } int urg_open(urg_t *urg, urg_connection_type_t connection_type, const char *device_or_address, long baudrate_or_port) { int ret; long baudrate = baudrate_or_port; urg->is_active = URG_FALSE; urg->is_sending = URG_TRUE; urg->last_errno = URG_NOT_CONNECTED; urg->timeout = MAX_TIMEOUT; urg->scanning_skip_scan = 0; urg->error_handler = NULL; // デバイスへの接続 ret = connection_open(&urg->connection, connection_type, device_or_address, baudrate_or_port); if (ret < 0) { switch (connection_type) { case URG_SERIAL: urg->last_errno = URG_SERIAL_OPEN_ERROR; break; case URG_ETHERNET: urg->last_errno = URG_ETHERNET_OPEN_ERROR; break; default: urg->last_errno = URG_INVALID_RESPONSE; break; } return urg->last_errno; } // 指定したボーレートで URG と通信できるように調整 if (connection_type == URG_ETHERNET) { // Ethernet のときは仮の通信速度を指定しておく baudrate = 115200; } if (connect_urg_device(urg, baudrate) != URG_NO_ERROR) { return set_errno_and_return(urg, ret); } urg->is_sending = URG_FALSE; // 変数の初期化 urg->last_errno = URG_NO_ERROR; urg->range_data_byte = URG_COMMUNICATION_3_BYTE; urg->specified_scan_times = 0; urg->scanning_remain_times = 0; urg->is_laser_on = URG_FALSE; // パラメータ情報を取得 ret = receive_parameter(urg); if (ret == URG_NO_ERROR) { urg->is_active = URG_TRUE; } return ret; } void urg_close(urg_t *urg) { if (urg->is_active) { ignore_receive_data_with_qt(urg, urg->timeout); } connection_close(&urg->connection); urg->is_active = URG_FALSE; } void urg_set_timeout_msec(urg_t *urg, int msec) { urg->timeout = msec; } int urg_start_time_stamp_mode(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM0 を発行する n = scip_response(urg, "TM0\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } long urg_time_stamp(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; char buffer[BUFFER_SIZE]; char *p; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } ret = scip_response(urg, "TM1\n", expected, urg->timeout, buffer, BUFFER_SIZE); if (ret < 0) { return ret; } // buffer からタイムスタンプを取得し、デコードして返す if (strcmp(buffer, "00P")) { // 最初の応答が "00P" でなければ戻る return set_errno_and_return(urg, URG_RECEIVE_ERROR); } p = buffer + 4; if (strlen(p) != 5) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (p[5] == scip_checksum(p, 4)) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } return urg_scip_decode(p, 4); } int urg_stop_time_stamp_mode(urg_t *urg) { int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM2 を発行する n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } static int send_distance_command(urg_t *urg, int scan_times, int skip_scan, char single_scan_ch, char continuous_scan_ch, char scan_type_ch) { char buffer[BUFFER_SIZE]; int write_size = 0; int front_index = urg->front_data_index; int n; urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times; urg->scanning_remain_times = urg->specified_scan_times; urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan; if (scan_times >= 100) { // 計測回数が 99 を越える場合は、無限回のスキャンを行う urg->specified_scan_times = 0; } if (urg->scanning_remain_times == 1) { // レーザ発光を指示 urg_laser_on(urg); write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n", single_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step); } else { write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n", continuous_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step, skip_scan, urg->specified_scan_times); urg->is_sending = URG_TRUE; } n = connection_write(&urg->connection, buffer, write_size); if (n != write_size) { return set_errno_and_return(urg, URG_SEND_ERROR); } return 0; } int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan) { char range_byte_ch; int ret = 0; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((skip_scan < 0) || (skip_scan > 9)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // !!! Mx 系, Nx 系の計測中のときは、QT を発行してから // !!! 計測開始コマンドを送信するようにする // !!! ただし、MD 計測中に MD を発行するように、同じコマンドの場合は // !!! Mx 系, Nx 系の計測は上書きすることができるようにする // 指定されたタイプのパケットを生成し、送信する switch (type) { case URG_DISTANCE: range_byte_ch = (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D'; ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', range_byte_ch); break; case URG_DISTANCE_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', 'E'); break; case URG_MULTIECHO: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'D'); break; case URG_MULTIECHO_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'E'); break; case URG_STOP: case URG_UNKNOWN: default: ignore_receive_data_with_qt(urg, urg->timeout); urg->last_errno = URG_INVALID_PARAMETER; ret = urg->last_errno; break; } return ret; } int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, NULL, time_stamp, system_time_stamp); } int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp); } int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp); } int urg_stop_measurement(urg_t *urg) { enum { MAX_READ_TIMES = 3 }; int ret = URG_INVALID_RESPONSE; int n; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // QT を発行する n = connection_write(&urg->connection, "QT\n", 3); if (n != 3) { return set_errno_and_return(urg, URG_SEND_ERROR); } for (i = 0; i < MAX_READ_TIMES; ++i) { // QT の応答が返されるまで、距離データを読み捨てる ret = receive_data(urg, NULL, NULL, NULL, NULL); if (ret == URG_NO_ERROR) { // 正常応答 urg->is_laser_on = URG_FALSE; urg->is_sending = URG_FALSE; return set_errno_and_return(urg, URG_NO_ERROR); } } return ret; } int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step) { // 設定の範囲外を指定したときは、エラーを返す if (((skip_step < 0) || (skip_step >= 100)) || (first_step > last_step) || (first_step < -urg->front_data_index) || (last_step > (urg->last_data_index - urg->front_data_index))) { return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR); } urg->scanning_first_step = first_step; urg->scanning_last_step = last_step; urg->scanning_skip_step = skip_step; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_set_connection_data_size(urg_t *urg, urg_range_data_byte_t data_byte) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((data_byte != URG_COMMUNICATION_3_BYTE) || (data_byte != URG_COMMUNICATION_2_BYTE)) { return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR); } urg->range_data_byte = data_byte; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_laser_on(urg_t *urg) { int expected[] = { 0, 2, EXPECTED_END }; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if (urg->is_laser_on != URG_FALSE) { // 既にレーザが発光しているときは、コマンドを送信しないようにする urg->last_errno = 0; return urg->last_errno; } ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0); if (ret >= 0) { urg->is_laser_on = URG_TRUE; ret = 0; } return ret; } int urg_laser_off(urg_t *urg) { return urg_stop_measurement(urg); } int urg_reboot(urg_t *urg) { int expected[] = { 0, 1, EXPECTED_END }; int ret; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // ２回目の RB 送信後、接続を切断する for (i = 0; i < 2; ++i) { ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0); if (ret < 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } urg->is_active = URG_FALSE; urg_close(urg); urg->last_errno = 0; return urg->last_errno; } void urg_sleep(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = 4 }; int sl_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE]; if (urg_stop_measurement(urg) != URG_NO_ERROR) { return; } scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); } void urg_wakeup(urg_t *urg) { urg_stop_measurement(urg); } int urg_is_stable(urg_t *urg) { const char *stat = urg_sensor_status(urg); return strncmp("Stable", stat, 6) ? 0 : 1; } static char *copy_token(char *dest, char *receive_buffer, const char *start_str, const char *end_ch, int lines) { size_t start_str_len = strlen(start_str); size_t end_ch_len = strlen(end_ch); int i; size_t j; for (j = 0; j < end_ch_len; ++j) { const char *p = receive_buffer; for (i = 0; i < lines; ++i) { if (!strncmp(p, start_str, start_str_len)) { char *last_p = strchr(p + start_str_len, end_ch[j]); if (last_p) { *last_p = '\0'; memcpy(dest, p + start_str_len, last_p - (p + start_str_len) + 1); return dest; } } p += strlen(p) + 1; } } return NULL; } static const char *receive_command_response(urg_t *urg, char *buffer, int buffer_size, const char* command, int response_lines) { const int vv_expected[] = { 0, EXPECTED_END }; int ret; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = scip_response(urg, command, vv_expected, urg->timeout, buffer, buffer_size); if (ret < response_lines) { return RECEIVE_ERROR_MESSAGE; } return NULL; } const char *urg_sensor_product_type(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROD:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_serial_id(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "SERI:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_vendor(urg_t *urg){ enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "VEND:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", "(", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_date(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } // Get the firmware version and append a '(', this will be what's before the date char firmware_version[50]; strcpy(firmware_version, urg_sensor_firmware_version(urg)); strcat(firmware_version, "("); ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } // Strip out the actual date from between the '(' and ')' char *date; p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", ";", VV_RESPONSE_LINES); date = copy_token(urg->return_buffer, p, firmware_version, ")", 1); return (date) ? date : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_protocol_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROT:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_status(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "STAT:", ";", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_state(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "MESM:", " (", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } void urg_set_error_handler(urg_t *urg, urg_error_handler handler) { urg->error_handler = handler; }
237  ret = scip_response(urg, "QT\n", qt_expected, MAX_TIMEOUT,
238  receive_buffer, RECEIVE_BUFFER_SIZE);
239  if (ret > 0) {
240  if (!strcmp(receive_buffer, "E")) {
241  int scip20_expected[] = { 0, EXPECTED_END };
242 
243  // QT 応答の最後の改行を読み飛ばす
244  ignore_receive_data(urg, MAX_TIMEOUT);
245 
246  // "E" が返された場合は、SCIP 1.1 とみなし "SCIP2.0" を送信する ret = scip_response(urg, "SCIP2.0\n", scip20_expected, MAX_TIMEOUT, NULL, 0); // SCIP2.0 応答の最後の改行を読み飛ばす ignore_receive_data(urg, MAX_TIMEOUT); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } else if (!strcmp(receive_buffer, "0Ee")) { int tm2_expected[] = { 0, EXPECTED_END }; // "0Ee" が返された場合は、TM モードとみなし "TM2" を送信する scip_response(urg, "TM2\n", tm2_expected, MAX_TIMEOUT, NULL, 0); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } if (ret <= 0) { if (ret == URG_INVALID_RESPONSE) { // 異常なエコーバックのときは、距離データ受信中とみなして // データを読み飛ばす ignore_receive_data_with_qt(urg, MAX_TIMEOUT); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } else { // 応答がないときは、ボーレートを変更して、再度接続を行う ignore_receive_data_with_qt(urg, MAX_TIMEOUT); continue; } } else if (!strcmp("00P", receive_buffer)) { // センサとホストのボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } return set_errno_and_return(urg, URG_NOT_DETECT_BAUDRATE_ERROR); } // PP コマンドの応答を urg_t に格納する static int receive_parameter(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * 9, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; int pp_expected[] = { 0, EXPECTED_END }; unsigned short received_bits = 0x0000; char *p; int i; int ret = scip_response(urg, "PP\n", pp_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); if (ret < 0) { return ret; } else if (ret < PP_RESPONSE_LINES) { ignore_receive_data_with_qt(urg, MAX_TIMEOUT); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } p = receive_buffer; for (i = 0; i < (ret - 1); ++i) { if (!strncmp(p, "DMIN:", 5)) { urg->min_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0001; } else if (!strncmp(p, "DMAX:", 5)) { urg->max_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0002; } else if (!strncmp(p, "ARES:", 5)) { urg->area_resolution = strtol(p + 5, NULL, 10); received_bits |= 0x0004; } else if (!strncmp(p, "AMIN:", 5)) { urg->first_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0008; } else if (!strncmp(p, "AMAX:", 5)) { urg->last_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0010; } else if (!strncmp(p, "AFRT:", 5)) { urg->front_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0020; } else if (!strncmp(p, "SCAN:", 5)) { int rpm = strtol(p + 5, NULL, 10); // タイムアウト時間は、計測周期の 16 倍程度の値にする urg->scan_usec = 1000 * 1000 * 60 / rpm; urg->timeout = urg->scan_usec >> (10 - 4); received_bits |= 0x0040; } p += strlen(p) + 1; } // 全てのパラメータを受信したか確認 if (received_bits != 0x007f) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } urg_set_scanning_parameter(urg, urg->first_data_index - urg->front_data_index, urg->last_data_index - urg->front_data_index, 1); return set_errno_and_return(urg, URG_NO_ERROR); } //! SCIP 文字列のデコード long urg_scip_decode(const char data[], int size) { const char* p = data; const char* last_p = p + size; int value = 0; while (p < last_p) { value <<= 6; value &= ~0x3f; value |= *p++ - 0x30; } return value; } static int parse_parameter(const char *parameter, int size) { char buffer[5]; memcpy(buffer, parameter, size); buffer[size] = '\0'; return strtol(buffer, NULL, 10); } static urg_measurement_type_t parse_distance_parameter(urg_t *urg, const char echoback[]) { urg_measurement_type_t ret_type = URG_UNKNOWN; urg->received_range_data_byte = URG_COMMUNICATION_3_BYTE; if (echoback[1] == 'S') { urg->received_range_data_byte = URG_COMMUNICATION_2_BYTE; ret_type = URG_DISTANCE; } else if (echoback[1] == 'D') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO; } } else if (echoback[1] == 'E') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE_INTENSITY; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO_INTENSITY; } } else { return URG_UNKNOWN; } // パラメータの格納 urg->received_first_index = parse_parameter(&echoback[2], 4); urg->received_last_index = parse_parameter(&echoback[6], 4); urg->received_skip_step = parse_parameter(&echoback[10], 2); return ret_type; } static urg_measurement_type_t parse_distance_echoback(urg_t *urg, const char echoback[]) { size_t line_length; urg_measurement_type_t ret_type = URG_UNKNOWN; if (!strcmp("QT", echoback)) { return URG_STOP; } line_length = strlen(echoback); if ((line_length == 12) && ((echoback[0] == 'G') || (echoback[0] == 'H'))) { ret_type = parse_distance_parameter(urg, echoback); } else if ((line_length == 15) && ((echoback[0] == 'M') || (echoback[0] == 'N'))) { ret_type = parse_distance_parameter(urg, echoback); } return ret_type; } static int receive_length_data(urg_t *urg, long length[], unsigned short intensity[], urg_measurement_type_t type, char buffer[]) { int n; int step_filled = 0; int line_filled = 0; int multiecho_index = 0; int each_size = (urg->received_range_data_byte == URG_COMMUNICATION_2_BYTE) ? 2 : 3; int data_size = each_size; int is_intensity = URG_FALSE; int is_multiecho = URG_FALSE; int multiecho_max_size = 1; if ((type == URG_DISTANCE_INTENSITY) || (type == URG_MULTIECHO_INTENSITY)) { data_size *= 2; is_intensity = URG_TRUE; } if ((type == URG_MULTIECHO) || (type == URG_MULTIECHO_INTENSITY)) { is_multiecho = URG_TRUE; multiecho_max_size = URG_MAX_ECHO; } do { char *p = buffer; char *last_p; n = connection_readline(&urg->connection, &buffer[line_filled], BUFFER_SIZE - line_filled, urg->timeout); if (n > 0) { // チェックサムの評価 if (buffer[line_filled + n - 1] != scip_checksum(&buffer[line_filled], n - 1)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } } if (n > 0) { line_filled += n - 1; } last_p = p + line_filled; while ((last_p - p) >= data_size) { int index; if (*p == '&') { // 先頭文字が '&' だったときは、マルチエコーのデータとみなす if ((last_p - (p + 1)) < data_size) { // '&' を除いて、data_size 分データが無ければ抜ける break; } --step_filled; ++multiecho_index; ++p; --line_filled; } else { // 次のデータ multiecho_index = 0; } index = (step_filled * multiecho_max_size) + multiecho_index; if (step_filled > (urg->received_last_index - urg->received_first_index)) { // データが多過ぎる場合は、残りのデータを無視して戻る ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (is_multiecho && (multiecho_index == 0)) { // マルチエコーのデータ格納先をダミーデータで埋める int i; if (length) { for (i = 1; i < multiecho_max_size; ++i) { length[index + i] = 0; } } if (intensity) { for (i = 1; i < multiecho_max_size; ++i) { intensity[index + i] = 0; } } } // 距離データの格納 if (length) { length[index] = urg_scip_decode(p, 3); } p += 3; // 強度データの格納 if (is_intensity) { if (intensity) { intensity[index] = (unsigned short)urg_scip_decode(p, 3); } p += 3; } ++step_filled; line_filled -= data_size; } // 次に処理する文字を退避 memmove(buffer, p, line_filled); } while (n > 0); return step_filled; } //! 距離データの取得 static int receive_data(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { urg_measurement_type_t type; char buffer[BUFFER_SIZE]; int ret = 0; int n; int extended_timeout = urg->timeout + 2 * (urg->scan_usec * (urg->scanning_skip_scan) / 1000); // エコーバックの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, extended_timeout); if (n <= 0) { return set_errno_and_return(urg, URG_NO_RESPONSE); } // エコーバックの解析 type = parse_distance_echoback(urg, buffer); // 応答の取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 3) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } if (buffer[n - 1] != scip_checksum(buffer, n - 1)) { // チェックサムの評価 ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } if (type == URG_STOP) { // QT 応答の場合には、最後の改行を読み捨て、正常応答として処理する n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n == 0) { return 0; } else { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } if (urg->specified_scan_times != 1) { if (!strncmp(buffer, "00", 2)) { // "00" 応答の場合は、エコーバック応答とみなし、 // 最後の空行を読み捨て、次からのデータを返す n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 0) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } } } if (((urg->specified_scan_times == 1) && (strncmp(buffer, "00", 2))) || ((urg->specified_scan_times != 1) && (strncmp(buffer, "99", 2)))) { if (urg->error_handler) { type = urg->error_handler(buffer, urg); } if (type == URG_UNKNOWN) { // Gx, Hx のときは 00P が返されたときがデータ // Mx, Nx のときは 99b が返されたときがデータ ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } // タイムスタンプの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n > 0) { if (time_stamp) { *time_stamp = urg_scip_decode(buffer, 4); } if (system_time_stamp) { urg_get_walltime(system_time_stamp); } } // データの取得 switch (type) { case URG_DISTANCE: case URG_MULTIECHO: ret = receive_length_data(urg, data, NULL, type, buffer); break; case URG_DISTANCE_INTENSITY: case URG_MULTIECHO_INTENSITY: ret = receive_length_data(urg, data, intensity, type, buffer); break; case URG_STOP: case URG_UNKNOWN: ret = 0; break; } // specified_scan_times == 1 のときは Gx 系コマンドが使われるため // データを明示的に停止しなくてよい if ((urg->specified_scan_times > 1) && (urg->scanning_remain_times > 0)) { if (--urg->scanning_remain_times <= 0) { // データの停止のみを行う urg_stop_measurement(urg); } } return ret; } int urg_open(urg_t *urg, urg_connection_type_t connection_type, const char *device_or_address, long baudrate_or_port) { int ret; long baudrate = baudrate_or_port; urg->is_active = URG_FALSE; urg->is_sending = URG_TRUE; urg->last_errno = URG_NOT_CONNECTED; urg->timeout = MAX_TIMEOUT; urg->scanning_skip_scan = 0; urg->error_handler = NULL; // デバイスへの接続 ret = connection_open(&urg->connection, connection_type, device_or_address, baudrate_or_port); if (ret < 0) { switch (connection_type) { case URG_SERIAL: urg->last_errno = URG_SERIAL_OPEN_ERROR; break; case URG_ETHERNET: urg->last_errno = URG_ETHERNET_OPEN_ERROR; break; default: urg->last_errno = URG_INVALID_RESPONSE; break; } return urg->last_errno; } // 指定したボーレートで URG と通信できるように調整 if (connection_type == URG_ETHERNET) { // Ethernet のときは仮の通信速度を指定しておく baudrate = 115200; } if (connect_urg_device(urg, baudrate) != URG_NO_ERROR) { return set_errno_and_return(urg, ret); } urg->is_sending = URG_FALSE; // 変数の初期化 urg->last_errno = URG_NO_ERROR; urg->range_data_byte = URG_COMMUNICATION_3_BYTE; urg->specified_scan_times = 0; urg->scanning_remain_times = 0; urg->is_laser_on = URG_FALSE; // パラメータ情報を取得 ret = receive_parameter(urg); if (ret == URG_NO_ERROR) { urg->is_active = URG_TRUE; } return ret; } void urg_close(urg_t *urg) { if (urg->is_active) { ignore_receive_data_with_qt(urg, urg->timeout); } connection_close(&urg->connection); urg->is_active = URG_FALSE; } void urg_set_timeout_msec(urg_t *urg, int msec) { urg->timeout = msec; } int urg_start_time_stamp_mode(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM0 を発行する n = scip_response(urg, "TM0\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } long urg_time_stamp(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; char buffer[BUFFER_SIZE]; char *p; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } ret = scip_response(urg, "TM1\n", expected, urg->timeout, buffer, BUFFER_SIZE); if (ret < 0) { return ret; } // buffer からタイムスタンプを取得し、デコードして返す if (strcmp(buffer, "00P")) { // 最初の応答が "00P" でなければ戻る return set_errno_and_return(urg, URG_RECEIVE_ERROR); } p = buffer + 4; if (strlen(p) != 5) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (p[5] == scip_checksum(p, 4)) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } return urg_scip_decode(p, 4); } int urg_stop_time_stamp_mode(urg_t *urg) { int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM2 を発行する n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } static int send_distance_command(urg_t *urg, int scan_times, int skip_scan, char single_scan_ch, char continuous_scan_ch, char scan_type_ch) { char buffer[BUFFER_SIZE]; int write_size = 0; int front_index = urg->front_data_index; int n; urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times; urg->scanning_remain_times = urg->specified_scan_times; urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan; if (scan_times >= 100) { // 計測回数が 99 を越える場合は、無限回のスキャンを行う urg->specified_scan_times = 0; } if (urg->scanning_remain_times == 1) { // レーザ発光を指示 urg_laser_on(urg); write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n", single_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step); } else { write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n", continuous_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step, skip_scan, urg->specified_scan_times); urg->is_sending = URG_TRUE; } n = connection_write(&urg->connection, buffer, write_size); if (n != write_size) { return set_errno_and_return(urg, URG_SEND_ERROR); } return 0; } int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan) { char range_byte_ch; int ret = 0; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((skip_scan < 0) || (skip_scan > 9)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // !!! Mx 系, Nx 系の計測中のときは、QT を発行してから // !!! 計測開始コマンドを送信するようにする // !!! ただし、MD 計測中に MD を発行するように、同じコマンドの場合は // !!! Mx 系, Nx 系の計測は上書きすることができるようにする // 指定されたタイプのパケットを生成し、送信する switch (type) { case URG_DISTANCE: range_byte_ch = (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D'; ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', range_byte_ch); break; case URG_DISTANCE_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', 'E'); break; case URG_MULTIECHO: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'D'); break; case URG_MULTIECHO_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'E'); break; case URG_STOP: case URG_UNKNOWN: default: ignore_receive_data_with_qt(urg, urg->timeout); urg->last_errno = URG_INVALID_PARAMETER; ret = urg->last_errno; break; } return ret; } int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, NULL, time_stamp, system_time_stamp); } int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp); } int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp); } int urg_stop_measurement(urg_t *urg) { enum { MAX_READ_TIMES = 3 }; int ret = URG_INVALID_RESPONSE; int n; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // QT を発行する n = connection_write(&urg->connection, "QT\n", 3); if (n != 3) { return set_errno_and_return(urg, URG_SEND_ERROR); } for (i = 0; i < MAX_READ_TIMES; ++i) { // QT の応答が返されるまで、距離データを読み捨てる ret = receive_data(urg, NULL, NULL, NULL, NULL); if (ret == URG_NO_ERROR) { // 正常応答 urg->is_laser_on = URG_FALSE; urg->is_sending = URG_FALSE; return set_errno_and_return(urg, URG_NO_ERROR); } } return ret; } int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step) { // 設定の範囲外を指定したときは、エラーを返す if (((skip_step < 0) || (skip_step >= 100)) || (first_step > last_step) || (first_step < -urg->front_data_index) || (last_step > (urg->last_data_index - urg->front_data_index))) { return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR); } urg->scanning_first_step = first_step; urg->scanning_last_step = last_step; urg->scanning_skip_step = skip_step; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_set_connection_data_size(urg_t *urg, urg_range_data_byte_t data_byte) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((data_byte != URG_COMMUNICATION_3_BYTE) || (data_byte != URG_COMMUNICATION_2_BYTE)) { return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR); } urg->range_data_byte = data_byte; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_laser_on(urg_t *urg) { int expected[] = { 0, 2, EXPECTED_END }; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if (urg->is_laser_on != URG_FALSE) { // 既にレーザが発光しているときは、コマンドを送信しないようにする urg->last_errno = 0; return urg->last_errno; } ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0); if (ret >= 0) { urg->is_laser_on = URG_TRUE; ret = 0; } return ret; } int urg_laser_off(urg_t *urg) { return urg_stop_measurement(urg); } int urg_reboot(urg_t *urg) { int expected[] = { 0, 1, EXPECTED_END }; int ret; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // ２回目の RB 送信後、接続を切断する for (i = 0; i < 2; ++i) { ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0); if (ret < 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } urg->is_active = URG_FALSE; urg_close(urg); urg->last_errno = 0; return urg->last_errno; } void urg_sleep(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = 4 }; int sl_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE]; if (urg_stop_measurement(urg) != URG_NO_ERROR) { return; } scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); } void urg_wakeup(urg_t *urg) { urg_stop_measurement(urg); } int urg_is_stable(urg_t *urg) { const char *stat = urg_sensor_status(urg); return strncmp("Stable", stat, 6) ? 0 : 1; } static char *copy_token(char *dest, char *receive_buffer, const char *start_str, const char *end_ch, int lines) { size_t start_str_len = strlen(start_str); size_t end_ch_len = strlen(end_ch); int i; size_t j; for (j = 0; j < end_ch_len; ++j) { const char *p = receive_buffer; for (i = 0; i < lines; ++i) { if (!strncmp(p, start_str, start_str_len)) { char *last_p = strchr(p + start_str_len, end_ch[j]); if (last_p) { *last_p = '\0'; memcpy(dest, p + start_str_len, last_p - (p + start_str_len) + 1); return dest; } } p += strlen(p) + 1; } } return NULL; } static const char *receive_command_response(urg_t *urg, char *buffer, int buffer_size, const char* command, int response_lines) { const int vv_expected[] = { 0, EXPECTED_END }; int ret; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = scip_response(urg, command, vv_expected, urg->timeout, buffer, buffer_size); if (ret < response_lines) { return RECEIVE_ERROR_MESSAGE; } return NULL; } const char *urg_sensor_product_type(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROD:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_serial_id(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "SERI:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_vendor(urg_t *urg){ enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "VEND:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", "(", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_date(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } // Get the firmware version and append a '(', this will be what's before the date char firmware_version[50]; strcpy(firmware_version, urg_sensor_firmware_version(urg)); strcat(firmware_version, "("); ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } // Strip out the actual date from between the '(' and ')' char *date; p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", ";", VV_RESPONSE_LINES); date = copy_token(urg->return_buffer, p, firmware_version, ")", 1); return (date) ? date : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_protocol_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROT:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_status(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "STAT:", ";", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_state(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "MESM:", " (", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } void urg_set_error_handler(urg_t *urg, urg_error_handler handler) { urg->error_handler = handler; }
247  ret = scip_response(urg, "SCIP2.0\n", scip20_expected,
248  MAX_TIMEOUT, NULL, 0);
249 
250  // SCIP2.0 応答の最後の改行を読み飛ばす
251  ignore_receive_data(urg, MAX_TIMEOUT);
252 
253  // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } else if (!strcmp(receive_buffer, "0Ee")) { int tm2_expected[] = { 0, EXPECTED_END }; // "0Ee" が返された場合は、TM モードとみなし "TM2" を送信する scip_response(urg, "TM2\n", tm2_expected, MAX_TIMEOUT, NULL, 0); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } if (ret <= 0) { if (ret == URG_INVALID_RESPONSE) { // 異常なエコーバックのときは、距離データ受信中とみなして // データを読み飛ばす ignore_receive_data_with_qt(urg, MAX_TIMEOUT); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } else { // 応答がないときは、ボーレートを変更して、再度接続を行う ignore_receive_data_with_qt(urg, MAX_TIMEOUT); continue; } } else if (!strcmp("00P", receive_buffer)) { // センサとホストのボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } return set_errno_and_return(urg, URG_NOT_DETECT_BAUDRATE_ERROR); } // PP コマンドの応答を urg_t に格納する static int receive_parameter(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * 9, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; int pp_expected[] = { 0, EXPECTED_END }; unsigned short received_bits = 0x0000; char *p; int i; int ret = scip_response(urg, "PP\n", pp_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); if (ret < 0) { return ret; } else if (ret < PP_RESPONSE_LINES) { ignore_receive_data_with_qt(urg, MAX_TIMEOUT); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } p = receive_buffer; for (i = 0; i < (ret - 1); ++i) { if (!strncmp(p, "DMIN:", 5)) { urg->min_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0001; } else if (!strncmp(p, "DMAX:", 5)) { urg->max_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0002; } else if (!strncmp(p, "ARES:", 5)) { urg->area_resolution = strtol(p + 5, NULL, 10); received_bits |= 0x0004; } else if (!strncmp(p, "AMIN:", 5)) { urg->first_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0008; } else if (!strncmp(p, "AMAX:", 5)) { urg->last_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0010; } else if (!strncmp(p, "AFRT:", 5)) { urg->front_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0020; } else if (!strncmp(p, "SCAN:", 5)) { int rpm = strtol(p + 5, NULL, 10); // タイムアウト時間は、計測周期の 16 倍程度の値にする urg->scan_usec = 1000 * 1000 * 60 / rpm; urg->timeout = urg->scan_usec >> (10 - 4); received_bits |= 0x0040; } p += strlen(p) + 1; } // 全てのパラメータを受信したか確認 if (received_bits != 0x007f) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } urg_set_scanning_parameter(urg, urg->first_data_index - urg->front_data_index, urg->last_data_index - urg->front_data_index, 1); return set_errno_and_return(urg, URG_NO_ERROR); } //! SCIP 文字列のデコード long urg_scip_decode(const char data[], int size) { const char* p = data; const char* last_p = p + size; int value = 0; while (p < last_p) { value <<= 6; value &= ~0x3f; value |= *p++ - 0x30; } return value; } static int parse_parameter(const char *parameter, int size) { char buffer[5]; memcpy(buffer, parameter, size); buffer[size] = '\0'; return strtol(buffer, NULL, 10); } static urg_measurement_type_t parse_distance_parameter(urg_t *urg, const char echoback[]) { urg_measurement_type_t ret_type = URG_UNKNOWN; urg->received_range_data_byte = URG_COMMUNICATION_3_BYTE; if (echoback[1] == 'S') { urg->received_range_data_byte = URG_COMMUNICATION_2_BYTE; ret_type = URG_DISTANCE; } else if (echoback[1] == 'D') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO; } } else if (echoback[1] == 'E') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE_INTENSITY; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO_INTENSITY; } } else { return URG_UNKNOWN; } // パラメータの格納 urg->received_first_index = parse_parameter(&echoback[2], 4); urg->received_last_index = parse_parameter(&echoback[6], 4); urg->received_skip_step = parse_parameter(&echoback[10], 2); return ret_type; } static urg_measurement_type_t parse_distance_echoback(urg_t *urg, const char echoback[]) { size_t line_length; urg_measurement_type_t ret_type = URG_UNKNOWN; if (!strcmp("QT", echoback)) { return URG_STOP; } line_length = strlen(echoback); if ((line_length == 12) && ((echoback[0] == 'G') || (echoback[0] == 'H'))) { ret_type = parse_distance_parameter(urg, echoback); } else if ((line_length == 15) && ((echoback[0] == 'M') || (echoback[0] == 'N'))) { ret_type = parse_distance_parameter(urg, echoback); } return ret_type; } static int receive_length_data(urg_t *urg, long length[], unsigned short intensity[], urg_measurement_type_t type, char buffer[]) { int n; int step_filled = 0; int line_filled = 0; int multiecho_index = 0; int each_size = (urg->received_range_data_byte == URG_COMMUNICATION_2_BYTE) ? 2 : 3; int data_size = each_size; int is_intensity = URG_FALSE; int is_multiecho = URG_FALSE; int multiecho_max_size = 1; if ((type == URG_DISTANCE_INTENSITY) || (type == URG_MULTIECHO_INTENSITY)) { data_size *= 2; is_intensity = URG_TRUE; } if ((type == URG_MULTIECHO) || (type == URG_MULTIECHO_INTENSITY)) { is_multiecho = URG_TRUE; multiecho_max_size = URG_MAX_ECHO; } do { char *p = buffer; char *last_p; n = connection_readline(&urg->connection, &buffer[line_filled], BUFFER_SIZE - line_filled, urg->timeout); if (n > 0) { // チェックサムの評価 if (buffer[line_filled + n - 1] != scip_checksum(&buffer[line_filled], n - 1)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } } if (n > 0) { line_filled += n - 1; } last_p = p + line_filled; while ((last_p - p) >= data_size) { int index; if (*p == '&') { // 先頭文字が '&' だったときは、マルチエコーのデータとみなす if ((last_p - (p + 1)) < data_size) { // '&' を除いて、data_size 分データが無ければ抜ける break; } --step_filled; ++multiecho_index; ++p; --line_filled; } else { // 次のデータ multiecho_index = 0; } index = (step_filled * multiecho_max_size) + multiecho_index; if (step_filled > (urg->received_last_index - urg->received_first_index)) { // データが多過ぎる場合は、残りのデータを無視して戻る ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (is_multiecho && (multiecho_index == 0)) { // マルチエコーのデータ格納先をダミーデータで埋める int i; if (length) { for (i = 1; i < multiecho_max_size; ++i) { length[index + i] = 0; } } if (intensity) { for (i = 1; i < multiecho_max_size; ++i) { intensity[index + i] = 0; } } } // 距離データの格納 if (length) { length[index] = urg_scip_decode(p, 3); } p += 3; // 強度データの格納 if (is_intensity) { if (intensity) { intensity[index] = (unsigned short)urg_scip_decode(p, 3); } p += 3; } ++step_filled; line_filled -= data_size; } // 次に処理する文字を退避 memmove(buffer, p, line_filled); } while (n > 0); return step_filled; } //! 距離データの取得 static int receive_data(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { urg_measurement_type_t type; char buffer[BUFFER_SIZE]; int ret = 0; int n; int extended_timeout = urg->timeout + 2 * (urg->scan_usec * (urg->scanning_skip_scan) / 1000); // エコーバックの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, extended_timeout); if (n <= 0) { return set_errno_and_return(urg, URG_NO_RESPONSE); } // エコーバックの解析 type = parse_distance_echoback(urg, buffer); // 応答の取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 3) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } if (buffer[n - 1] != scip_checksum(buffer, n - 1)) { // チェックサムの評価 ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } if (type == URG_STOP) { // QT 応答の場合には、最後の改行を読み捨て、正常応答として処理する n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n == 0) { return 0; } else { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } if (urg->specified_scan_times != 1) { if (!strncmp(buffer, "00", 2)) { // "00" 応答の場合は、エコーバック応答とみなし、 // 最後の空行を読み捨て、次からのデータを返す n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 0) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } } } if (((urg->specified_scan_times == 1) && (strncmp(buffer, "00", 2))) || ((urg->specified_scan_times != 1) && (strncmp(buffer, "99", 2)))) { if (urg->error_handler) { type = urg->error_handler(buffer, urg); } if (type == URG_UNKNOWN) { // Gx, Hx のときは 00P が返されたときがデータ // Mx, Nx のときは 99b が返されたときがデータ ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } // タイムスタンプの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n > 0) { if (time_stamp) { *time_stamp = urg_scip_decode(buffer, 4); } if (system_time_stamp) { urg_get_walltime(system_time_stamp); } } // データの取得 switch (type) { case URG_DISTANCE: case URG_MULTIECHO: ret = receive_length_data(urg, data, NULL, type, buffer); break; case URG_DISTANCE_INTENSITY: case URG_MULTIECHO_INTENSITY: ret = receive_length_data(urg, data, intensity, type, buffer); break; case URG_STOP: case URG_UNKNOWN: ret = 0; break; } // specified_scan_times == 1 のときは Gx 系コマンドが使われるため // データを明示的に停止しなくてよい if ((urg->specified_scan_times > 1) && (urg->scanning_remain_times > 0)) { if (--urg->scanning_remain_times <= 0) { // データの停止のみを行う urg_stop_measurement(urg); } } return ret; } int urg_open(urg_t *urg, urg_connection_type_t connection_type, const char *device_or_address, long baudrate_or_port) { int ret; long baudrate = baudrate_or_port; urg->is_active = URG_FALSE; urg->is_sending = URG_TRUE; urg->last_errno = URG_NOT_CONNECTED; urg->timeout = MAX_TIMEOUT; urg->scanning_skip_scan = 0; urg->error_handler = NULL; // デバイスへの接続 ret = connection_open(&urg->connection, connection_type, device_or_address, baudrate_or_port); if (ret < 0) { switch (connection_type) { case URG_SERIAL: urg->last_errno = URG_SERIAL_OPEN_ERROR; break; case URG_ETHERNET: urg->last_errno = URG_ETHERNET_OPEN_ERROR; break; default: urg->last_errno = URG_INVALID_RESPONSE; break; } return urg->last_errno; } // 指定したボーレートで URG と通信できるように調整 if (connection_type == URG_ETHERNET) { // Ethernet のときは仮の通信速度を指定しておく baudrate = 115200; } if (connect_urg_device(urg, baudrate) != URG_NO_ERROR) { return set_errno_and_return(urg, ret); } urg->is_sending = URG_FALSE; // 変数の初期化 urg->last_errno = URG_NO_ERROR; urg->range_data_byte = URG_COMMUNICATION_3_BYTE; urg->specified_scan_times = 0; urg->scanning_remain_times = 0; urg->is_laser_on = URG_FALSE; // パラメータ情報を取得 ret = receive_parameter(urg); if (ret == URG_NO_ERROR) { urg->is_active = URG_TRUE; } return ret; } void urg_close(urg_t *urg) { if (urg->is_active) { ignore_receive_data_with_qt(urg, urg->timeout); } connection_close(&urg->connection); urg->is_active = URG_FALSE; } void urg_set_timeout_msec(urg_t *urg, int msec) { urg->timeout = msec; } int urg_start_time_stamp_mode(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM0 を発行する n = scip_response(urg, "TM0\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } long urg_time_stamp(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; char buffer[BUFFER_SIZE]; char *p; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } ret = scip_response(urg, "TM1\n", expected, urg->timeout, buffer, BUFFER_SIZE); if (ret < 0) { return ret; } // buffer からタイムスタンプを取得し、デコードして返す if (strcmp(buffer, "00P")) { // 最初の応答が "00P" でなければ戻る return set_errno_and_return(urg, URG_RECEIVE_ERROR); } p = buffer + 4; if (strlen(p) != 5) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (p[5] == scip_checksum(p, 4)) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } return urg_scip_decode(p, 4); } int urg_stop_time_stamp_mode(urg_t *urg) { int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM2 を発行する n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } static int send_distance_command(urg_t *urg, int scan_times, int skip_scan, char single_scan_ch, char continuous_scan_ch, char scan_type_ch) { char buffer[BUFFER_SIZE]; int write_size = 0; int front_index = urg->front_data_index; int n; urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times; urg->scanning_remain_times = urg->specified_scan_times; urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan; if (scan_times >= 100) { // 計測回数が 99 を越える場合は、無限回のスキャンを行う urg->specified_scan_times = 0; } if (urg->scanning_remain_times == 1) { // レーザ発光を指示 urg_laser_on(urg); write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n", single_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step); } else { write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n", continuous_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step, skip_scan, urg->specified_scan_times); urg->is_sending = URG_TRUE; } n = connection_write(&urg->connection, buffer, write_size); if (n != write_size) { return set_errno_and_return(urg, URG_SEND_ERROR); } return 0; } int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan) { char range_byte_ch; int ret = 0; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((skip_scan < 0) || (skip_scan > 9)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // !!! Mx 系, Nx 系の計測中のときは、QT を発行してから // !!! 計測開始コマンドを送信するようにする // !!! ただし、MD 計測中に MD を発行するように、同じコマンドの場合は // !!! Mx 系, Nx 系の計測は上書きすることができるようにする // 指定されたタイプのパケットを生成し、送信する switch (type) { case URG_DISTANCE: range_byte_ch = (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D'; ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', range_byte_ch); break; case URG_DISTANCE_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', 'E'); break; case URG_MULTIECHO: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'D'); break; case URG_MULTIECHO_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'E'); break; case URG_STOP: case URG_UNKNOWN: default: ignore_receive_data_with_qt(urg, urg->timeout); urg->last_errno = URG_INVALID_PARAMETER; ret = urg->last_errno; break; } return ret; } int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, NULL, time_stamp, system_time_stamp); } int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp); } int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp); } int urg_stop_measurement(urg_t *urg) { enum { MAX_READ_TIMES = 3 }; int ret = URG_INVALID_RESPONSE; int n; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // QT を発行する n = connection_write(&urg->connection, "QT\n", 3); if (n != 3) { return set_errno_and_return(urg, URG_SEND_ERROR); } for (i = 0; i < MAX_READ_TIMES; ++i) { // QT の応答が返されるまで、距離データを読み捨てる ret = receive_data(urg, NULL, NULL, NULL, NULL); if (ret == URG_NO_ERROR) { // 正常応答 urg->is_laser_on = URG_FALSE; urg->is_sending = URG_FALSE; return set_errno_and_return(urg, URG_NO_ERROR); } } return ret; } int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step) { // 設定の範囲外を指定したときは、エラーを返す if (((skip_step < 0) || (skip_step >= 100)) || (first_step > last_step) || (first_step < -urg->front_data_index) || (last_step > (urg->last_data_index - urg->front_data_index))) { return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR); } urg->scanning_first_step = first_step; urg->scanning_last_step = last_step; urg->scanning_skip_step = skip_step; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_set_connection_data_size(urg_t *urg, urg_range_data_byte_t data_byte) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((data_byte != URG_COMMUNICATION_3_BYTE) || (data_byte != URG_COMMUNICATION_2_BYTE)) { return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR); } urg->range_data_byte = data_byte; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_laser_on(urg_t *urg) { int expected[] = { 0, 2, EXPECTED_END }; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if (urg->is_laser_on != URG_FALSE) { // 既にレーザが発光しているときは、コマンドを送信しないようにする urg->last_errno = 0; return urg->last_errno; } ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0); if (ret >= 0) { urg->is_laser_on = URG_TRUE; ret = 0; } return ret; } int urg_laser_off(urg_t *urg) { return urg_stop_measurement(urg); } int urg_reboot(urg_t *urg) { int expected[] = { 0, 1, EXPECTED_END }; int ret; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // ２回目の RB 送信後、接続を切断する for (i = 0; i < 2; ++i) { ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0); if (ret < 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } urg->is_active = URG_FALSE; urg_close(urg); urg->last_errno = 0; return urg->last_errno; } void urg_sleep(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = 4 }; int sl_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE]; if (urg_stop_measurement(urg) != URG_NO_ERROR) { return; } scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); } void urg_wakeup(urg_t *urg) { urg_stop_measurement(urg); } int urg_is_stable(urg_t *urg) { const char *stat = urg_sensor_status(urg); return strncmp("Stable", stat, 6) ? 0 : 1; } static char *copy_token(char *dest, char *receive_buffer, const char *start_str, const char *end_ch, int lines) { size_t start_str_len = strlen(start_str); size_t end_ch_len = strlen(end_ch); int i; size_t j; for (j = 0; j < end_ch_len; ++j) { const char *p = receive_buffer; for (i = 0; i < lines; ++i) { if (!strncmp(p, start_str, start_str_len)) { char *last_p = strchr(p + start_str_len, end_ch[j]); if (last_p) { *last_p = '\0'; memcpy(dest, p + start_str_len, last_p - (p + start_str_len) + 1); return dest; } } p += strlen(p) + 1; } } return NULL; } static const char *receive_command_response(urg_t *urg, char *buffer, int buffer_size, const char* command, int response_lines) { const int vv_expected[] = { 0, EXPECTED_END }; int ret; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = scip_response(urg, command, vv_expected, urg->timeout, buffer, buffer_size); if (ret < response_lines) { return RECEIVE_ERROR_MESSAGE; } return NULL; } const char *urg_sensor_product_type(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROD:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_serial_id(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "SERI:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_vendor(urg_t *urg){ enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "VEND:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", "(", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_date(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } // Get the firmware version and append a '(', this will be what's before the date char firmware_version[50]; strcpy(firmware_version, urg_sensor_firmware_version(urg)); strcat(firmware_version, "("); ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } // Strip out the actual date from between the '(' and ')' char *date; p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", ";", VV_RESPONSE_LINES); date = copy_token(urg->return_buffer, p, firmware_version, ")", 1); return (date) ? date : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_protocol_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROT:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_status(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "STAT:", ";", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_state(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "MESM:", " (", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } void urg_set_error_handler(urg_t *urg, urg_error_handler handler) { urg->error_handler = handler; }
254  return change_sensor_baudrate(urg, try_baudrate[i], baudrate);
255 
256  } else if (!strcmp(receive_buffer, "0Ee")) {
257  int tm2_expected[] = { 0, EXPECTED_END };
258 
259  // "0Ee" が返された場合は、TM モードとみなし "TM2" を送信する scip_response(urg, "TM2\n", tm2_expected, MAX_TIMEOUT, NULL, 0); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } if (ret <= 0) { if (ret == URG_INVALID_RESPONSE) { // 異常なエコーバックのときは、距離データ受信中とみなして // データを読み飛ばす ignore_receive_data_with_qt(urg, MAX_TIMEOUT); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } else { // 応答がないときは、ボーレートを変更して、再度接続を行う ignore_receive_data_with_qt(urg, MAX_TIMEOUT); continue; } } else if (!strcmp("00P", receive_buffer)) { // センサとホストのボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } return set_errno_and_return(urg, URG_NOT_DETECT_BAUDRATE_ERROR); } // PP コマンドの応答を urg_t に格納する static int receive_parameter(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * 9, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; int pp_expected[] = { 0, EXPECTED_END }; unsigned short received_bits = 0x0000; char *p; int i; int ret = scip_response(urg, "PP\n", pp_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); if (ret < 0) { return ret; } else if (ret < PP_RESPONSE_LINES) { ignore_receive_data_with_qt(urg, MAX_TIMEOUT); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } p = receive_buffer; for (i = 0; i < (ret - 1); ++i) { if (!strncmp(p, "DMIN:", 5)) { urg->min_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0001; } else if (!strncmp(p, "DMAX:", 5)) { urg->max_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0002; } else if (!strncmp(p, "ARES:", 5)) { urg->area_resolution = strtol(p + 5, NULL, 10); received_bits |= 0x0004; } else if (!strncmp(p, "AMIN:", 5)) { urg->first_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0008; } else if (!strncmp(p, "AMAX:", 5)) { urg->last_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0010; } else if (!strncmp(p, "AFRT:", 5)) { urg->front_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0020; } else if (!strncmp(p, "SCAN:", 5)) { int rpm = strtol(p + 5, NULL, 10); // タイムアウト時間は、計測周期の 16 倍程度の値にする urg->scan_usec = 1000 * 1000 * 60 / rpm; urg->timeout = urg->scan_usec >> (10 - 4); received_bits |= 0x0040; } p += strlen(p) + 1; } // 全てのパラメータを受信したか確認 if (received_bits != 0x007f) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } urg_set_scanning_parameter(urg, urg->first_data_index - urg->front_data_index, urg->last_data_index - urg->front_data_index, 1); return set_errno_and_return(urg, URG_NO_ERROR); } //! SCIP 文字列のデコード long urg_scip_decode(const char data[], int size) { const char* p = data; const char* last_p = p + size; int value = 0; while (p < last_p) { value <<= 6; value &= ~0x3f; value |= *p++ - 0x30; } return value; } static int parse_parameter(const char *parameter, int size) { char buffer[5]; memcpy(buffer, parameter, size); buffer[size] = '\0'; return strtol(buffer, NULL, 10); } static urg_measurement_type_t parse_distance_parameter(urg_t *urg, const char echoback[]) { urg_measurement_type_t ret_type = URG_UNKNOWN; urg->received_range_data_byte = URG_COMMUNICATION_3_BYTE; if (echoback[1] == 'S') { urg->received_range_data_byte = URG_COMMUNICATION_2_BYTE; ret_type = URG_DISTANCE; } else if (echoback[1] == 'D') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO; } } else if (echoback[1] == 'E') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE_INTENSITY; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO_INTENSITY; } } else { return URG_UNKNOWN; } // パラメータの格納 urg->received_first_index = parse_parameter(&echoback[2], 4); urg->received_last_index = parse_parameter(&echoback[6], 4); urg->received_skip_step = parse_parameter(&echoback[10], 2); return ret_type; } static urg_measurement_type_t parse_distance_echoback(urg_t *urg, const char echoback[]) { size_t line_length; urg_measurement_type_t ret_type = URG_UNKNOWN; if (!strcmp("QT", echoback)) { return URG_STOP; } line_length = strlen(echoback); if ((line_length == 12) && ((echoback[0] == 'G') || (echoback[0] == 'H'))) { ret_type = parse_distance_parameter(urg, echoback); } else if ((line_length == 15) && ((echoback[0] == 'M') || (echoback[0] == 'N'))) { ret_type = parse_distance_parameter(urg, echoback); } return ret_type; } static int receive_length_data(urg_t *urg, long length[], unsigned short intensity[], urg_measurement_type_t type, char buffer[]) { int n; int step_filled = 0; int line_filled = 0; int multiecho_index = 0; int each_size = (urg->received_range_data_byte == URG_COMMUNICATION_2_BYTE) ? 2 : 3; int data_size = each_size; int is_intensity = URG_FALSE; int is_multiecho = URG_FALSE; int multiecho_max_size = 1; if ((type == URG_DISTANCE_INTENSITY) || (type == URG_MULTIECHO_INTENSITY)) { data_size *= 2; is_intensity = URG_TRUE; } if ((type == URG_MULTIECHO) || (type == URG_MULTIECHO_INTENSITY)) { is_multiecho = URG_TRUE; multiecho_max_size = URG_MAX_ECHO; } do { char *p = buffer; char *last_p; n = connection_readline(&urg->connection, &buffer[line_filled], BUFFER_SIZE - line_filled, urg->timeout); if (n > 0) { // チェックサムの評価 if (buffer[line_filled + n - 1] != scip_checksum(&buffer[line_filled], n - 1)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } } if (n > 0) { line_filled += n - 1; } last_p = p + line_filled; while ((last_p - p) >= data_size) { int index; if (*p == '&') { // 先頭文字が '&' だったときは、マルチエコーのデータとみなす if ((last_p - (p + 1)) < data_size) { // '&' を除いて、data_size 分データが無ければ抜ける break; } --step_filled; ++multiecho_index; ++p; --line_filled; } else { // 次のデータ multiecho_index = 0; } index = (step_filled * multiecho_max_size) + multiecho_index; if (step_filled > (urg->received_last_index - urg->received_first_index)) { // データが多過ぎる場合は、残りのデータを無視して戻る ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (is_multiecho && (multiecho_index == 0)) { // マルチエコーのデータ格納先をダミーデータで埋める int i; if (length) { for (i = 1; i < multiecho_max_size; ++i) { length[index + i] = 0; } } if (intensity) { for (i = 1; i < multiecho_max_size; ++i) { intensity[index + i] = 0; } } } // 距離データの格納 if (length) { length[index] = urg_scip_decode(p, 3); } p += 3; // 強度データの格納 if (is_intensity) { if (intensity) { intensity[index] = (unsigned short)urg_scip_decode(p, 3); } p += 3; } ++step_filled; line_filled -= data_size; } // 次に処理する文字を退避 memmove(buffer, p, line_filled); } while (n > 0); return step_filled; } //! 距離データの取得 static int receive_data(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { urg_measurement_type_t type; char buffer[BUFFER_SIZE]; int ret = 0; int n; int extended_timeout = urg->timeout + 2 * (urg->scan_usec * (urg->scanning_skip_scan) / 1000); // エコーバックの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, extended_timeout); if (n <= 0) { return set_errno_and_return(urg, URG_NO_RESPONSE); } // エコーバックの解析 type = parse_distance_echoback(urg, buffer); // 応答の取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 3) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } if (buffer[n - 1] != scip_checksum(buffer, n - 1)) { // チェックサムの評価 ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } if (type == URG_STOP) { // QT 応答の場合には、最後の改行を読み捨て、正常応答として処理する n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n == 0) { return 0; } else { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } if (urg->specified_scan_times != 1) { if (!strncmp(buffer, "00", 2)) { // "00" 応答の場合は、エコーバック応答とみなし、 // 最後の空行を読み捨て、次からのデータを返す n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 0) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } } } if (((urg->specified_scan_times == 1) && (strncmp(buffer, "00", 2))) || ((urg->specified_scan_times != 1) && (strncmp(buffer, "99", 2)))) { if (urg->error_handler) { type = urg->error_handler(buffer, urg); } if (type == URG_UNKNOWN) { // Gx, Hx のときは 00P が返されたときがデータ // Mx, Nx のときは 99b が返されたときがデータ ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } // タイムスタンプの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n > 0) { if (time_stamp) { *time_stamp = urg_scip_decode(buffer, 4); } if (system_time_stamp) { urg_get_walltime(system_time_stamp); } } // データの取得 switch (type) { case URG_DISTANCE: case URG_MULTIECHO: ret = receive_length_data(urg, data, NULL, type, buffer); break; case URG_DISTANCE_INTENSITY: case URG_MULTIECHO_INTENSITY: ret = receive_length_data(urg, data, intensity, type, buffer); break; case URG_STOP: case URG_UNKNOWN: ret = 0; break; } // specified_scan_times == 1 のときは Gx 系コマンドが使われるため // データを明示的に停止しなくてよい if ((urg->specified_scan_times > 1) && (urg->scanning_remain_times > 0)) { if (--urg->scanning_remain_times <= 0) { // データの停止のみを行う urg_stop_measurement(urg); } } return ret; } int urg_open(urg_t *urg, urg_connection_type_t connection_type, const char *device_or_address, long baudrate_or_port) { int ret; long baudrate = baudrate_or_port; urg->is_active = URG_FALSE; urg->is_sending = URG_TRUE; urg->last_errno = URG_NOT_CONNECTED; urg->timeout = MAX_TIMEOUT; urg->scanning_skip_scan = 0; urg->error_handler = NULL; // デバイスへの接続 ret = connection_open(&urg->connection, connection_type, device_or_address, baudrate_or_port); if (ret < 0) { switch (connection_type) { case URG_SERIAL: urg->last_errno = URG_SERIAL_OPEN_ERROR; break; case URG_ETHERNET: urg->last_errno = URG_ETHERNET_OPEN_ERROR; break; default: urg->last_errno = URG_INVALID_RESPONSE; break; } return urg->last_errno; } // 指定したボーレートで URG と通信できるように調整 if (connection_type == URG_ETHERNET) { // Ethernet のときは仮の通信速度を指定しておく baudrate = 115200; } if (connect_urg_device(urg, baudrate) != URG_NO_ERROR) { return set_errno_and_return(urg, ret); } urg->is_sending = URG_FALSE; // 変数の初期化 urg->last_errno = URG_NO_ERROR; urg->range_data_byte = URG_COMMUNICATION_3_BYTE; urg->specified_scan_times = 0; urg->scanning_remain_times = 0; urg->is_laser_on = URG_FALSE; // パラメータ情報を取得 ret = receive_parameter(urg); if (ret == URG_NO_ERROR) { urg->is_active = URG_TRUE; } return ret; } void urg_close(urg_t *urg) { if (urg->is_active) { ignore_receive_data_with_qt(urg, urg->timeout); } connection_close(&urg->connection); urg->is_active = URG_FALSE; } void urg_set_timeout_msec(urg_t *urg, int msec) { urg->timeout = msec; } int urg_start_time_stamp_mode(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM0 を発行する n = scip_response(urg, "TM0\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } long urg_time_stamp(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; char buffer[BUFFER_SIZE]; char *p; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } ret = scip_response(urg, "TM1\n", expected, urg->timeout, buffer, BUFFER_SIZE); if (ret < 0) { return ret; } // buffer からタイムスタンプを取得し、デコードして返す if (strcmp(buffer, "00P")) { // 最初の応答が "00P" でなければ戻る return set_errno_and_return(urg, URG_RECEIVE_ERROR); } p = buffer + 4; if (strlen(p) != 5) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (p[5] == scip_checksum(p, 4)) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } return urg_scip_decode(p, 4); } int urg_stop_time_stamp_mode(urg_t *urg) { int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM2 を発行する n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } static int send_distance_command(urg_t *urg, int scan_times, int skip_scan, char single_scan_ch, char continuous_scan_ch, char scan_type_ch) { char buffer[BUFFER_SIZE]; int write_size = 0; int front_index = urg->front_data_index; int n; urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times; urg->scanning_remain_times = urg->specified_scan_times; urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan; if (scan_times >= 100) { // 計測回数が 99 を越える場合は、無限回のスキャンを行う urg->specified_scan_times = 0; } if (urg->scanning_remain_times == 1) { // レーザ発光を指示 urg_laser_on(urg); write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n", single_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step); } else { write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n", continuous_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step, skip_scan, urg->specified_scan_times); urg->is_sending = URG_TRUE; } n = connection_write(&urg->connection, buffer, write_size); if (n != write_size) { return set_errno_and_return(urg, URG_SEND_ERROR); } return 0; } int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan) { char range_byte_ch; int ret = 0; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((skip_scan < 0) || (skip_scan > 9)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // !!! Mx 系, Nx 系の計測中のときは、QT を発行してから // !!! 計測開始コマンドを送信するようにする // !!! ただし、MD 計測中に MD を発行するように、同じコマンドの場合は // !!! Mx 系, Nx 系の計測は上書きすることができるようにする // 指定されたタイプのパケットを生成し、送信する switch (type) { case URG_DISTANCE: range_byte_ch = (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D'; ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', range_byte_ch); break; case URG_DISTANCE_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', 'E'); break; case URG_MULTIECHO: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'D'); break; case URG_MULTIECHO_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'E'); break; case URG_STOP: case URG_UNKNOWN: default: ignore_receive_data_with_qt(urg, urg->timeout); urg->last_errno = URG_INVALID_PARAMETER; ret = urg->last_errno; break; } return ret; } int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, NULL, time_stamp, system_time_stamp); } int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp); } int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp); } int urg_stop_measurement(urg_t *urg) { enum { MAX_READ_TIMES = 3 }; int ret = URG_INVALID_RESPONSE; int n; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // QT を発行する n = connection_write(&urg->connection, "QT\n", 3); if (n != 3) { return set_errno_and_return(urg, URG_SEND_ERROR); } for (i = 0; i < MAX_READ_TIMES; ++i) { // QT の応答が返されるまで、距離データを読み捨てる ret = receive_data(urg, NULL, NULL, NULL, NULL); if (ret == URG_NO_ERROR) { // 正常応答 urg->is_laser_on = URG_FALSE; urg->is_sending = URG_FALSE; return set_errno_and_return(urg, URG_NO_ERROR); } } return ret; } int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step) { // 設定の範囲外を指定したときは、エラーを返す if (((skip_step < 0) || (skip_step >= 100)) || (first_step > last_step) || (first_step < -urg->front_data_index) || (last_step > (urg->last_data_index - urg->front_data_index))) { return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR); } urg->scanning_first_step = first_step; urg->scanning_last_step = last_step; urg->scanning_skip_step = skip_step; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_set_connection_data_size(urg_t *urg, urg_range_data_byte_t data_byte) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((data_byte != URG_COMMUNICATION_3_BYTE) || (data_byte != URG_COMMUNICATION_2_BYTE)) { return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR); } urg->range_data_byte = data_byte; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_laser_on(urg_t *urg) { int expected[] = { 0, 2, EXPECTED_END }; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if (urg->is_laser_on != URG_FALSE) { // 既にレーザが発光しているときは、コマンドを送信しないようにする urg->last_errno = 0; return urg->last_errno; } ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0); if (ret >= 0) { urg->is_laser_on = URG_TRUE; ret = 0; } return ret; } int urg_laser_off(urg_t *urg) { return urg_stop_measurement(urg); } int urg_reboot(urg_t *urg) { int expected[] = { 0, 1, EXPECTED_END }; int ret; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // ２回目の RB 送信後、接続を切断する for (i = 0; i < 2; ++i) { ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0); if (ret < 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } urg->is_active = URG_FALSE; urg_close(urg); urg->last_errno = 0; return urg->last_errno; } void urg_sleep(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = 4 }; int sl_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE]; if (urg_stop_measurement(urg) != URG_NO_ERROR) { return; } scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); } void urg_wakeup(urg_t *urg) { urg_stop_measurement(urg); } int urg_is_stable(urg_t *urg) { const char *stat = urg_sensor_status(urg); return strncmp("Stable", stat, 6) ? 0 : 1; } static char *copy_token(char *dest, char *receive_buffer, const char *start_str, const char *end_ch, int lines) { size_t start_str_len = strlen(start_str); size_t end_ch_len = strlen(end_ch); int i; size_t j; for (j = 0; j < end_ch_len; ++j) { const char *p = receive_buffer; for (i = 0; i < lines; ++i) { if (!strncmp(p, start_str, start_str_len)) { char *last_p = strchr(p + start_str_len, end_ch[j]); if (last_p) { *last_p = '\0'; memcpy(dest, p + start_str_len, last_p - (p + start_str_len) + 1); return dest; } } p += strlen(p) + 1; } } return NULL; } static const char *receive_command_response(urg_t *urg, char *buffer, int buffer_size, const char* command, int response_lines) { const int vv_expected[] = { 0, EXPECTED_END }; int ret; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = scip_response(urg, command, vv_expected, urg->timeout, buffer, buffer_size); if (ret < response_lines) { return RECEIVE_ERROR_MESSAGE; } return NULL; } const char *urg_sensor_product_type(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROD:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_serial_id(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "SERI:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_vendor(urg_t *urg){ enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "VEND:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", "(", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_date(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } // Get the firmware version and append a '(', this will be what's before the date char firmware_version[50]; strcpy(firmware_version, urg_sensor_firmware_version(urg)); strcat(firmware_version, "("); ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } // Strip out the actual date from between the '(' and ')' char *date; p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", ";", VV_RESPONSE_LINES); date = copy_token(urg->return_buffer, p, firmware_version, ")", 1); return (date) ? date : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_protocol_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROT:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_status(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "STAT:", ";", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_state(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "MESM:", " (", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } void urg_set_error_handler(urg_t *urg, urg_error_handler handler) { urg->error_handler = handler; }
260  scip_response(urg, "TM2\n", tm2_expected,
261  MAX_TIMEOUT, NULL, 0);
262 
263  // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } if (ret <= 0) { if (ret == URG_INVALID_RESPONSE) { // 異常なエコーバックのときは、距離データ受信中とみなして // データを読み飛ばす ignore_receive_data_with_qt(urg, MAX_TIMEOUT); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } else { // 応答がないときは、ボーレートを変更して、再度接続を行う ignore_receive_data_with_qt(urg, MAX_TIMEOUT); continue; } } else if (!strcmp("00P", receive_buffer)) { // センサとホストのボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } return set_errno_and_return(urg, URG_NOT_DETECT_BAUDRATE_ERROR); } // PP コマンドの応答を urg_t に格納する static int receive_parameter(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * 9, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; int pp_expected[] = { 0, EXPECTED_END }; unsigned short received_bits = 0x0000; char *p; int i; int ret = scip_response(urg, "PP\n", pp_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); if (ret < 0) { return ret; } else if (ret < PP_RESPONSE_LINES) { ignore_receive_data_with_qt(urg, MAX_TIMEOUT); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } p = receive_buffer; for (i = 0; i < (ret - 1); ++i) { if (!strncmp(p, "DMIN:", 5)) { urg->min_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0001; } else if (!strncmp(p, "DMAX:", 5)) { urg->max_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0002; } else if (!strncmp(p, "ARES:", 5)) { urg->area_resolution = strtol(p + 5, NULL, 10); received_bits |= 0x0004; } else if (!strncmp(p, "AMIN:", 5)) { urg->first_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0008; } else if (!strncmp(p, "AMAX:", 5)) { urg->last_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0010; } else if (!strncmp(p, "AFRT:", 5)) { urg->front_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0020; } else if (!strncmp(p, "SCAN:", 5)) { int rpm = strtol(p + 5, NULL, 10); // タイムアウト時間は、計測周期の 16 倍程度の値にする urg->scan_usec = 1000 * 1000 * 60 / rpm; urg->timeout = urg->scan_usec >> (10 - 4); received_bits |= 0x0040; } p += strlen(p) + 1; } // 全てのパラメータを受信したか確認 if (received_bits != 0x007f) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } urg_set_scanning_parameter(urg, urg->first_data_index - urg->front_data_index, urg->last_data_index - urg->front_data_index, 1); return set_errno_and_return(urg, URG_NO_ERROR); } //! SCIP 文字列のデコード long urg_scip_decode(const char data[], int size) { const char* p = data; const char* last_p = p + size; int value = 0; while (p < last_p) { value <<= 6; value &= ~0x3f; value |= *p++ - 0x30; } return value; } static int parse_parameter(const char *parameter, int size) { char buffer[5]; memcpy(buffer, parameter, size); buffer[size] = '\0'; return strtol(buffer, NULL, 10); } static urg_measurement_type_t parse_distance_parameter(urg_t *urg, const char echoback[]) { urg_measurement_type_t ret_type = URG_UNKNOWN; urg->received_range_data_byte = URG_COMMUNICATION_3_BYTE; if (echoback[1] == 'S') { urg->received_range_data_byte = URG_COMMUNICATION_2_BYTE; ret_type = URG_DISTANCE; } else if (echoback[1] == 'D') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO; } } else if (echoback[1] == 'E') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE_INTENSITY; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO_INTENSITY; } } else { return URG_UNKNOWN; } // パラメータの格納 urg->received_first_index = parse_parameter(&echoback[2], 4); urg->received_last_index = parse_parameter(&echoback[6], 4); urg->received_skip_step = parse_parameter(&echoback[10], 2); return ret_type; } static urg_measurement_type_t parse_distance_echoback(urg_t *urg, const char echoback[]) { size_t line_length; urg_measurement_type_t ret_type = URG_UNKNOWN; if (!strcmp("QT", echoback)) { return URG_STOP; } line_length = strlen(echoback); if ((line_length == 12) && ((echoback[0] == 'G') || (echoback[0] == 'H'))) { ret_type = parse_distance_parameter(urg, echoback); } else if ((line_length == 15) && ((echoback[0] == 'M') || (echoback[0] == 'N'))) { ret_type = parse_distance_parameter(urg, echoback); } return ret_type; } static int receive_length_data(urg_t *urg, long length[], unsigned short intensity[], urg_measurement_type_t type, char buffer[]) { int n; int step_filled = 0; int line_filled = 0; int multiecho_index = 0; int each_size = (urg->received_range_data_byte == URG_COMMUNICATION_2_BYTE) ? 2 : 3; int data_size = each_size; int is_intensity = URG_FALSE; int is_multiecho = URG_FALSE; int multiecho_max_size = 1; if ((type == URG_DISTANCE_INTENSITY) || (type == URG_MULTIECHO_INTENSITY)) { data_size *= 2; is_intensity = URG_TRUE; } if ((type == URG_MULTIECHO) || (type == URG_MULTIECHO_INTENSITY)) { is_multiecho = URG_TRUE; multiecho_max_size = URG_MAX_ECHO; } do { char *p = buffer; char *last_p; n = connection_readline(&urg->connection, &buffer[line_filled], BUFFER_SIZE - line_filled, urg->timeout); if (n > 0) { // チェックサムの評価 if (buffer[line_filled + n - 1] != scip_checksum(&buffer[line_filled], n - 1)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } } if (n > 0) { line_filled += n - 1; } last_p = p + line_filled; while ((last_p - p) >= data_size) { int index; if (*p == '&') { // 先頭文字が '&' だったときは、マルチエコーのデータとみなす if ((last_p - (p + 1)) < data_size) { // '&' を除いて、data_size 分データが無ければ抜ける break; } --step_filled; ++multiecho_index; ++p; --line_filled; } else { // 次のデータ multiecho_index = 0; } index = (step_filled * multiecho_max_size) + multiecho_index; if (step_filled > (urg->received_last_index - urg->received_first_index)) { // データが多過ぎる場合は、残りのデータを無視して戻る ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (is_multiecho && (multiecho_index == 0)) { // マルチエコーのデータ格納先をダミーデータで埋める int i; if (length) { for (i = 1; i < multiecho_max_size; ++i) { length[index + i] = 0; } } if (intensity) { for (i = 1; i < multiecho_max_size; ++i) { intensity[index + i] = 0; } } } // 距離データの格納 if (length) { length[index] = urg_scip_decode(p, 3); } p += 3; // 強度データの格納 if (is_intensity) { if (intensity) { intensity[index] = (unsigned short)urg_scip_decode(p, 3); } p += 3; } ++step_filled; line_filled -= data_size; } // 次に処理する文字を退避 memmove(buffer, p, line_filled); } while (n > 0); return step_filled; } //! 距離データの取得 static int receive_data(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { urg_measurement_type_t type; char buffer[BUFFER_SIZE]; int ret = 0; int n; int extended_timeout = urg->timeout + 2 * (urg->scan_usec * (urg->scanning_skip_scan) / 1000); // エコーバックの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, extended_timeout); if (n <= 0) { return set_errno_and_return(urg, URG_NO_RESPONSE); } // エコーバックの解析 type = parse_distance_echoback(urg, buffer); // 応答の取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 3) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } if (buffer[n - 1] != scip_checksum(buffer, n - 1)) { // チェックサムの評価 ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } if (type == URG_STOP) { // QT 応答の場合には、最後の改行を読み捨て、正常応答として処理する n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n == 0) { return 0; } else { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } if (urg->specified_scan_times != 1) { if (!strncmp(buffer, "00", 2)) { // "00" 応答の場合は、エコーバック応答とみなし、 // 最後の空行を読み捨て、次からのデータを返す n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 0) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } } } if (((urg->specified_scan_times == 1) && (strncmp(buffer, "00", 2))) || ((urg->specified_scan_times != 1) && (strncmp(buffer, "99", 2)))) { if (urg->error_handler) { type = urg->error_handler(buffer, urg); } if (type == URG_UNKNOWN) { // Gx, Hx のときは 00P が返されたときがデータ // Mx, Nx のときは 99b が返されたときがデータ ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } // タイムスタンプの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n > 0) { if (time_stamp) { *time_stamp = urg_scip_decode(buffer, 4); } if (system_time_stamp) { urg_get_walltime(system_time_stamp); } } // データの取得 switch (type) { case URG_DISTANCE: case URG_MULTIECHO: ret = receive_length_data(urg, data, NULL, type, buffer); break; case URG_DISTANCE_INTENSITY: case URG_MULTIECHO_INTENSITY: ret = receive_length_data(urg, data, intensity, type, buffer); break; case URG_STOP: case URG_UNKNOWN: ret = 0; break; } // specified_scan_times == 1 のときは Gx 系コマンドが使われるため // データを明示的に停止しなくてよい if ((urg->specified_scan_times > 1) && (urg->scanning_remain_times > 0)) { if (--urg->scanning_remain_times <= 0) { // データの停止のみを行う urg_stop_measurement(urg); } } return ret; } int urg_open(urg_t *urg, urg_connection_type_t connection_type, const char *device_or_address, long baudrate_or_port) { int ret; long baudrate = baudrate_or_port; urg->is_active = URG_FALSE; urg->is_sending = URG_TRUE; urg->last_errno = URG_NOT_CONNECTED; urg->timeout = MAX_TIMEOUT; urg->scanning_skip_scan = 0; urg->error_handler = NULL; // デバイスへの接続 ret = connection_open(&urg->connection, connection_type, device_or_address, baudrate_or_port); if (ret < 0) { switch (connection_type) { case URG_SERIAL: urg->last_errno = URG_SERIAL_OPEN_ERROR; break; case URG_ETHERNET: urg->last_errno = URG_ETHERNET_OPEN_ERROR; break; default: urg->last_errno = URG_INVALID_RESPONSE; break; } return urg->last_errno; } // 指定したボーレートで URG と通信できるように調整 if (connection_type == URG_ETHERNET) { // Ethernet のときは仮の通信速度を指定しておく baudrate = 115200; } if (connect_urg_device(urg, baudrate) != URG_NO_ERROR) { return set_errno_and_return(urg, ret); } urg->is_sending = URG_FALSE; // 変数の初期化 urg->last_errno = URG_NO_ERROR; urg->range_data_byte = URG_COMMUNICATION_3_BYTE; urg->specified_scan_times = 0; urg->scanning_remain_times = 0; urg->is_laser_on = URG_FALSE; // パラメータ情報を取得 ret = receive_parameter(urg); if (ret == URG_NO_ERROR) { urg->is_active = URG_TRUE; } return ret; } void urg_close(urg_t *urg) { if (urg->is_active) { ignore_receive_data_with_qt(urg, urg->timeout); } connection_close(&urg->connection); urg->is_active = URG_FALSE; } void urg_set_timeout_msec(urg_t *urg, int msec) { urg->timeout = msec; } int urg_start_time_stamp_mode(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM0 を発行する n = scip_response(urg, "TM0\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } long urg_time_stamp(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; char buffer[BUFFER_SIZE]; char *p; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } ret = scip_response(urg, "TM1\n", expected, urg->timeout, buffer, BUFFER_SIZE); if (ret < 0) { return ret; } // buffer からタイムスタンプを取得し、デコードして返す if (strcmp(buffer, "00P")) { // 最初の応答が "00P" でなければ戻る return set_errno_and_return(urg, URG_RECEIVE_ERROR); } p = buffer + 4; if (strlen(p) != 5) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (p[5] == scip_checksum(p, 4)) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } return urg_scip_decode(p, 4); } int urg_stop_time_stamp_mode(urg_t *urg) { int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM2 を発行する n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } static int send_distance_command(urg_t *urg, int scan_times, int skip_scan, char single_scan_ch, char continuous_scan_ch, char scan_type_ch) { char buffer[BUFFER_SIZE]; int write_size = 0; int front_index = urg->front_data_index; int n; urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times; urg->scanning_remain_times = urg->specified_scan_times; urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan; if (scan_times >= 100) { // 計測回数が 99 を越える場合は、無限回のスキャンを行う urg->specified_scan_times = 0; } if (urg->scanning_remain_times == 1) { // レーザ発光を指示 urg_laser_on(urg); write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n", single_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step); } else { write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n", continuous_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step, skip_scan, urg->specified_scan_times); urg->is_sending = URG_TRUE; } n = connection_write(&urg->connection, buffer, write_size); if (n != write_size) { return set_errno_and_return(urg, URG_SEND_ERROR); } return 0; } int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan) { char range_byte_ch; int ret = 0; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((skip_scan < 0) || (skip_scan > 9)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // !!! Mx 系, Nx 系の計測中のときは、QT を発行してから // !!! 計測開始コマンドを送信するようにする // !!! ただし、MD 計測中に MD を発行するように、同じコマンドの場合は // !!! Mx 系, Nx 系の計測は上書きすることができるようにする // 指定されたタイプのパケットを生成し、送信する switch (type) { case URG_DISTANCE: range_byte_ch = (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D'; ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', range_byte_ch); break; case URG_DISTANCE_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', 'E'); break; case URG_MULTIECHO: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'D'); break; case URG_MULTIECHO_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'E'); break; case URG_STOP: case URG_UNKNOWN: default: ignore_receive_data_with_qt(urg, urg->timeout); urg->last_errno = URG_INVALID_PARAMETER; ret = urg->last_errno; break; } return ret; } int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, NULL, time_stamp, system_time_stamp); } int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp); } int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp); } int urg_stop_measurement(urg_t *urg) { enum { MAX_READ_TIMES = 3 }; int ret = URG_INVALID_RESPONSE; int n; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // QT を発行する n = connection_write(&urg->connection, "QT\n", 3); if (n != 3) { return set_errno_and_return(urg, URG_SEND_ERROR); } for (i = 0; i < MAX_READ_TIMES; ++i) { // QT の応答が返されるまで、距離データを読み捨てる ret = receive_data(urg, NULL, NULL, NULL, NULL); if (ret == URG_NO_ERROR) { // 正常応答 urg->is_laser_on = URG_FALSE; urg->is_sending = URG_FALSE; return set_errno_and_return(urg, URG_NO_ERROR); } } return ret; } int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step) { // 設定の範囲外を指定したときは、エラーを返す if (((skip_step < 0) || (skip_step >= 100)) || (first_step > last_step) || (first_step < -urg->front_data_index) || (last_step > (urg->last_data_index - urg->front_data_index))) { return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR); } urg->scanning_first_step = first_step; urg->scanning_last_step = last_step; urg->scanning_skip_step = skip_step; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_set_connection_data_size(urg_t *urg, urg_range_data_byte_t data_byte) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((data_byte != URG_COMMUNICATION_3_BYTE) || (data_byte != URG_COMMUNICATION_2_BYTE)) { return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR); } urg->range_data_byte = data_byte; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_laser_on(urg_t *urg) { int expected[] = { 0, 2, EXPECTED_END }; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if (urg->is_laser_on != URG_FALSE) { // 既にレーザが発光しているときは、コマンドを送信しないようにする urg->last_errno = 0; return urg->last_errno; } ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0); if (ret >= 0) { urg->is_laser_on = URG_TRUE; ret = 0; } return ret; } int urg_laser_off(urg_t *urg) { return urg_stop_measurement(urg); } int urg_reboot(urg_t *urg) { int expected[] = { 0, 1, EXPECTED_END }; int ret; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // ２回目の RB 送信後、接続を切断する for (i = 0; i < 2; ++i) { ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0); if (ret < 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } urg->is_active = URG_FALSE; urg_close(urg); urg->last_errno = 0; return urg->last_errno; } void urg_sleep(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = 4 }; int sl_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE]; if (urg_stop_measurement(urg) != URG_NO_ERROR) { return; } scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); } void urg_wakeup(urg_t *urg) { urg_stop_measurement(urg); } int urg_is_stable(urg_t *urg) { const char *stat = urg_sensor_status(urg); return strncmp("Stable", stat, 6) ? 0 : 1; } static char *copy_token(char *dest, char *receive_buffer, const char *start_str, const char *end_ch, int lines) { size_t start_str_len = strlen(start_str); size_t end_ch_len = strlen(end_ch); int i; size_t j; for (j = 0; j < end_ch_len; ++j) { const char *p = receive_buffer; for (i = 0; i < lines; ++i) { if (!strncmp(p, start_str, start_str_len)) { char *last_p = strchr(p + start_str_len, end_ch[j]); if (last_p) { *last_p = '\0'; memcpy(dest, p + start_str_len, last_p - (p + start_str_len) + 1); return dest; } } p += strlen(p) + 1; } } return NULL; } static const char *receive_command_response(urg_t *urg, char *buffer, int buffer_size, const char* command, int response_lines) { const int vv_expected[] = { 0, EXPECTED_END }; int ret; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = scip_response(urg, command, vv_expected, urg->timeout, buffer, buffer_size); if (ret < response_lines) { return RECEIVE_ERROR_MESSAGE; } return NULL; } const char *urg_sensor_product_type(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROD:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_serial_id(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "SERI:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_vendor(urg_t *urg){ enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "VEND:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", "(", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_date(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } // Get the firmware version and append a '(', this will be what's before the date char firmware_version[50]; strcpy(firmware_version, urg_sensor_firmware_version(urg)); strcat(firmware_version, "("); ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } // Strip out the actual date from between the '(' and ')' char *date; p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", ";", VV_RESPONSE_LINES); date = copy_token(urg->return_buffer, p, firmware_version, ")", 1); return (date) ? date : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_protocol_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROT:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_status(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "STAT:", ";", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_state(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "MESM:", " (", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } void urg_set_error_handler(urg_t *urg, urg_error_handler handler) { urg->error_handler = handler; }
264  return change_sensor_baudrate(urg, try_baudrate[i], baudrate);
265  }
266  }
267 
268  if (ret <= 0) {
269  if (ret == URG_INVALID_RESPONSE) {
270  // 異常なエコーバックのときは、距離データ受信中とみなして // データを読み飛ばす ignore_receive_data_with_qt(urg, MAX_TIMEOUT); // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } else { // 応答がないときは、ボーレートを変更して、再度接続を行う ignore_receive_data_with_qt(urg, MAX_TIMEOUT); continue; } } else if (!strcmp("00P", receive_buffer)) { // センサとホストのボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } return set_errno_and_return(urg, URG_NOT_DETECT_BAUDRATE_ERROR); } // PP コマンドの応答を urg_t に格納する static int receive_parameter(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * 9, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; int pp_expected[] = { 0, EXPECTED_END }; unsigned short received_bits = 0x0000; char *p; int i; int ret = scip_response(urg, "PP\n", pp_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); if (ret < 0) { return ret; } else if (ret < PP_RESPONSE_LINES) { ignore_receive_data_with_qt(urg, MAX_TIMEOUT); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } p = receive_buffer; for (i = 0; i < (ret - 1); ++i) { if (!strncmp(p, "DMIN:", 5)) { urg->min_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0001; } else if (!strncmp(p, "DMAX:", 5)) { urg->max_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0002; } else if (!strncmp(p, "ARES:", 5)) { urg->area_resolution = strtol(p + 5, NULL, 10); received_bits |= 0x0004; } else if (!strncmp(p, "AMIN:", 5)) { urg->first_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0008; } else if (!strncmp(p, "AMAX:", 5)) { urg->last_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0010; } else if (!strncmp(p, "AFRT:", 5)) { urg->front_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0020; } else if (!strncmp(p, "SCAN:", 5)) { int rpm = strtol(p + 5, NULL, 10); // タイムアウト時間は、計測周期の 16 倍程度の値にする urg->scan_usec = 1000 * 1000 * 60 / rpm; urg->timeout = urg->scan_usec >> (10 - 4); received_bits |= 0x0040; } p += strlen(p) + 1; } // 全てのパラメータを受信したか確認 if (received_bits != 0x007f) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } urg_set_scanning_parameter(urg, urg->first_data_index - urg->front_data_index, urg->last_data_index - urg->front_data_index, 1); return set_errno_and_return(urg, URG_NO_ERROR); } //! SCIP 文字列のデコード long urg_scip_decode(const char data[], int size) { const char* p = data; const char* last_p = p + size; int value = 0; while (p < last_p) { value <<= 6; value &= ~0x3f; value |= *p++ - 0x30; } return value; } static int parse_parameter(const char *parameter, int size) { char buffer[5]; memcpy(buffer, parameter, size); buffer[size] = '\0'; return strtol(buffer, NULL, 10); } static urg_measurement_type_t parse_distance_parameter(urg_t *urg, const char echoback[]) { urg_measurement_type_t ret_type = URG_UNKNOWN; urg->received_range_data_byte = URG_COMMUNICATION_3_BYTE; if (echoback[1] == 'S') { urg->received_range_data_byte = URG_COMMUNICATION_2_BYTE; ret_type = URG_DISTANCE; } else if (echoback[1] == 'D') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO; } } else if (echoback[1] == 'E') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE_INTENSITY; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO_INTENSITY; } } else { return URG_UNKNOWN; } // パラメータの格納 urg->received_first_index = parse_parameter(&echoback[2], 4); urg->received_last_index = parse_parameter(&echoback[6], 4); urg->received_skip_step = parse_parameter(&echoback[10], 2); return ret_type; } static urg_measurement_type_t parse_distance_echoback(urg_t *urg, const char echoback[]) { size_t line_length; urg_measurement_type_t ret_type = URG_UNKNOWN; if (!strcmp("QT", echoback)) { return URG_STOP; } line_length = strlen(echoback); if ((line_length == 12) && ((echoback[0] == 'G') || (echoback[0] == 'H'))) { ret_type = parse_distance_parameter(urg, echoback); } else if ((line_length == 15) && ((echoback[0] == 'M') || (echoback[0] == 'N'))) { ret_type = parse_distance_parameter(urg, echoback); } return ret_type; } static int receive_length_data(urg_t *urg, long length[], unsigned short intensity[], urg_measurement_type_t type, char buffer[]) { int n; int step_filled = 0; int line_filled = 0; int multiecho_index = 0; int each_size = (urg->received_range_data_byte == URG_COMMUNICATION_2_BYTE) ? 2 : 3; int data_size = each_size; int is_intensity = URG_FALSE; int is_multiecho = URG_FALSE; int multiecho_max_size = 1; if ((type == URG_DISTANCE_INTENSITY) || (type == URG_MULTIECHO_INTENSITY)) { data_size *= 2; is_intensity = URG_TRUE; } if ((type == URG_MULTIECHO) || (type == URG_MULTIECHO_INTENSITY)) { is_multiecho = URG_TRUE; multiecho_max_size = URG_MAX_ECHO; } do { char *p = buffer; char *last_p; n = connection_readline(&urg->connection, &buffer[line_filled], BUFFER_SIZE - line_filled, urg->timeout); if (n > 0) { // チェックサムの評価 if (buffer[line_filled + n - 1] != scip_checksum(&buffer[line_filled], n - 1)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } } if (n > 0) { line_filled += n - 1; } last_p = p + line_filled; while ((last_p - p) >= data_size) { int index; if (*p == '&') { // 先頭文字が '&' だったときは、マルチエコーのデータとみなす if ((last_p - (p + 1)) < data_size) { // '&' を除いて、data_size 分データが無ければ抜ける break; } --step_filled; ++multiecho_index; ++p; --line_filled; } else { // 次のデータ multiecho_index = 0; } index = (step_filled * multiecho_max_size) + multiecho_index; if (step_filled > (urg->received_last_index - urg->received_first_index)) { // データが多過ぎる場合は、残りのデータを無視して戻る ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (is_multiecho && (multiecho_index == 0)) { // マルチエコーのデータ格納先をダミーデータで埋める int i; if (length) { for (i = 1; i < multiecho_max_size; ++i) { length[index + i] = 0; } } if (intensity) { for (i = 1; i < multiecho_max_size; ++i) { intensity[index + i] = 0; } } } // 距離データの格納 if (length) { length[index] = urg_scip_decode(p, 3); } p += 3; // 強度データの格納 if (is_intensity) { if (intensity) { intensity[index] = (unsigned short)urg_scip_decode(p, 3); } p += 3; } ++step_filled; line_filled -= data_size; } // 次に処理する文字を退避 memmove(buffer, p, line_filled); } while (n > 0); return step_filled; } //! 距離データの取得 static int receive_data(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { urg_measurement_type_t type; char buffer[BUFFER_SIZE]; int ret = 0; int n; int extended_timeout = urg->timeout + 2 * (urg->scan_usec * (urg->scanning_skip_scan) / 1000); // エコーバックの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, extended_timeout); if (n <= 0) { return set_errno_and_return(urg, URG_NO_RESPONSE); } // エコーバックの解析 type = parse_distance_echoback(urg, buffer); // 応答の取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 3) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } if (buffer[n - 1] != scip_checksum(buffer, n - 1)) { // チェックサムの評価 ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } if (type == URG_STOP) { // QT 応答の場合には、最後の改行を読み捨て、正常応答として処理する n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n == 0) { return 0; } else { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } if (urg->specified_scan_times != 1) { if (!strncmp(buffer, "00", 2)) { // "00" 応答の場合は、エコーバック応答とみなし、 // 最後の空行を読み捨て、次からのデータを返す n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 0) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } } } if (((urg->specified_scan_times == 1) && (strncmp(buffer, "00", 2))) || ((urg->specified_scan_times != 1) && (strncmp(buffer, "99", 2)))) { if (urg->error_handler) { type = urg->error_handler(buffer, urg); } if (type == URG_UNKNOWN) { // Gx, Hx のときは 00P が返されたときがデータ // Mx, Nx のときは 99b が返されたときがデータ ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } // タイムスタンプの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n > 0) { if (time_stamp) { *time_stamp = urg_scip_decode(buffer, 4); } if (system_time_stamp) { urg_get_walltime(system_time_stamp); } } // データの取得 switch (type) { case URG_DISTANCE: case URG_MULTIECHO: ret = receive_length_data(urg, data, NULL, type, buffer); break; case URG_DISTANCE_INTENSITY: case URG_MULTIECHO_INTENSITY: ret = receive_length_data(urg, data, intensity, type, buffer); break; case URG_STOP: case URG_UNKNOWN: ret = 0; break; } // specified_scan_times == 1 のときは Gx 系コマンドが使われるため // データを明示的に停止しなくてよい if ((urg->specified_scan_times > 1) && (urg->scanning_remain_times > 0)) { if (--urg->scanning_remain_times <= 0) { // データの停止のみを行う urg_stop_measurement(urg); } } return ret; } int urg_open(urg_t *urg, urg_connection_type_t connection_type, const char *device_or_address, long baudrate_or_port) { int ret; long baudrate = baudrate_or_port; urg->is_active = URG_FALSE; urg->is_sending = URG_TRUE; urg->last_errno = URG_NOT_CONNECTED; urg->timeout = MAX_TIMEOUT; urg->scanning_skip_scan = 0; urg->error_handler = NULL; // デバイスへの接続 ret = connection_open(&urg->connection, connection_type, device_or_address, baudrate_or_port); if (ret < 0) { switch (connection_type) { case URG_SERIAL: urg->last_errno = URG_SERIAL_OPEN_ERROR; break; case URG_ETHERNET: urg->last_errno = URG_ETHERNET_OPEN_ERROR; break; default: urg->last_errno = URG_INVALID_RESPONSE; break; } return urg->last_errno; } // 指定したボーレートで URG と通信できるように調整 if (connection_type == URG_ETHERNET) { // Ethernet のときは仮の通信速度を指定しておく baudrate = 115200; } if (connect_urg_device(urg, baudrate) != URG_NO_ERROR) { return set_errno_and_return(urg, ret); } urg->is_sending = URG_FALSE; // 変数の初期化 urg->last_errno = URG_NO_ERROR; urg->range_data_byte = URG_COMMUNICATION_3_BYTE; urg->specified_scan_times = 0; urg->scanning_remain_times = 0; urg->is_laser_on = URG_FALSE; // パラメータ情報を取得 ret = receive_parameter(urg); if (ret == URG_NO_ERROR) { urg->is_active = URG_TRUE; } return ret; } void urg_close(urg_t *urg) { if (urg->is_active) { ignore_receive_data_with_qt(urg, urg->timeout); } connection_close(&urg->connection); urg->is_active = URG_FALSE; } void urg_set_timeout_msec(urg_t *urg, int msec) { urg->timeout = msec; } int urg_start_time_stamp_mode(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM0 を発行する n = scip_response(urg, "TM0\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } long urg_time_stamp(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; char buffer[BUFFER_SIZE]; char *p; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } ret = scip_response(urg, "TM1\n", expected, urg->timeout, buffer, BUFFER_SIZE); if (ret < 0) { return ret; } // buffer からタイムスタンプを取得し、デコードして返す if (strcmp(buffer, "00P")) { // 最初の応答が "00P" でなければ戻る return set_errno_and_return(urg, URG_RECEIVE_ERROR); } p = buffer + 4; if (strlen(p) != 5) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (p[5] == scip_checksum(p, 4)) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } return urg_scip_decode(p, 4); } int urg_stop_time_stamp_mode(urg_t *urg) { int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM2 を発行する n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } static int send_distance_command(urg_t *urg, int scan_times, int skip_scan, char single_scan_ch, char continuous_scan_ch, char scan_type_ch) { char buffer[BUFFER_SIZE]; int write_size = 0; int front_index = urg->front_data_index; int n; urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times; urg->scanning_remain_times = urg->specified_scan_times; urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan; if (scan_times >= 100) { // 計測回数が 99 を越える場合は、無限回のスキャンを行う urg->specified_scan_times = 0; } if (urg->scanning_remain_times == 1) { // レーザ発光を指示 urg_laser_on(urg); write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n", single_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step); } else { write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n", continuous_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step, skip_scan, urg->specified_scan_times); urg->is_sending = URG_TRUE; } n = connection_write(&urg->connection, buffer, write_size); if (n != write_size) { return set_errno_and_return(urg, URG_SEND_ERROR); } return 0; } int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan) { char range_byte_ch; int ret = 0; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((skip_scan < 0) || (skip_scan > 9)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // !!! Mx 系, Nx 系の計測中のときは、QT を発行してから // !!! 計測開始コマンドを送信するようにする // !!! ただし、MD 計測中に MD を発行するように、同じコマンドの場合は // !!! Mx 系, Nx 系の計測は上書きすることができるようにする // 指定されたタイプのパケットを生成し、送信する switch (type) { case URG_DISTANCE: range_byte_ch = (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D'; ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', range_byte_ch); break; case URG_DISTANCE_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', 'E'); break; case URG_MULTIECHO: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'D'); break; case URG_MULTIECHO_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'E'); break; case URG_STOP: case URG_UNKNOWN: default: ignore_receive_data_with_qt(urg, urg->timeout); urg->last_errno = URG_INVALID_PARAMETER; ret = urg->last_errno; break; } return ret; } int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, NULL, time_stamp, system_time_stamp); } int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp); } int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp); } int urg_stop_measurement(urg_t *urg) { enum { MAX_READ_TIMES = 3 }; int ret = URG_INVALID_RESPONSE; int n; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // QT を発行する n = connection_write(&urg->connection, "QT\n", 3); if (n != 3) { return set_errno_and_return(urg, URG_SEND_ERROR); } for (i = 0; i < MAX_READ_TIMES; ++i) { // QT の応答が返されるまで、距離データを読み捨てる ret = receive_data(urg, NULL, NULL, NULL, NULL); if (ret == URG_NO_ERROR) { // 正常応答 urg->is_laser_on = URG_FALSE; urg->is_sending = URG_FALSE; return set_errno_and_return(urg, URG_NO_ERROR); } } return ret; } int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step) { // 設定の範囲外を指定したときは、エラーを返す if (((skip_step < 0) || (skip_step >= 100)) || (first_step > last_step) || (first_step < -urg->front_data_index) || (last_step > (urg->last_data_index - urg->front_data_index))) { return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR); } urg->scanning_first_step = first_step; urg->scanning_last_step = last_step; urg->scanning_skip_step = skip_step; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_set_connection_data_size(urg_t *urg, urg_range_data_byte_t data_byte) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((data_byte != URG_COMMUNICATION_3_BYTE) || (data_byte != URG_COMMUNICATION_2_BYTE)) { return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR); } urg->range_data_byte = data_byte; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_laser_on(urg_t *urg) { int expected[] = { 0, 2, EXPECTED_END }; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if (urg->is_laser_on != URG_FALSE) { // 既にレーザが発光しているときは、コマンドを送信しないようにする urg->last_errno = 0; return urg->last_errno; } ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0); if (ret >= 0) { urg->is_laser_on = URG_TRUE; ret = 0; } return ret; } int urg_laser_off(urg_t *urg) { return urg_stop_measurement(urg); } int urg_reboot(urg_t *urg) { int expected[] = { 0, 1, EXPECTED_END }; int ret; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // ２回目の RB 送信後、接続を切断する for (i = 0; i < 2; ++i) { ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0); if (ret < 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } urg->is_active = URG_FALSE; urg_close(urg); urg->last_errno = 0; return urg->last_errno; } void urg_sleep(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = 4 }; int sl_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE]; if (urg_stop_measurement(urg) != URG_NO_ERROR) { return; } scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); } void urg_wakeup(urg_t *urg) { urg_stop_measurement(urg); } int urg_is_stable(urg_t *urg) { const char *stat = urg_sensor_status(urg); return strncmp("Stable", stat, 6) ? 0 : 1; } static char *copy_token(char *dest, char *receive_buffer, const char *start_str, const char *end_ch, int lines) { size_t start_str_len = strlen(start_str); size_t end_ch_len = strlen(end_ch); int i; size_t j; for (j = 0; j < end_ch_len; ++j) { const char *p = receive_buffer; for (i = 0; i < lines; ++i) { if (!strncmp(p, start_str, start_str_len)) { char *last_p = strchr(p + start_str_len, end_ch[j]); if (last_p) { *last_p = '\0'; memcpy(dest, p + start_str_len, last_p - (p + start_str_len) + 1); return dest; } } p += strlen(p) + 1; } } return NULL; } static const char *receive_command_response(urg_t *urg, char *buffer, int buffer_size, const char* command, int response_lines) { const int vv_expected[] = { 0, EXPECTED_END }; int ret; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = scip_response(urg, command, vv_expected, urg->timeout, buffer, buffer_size); if (ret < response_lines) { return RECEIVE_ERROR_MESSAGE; } return NULL; } const char *urg_sensor_product_type(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROD:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_serial_id(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "SERI:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_vendor(urg_t *urg){ enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "VEND:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", "(", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_date(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } // Get the firmware version and append a '(', this will be what's before the date char firmware_version[50]; strcpy(firmware_version, urg_sensor_firmware_version(urg)); strcat(firmware_version, "("); ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } // Strip out the actual date from between the '(' and ')' char *date; p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", ";", VV_RESPONSE_LINES); date = copy_token(urg->return_buffer, p, firmware_version, ")", 1); return (date) ? date : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_protocol_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROT:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_status(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "STAT:", ";", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_state(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "MESM:", " (", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } void urg_set_error_handler(urg_t *urg, urg_error_handler handler) { urg->error_handler = handler; }
271  // データを読み飛ばす
272  ignore_receive_data_with_qt(urg, MAX_TIMEOUT);
273 
274  // ボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } else { // 応答がないときは、ボーレートを変更して、再度接続を行う ignore_receive_data_with_qt(urg, MAX_TIMEOUT); continue; } } else if (!strcmp("00P", receive_buffer)) { // センサとホストのボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } return set_errno_and_return(urg, URG_NOT_DETECT_BAUDRATE_ERROR); } // PP コマンドの応答を urg_t に格納する static int receive_parameter(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * 9, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; int pp_expected[] = { 0, EXPECTED_END }; unsigned short received_bits = 0x0000; char *p; int i; int ret = scip_response(urg, "PP\n", pp_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); if (ret < 0) { return ret; } else if (ret < PP_RESPONSE_LINES) { ignore_receive_data_with_qt(urg, MAX_TIMEOUT); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } p = receive_buffer; for (i = 0; i < (ret - 1); ++i) { if (!strncmp(p, "DMIN:", 5)) { urg->min_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0001; } else if (!strncmp(p, "DMAX:", 5)) { urg->max_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0002; } else if (!strncmp(p, "ARES:", 5)) { urg->area_resolution = strtol(p + 5, NULL, 10); received_bits |= 0x0004; } else if (!strncmp(p, "AMIN:", 5)) { urg->first_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0008; } else if (!strncmp(p, "AMAX:", 5)) { urg->last_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0010; } else if (!strncmp(p, "AFRT:", 5)) { urg->front_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0020; } else if (!strncmp(p, "SCAN:", 5)) { int rpm = strtol(p + 5, NULL, 10); // タイムアウト時間は、計測周期の 16 倍程度の値にする urg->scan_usec = 1000 * 1000 * 60 / rpm; urg->timeout = urg->scan_usec >> (10 - 4); received_bits |= 0x0040; } p += strlen(p) + 1; } // 全てのパラメータを受信したか確認 if (received_bits != 0x007f) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } urg_set_scanning_parameter(urg, urg->first_data_index - urg->front_data_index, urg->last_data_index - urg->front_data_index, 1); return set_errno_and_return(urg, URG_NO_ERROR); } //! SCIP 文字列のデコード long urg_scip_decode(const char data[], int size) { const char* p = data; const char* last_p = p + size; int value = 0; while (p < last_p) { value <<= 6; value &= ~0x3f; value |= *p++ - 0x30; } return value; } static int parse_parameter(const char *parameter, int size) { char buffer[5]; memcpy(buffer, parameter, size); buffer[size] = '\0'; return strtol(buffer, NULL, 10); } static urg_measurement_type_t parse_distance_parameter(urg_t *urg, const char echoback[]) { urg_measurement_type_t ret_type = URG_UNKNOWN; urg->received_range_data_byte = URG_COMMUNICATION_3_BYTE; if (echoback[1] == 'S') { urg->received_range_data_byte = URG_COMMUNICATION_2_BYTE; ret_type = URG_DISTANCE; } else if (echoback[1] == 'D') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO; } } else if (echoback[1] == 'E') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE_INTENSITY; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO_INTENSITY; } } else { return URG_UNKNOWN; } // パラメータの格納 urg->received_first_index = parse_parameter(&echoback[2], 4); urg->received_last_index = parse_parameter(&echoback[6], 4); urg->received_skip_step = parse_parameter(&echoback[10], 2); return ret_type; } static urg_measurement_type_t parse_distance_echoback(urg_t *urg, const char echoback[]) { size_t line_length; urg_measurement_type_t ret_type = URG_UNKNOWN; if (!strcmp("QT", echoback)) { return URG_STOP; } line_length = strlen(echoback); if ((line_length == 12) && ((echoback[0] == 'G') || (echoback[0] == 'H'))) { ret_type = parse_distance_parameter(urg, echoback); } else if ((line_length == 15) && ((echoback[0] == 'M') || (echoback[0] == 'N'))) { ret_type = parse_distance_parameter(urg, echoback); } return ret_type; } static int receive_length_data(urg_t *urg, long length[], unsigned short intensity[], urg_measurement_type_t type, char buffer[]) { int n; int step_filled = 0; int line_filled = 0; int multiecho_index = 0; int each_size = (urg->received_range_data_byte == URG_COMMUNICATION_2_BYTE) ? 2 : 3; int data_size = each_size; int is_intensity = URG_FALSE; int is_multiecho = URG_FALSE; int multiecho_max_size = 1; if ((type == URG_DISTANCE_INTENSITY) || (type == URG_MULTIECHO_INTENSITY)) { data_size *= 2; is_intensity = URG_TRUE; } if ((type == URG_MULTIECHO) || (type == URG_MULTIECHO_INTENSITY)) { is_multiecho = URG_TRUE; multiecho_max_size = URG_MAX_ECHO; } do { char *p = buffer; char *last_p; n = connection_readline(&urg->connection, &buffer[line_filled], BUFFER_SIZE - line_filled, urg->timeout); if (n > 0) { // チェックサムの評価 if (buffer[line_filled + n - 1] != scip_checksum(&buffer[line_filled], n - 1)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } } if (n > 0) { line_filled += n - 1; } last_p = p + line_filled; while ((last_p - p) >= data_size) { int index; if (*p == '&') { // 先頭文字が '&' だったときは、マルチエコーのデータとみなす if ((last_p - (p + 1)) < data_size) { // '&' を除いて、data_size 分データが無ければ抜ける break; } --step_filled; ++multiecho_index; ++p; --line_filled; } else { // 次のデータ multiecho_index = 0; } index = (step_filled * multiecho_max_size) + multiecho_index; if (step_filled > (urg->received_last_index - urg->received_first_index)) { // データが多過ぎる場合は、残りのデータを無視して戻る ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (is_multiecho && (multiecho_index == 0)) { // マルチエコーのデータ格納先をダミーデータで埋める int i; if (length) { for (i = 1; i < multiecho_max_size; ++i) { length[index + i] = 0; } } if (intensity) { for (i = 1; i < multiecho_max_size; ++i) { intensity[index + i] = 0; } } } // 距離データの格納 if (length) { length[index] = urg_scip_decode(p, 3); } p += 3; // 強度データの格納 if (is_intensity) { if (intensity) { intensity[index] = (unsigned short)urg_scip_decode(p, 3); } p += 3; } ++step_filled; line_filled -= data_size; } // 次に処理する文字を退避 memmove(buffer, p, line_filled); } while (n > 0); return step_filled; } //! 距離データの取得 static int receive_data(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { urg_measurement_type_t type; char buffer[BUFFER_SIZE]; int ret = 0; int n; int extended_timeout = urg->timeout + 2 * (urg->scan_usec * (urg->scanning_skip_scan) / 1000); // エコーバックの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, extended_timeout); if (n <= 0) { return set_errno_and_return(urg, URG_NO_RESPONSE); } // エコーバックの解析 type = parse_distance_echoback(urg, buffer); // 応答の取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 3) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } if (buffer[n - 1] != scip_checksum(buffer, n - 1)) { // チェックサムの評価 ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } if (type == URG_STOP) { // QT 応答の場合には、最後の改行を読み捨て、正常応答として処理する n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n == 0) { return 0; } else { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } if (urg->specified_scan_times != 1) { if (!strncmp(buffer, "00", 2)) { // "00" 応答の場合は、エコーバック応答とみなし、 // 最後の空行を読み捨て、次からのデータを返す n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 0) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } } } if (((urg->specified_scan_times == 1) && (strncmp(buffer, "00", 2))) || ((urg->specified_scan_times != 1) && (strncmp(buffer, "99", 2)))) { if (urg->error_handler) { type = urg->error_handler(buffer, urg); } if (type == URG_UNKNOWN) { // Gx, Hx のときは 00P が返されたときがデータ // Mx, Nx のときは 99b が返されたときがデータ ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } // タイムスタンプの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n > 0) { if (time_stamp) { *time_stamp = urg_scip_decode(buffer, 4); } if (system_time_stamp) { urg_get_walltime(system_time_stamp); } } // データの取得 switch (type) { case URG_DISTANCE: case URG_MULTIECHO: ret = receive_length_data(urg, data, NULL, type, buffer); break; case URG_DISTANCE_INTENSITY: case URG_MULTIECHO_INTENSITY: ret = receive_length_data(urg, data, intensity, type, buffer); break; case URG_STOP: case URG_UNKNOWN: ret = 0; break; } // specified_scan_times == 1 のときは Gx 系コマンドが使われるため // データを明示的に停止しなくてよい if ((urg->specified_scan_times > 1) && (urg->scanning_remain_times > 0)) { if (--urg->scanning_remain_times <= 0) { // データの停止のみを行う urg_stop_measurement(urg); } } return ret; } int urg_open(urg_t *urg, urg_connection_type_t connection_type, const char *device_or_address, long baudrate_or_port) { int ret; long baudrate = baudrate_or_port; urg->is_active = URG_FALSE; urg->is_sending = URG_TRUE; urg->last_errno = URG_NOT_CONNECTED; urg->timeout = MAX_TIMEOUT; urg->scanning_skip_scan = 0; urg->error_handler = NULL; // デバイスへの接続 ret = connection_open(&urg->connection, connection_type, device_or_address, baudrate_or_port); if (ret < 0) { switch (connection_type) { case URG_SERIAL: urg->last_errno = URG_SERIAL_OPEN_ERROR; break; case URG_ETHERNET: urg->last_errno = URG_ETHERNET_OPEN_ERROR; break; default: urg->last_errno = URG_INVALID_RESPONSE; break; } return urg->last_errno; } // 指定したボーレートで URG と通信できるように調整 if (connection_type == URG_ETHERNET) { // Ethernet のときは仮の通信速度を指定しておく baudrate = 115200; } if (connect_urg_device(urg, baudrate) != URG_NO_ERROR) { return set_errno_and_return(urg, ret); } urg->is_sending = URG_FALSE; // 変数の初期化 urg->last_errno = URG_NO_ERROR; urg->range_data_byte = URG_COMMUNICATION_3_BYTE; urg->specified_scan_times = 0; urg->scanning_remain_times = 0; urg->is_laser_on = URG_FALSE; // パラメータ情報を取得 ret = receive_parameter(urg); if (ret == URG_NO_ERROR) { urg->is_active = URG_TRUE; } return ret; } void urg_close(urg_t *urg) { if (urg->is_active) { ignore_receive_data_with_qt(urg, urg->timeout); } connection_close(&urg->connection); urg->is_active = URG_FALSE; } void urg_set_timeout_msec(urg_t *urg, int msec) { urg->timeout = msec; } int urg_start_time_stamp_mode(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM0 を発行する n = scip_response(urg, "TM0\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } long urg_time_stamp(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; char buffer[BUFFER_SIZE]; char *p; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } ret = scip_response(urg, "TM1\n", expected, urg->timeout, buffer, BUFFER_SIZE); if (ret < 0) { return ret; } // buffer からタイムスタンプを取得し、デコードして返す if (strcmp(buffer, "00P")) { // 最初の応答が "00P" でなければ戻る return set_errno_and_return(urg, URG_RECEIVE_ERROR); } p = buffer + 4; if (strlen(p) != 5) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (p[5] == scip_checksum(p, 4)) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } return urg_scip_decode(p, 4); } int urg_stop_time_stamp_mode(urg_t *urg) { int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM2 を発行する n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } static int send_distance_command(urg_t *urg, int scan_times, int skip_scan, char single_scan_ch, char continuous_scan_ch, char scan_type_ch) { char buffer[BUFFER_SIZE]; int write_size = 0; int front_index = urg->front_data_index; int n; urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times; urg->scanning_remain_times = urg->specified_scan_times; urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan; if (scan_times >= 100) { // 計測回数が 99 を越える場合は、無限回のスキャンを行う urg->specified_scan_times = 0; } if (urg->scanning_remain_times == 1) { // レーザ発光を指示 urg_laser_on(urg); write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n", single_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step); } else { write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n", continuous_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step, skip_scan, urg->specified_scan_times); urg->is_sending = URG_TRUE; } n = connection_write(&urg->connection, buffer, write_size); if (n != write_size) { return set_errno_and_return(urg, URG_SEND_ERROR); } return 0; } int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan) { char range_byte_ch; int ret = 0; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((skip_scan < 0) || (skip_scan > 9)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // !!! Mx 系, Nx 系の計測中のときは、QT を発行してから // !!! 計測開始コマンドを送信するようにする // !!! ただし、MD 計測中に MD を発行するように、同じコマンドの場合は // !!! Mx 系, Nx 系の計測は上書きすることができるようにする // 指定されたタイプのパケットを生成し、送信する switch (type) { case URG_DISTANCE: range_byte_ch = (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D'; ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', range_byte_ch); break; case URG_DISTANCE_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', 'E'); break; case URG_MULTIECHO: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'D'); break; case URG_MULTIECHO_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'E'); break; case URG_STOP: case URG_UNKNOWN: default: ignore_receive_data_with_qt(urg, urg->timeout); urg->last_errno = URG_INVALID_PARAMETER; ret = urg->last_errno; break; } return ret; } int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, NULL, time_stamp, system_time_stamp); } int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp); } int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp); } int urg_stop_measurement(urg_t *urg) { enum { MAX_READ_TIMES = 3 }; int ret = URG_INVALID_RESPONSE; int n; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // QT を発行する n = connection_write(&urg->connection, "QT\n", 3); if (n != 3) { return set_errno_and_return(urg, URG_SEND_ERROR); } for (i = 0; i < MAX_READ_TIMES; ++i) { // QT の応答が返されるまで、距離データを読み捨てる ret = receive_data(urg, NULL, NULL, NULL, NULL); if (ret == URG_NO_ERROR) { // 正常応答 urg->is_laser_on = URG_FALSE; urg->is_sending = URG_FALSE; return set_errno_and_return(urg, URG_NO_ERROR); } } return ret; } int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step) { // 設定の範囲外を指定したときは、エラーを返す if (((skip_step < 0) || (skip_step >= 100)) || (first_step > last_step) || (first_step < -urg->front_data_index) || (last_step > (urg->last_data_index - urg->front_data_index))) { return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR); } urg->scanning_first_step = first_step; urg->scanning_last_step = last_step; urg->scanning_skip_step = skip_step; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_set_connection_data_size(urg_t *urg, urg_range_data_byte_t data_byte) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((data_byte != URG_COMMUNICATION_3_BYTE) || (data_byte != URG_COMMUNICATION_2_BYTE)) { return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR); } urg->range_data_byte = data_byte; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_laser_on(urg_t *urg) { int expected[] = { 0, 2, EXPECTED_END }; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if (urg->is_laser_on != URG_FALSE) { // 既にレーザが発光しているときは、コマンドを送信しないようにする urg->last_errno = 0; return urg->last_errno; } ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0); if (ret >= 0) { urg->is_laser_on = URG_TRUE; ret = 0; } return ret; } int urg_laser_off(urg_t *urg) { return urg_stop_measurement(urg); } int urg_reboot(urg_t *urg) { int expected[] = { 0, 1, EXPECTED_END }; int ret; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // ２回目の RB 送信後、接続を切断する for (i = 0; i < 2; ++i) { ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0); if (ret < 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } urg->is_active = URG_FALSE; urg_close(urg); urg->last_errno = 0; return urg->last_errno; } void urg_sleep(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = 4 }; int sl_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE]; if (urg_stop_measurement(urg) != URG_NO_ERROR) { return; } scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); } void urg_wakeup(urg_t *urg) { urg_stop_measurement(urg); } int urg_is_stable(urg_t *urg) { const char *stat = urg_sensor_status(urg); return strncmp("Stable", stat, 6) ? 0 : 1; } static char *copy_token(char *dest, char *receive_buffer, const char *start_str, const char *end_ch, int lines) { size_t start_str_len = strlen(start_str); size_t end_ch_len = strlen(end_ch); int i; size_t j; for (j = 0; j < end_ch_len; ++j) { const char *p = receive_buffer; for (i = 0; i < lines; ++i) { if (!strncmp(p, start_str, start_str_len)) { char *last_p = strchr(p + start_str_len, end_ch[j]); if (last_p) { *last_p = '\0'; memcpy(dest, p + start_str_len, last_p - (p + start_str_len) + 1); return dest; } } p += strlen(p) + 1; } } return NULL; } static const char *receive_command_response(urg_t *urg, char *buffer, int buffer_size, const char* command, int response_lines) { const int vv_expected[] = { 0, EXPECTED_END }; int ret; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = scip_response(urg, command, vv_expected, urg->timeout, buffer, buffer_size); if (ret < response_lines) { return RECEIVE_ERROR_MESSAGE; } return NULL; } const char *urg_sensor_product_type(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROD:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_serial_id(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "SERI:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_vendor(urg_t *urg){ enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "VEND:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", "(", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_date(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } // Get the firmware version and append a '(', this will be what's before the date char firmware_version[50]; strcpy(firmware_version, urg_sensor_firmware_version(urg)); strcat(firmware_version, "("); ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } // Strip out the actual date from between the '(' and ')' char *date; p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", ";", VV_RESPONSE_LINES); date = copy_token(urg->return_buffer, p, firmware_version, ")", 1); return (date) ? date : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_protocol_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROT:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_status(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "STAT:", ";", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_state(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "MESM:", " (", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } void urg_set_error_handler(urg_t *urg, urg_error_handler handler) { urg->error_handler = handler; }
275  return change_sensor_baudrate(urg, try_baudrate[i], baudrate);
276 
277  } else {
278  // 応答がないときは、ボーレートを変更して、再度接続を行う
279  ignore_receive_data_with_qt(urg, MAX_TIMEOUT);
280  continue;
281  }
282  } else if (!strcmp("00P", receive_buffer)) {
283 
284  // センサとホストのボーレートを変更して戻る return change_sensor_baudrate(urg, try_baudrate[i], baudrate); } } return set_errno_and_return(urg, URG_NOT_DETECT_BAUDRATE_ERROR); } // PP コマンドの応答を urg_t に格納する static int receive_parameter(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * 9, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; int pp_expected[] = { 0, EXPECTED_END }; unsigned short received_bits = 0x0000; char *p; int i; int ret = scip_response(urg, "PP\n", pp_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); if (ret < 0) { return ret; } else if (ret < PP_RESPONSE_LINES) { ignore_receive_data_with_qt(urg, MAX_TIMEOUT); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } p = receive_buffer; for (i = 0; i < (ret - 1); ++i) { if (!strncmp(p, "DMIN:", 5)) { urg->min_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0001; } else if (!strncmp(p, "DMAX:", 5)) { urg->max_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0002; } else if (!strncmp(p, "ARES:", 5)) { urg->area_resolution = strtol(p + 5, NULL, 10); received_bits |= 0x0004; } else if (!strncmp(p, "AMIN:", 5)) { urg->first_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0008; } else if (!strncmp(p, "AMAX:", 5)) { urg->last_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0010; } else if (!strncmp(p, "AFRT:", 5)) { urg->front_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0020; } else if (!strncmp(p, "SCAN:", 5)) { int rpm = strtol(p + 5, NULL, 10); // タイムアウト時間は、計測周期の 16 倍程度の値にする urg->scan_usec = 1000 * 1000 * 60 / rpm; urg->timeout = urg->scan_usec >> (10 - 4); received_bits |= 0x0040; } p += strlen(p) + 1; } // 全てのパラメータを受信したか確認 if (received_bits != 0x007f) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } urg_set_scanning_parameter(urg, urg->first_data_index - urg->front_data_index, urg->last_data_index - urg->front_data_index, 1); return set_errno_and_return(urg, URG_NO_ERROR); } //! SCIP 文字列のデコード long urg_scip_decode(const char data[], int size) { const char* p = data; const char* last_p = p + size; int value = 0; while (p < last_p) { value <<= 6; value &= ~0x3f; value |= *p++ - 0x30; } return value; } static int parse_parameter(const char *parameter, int size) { char buffer[5]; memcpy(buffer, parameter, size); buffer[size] = '\0'; return strtol(buffer, NULL, 10); } static urg_measurement_type_t parse_distance_parameter(urg_t *urg, const char echoback[]) { urg_measurement_type_t ret_type = URG_UNKNOWN; urg->received_range_data_byte = URG_COMMUNICATION_3_BYTE; if (echoback[1] == 'S') { urg->received_range_data_byte = URG_COMMUNICATION_2_BYTE; ret_type = URG_DISTANCE; } else if (echoback[1] == 'D') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO; } } else if (echoback[1] == 'E') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE_INTENSITY; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO_INTENSITY; } } else { return URG_UNKNOWN; } // パラメータの格納 urg->received_first_index = parse_parameter(&echoback[2], 4); urg->received_last_index = parse_parameter(&echoback[6], 4); urg->received_skip_step = parse_parameter(&echoback[10], 2); return ret_type; } static urg_measurement_type_t parse_distance_echoback(urg_t *urg, const char echoback[]) { size_t line_length; urg_measurement_type_t ret_type = URG_UNKNOWN; if (!strcmp("QT", echoback)) { return URG_STOP; } line_length = strlen(echoback); if ((line_length == 12) && ((echoback[0] == 'G') || (echoback[0] == 'H'))) { ret_type = parse_distance_parameter(urg, echoback); } else if ((line_length == 15) && ((echoback[0] == 'M') || (echoback[0] == 'N'))) { ret_type = parse_distance_parameter(urg, echoback); } return ret_type; } static int receive_length_data(urg_t *urg, long length[], unsigned short intensity[], urg_measurement_type_t type, char buffer[]) { int n; int step_filled = 0; int line_filled = 0; int multiecho_index = 0; int each_size = (urg->received_range_data_byte == URG_COMMUNICATION_2_BYTE) ? 2 : 3; int data_size = each_size; int is_intensity = URG_FALSE; int is_multiecho = URG_FALSE; int multiecho_max_size = 1; if ((type == URG_DISTANCE_INTENSITY) || (type == URG_MULTIECHO_INTENSITY)) { data_size *= 2; is_intensity = URG_TRUE; } if ((type == URG_MULTIECHO) || (type == URG_MULTIECHO_INTENSITY)) { is_multiecho = URG_TRUE; multiecho_max_size = URG_MAX_ECHO; } do { char *p = buffer; char *last_p; n = connection_readline(&urg->connection, &buffer[line_filled], BUFFER_SIZE - line_filled, urg->timeout); if (n > 0) { // チェックサムの評価 if (buffer[line_filled + n - 1] != scip_checksum(&buffer[line_filled], n - 1)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } } if (n > 0) { line_filled += n - 1; } last_p = p + line_filled; while ((last_p - p) >= data_size) { int index; if (*p == '&') { // 先頭文字が '&' だったときは、マルチエコーのデータとみなす if ((last_p - (p + 1)) < data_size) { // '&' を除いて、data_size 分データが無ければ抜ける break; } --step_filled; ++multiecho_index; ++p; --line_filled; } else { // 次のデータ multiecho_index = 0; } index = (step_filled * multiecho_max_size) + multiecho_index; if (step_filled > (urg->received_last_index - urg->received_first_index)) { // データが多過ぎる場合は、残りのデータを無視して戻る ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (is_multiecho && (multiecho_index == 0)) { // マルチエコーのデータ格納先をダミーデータで埋める int i; if (length) { for (i = 1; i < multiecho_max_size; ++i) { length[index + i] = 0; } } if (intensity) { for (i = 1; i < multiecho_max_size; ++i) { intensity[index + i] = 0; } } } // 距離データの格納 if (length) { length[index] = urg_scip_decode(p, 3); } p += 3; // 強度データの格納 if (is_intensity) { if (intensity) { intensity[index] = (unsigned short)urg_scip_decode(p, 3); } p += 3; } ++step_filled; line_filled -= data_size; } // 次に処理する文字を退避 memmove(buffer, p, line_filled); } while (n > 0); return step_filled; } //! 距離データの取得 static int receive_data(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { urg_measurement_type_t type; char buffer[BUFFER_SIZE]; int ret = 0; int n; int extended_timeout = urg->timeout + 2 * (urg->scan_usec * (urg->scanning_skip_scan) / 1000); // エコーバックの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, extended_timeout); if (n <= 0) { return set_errno_and_return(urg, URG_NO_RESPONSE); } // エコーバックの解析 type = parse_distance_echoback(urg, buffer); // 応答の取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 3) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } if (buffer[n - 1] != scip_checksum(buffer, n - 1)) { // チェックサムの評価 ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } if (type == URG_STOP) { // QT 応答の場合には、最後の改行を読み捨て、正常応答として処理する n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n == 0) { return 0; } else { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } if (urg->specified_scan_times != 1) { if (!strncmp(buffer, "00", 2)) { // "00" 応答の場合は、エコーバック応答とみなし、 // 最後の空行を読み捨て、次からのデータを返す n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 0) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } } } if (((urg->specified_scan_times == 1) && (strncmp(buffer, "00", 2))) || ((urg->specified_scan_times != 1) && (strncmp(buffer, "99", 2)))) { if (urg->error_handler) { type = urg->error_handler(buffer, urg); } if (type == URG_UNKNOWN) { // Gx, Hx のときは 00P が返されたときがデータ // Mx, Nx のときは 99b が返されたときがデータ ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } // タイムスタンプの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n > 0) { if (time_stamp) { *time_stamp = urg_scip_decode(buffer, 4); } if (system_time_stamp) { urg_get_walltime(system_time_stamp); } } // データの取得 switch (type) { case URG_DISTANCE: case URG_MULTIECHO: ret = receive_length_data(urg, data, NULL, type, buffer); break; case URG_DISTANCE_INTENSITY: case URG_MULTIECHO_INTENSITY: ret = receive_length_data(urg, data, intensity, type, buffer); break; case URG_STOP: case URG_UNKNOWN: ret = 0; break; } // specified_scan_times == 1 のときは Gx 系コマンドが使われるため // データを明示的に停止しなくてよい if ((urg->specified_scan_times > 1) && (urg->scanning_remain_times > 0)) { if (--urg->scanning_remain_times <= 0) { // データの停止のみを行う urg_stop_measurement(urg); } } return ret; } int urg_open(urg_t *urg, urg_connection_type_t connection_type, const char *device_or_address, long baudrate_or_port) { int ret; long baudrate = baudrate_or_port; urg->is_active = URG_FALSE; urg->is_sending = URG_TRUE; urg->last_errno = URG_NOT_CONNECTED; urg->timeout = MAX_TIMEOUT; urg->scanning_skip_scan = 0; urg->error_handler = NULL; // デバイスへの接続 ret = connection_open(&urg->connection, connection_type, device_or_address, baudrate_or_port); if (ret < 0) { switch (connection_type) { case URG_SERIAL: urg->last_errno = URG_SERIAL_OPEN_ERROR; break; case URG_ETHERNET: urg->last_errno = URG_ETHERNET_OPEN_ERROR; break; default: urg->last_errno = URG_INVALID_RESPONSE; break; } return urg->last_errno; } // 指定したボーレートで URG と通信できるように調整 if (connection_type == URG_ETHERNET) { // Ethernet のときは仮の通信速度を指定しておく baudrate = 115200; } if (connect_urg_device(urg, baudrate) != URG_NO_ERROR) { return set_errno_and_return(urg, ret); } urg->is_sending = URG_FALSE; // 変数の初期化 urg->last_errno = URG_NO_ERROR; urg->range_data_byte = URG_COMMUNICATION_3_BYTE; urg->specified_scan_times = 0; urg->scanning_remain_times = 0; urg->is_laser_on = URG_FALSE; // パラメータ情報を取得 ret = receive_parameter(urg); if (ret == URG_NO_ERROR) { urg->is_active = URG_TRUE; } return ret; } void urg_close(urg_t *urg) { if (urg->is_active) { ignore_receive_data_with_qt(urg, urg->timeout); } connection_close(&urg->connection); urg->is_active = URG_FALSE; } void urg_set_timeout_msec(urg_t *urg, int msec) { urg->timeout = msec; } int urg_start_time_stamp_mode(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM0 を発行する n = scip_response(urg, "TM0\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } long urg_time_stamp(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; char buffer[BUFFER_SIZE]; char *p; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } ret = scip_response(urg, "TM1\n", expected, urg->timeout, buffer, BUFFER_SIZE); if (ret < 0) { return ret; } // buffer からタイムスタンプを取得し、デコードして返す if (strcmp(buffer, "00P")) { // 最初の応答が "00P" でなければ戻る return set_errno_and_return(urg, URG_RECEIVE_ERROR); } p = buffer + 4; if (strlen(p) != 5) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (p[5] == scip_checksum(p, 4)) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } return urg_scip_decode(p, 4); } int urg_stop_time_stamp_mode(urg_t *urg) { int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM2 を発行する n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } static int send_distance_command(urg_t *urg, int scan_times, int skip_scan, char single_scan_ch, char continuous_scan_ch, char scan_type_ch) { char buffer[BUFFER_SIZE]; int write_size = 0; int front_index = urg->front_data_index; int n; urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times; urg->scanning_remain_times = urg->specified_scan_times; urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan; if (scan_times >= 100) { // 計測回数が 99 を越える場合は、無限回のスキャンを行う urg->specified_scan_times = 0; } if (urg->scanning_remain_times == 1) { // レーザ発光を指示 urg_laser_on(urg); write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n", single_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step); } else { write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n", continuous_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step, skip_scan, urg->specified_scan_times); urg->is_sending = URG_TRUE; } n = connection_write(&urg->connection, buffer, write_size); if (n != write_size) { return set_errno_and_return(urg, URG_SEND_ERROR); } return 0; } int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan) { char range_byte_ch; int ret = 0; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((skip_scan < 0) || (skip_scan > 9)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // !!! Mx 系, Nx 系の計測中のときは、QT を発行してから // !!! 計測開始コマンドを送信するようにする // !!! ただし、MD 計測中に MD を発行するように、同じコマンドの場合は // !!! Mx 系, Nx 系の計測は上書きすることができるようにする // 指定されたタイプのパケットを生成し、送信する switch (type) { case URG_DISTANCE: range_byte_ch = (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D'; ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', range_byte_ch); break; case URG_DISTANCE_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', 'E'); break; case URG_MULTIECHO: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'D'); break; case URG_MULTIECHO_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'E'); break; case URG_STOP: case URG_UNKNOWN: default: ignore_receive_data_with_qt(urg, urg->timeout); urg->last_errno = URG_INVALID_PARAMETER; ret = urg->last_errno; break; } return ret; } int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, NULL, time_stamp, system_time_stamp); } int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp); } int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp); } int urg_stop_measurement(urg_t *urg) { enum { MAX_READ_TIMES = 3 }; int ret = URG_INVALID_RESPONSE; int n; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // QT を発行する n = connection_write(&urg->connection, "QT\n", 3); if (n != 3) { return set_errno_and_return(urg, URG_SEND_ERROR); } for (i = 0; i < MAX_READ_TIMES; ++i) { // QT の応答が返されるまで、距離データを読み捨てる ret = receive_data(urg, NULL, NULL, NULL, NULL); if (ret == URG_NO_ERROR) { // 正常応答 urg->is_laser_on = URG_FALSE; urg->is_sending = URG_FALSE; return set_errno_and_return(urg, URG_NO_ERROR); } } return ret; } int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step) { // 設定の範囲外を指定したときは、エラーを返す if (((skip_step < 0) || (skip_step >= 100)) || (first_step > last_step) || (first_step < -urg->front_data_index) || (last_step > (urg->last_data_index - urg->front_data_index))) { return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR); } urg->scanning_first_step = first_step; urg->scanning_last_step = last_step; urg->scanning_skip_step = skip_step; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_set_connection_data_size(urg_t *urg, urg_range_data_byte_t data_byte) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((data_byte != URG_COMMUNICATION_3_BYTE) || (data_byte != URG_COMMUNICATION_2_BYTE)) { return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR); } urg->range_data_byte = data_byte; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_laser_on(urg_t *urg) { int expected[] = { 0, 2, EXPECTED_END }; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if (urg->is_laser_on != URG_FALSE) { // 既にレーザが発光しているときは、コマンドを送信しないようにする urg->last_errno = 0; return urg->last_errno; } ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0); if (ret >= 0) { urg->is_laser_on = URG_TRUE; ret = 0; } return ret; } int urg_laser_off(urg_t *urg) { return urg_stop_measurement(urg); } int urg_reboot(urg_t *urg) { int expected[] = { 0, 1, EXPECTED_END }; int ret; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // ２回目の RB 送信後、接続を切断する for (i = 0; i < 2; ++i) { ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0); if (ret < 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } urg->is_active = URG_FALSE; urg_close(urg); urg->last_errno = 0; return urg->last_errno; } void urg_sleep(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = 4 }; int sl_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE]; if (urg_stop_measurement(urg) != URG_NO_ERROR) { return; } scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); } void urg_wakeup(urg_t *urg) { urg_stop_measurement(urg); } int urg_is_stable(urg_t *urg) { const char *stat = urg_sensor_status(urg); return strncmp("Stable", stat, 6) ? 0 : 1; } static char *copy_token(char *dest, char *receive_buffer, const char *start_str, const char *end_ch, int lines) { size_t start_str_len = strlen(start_str); size_t end_ch_len = strlen(end_ch); int i; size_t j; for (j = 0; j < end_ch_len; ++j) { const char *p = receive_buffer; for (i = 0; i < lines; ++i) { if (!strncmp(p, start_str, start_str_len)) { char *last_p = strchr(p + start_str_len, end_ch[j]); if (last_p) { *last_p = '\0'; memcpy(dest, p + start_str_len, last_p - (p + start_str_len) + 1); return dest; } } p += strlen(p) + 1; } } return NULL; } static const char *receive_command_response(urg_t *urg, char *buffer, int buffer_size, const char* command, int response_lines) { const int vv_expected[] = { 0, EXPECTED_END }; int ret; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = scip_response(urg, command, vv_expected, urg->timeout, buffer, buffer_size); if (ret < response_lines) { return RECEIVE_ERROR_MESSAGE; } return NULL; } const char *urg_sensor_product_type(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROD:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_serial_id(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "SERI:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_vendor(urg_t *urg){ enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "VEND:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", "(", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_date(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } // Get the firmware version and append a '(', this will be what's before the date char firmware_version[50]; strcpy(firmware_version, urg_sensor_firmware_version(urg)); strcat(firmware_version, "("); ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } // Strip out the actual date from between the '(' and ')' char *date; p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", ";", VV_RESPONSE_LINES); date = copy_token(urg->return_buffer, p, firmware_version, ")", 1); return (date) ? date : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_protocol_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROT:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_status(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "STAT:", ";", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_state(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "MESM:", " (", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } void urg_set_error_handler(urg_t *urg, urg_error_handler handler) { urg->error_handler = handler; }
285  return change_sensor_baudrate(urg, try_baudrate[i], baudrate);
286  }
287  }
288 
289  return set_errno_and_return(urg, URG_NOT_DETECT_BAUDRATE_ERROR);
290 }
291 
292 
293 // PP コマンドの応答を urg_t に格納するstatic int receive_parameter(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * 9, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; int pp_expected[] = { 0, EXPECTED_END }; unsigned short received_bits = 0x0000; char *p; int i; int ret = scip_response(urg, "PP\n", pp_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); if (ret < 0) { return ret; } else if (ret < PP_RESPONSE_LINES) { ignore_receive_data_with_qt(urg, MAX_TIMEOUT); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } p = receive_buffer; for (i = 0; i < (ret - 1); ++i) { if (!strncmp(p, "DMIN:", 5)) { urg->min_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0001; } else if (!strncmp(p, "DMAX:", 5)) { urg->max_distance = strtol(p + 5, NULL, 10); received_bits |= 0x0002; } else if (!strncmp(p, "ARES:", 5)) { urg->area_resolution = strtol(p + 5, NULL, 10); received_bits |= 0x0004; } else if (!strncmp(p, "AMIN:", 5)) { urg->first_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0008; } else if (!strncmp(p, "AMAX:", 5)) { urg->last_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0010; } else if (!strncmp(p, "AFRT:", 5)) { urg->front_data_index = strtol(p + 5, NULL, 10); received_bits |= 0x0020; } else if (!strncmp(p, "SCAN:", 5)) { int rpm = strtol(p + 5, NULL, 10); // タイムアウト時間は、計測周期の 16 倍程度の値にする urg->scan_usec = 1000 * 1000 * 60 / rpm; urg->timeout = urg->scan_usec >> (10 - 4); received_bits |= 0x0040; } p += strlen(p) + 1; } // 全てのパラメータを受信したか確認 if (received_bits != 0x007f) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } urg_set_scanning_parameter(urg, urg->first_data_index - urg->front_data_index, urg->last_data_index - urg->front_data_index, 1); return set_errno_and_return(urg, URG_NO_ERROR); } //! SCIP 文字列のデコード long urg_scip_decode(const char data[], int size) { const char* p = data; const char* last_p = p + size; int value = 0; while (p < last_p) { value <<= 6; value &= ~0x3f; value |= *p++ - 0x30; } return value; } static int parse_parameter(const char *parameter, int size) { char buffer[5]; memcpy(buffer, parameter, size); buffer[size] = '\0'; return strtol(buffer, NULL, 10); } static urg_measurement_type_t parse_distance_parameter(urg_t *urg, const char echoback[]) { urg_measurement_type_t ret_type = URG_UNKNOWN; urg->received_range_data_byte = URG_COMMUNICATION_3_BYTE; if (echoback[1] == 'S') { urg->received_range_data_byte = URG_COMMUNICATION_2_BYTE; ret_type = URG_DISTANCE; } else if (echoback[1] == 'D') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO; } } else if (echoback[1] == 'E') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE_INTENSITY; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO_INTENSITY; } } else { return URG_UNKNOWN; } // パラメータの格納 urg->received_first_index = parse_parameter(&echoback[2], 4); urg->received_last_index = parse_parameter(&echoback[6], 4); urg->received_skip_step = parse_parameter(&echoback[10], 2); return ret_type; } static urg_measurement_type_t parse_distance_echoback(urg_t *urg, const char echoback[]) { size_t line_length; urg_measurement_type_t ret_type = URG_UNKNOWN; if (!strcmp("QT", echoback)) { return URG_STOP; } line_length = strlen(echoback); if ((line_length == 12) && ((echoback[0] == 'G') || (echoback[0] == 'H'))) { ret_type = parse_distance_parameter(urg, echoback); } else if ((line_length == 15) && ((echoback[0] == 'M') || (echoback[0] == 'N'))) { ret_type = parse_distance_parameter(urg, echoback); } return ret_type; } static int receive_length_data(urg_t *urg, long length[], unsigned short intensity[], urg_measurement_type_t type, char buffer[]) { int n; int step_filled = 0; int line_filled = 0; int multiecho_index = 0; int each_size = (urg->received_range_data_byte == URG_COMMUNICATION_2_BYTE) ? 2 : 3; int data_size = each_size; int is_intensity = URG_FALSE; int is_multiecho = URG_FALSE; int multiecho_max_size = 1; if ((type == URG_DISTANCE_INTENSITY) || (type == URG_MULTIECHO_INTENSITY)) { data_size *= 2; is_intensity = URG_TRUE; } if ((type == URG_MULTIECHO) || (type == URG_MULTIECHO_INTENSITY)) { is_multiecho = URG_TRUE; multiecho_max_size = URG_MAX_ECHO; } do { char *p = buffer; char *last_p; n = connection_readline(&urg->connection, &buffer[line_filled], BUFFER_SIZE - line_filled, urg->timeout); if (n > 0) { // チェックサムの評価 if (buffer[line_filled + n - 1] != scip_checksum(&buffer[line_filled], n - 1)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } } if (n > 0) { line_filled += n - 1; } last_p = p + line_filled; while ((last_p - p) >= data_size) { int index; if (*p == '&') { // 先頭文字が '&' だったときは、マルチエコーのデータとみなす if ((last_p - (p + 1)) < data_size) { // '&' を除いて、data_size 分データが無ければ抜ける break; } --step_filled; ++multiecho_index; ++p; --line_filled; } else { // 次のデータ multiecho_index = 0; } index = (step_filled * multiecho_max_size) + multiecho_index; if (step_filled > (urg->received_last_index - urg->received_first_index)) { // データが多過ぎる場合は、残りのデータを無視して戻る ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (is_multiecho && (multiecho_index == 0)) { // マルチエコーのデータ格納先をダミーデータで埋める int i; if (length) { for (i = 1; i < multiecho_max_size; ++i) { length[index + i] = 0; } } if (intensity) { for (i = 1; i < multiecho_max_size; ++i) { intensity[index + i] = 0; } } } // 距離データの格納 if (length) { length[index] = urg_scip_decode(p, 3); } p += 3; // 強度データの格納 if (is_intensity) { if (intensity) { intensity[index] = (unsigned short)urg_scip_decode(p, 3); } p += 3; } ++step_filled; line_filled -= data_size; } // 次に処理する文字を退避 memmove(buffer, p, line_filled); } while (n > 0); return step_filled; } //! 距離データの取得 static int receive_data(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { urg_measurement_type_t type; char buffer[BUFFER_SIZE]; int ret = 0; int n; int extended_timeout = urg->timeout + 2 * (urg->scan_usec * (urg->scanning_skip_scan) / 1000); // エコーバックの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, extended_timeout); if (n <= 0) { return set_errno_and_return(urg, URG_NO_RESPONSE); } // エコーバックの解析 type = parse_distance_echoback(urg, buffer); // 応答の取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 3) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } if (buffer[n - 1] != scip_checksum(buffer, n - 1)) { // チェックサムの評価 ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } if (type == URG_STOP) { // QT 応答の場合には、最後の改行を読み捨て、正常応答として処理する n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n == 0) { return 0; } else { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } if (urg->specified_scan_times != 1) { if (!strncmp(buffer, "00", 2)) { // "00" 応答の場合は、エコーバック応答とみなし、 // 最後の空行を読み捨て、次からのデータを返す n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 0) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } } } if (((urg->specified_scan_times == 1) && (strncmp(buffer, "00", 2))) || ((urg->specified_scan_times != 1) && (strncmp(buffer, "99", 2)))) { if (urg->error_handler) { type = urg->error_handler(buffer, urg); } if (type == URG_UNKNOWN) { // Gx, Hx のときは 00P が返されたときがデータ // Mx, Nx のときは 99b が返されたときがデータ ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } // タイムスタンプの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n > 0) { if (time_stamp) { *time_stamp = urg_scip_decode(buffer, 4); } if (system_time_stamp) { urg_get_walltime(system_time_stamp); } } // データの取得 switch (type) { case URG_DISTANCE: case URG_MULTIECHO: ret = receive_length_data(urg, data, NULL, type, buffer); break; case URG_DISTANCE_INTENSITY: case URG_MULTIECHO_INTENSITY: ret = receive_length_data(urg, data, intensity, type, buffer); break; case URG_STOP: case URG_UNKNOWN: ret = 0; break; } // specified_scan_times == 1 のときは Gx 系コマンドが使われるため // データを明示的に停止しなくてよい if ((urg->specified_scan_times > 1) && (urg->scanning_remain_times > 0)) { if (--urg->scanning_remain_times <= 0) { // データの停止のみを行う urg_stop_measurement(urg); } } return ret; } int urg_open(urg_t *urg, urg_connection_type_t connection_type, const char *device_or_address, long baudrate_or_port) { int ret; long baudrate = baudrate_or_port; urg->is_active = URG_FALSE; urg->is_sending = URG_TRUE; urg->last_errno = URG_NOT_CONNECTED; urg->timeout = MAX_TIMEOUT; urg->scanning_skip_scan = 0; urg->error_handler = NULL; // デバイスへの接続 ret = connection_open(&urg->connection, connection_type, device_or_address, baudrate_or_port); if (ret < 0) { switch (connection_type) { case URG_SERIAL: urg->last_errno = URG_SERIAL_OPEN_ERROR; break; case URG_ETHERNET: urg->last_errno = URG_ETHERNET_OPEN_ERROR; break; default: urg->last_errno = URG_INVALID_RESPONSE; break; } return urg->last_errno; } // 指定したボーレートで URG と通信できるように調整 if (connection_type == URG_ETHERNET) { // Ethernet のときは仮の通信速度を指定しておく baudrate = 115200; } if (connect_urg_device(urg, baudrate) != URG_NO_ERROR) { return set_errno_and_return(urg, ret); } urg->is_sending = URG_FALSE; // 変数の初期化 urg->last_errno = URG_NO_ERROR; urg->range_data_byte = URG_COMMUNICATION_3_BYTE; urg->specified_scan_times = 0; urg->scanning_remain_times = 0; urg->is_laser_on = URG_FALSE; // パラメータ情報を取得 ret = receive_parameter(urg); if (ret == URG_NO_ERROR) { urg->is_active = URG_TRUE; } return ret; } void urg_close(urg_t *urg) { if (urg->is_active) { ignore_receive_data_with_qt(urg, urg->timeout); } connection_close(&urg->connection); urg->is_active = URG_FALSE; } void urg_set_timeout_msec(urg_t *urg, int msec) { urg->timeout = msec; } int urg_start_time_stamp_mode(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM0 を発行する n = scip_response(urg, "TM0\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } long urg_time_stamp(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; char buffer[BUFFER_SIZE]; char *p; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } ret = scip_response(urg, "TM1\n", expected, urg->timeout, buffer, BUFFER_SIZE); if (ret < 0) { return ret; } // buffer からタイムスタンプを取得し、デコードして返す if (strcmp(buffer, "00P")) { // 最初の応答が "00P" でなければ戻る return set_errno_and_return(urg, URG_RECEIVE_ERROR); } p = buffer + 4; if (strlen(p) != 5) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (p[5] == scip_checksum(p, 4)) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } return urg_scip_decode(p, 4); } int urg_stop_time_stamp_mode(urg_t *urg) { int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM2 を発行する n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } static int send_distance_command(urg_t *urg, int scan_times, int skip_scan, char single_scan_ch, char continuous_scan_ch, char scan_type_ch) { char buffer[BUFFER_SIZE]; int write_size = 0; int front_index = urg->front_data_index; int n; urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times; urg->scanning_remain_times = urg->specified_scan_times; urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan; if (scan_times >= 100) { // 計測回数が 99 を越える場合は、無限回のスキャンを行う urg->specified_scan_times = 0; } if (urg->scanning_remain_times == 1) { // レーザ発光を指示 urg_laser_on(urg); write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n", single_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step); } else { write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n", continuous_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step, skip_scan, urg->specified_scan_times); urg->is_sending = URG_TRUE; } n = connection_write(&urg->connection, buffer, write_size); if (n != write_size) { return set_errno_and_return(urg, URG_SEND_ERROR); } return 0; } int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan) { char range_byte_ch; int ret = 0; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((skip_scan < 0) || (skip_scan > 9)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // !!! Mx 系, Nx 系の計測中のときは、QT を発行してから // !!! 計測開始コマンドを送信するようにする // !!! ただし、MD 計測中に MD を発行するように、同じコマンドの場合は // !!! Mx 系, Nx 系の計測は上書きすることができるようにする // 指定されたタイプのパケットを生成し、送信する switch (type) { case URG_DISTANCE: range_byte_ch = (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D'; ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', range_byte_ch); break; case URG_DISTANCE_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', 'E'); break; case URG_MULTIECHO: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'D'); break; case URG_MULTIECHO_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'E'); break; case URG_STOP: case URG_UNKNOWN: default: ignore_receive_data_with_qt(urg, urg->timeout); urg->last_errno = URG_INVALID_PARAMETER; ret = urg->last_errno; break; } return ret; } int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, NULL, time_stamp, system_time_stamp); } int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp); } int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp); } int urg_stop_measurement(urg_t *urg) { enum { MAX_READ_TIMES = 3 }; int ret = URG_INVALID_RESPONSE; int n; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // QT を発行する n = connection_write(&urg->connection, "QT\n", 3); if (n != 3) { return set_errno_and_return(urg, URG_SEND_ERROR); } for (i = 0; i < MAX_READ_TIMES; ++i) { // QT の応答が返されるまで、距離データを読み捨てる ret = receive_data(urg, NULL, NULL, NULL, NULL); if (ret == URG_NO_ERROR) { // 正常応答 urg->is_laser_on = URG_FALSE; urg->is_sending = URG_FALSE; return set_errno_and_return(urg, URG_NO_ERROR); } } return ret; } int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step) { // 設定の範囲外を指定したときは、エラーを返す if (((skip_step < 0) || (skip_step >= 100)) || (first_step > last_step) || (first_step < -urg->front_data_index) || (last_step > (urg->last_data_index - urg->front_data_index))) { return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR); } urg->scanning_first_step = first_step; urg->scanning_last_step = last_step; urg->scanning_skip_step = skip_step; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_set_connection_data_size(urg_t *urg, urg_range_data_byte_t data_byte) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((data_byte != URG_COMMUNICATION_3_BYTE) || (data_byte != URG_COMMUNICATION_2_BYTE)) { return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR); } urg->range_data_byte = data_byte; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_laser_on(urg_t *urg) { int expected[] = { 0, 2, EXPECTED_END }; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if (urg->is_laser_on != URG_FALSE) { // 既にレーザが発光しているときは、コマンドを送信しないようにする urg->last_errno = 0; return urg->last_errno; } ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0); if (ret >= 0) { urg->is_laser_on = URG_TRUE; ret = 0; } return ret; } int urg_laser_off(urg_t *urg) { return urg_stop_measurement(urg); } int urg_reboot(urg_t *urg) { int expected[] = { 0, 1, EXPECTED_END }; int ret; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // ２回目の RB 送信後、接続を切断する for (i = 0; i < 2; ++i) { ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0); if (ret < 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } urg->is_active = URG_FALSE; urg_close(urg); urg->last_errno = 0; return urg->last_errno; } void urg_sleep(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = 4 }; int sl_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE]; if (urg_stop_measurement(urg) != URG_NO_ERROR) { return; } scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); } void urg_wakeup(urg_t *urg) { urg_stop_measurement(urg); } int urg_is_stable(urg_t *urg) { const char *stat = urg_sensor_status(urg); return strncmp("Stable", stat, 6) ? 0 : 1; } static char *copy_token(char *dest, char *receive_buffer, const char *start_str, const char *end_ch, int lines) { size_t start_str_len = strlen(start_str); size_t end_ch_len = strlen(end_ch); int i; size_t j; for (j = 0; j < end_ch_len; ++j) { const char *p = receive_buffer; for (i = 0; i < lines; ++i) { if (!strncmp(p, start_str, start_str_len)) { char *last_p = strchr(p + start_str_len, end_ch[j]); if (last_p) { *last_p = '\0'; memcpy(dest, p + start_str_len, last_p - (p + start_str_len) + 1); return dest; } } p += strlen(p) + 1; } } return NULL; } static const char *receive_command_response(urg_t *urg, char *buffer, int buffer_size, const char* command, int response_lines) { const int vv_expected[] = { 0, EXPECTED_END }; int ret; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = scip_response(urg, command, vv_expected, urg->timeout, buffer, buffer_size); if (ret < response_lines) { return RECEIVE_ERROR_MESSAGE; } return NULL; } const char *urg_sensor_product_type(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROD:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_serial_id(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "SERI:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_vendor(urg_t *urg){ enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "VEND:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", "(", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_date(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } // Get the firmware version and append a '(', this will be what's before the date char firmware_version[50]; strcpy(firmware_version, urg_sensor_firmware_version(urg)); strcat(firmware_version, "("); ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } // Strip out the actual date from between the '(' and ')' char *date; p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", ";", VV_RESPONSE_LINES); date = copy_token(urg->return_buffer, p, firmware_version, ")", 1); return (date) ? date : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_protocol_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROT:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_status(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "STAT:", ";", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_state(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "MESM:", " (", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } void urg_set_error_handler(urg_t *urg, urg_error_handler handler) { urg->error_handler = handler; }
294 static int receive_parameter(urg_t *urg)
295 {
296  enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * 9, };
297  char receive_buffer[RECEIVE_BUFFER_SIZE];
298  int pp_expected[] = { 0, EXPECTED_END };
299  unsigned short received_bits = 0x0000;
300  char *p;
301  int i;
302 
303  int ret = scip_response(urg, "PP\n", pp_expected, MAX_TIMEOUT,
304  receive_buffer, RECEIVE_BUFFER_SIZE);
305  if (ret < 0) {
306  return ret;
307  } else if (ret < PP_RESPONSE_LINES) {
308  ignore_receive_data_with_qt(urg, MAX_TIMEOUT);
309  return set_errno_and_return(urg, URG_INVALID_RESPONSE);
310  }
311 
312  p = receive_buffer;
313  for (i = 0; i < (ret - 1); ++i) {
314 
315  if (!strncmp(p, "DMIN:", 5)) {
316  urg->min_distance = strtol(p + 5, NULL, 10);
317  received_bits |= 0x0001;
318 
319  } else if (!strncmp(p, "DMAX:", 5)) {
320  urg->max_distance = strtol(p + 5, NULL, 10);
321  received_bits |= 0x0002;
322 
323  } else if (!strncmp(p, "ARES:", 5)) {
324  urg->area_resolution = strtol(p + 5, NULL, 10);
325  received_bits |= 0x0004;
326 
327  } else if (!strncmp(p, "AMIN:", 5)) {
328  urg->first_data_index = strtol(p + 5, NULL, 10);
329  received_bits |= 0x0008;
330 
331  } else if (!strncmp(p, "AMAX:", 5)) {
332  urg->last_data_index = strtol(p + 5, NULL, 10);
333  received_bits |= 0x0010;
334 
335  } else if (!strncmp(p, "AFRT:", 5)) {
336  urg->front_data_index = strtol(p + 5, NULL, 10);
337  received_bits |= 0x0020;
338 
339  } else if (!strncmp(p, "SCAN:", 5)) {
340  int rpm = strtol(p + 5, NULL, 10);
341  // タイムアウト時間は、計測周期の 16 倍程度の値にする urg->scan_usec = 1000 * 1000 * 60 / rpm; urg->timeout = urg->scan_usec >> (10 - 4); received_bits |= 0x0040; } p += strlen(p) + 1; } // 全てのパラメータを受信したか確認 if (received_bits != 0x007f) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } urg_set_scanning_parameter(urg, urg->first_data_index - urg->front_data_index, urg->last_data_index - urg->front_data_index, 1); return set_errno_and_return(urg, URG_NO_ERROR); } //! SCIP 文字列のデコード long urg_scip_decode(const char data[], int size) { const char* p = data; const char* last_p = p + size; int value = 0; while (p < last_p) { value <<= 6; value &= ~0x3f; value |= *p++ - 0x30; } return value; } static int parse_parameter(const char *parameter, int size) { char buffer[5]; memcpy(buffer, parameter, size); buffer[size] = '\0'; return strtol(buffer, NULL, 10); } static urg_measurement_type_t parse_distance_parameter(urg_t *urg, const char echoback[]) { urg_measurement_type_t ret_type = URG_UNKNOWN; urg->received_range_data_byte = URG_COMMUNICATION_3_BYTE; if (echoback[1] == 'S') { urg->received_range_data_byte = URG_COMMUNICATION_2_BYTE; ret_type = URG_DISTANCE; } else if (echoback[1] == 'D') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO; } } else if (echoback[1] == 'E') { if ((echoback[0] == 'G') || (echoback[0] == 'M')) { ret_type = URG_DISTANCE_INTENSITY; } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) { ret_type = URG_MULTIECHO_INTENSITY; } } else { return URG_UNKNOWN; } // パラメータの格納 urg->received_first_index = parse_parameter(&echoback[2], 4); urg->received_last_index = parse_parameter(&echoback[6], 4); urg->received_skip_step = parse_parameter(&echoback[10], 2); return ret_type; } static urg_measurement_type_t parse_distance_echoback(urg_t *urg, const char echoback[]) { size_t line_length; urg_measurement_type_t ret_type = URG_UNKNOWN; if (!strcmp("QT", echoback)) { return URG_STOP; } line_length = strlen(echoback); if ((line_length == 12) && ((echoback[0] == 'G') || (echoback[0] == 'H'))) { ret_type = parse_distance_parameter(urg, echoback); } else if ((line_length == 15) && ((echoback[0] == 'M') || (echoback[0] == 'N'))) { ret_type = parse_distance_parameter(urg, echoback); } return ret_type; } static int receive_length_data(urg_t *urg, long length[], unsigned short intensity[], urg_measurement_type_t type, char buffer[]) { int n; int step_filled = 0; int line_filled = 0; int multiecho_index = 0; int each_size = (urg->received_range_data_byte == URG_COMMUNICATION_2_BYTE) ? 2 : 3; int data_size = each_size; int is_intensity = URG_FALSE; int is_multiecho = URG_FALSE; int multiecho_max_size = 1; if ((type == URG_DISTANCE_INTENSITY) || (type == URG_MULTIECHO_INTENSITY)) { data_size *= 2; is_intensity = URG_TRUE; } if ((type == URG_MULTIECHO) || (type == URG_MULTIECHO_INTENSITY)) { is_multiecho = URG_TRUE; multiecho_max_size = URG_MAX_ECHO; } do { char *p = buffer; char *last_p; n = connection_readline(&urg->connection, &buffer[line_filled], BUFFER_SIZE - line_filled, urg->timeout); if (n > 0) { // チェックサムの評価 if (buffer[line_filled + n - 1] != scip_checksum(&buffer[line_filled], n - 1)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } } if (n > 0) { line_filled += n - 1; } last_p = p + line_filled; while ((last_p - p) >= data_size) { int index; if (*p == '&') { // 先頭文字が '&' だったときは、マルチエコーのデータとみなす if ((last_p - (p + 1)) < data_size) { // '&' を除いて、data_size 分データが無ければ抜ける break; } --step_filled; ++multiecho_index; ++p; --line_filled; } else { // 次のデータ multiecho_index = 0; } index = (step_filled * multiecho_max_size) + multiecho_index; if (step_filled > (urg->received_last_index - urg->received_first_index)) { // データが多過ぎる場合は、残りのデータを無視して戻る ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (is_multiecho && (multiecho_index == 0)) { // マルチエコーのデータ格納先をダミーデータで埋める int i; if (length) { for (i = 1; i < multiecho_max_size; ++i) { length[index + i] = 0; } } if (intensity) { for (i = 1; i < multiecho_max_size; ++i) { intensity[index + i] = 0; } } } // 距離データの格納 if (length) { length[index] = urg_scip_decode(p, 3); } p += 3; // 強度データの格納 if (is_intensity) { if (intensity) { intensity[index] = (unsigned short)urg_scip_decode(p, 3); } p += 3; } ++step_filled; line_filled -= data_size; } // 次に処理する文字を退避 memmove(buffer, p, line_filled); } while (n > 0); return step_filled; } //! 距離データの取得 static int receive_data(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { urg_measurement_type_t type; char buffer[BUFFER_SIZE]; int ret = 0; int n; int extended_timeout = urg->timeout + 2 * (urg->scan_usec * (urg->scanning_skip_scan) / 1000); // エコーバックの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, extended_timeout); if (n <= 0) { return set_errno_and_return(urg, URG_NO_RESPONSE); } // エコーバックの解析 type = parse_distance_echoback(urg, buffer); // 応答の取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 3) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } if (buffer[n - 1] != scip_checksum(buffer, n - 1)) { // チェックサムの評価 ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } if (type == URG_STOP) { // QT 応答の場合には、最後の改行を読み捨て、正常応答として処理する n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n == 0) { return 0; } else { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } if (urg->specified_scan_times != 1) { if (!strncmp(buffer, "00", 2)) { // "00" 応答の場合は、エコーバック応答とみなし、 // 最後の空行を読み捨て、次からのデータを返す n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 0) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } } } if (((urg->specified_scan_times == 1) && (strncmp(buffer, "00", 2))) || ((urg->specified_scan_times != 1) && (strncmp(buffer, "99", 2)))) { if (urg->error_handler) { type = urg->error_handler(buffer, urg); } if (type == URG_UNKNOWN) { // Gx, Hx のときは 00P が返されたときがデータ // Mx, Nx のときは 99b が返されたときがデータ ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } // タイムスタンプの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n > 0) { if (time_stamp) { *time_stamp = urg_scip_decode(buffer, 4); } if (system_time_stamp) { urg_get_walltime(system_time_stamp); } } // データの取得 switch (type) { case URG_DISTANCE: case URG_MULTIECHO: ret = receive_length_data(urg, data, NULL, type, buffer); break; case URG_DISTANCE_INTENSITY: case URG_MULTIECHO_INTENSITY: ret = receive_length_data(urg, data, intensity, type, buffer); break; case URG_STOP: case URG_UNKNOWN: ret = 0; break; } // specified_scan_times == 1 のときは Gx 系コマンドが使われるため // データを明示的に停止しなくてよい if ((urg->specified_scan_times > 1) && (urg->scanning_remain_times > 0)) { if (--urg->scanning_remain_times <= 0) { // データの停止のみを行う urg_stop_measurement(urg); } } return ret; } int urg_open(urg_t *urg, urg_connection_type_t connection_type, const char *device_or_address, long baudrate_or_port) { int ret; long baudrate = baudrate_or_port; urg->is_active = URG_FALSE; urg->is_sending = URG_TRUE; urg->last_errno = URG_NOT_CONNECTED; urg->timeout = MAX_TIMEOUT; urg->scanning_skip_scan = 0; urg->error_handler = NULL; // デバイスへの接続 ret = connection_open(&urg->connection, connection_type, device_or_address, baudrate_or_port); if (ret < 0) { switch (connection_type) { case URG_SERIAL: urg->last_errno = URG_SERIAL_OPEN_ERROR; break; case URG_ETHERNET: urg->last_errno = URG_ETHERNET_OPEN_ERROR; break; default: urg->last_errno = URG_INVALID_RESPONSE; break; } return urg->last_errno; } // 指定したボーレートで URG と通信できるように調整 if (connection_type == URG_ETHERNET) { // Ethernet のときは仮の通信速度を指定しておく baudrate = 115200; } if (connect_urg_device(urg, baudrate) != URG_NO_ERROR) { return set_errno_and_return(urg, ret); } urg->is_sending = URG_FALSE; // 変数の初期化 urg->last_errno = URG_NO_ERROR; urg->range_data_byte = URG_COMMUNICATION_3_BYTE; urg->specified_scan_times = 0; urg->scanning_remain_times = 0; urg->is_laser_on = URG_FALSE; // パラメータ情報を取得 ret = receive_parameter(urg); if (ret == URG_NO_ERROR) { urg->is_active = URG_TRUE; } return ret; } void urg_close(urg_t *urg) { if (urg->is_active) { ignore_receive_data_with_qt(urg, urg->timeout); } connection_close(&urg->connection); urg->is_active = URG_FALSE; } void urg_set_timeout_msec(urg_t *urg, int msec) { urg->timeout = msec; } int urg_start_time_stamp_mode(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM0 を発行する n = scip_response(urg, "TM0\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } long urg_time_stamp(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; char buffer[BUFFER_SIZE]; char *p; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } ret = scip_response(urg, "TM1\n", expected, urg->timeout, buffer, BUFFER_SIZE); if (ret < 0) { return ret; } // buffer からタイムスタンプを取得し、デコードして返す if (strcmp(buffer, "00P")) { // 最初の応答が "00P" でなければ戻る return set_errno_and_return(urg, URG_RECEIVE_ERROR); } p = buffer + 4; if (strlen(p) != 5) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (p[5] == scip_checksum(p, 4)) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } return urg_scip_decode(p, 4); } int urg_stop_time_stamp_mode(urg_t *urg) { int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM2 を発行する n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } static int send_distance_command(urg_t *urg, int scan_times, int skip_scan, char single_scan_ch, char continuous_scan_ch, char scan_type_ch) { char buffer[BUFFER_SIZE]; int write_size = 0; int front_index = urg->front_data_index; int n; urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times; urg->scanning_remain_times = urg->specified_scan_times; urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan; if (scan_times >= 100) { // 計測回数が 99 を越える場合は、無限回のスキャンを行う urg->specified_scan_times = 0; } if (urg->scanning_remain_times == 1) { // レーザ発光を指示 urg_laser_on(urg); write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n", single_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step); } else { write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n", continuous_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step, skip_scan, urg->specified_scan_times); urg->is_sending = URG_TRUE; } n = connection_write(&urg->connection, buffer, write_size); if (n != write_size) { return set_errno_and_return(urg, URG_SEND_ERROR); } return 0; } int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan) { char range_byte_ch; int ret = 0; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((skip_scan < 0) || (skip_scan > 9)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // !!! Mx 系, Nx 系の計測中のときは、QT を発行してから // !!! 計測開始コマンドを送信するようにする // !!! ただし、MD 計測中に MD を発行するように、同じコマンドの場合は // !!! Mx 系, Nx 系の計測は上書きすることができるようにする // 指定されたタイプのパケットを生成し、送信する switch (type) { case URG_DISTANCE: range_byte_ch = (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D'; ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', range_byte_ch); break; case URG_DISTANCE_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', 'E'); break; case URG_MULTIECHO: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'D'); break; case URG_MULTIECHO_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'E'); break; case URG_STOP: case URG_UNKNOWN: default: ignore_receive_data_with_qt(urg, urg->timeout); urg->last_errno = URG_INVALID_PARAMETER; ret = urg->last_errno; break; } return ret; } int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, NULL, time_stamp, system_time_stamp); } int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp); } int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp); } int urg_stop_measurement(urg_t *urg) { enum { MAX_READ_TIMES = 3 }; int ret = URG_INVALID_RESPONSE; int n; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // QT を発行する n = connection_write(&urg->connection, "QT\n", 3); if (n != 3) { return set_errno_and_return(urg, URG_SEND_ERROR); } for (i = 0; i < MAX_READ_TIMES; ++i) { // QT の応答が返されるまで、距離データを読み捨てる ret = receive_data(urg, NULL, NULL, NULL, NULL); if (ret == URG_NO_ERROR) { // 正常応答 urg->is_laser_on = URG_FALSE; urg->is_sending = URG_FALSE; return set_errno_and_return(urg, URG_NO_ERROR); } } return ret; } int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step) { // 設定の範囲外を指定したときは、エラーを返す if (((skip_step < 0) || (skip_step >= 100)) || (first_step > last_step) || (first_step < -urg->front_data_index) || (last_step > (urg->last_data_index - urg->front_data_index))) { return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR); } urg->scanning_first_step = first_step; urg->scanning_last_step = last_step; urg->scanning_skip_step = skip_step; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_set_connection_data_size(urg_t *urg, urg_range_data_byte_t data_byte) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((data_byte != URG_COMMUNICATION_3_BYTE) || (data_byte != URG_COMMUNICATION_2_BYTE)) { return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR); } urg->range_data_byte = data_byte; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_laser_on(urg_t *urg) { int expected[] = { 0, 2, EXPECTED_END }; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if (urg->is_laser_on != URG_FALSE) { // 既にレーザが発光しているときは、コマンドを送信しないようにする urg->last_errno = 0; return urg->last_errno; } ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0); if (ret >= 0) { urg->is_laser_on = URG_TRUE; ret = 0; } return ret; } int urg_laser_off(urg_t *urg) { return urg_stop_measurement(urg); } int urg_reboot(urg_t *urg) { int expected[] = { 0, 1, EXPECTED_END }; int ret; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // ２回目の RB 送信後、接続を切断する for (i = 0; i < 2; ++i) { ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0); if (ret < 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } urg->is_active = URG_FALSE; urg_close(urg); urg->last_errno = 0; return urg->last_errno; } void urg_sleep(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = 4 }; int sl_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE]; if (urg_stop_measurement(urg) != URG_NO_ERROR) { return; } scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); } void urg_wakeup(urg_t *urg) { urg_stop_measurement(urg); } int urg_is_stable(urg_t *urg) { const char *stat = urg_sensor_status(urg); return strncmp("Stable", stat, 6) ? 0 : 1; } static char *copy_token(char *dest, char *receive_buffer, const char *start_str, const char *end_ch, int lines) { size_t start_str_len = strlen(start_str); size_t end_ch_len = strlen(end_ch); int i; size_t j; for (j = 0; j < end_ch_len; ++j) { const char *p = receive_buffer; for (i = 0; i < lines; ++i) { if (!strncmp(p, start_str, start_str_len)) { char *last_p = strchr(p + start_str_len, end_ch[j]); if (last_p) { *last_p = '\0'; memcpy(dest, p + start_str_len, last_p - (p + start_str_len) + 1); return dest; } } p += strlen(p) + 1; } } return NULL; } static const char *receive_command_response(urg_t *urg, char *buffer, int buffer_size, const char* command, int response_lines) { const int vv_expected[] = { 0, EXPECTED_END }; int ret; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = scip_response(urg, command, vv_expected, urg->timeout, buffer, buffer_size); if (ret < response_lines) { return RECEIVE_ERROR_MESSAGE; } return NULL; } const char *urg_sensor_product_type(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROD:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_serial_id(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "SERI:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_vendor(urg_t *urg){ enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "VEND:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", "(", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_date(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } // Get the firmware version and append a '(', this will be what's before the date char firmware_version[50]; strcpy(firmware_version, urg_sensor_firmware_version(urg)); strcat(firmware_version, "("); ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } // Strip out the actual date from between the '(' and ')' char *date; p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", ";", VV_RESPONSE_LINES); date = copy_token(urg->return_buffer, p, firmware_version, ")", 1); return (date) ? date : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_protocol_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROT:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_status(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "STAT:", ";", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_state(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "MESM:", " (", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } void urg_set_error_handler(urg_t *urg, urg_error_handler handler) { urg->error_handler = handler; }
342  urg->scan_usec = 1000 * 1000 * 60 / rpm;
343  urg->timeout = urg->scan_usec >> (10 - 4);
344  received_bits |= 0x0040;
345  }
346  p += strlen(p) + 1;
347  }
348 
349  // 全てのパラメータを受信したか確認
350  if (received_bits != 0x007f) {
351  return set_errno_and_return(urg, URG_RECEIVE_ERROR);
352  }
353 
354  urg_set_scanning_parameter(urg,
355  urg->first_data_index - urg->front_data_index,
356  urg->last_data_index - urg->front_data_index,
357  1);
358 
359  return set_errno_and_return(urg, URG_NO_ERROR);
360 }
361 
362 
364 long urg_scip_decode(const char data[], int size)
365 {
366  const char* p = data;
367  const char* last_p = p + size;
368  int value = 0;
369 
370  while (p < last_p) {
371  value <<= 6;
372  value &= ~0x3f;
373  value |= *p++ - 0x30;
374  }
375  return value;
376 }
377 
378 
379 static int parse_parameter(const char *parameter, int size)
380 {
381  char buffer[5];
382 
383  memcpy(buffer, parameter, size);
384  buffer[size] = '\0';
385 
386  return strtol(buffer, NULL, 10);
387 }
388 
389 
390 static urg_measurement_type_t parse_distance_parameter(urg_t *urg,
391  const char echoback[])
392 {
393  urg_measurement_type_t ret_type = URG_UNKNOWN;
394 
395  urg->received_range_data_byte = URG_COMMUNICATION_3_BYTE;
396  if (echoback[1] == 'S') {
397  urg->received_range_data_byte = URG_COMMUNICATION_2_BYTE;
398  ret_type = URG_DISTANCE;
399 
400  } else if (echoback[1] == 'D') {
401  if ((echoback[0] == 'G') || (echoback[0] == 'M')) {
402  ret_type = URG_DISTANCE;
403  } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) {
404  ret_type = URG_MULTIECHO;
405  }
406  } else if (echoback[1] == 'E') {
407  if ((echoback[0] == 'G') || (echoback[0] == 'M')) {
408  ret_type = URG_DISTANCE_INTENSITY;
409  } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) {
410  ret_type = URG_MULTIECHO_INTENSITY;
411  }
412  } else {
413  return URG_UNKNOWN;
414  }
415 
416  // パラメータの格納
417  urg->received_first_index = parse_parameter(&echoback[2], 4);
418  urg->received_last_index = parse_parameter(&echoback[6], 4);
419  urg->received_skip_step = parse_parameter(&echoback[10], 2);
420 
421  return ret_type;
422 }
423 
424 
425 static urg_measurement_type_t parse_distance_echoback(urg_t *urg,
426  const char echoback[])
427 {
428  size_t line_length;
429  urg_measurement_type_t ret_type = URG_UNKNOWN;
430 
431  if (!strcmp("QT", echoback)) {
432  return URG_STOP;
433  }
434 
435  line_length = strlen(echoback);
436  if ((line_length == 12) &&
437  ((echoback[0] == 'G') || (echoback[0] == 'H'))) {
438  ret_type = parse_distance_parameter(urg, echoback);
439 
440  } else if ((line_length == 15) &&
441  ((echoback[0] == 'M') || (echoback[0] == 'N'))) {
442  ret_type = parse_distance_parameter(urg, echoback);
443  }
444  return ret_type;
445 }
446 
447 
448 static int receive_length_data(urg_t *urg, long length[],
449  unsigned short intensity[],
450  urg_measurement_type_t type, char buffer[])
451 {
452  int n;
453  int step_filled = 0;
454  int line_filled = 0;
455  int multiecho_index = 0;
456 
457  int each_size =
458  (urg->received_range_data_byte == URG_COMMUNICATION_2_BYTE) ? 2 : 3;
459  int data_size = each_size;
460  int is_intensity = URG_FALSE;
461  int is_multiecho = URG_FALSE;
462  int multiecho_max_size = 1;
463 
464  if ((type == URG_DISTANCE_INTENSITY) || (type == URG_MULTIECHO_INTENSITY)) {
465  data_size *= 2;
466  is_intensity = URG_TRUE;
467  }
468  if ((type == URG_MULTIECHO) || (type == URG_MULTIECHO_INTENSITY)) {
469  is_multiecho = URG_TRUE;
470  multiecho_max_size = URG_MAX_ECHO;
471  }
472 
473  do {
474  char *p = buffer;
475  char *last_p;
476 
477  n = connection_readline(&urg->connection,
478  &buffer[line_filled], BUFFER_SIZE - line_filled,
479  urg->timeout);
480 
481  if (n > 0) {
482  // チェックサムの評価
483  if (buffer[line_filled + n - 1] !=
484  scip_checksum(&buffer[line_filled], n - 1)) {
485  ignore_receive_data_with_qt(urg, urg->timeout);
486  return set_errno_and_return(urg, URG_CHECKSUM_ERROR);
487  }
488  }
489 
490  if (n > 0) {
491  line_filled += n - 1;
492  }
493  last_p = p + line_filled;
494 
495  while ((last_p - p) >= data_size) {
496  int index;
497 
498  if (*p == '&') {
499  // 先頭文字が '&' だったときは、マルチエコーのデータとみなす
500 
501  if ((last_p - (p + 1)) < data_size) {
502  // '&' を除いて、data_size 分データが無ければ抜ける break; } --step_filled; ++multiecho_index; ++p; --line_filled; } else { // 次のデータ multiecho_index = 0; } index = (step_filled * multiecho_max_size) + multiecho_index; if (step_filled > (urg->received_last_index - urg->received_first_index)) { // データが多過ぎる場合は、残りのデータを無視して戻る ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (is_multiecho && (multiecho_index == 0)) { // マルチエコーのデータ格納先をダミーデータで埋める int i; if (length) { for (i = 1; i < multiecho_max_size; ++i) { length[index + i] = 0; } } if (intensity) { for (i = 1; i < multiecho_max_size; ++i) { intensity[index + i] = 0; } } } // 距離データの格納 if (length) { length[index] = urg_scip_decode(p, 3); } p += 3; // 強度データの格納 if (is_intensity) { if (intensity) { intensity[index] = (unsigned short)urg_scip_decode(p, 3); } p += 3; } ++step_filled; line_filled -= data_size; } // 次に処理する文字を退避 memmove(buffer, p, line_filled); } while (n > 0); return step_filled; } //! 距離データの取得 static int receive_data(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { urg_measurement_type_t type; char buffer[BUFFER_SIZE]; int ret = 0; int n; int extended_timeout = urg->timeout + 2 * (urg->scan_usec * (urg->scanning_skip_scan) / 1000); // エコーバックの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, extended_timeout); if (n <= 0) { return set_errno_and_return(urg, URG_NO_RESPONSE); } // エコーバックの解析 type = parse_distance_echoback(urg, buffer); // 応答の取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 3) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } if (buffer[n - 1] != scip_checksum(buffer, n - 1)) { // チェックサムの評価 ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } if (type == URG_STOP) { // QT 応答の場合には、最後の改行を読み捨て、正常応答として処理する n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n == 0) { return 0; } else { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } if (urg->specified_scan_times != 1) { if (!strncmp(buffer, "00", 2)) { // "00" 応答の場合は、エコーバック応答とみなし、 // 最後の空行を読み捨て、次からのデータを返す n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 0) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } } } if (((urg->specified_scan_times == 1) && (strncmp(buffer, "00", 2))) || ((urg->specified_scan_times != 1) && (strncmp(buffer, "99", 2)))) { if (urg->error_handler) { type = urg->error_handler(buffer, urg); } if (type == URG_UNKNOWN) { // Gx, Hx のときは 00P が返されたときがデータ // Mx, Nx のときは 99b が返されたときがデータ ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } // タイムスタンプの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n > 0) { if (time_stamp) { *time_stamp = urg_scip_decode(buffer, 4); } if (system_time_stamp) { urg_get_walltime(system_time_stamp); } } // データの取得 switch (type) { case URG_DISTANCE: case URG_MULTIECHO: ret = receive_length_data(urg, data, NULL, type, buffer); break; case URG_DISTANCE_INTENSITY: case URG_MULTIECHO_INTENSITY: ret = receive_length_data(urg, data, intensity, type, buffer); break; case URG_STOP: case URG_UNKNOWN: ret = 0; break; } // specified_scan_times == 1 のときは Gx 系コマンドが使われるため // データを明示的に停止しなくてよい if ((urg->specified_scan_times > 1) && (urg->scanning_remain_times > 0)) { if (--urg->scanning_remain_times <= 0) { // データの停止のみを行う urg_stop_measurement(urg); } } return ret; } int urg_open(urg_t *urg, urg_connection_type_t connection_type, const char *device_or_address, long baudrate_or_port) { int ret; long baudrate = baudrate_or_port; urg->is_active = URG_FALSE; urg->is_sending = URG_TRUE; urg->last_errno = URG_NOT_CONNECTED; urg->timeout = MAX_TIMEOUT; urg->scanning_skip_scan = 0; urg->error_handler = NULL; // デバイスへの接続 ret = connection_open(&urg->connection, connection_type, device_or_address, baudrate_or_port); if (ret < 0) { switch (connection_type) { case URG_SERIAL: urg->last_errno = URG_SERIAL_OPEN_ERROR; break; case URG_ETHERNET: urg->last_errno = URG_ETHERNET_OPEN_ERROR; break; default: urg->last_errno = URG_INVALID_RESPONSE; break; } return urg->last_errno; } // 指定したボーレートで URG と通信できるように調整 if (connection_type == URG_ETHERNET) { // Ethernet のときは仮の通信速度を指定しておく baudrate = 115200; } if (connect_urg_device(urg, baudrate) != URG_NO_ERROR) { return set_errno_and_return(urg, ret); } urg->is_sending = URG_FALSE; // 変数の初期化 urg->last_errno = URG_NO_ERROR; urg->range_data_byte = URG_COMMUNICATION_3_BYTE; urg->specified_scan_times = 0; urg->scanning_remain_times = 0; urg->is_laser_on = URG_FALSE; // パラメータ情報を取得 ret = receive_parameter(urg); if (ret == URG_NO_ERROR) { urg->is_active = URG_TRUE; } return ret; } void urg_close(urg_t *urg) { if (urg->is_active) { ignore_receive_data_with_qt(urg, urg->timeout); } connection_close(&urg->connection); urg->is_active = URG_FALSE; } void urg_set_timeout_msec(urg_t *urg, int msec) { urg->timeout = msec; } int urg_start_time_stamp_mode(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM0 を発行する n = scip_response(urg, "TM0\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } long urg_time_stamp(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; char buffer[BUFFER_SIZE]; char *p; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } ret = scip_response(urg, "TM1\n", expected, urg->timeout, buffer, BUFFER_SIZE); if (ret < 0) { return ret; } // buffer からタイムスタンプを取得し、デコードして返す if (strcmp(buffer, "00P")) { // 最初の応答が "00P" でなければ戻る return set_errno_and_return(urg, URG_RECEIVE_ERROR); } p = buffer + 4; if (strlen(p) != 5) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (p[5] == scip_checksum(p, 4)) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } return urg_scip_decode(p, 4); } int urg_stop_time_stamp_mode(urg_t *urg) { int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM2 を発行する n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } static int send_distance_command(urg_t *urg, int scan_times, int skip_scan, char single_scan_ch, char continuous_scan_ch, char scan_type_ch) { char buffer[BUFFER_SIZE]; int write_size = 0; int front_index = urg->front_data_index; int n; urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times; urg->scanning_remain_times = urg->specified_scan_times; urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan; if (scan_times >= 100) { // 計測回数が 99 を越える場合は、無限回のスキャンを行う urg->specified_scan_times = 0; } if (urg->scanning_remain_times == 1) { // レーザ発光を指示 urg_laser_on(urg); write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n", single_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step); } else { write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n", continuous_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step, skip_scan, urg->specified_scan_times); urg->is_sending = URG_TRUE; } n = connection_write(&urg->connection, buffer, write_size); if (n != write_size) { return set_errno_and_return(urg, URG_SEND_ERROR); } return 0; } int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan) { char range_byte_ch; int ret = 0; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((skip_scan < 0) || (skip_scan > 9)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // !!! Mx 系, Nx 系の計測中のときは、QT を発行してから // !!! 計測開始コマンドを送信するようにする // !!! ただし、MD 計測中に MD を発行するように、同じコマンドの場合は // !!! Mx 系, Nx 系の計測は上書きすることができるようにする // 指定されたタイプのパケットを生成し、送信する switch (type) { case URG_DISTANCE: range_byte_ch = (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D'; ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', range_byte_ch); break; case URG_DISTANCE_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', 'E'); break; case URG_MULTIECHO: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'D'); break; case URG_MULTIECHO_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'E'); break; case URG_STOP: case URG_UNKNOWN: default: ignore_receive_data_with_qt(urg, urg->timeout); urg->last_errno = URG_INVALID_PARAMETER; ret = urg->last_errno; break; } return ret; } int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, NULL, time_stamp, system_time_stamp); } int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp); } int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp); } int urg_stop_measurement(urg_t *urg) { enum { MAX_READ_TIMES = 3 }; int ret = URG_INVALID_RESPONSE; int n; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // QT を発行する n = connection_write(&urg->connection, "QT\n", 3); if (n != 3) { return set_errno_and_return(urg, URG_SEND_ERROR); } for (i = 0; i < MAX_READ_TIMES; ++i) { // QT の応答が返されるまで、距離データを読み捨てる ret = receive_data(urg, NULL, NULL, NULL, NULL); if (ret == URG_NO_ERROR) { // 正常応答 urg->is_laser_on = URG_FALSE; urg->is_sending = URG_FALSE; return set_errno_and_return(urg, URG_NO_ERROR); } } return ret; } int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step) { // 設定の範囲外を指定したときは、エラーを返す if (((skip_step < 0) || (skip_step >= 100)) || (first_step > last_step) || (first_step < -urg->front_data_index) || (last_step > (urg->last_data_index - urg->front_data_index))) { return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR); } urg->scanning_first_step = first_step; urg->scanning_last_step = last_step; urg->scanning_skip_step = skip_step; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_set_connection_data_size(urg_t *urg, urg_range_data_byte_t data_byte) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((data_byte != URG_COMMUNICATION_3_BYTE) || (data_byte != URG_COMMUNICATION_2_BYTE)) { return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR); } urg->range_data_byte = data_byte; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_laser_on(urg_t *urg) { int expected[] = { 0, 2, EXPECTED_END }; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if (urg->is_laser_on != URG_FALSE) { // 既にレーザが発光しているときは、コマンドを送信しないようにする urg->last_errno = 0; return urg->last_errno; } ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0); if (ret >= 0) { urg->is_laser_on = URG_TRUE; ret = 0; } return ret; } int urg_laser_off(urg_t *urg) { return urg_stop_measurement(urg); } int urg_reboot(urg_t *urg) { int expected[] = { 0, 1, EXPECTED_END }; int ret; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // ２回目の RB 送信後、接続を切断する for (i = 0; i < 2; ++i) { ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0); if (ret < 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } urg->is_active = URG_FALSE; urg_close(urg); urg->last_errno = 0; return urg->last_errno; } void urg_sleep(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = 4 }; int sl_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE]; if (urg_stop_measurement(urg) != URG_NO_ERROR) { return; } scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); } void urg_wakeup(urg_t *urg) { urg_stop_measurement(urg); } int urg_is_stable(urg_t *urg) { const char *stat = urg_sensor_status(urg); return strncmp("Stable", stat, 6) ? 0 : 1; } static char *copy_token(char *dest, char *receive_buffer, const char *start_str, const char *end_ch, int lines) { size_t start_str_len = strlen(start_str); size_t end_ch_len = strlen(end_ch); int i; size_t j; for (j = 0; j < end_ch_len; ++j) { const char *p = receive_buffer; for (i = 0; i < lines; ++i) { if (!strncmp(p, start_str, start_str_len)) { char *last_p = strchr(p + start_str_len, end_ch[j]); if (last_p) { *last_p = '\0'; memcpy(dest, p + start_str_len, last_p - (p + start_str_len) + 1); return dest; } } p += strlen(p) + 1; } } return NULL; } static const char *receive_command_response(urg_t *urg, char *buffer, int buffer_size, const char* command, int response_lines) { const int vv_expected[] = { 0, EXPECTED_END }; int ret; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = scip_response(urg, command, vv_expected, urg->timeout, buffer, buffer_size); if (ret < response_lines) { return RECEIVE_ERROR_MESSAGE; } return NULL; } const char *urg_sensor_product_type(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROD:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_serial_id(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "SERI:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_vendor(urg_t *urg){ enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "VEND:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", "(", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_date(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } // Get the firmware version and append a '(', this will be what's before the date char firmware_version[50]; strcpy(firmware_version, urg_sensor_firmware_version(urg)); strcat(firmware_version, "("); ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } // Strip out the actual date from between the '(' and ')' char *date; p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", ";", VV_RESPONSE_LINES); date = copy_token(urg->return_buffer, p, firmware_version, ")", 1); return (date) ? date : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_protocol_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROT:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_status(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "STAT:", ";", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_state(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "MESM:", " (", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } void urg_set_error_handler(urg_t *urg, urg_error_handler handler) { urg->error_handler = handler; }
503  break;
504  }
505 
506  --step_filled;
507  ++multiecho_index;
508  ++p;
509  --line_filled;
510 
511  } else {
512  // 次のデータ
513  multiecho_index = 0;
514  }
515 
516  index = (step_filled * multiecho_max_size) + multiecho_index;
517 
518  if (step_filled >
519  (urg->received_last_index - urg->received_first_index)) {
520  // データが多過ぎる場合は、残りのデータを無視して戻る ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (is_multiecho && (multiecho_index == 0)) { // マルチエコーのデータ格納先をダミーデータで埋める int i; if (length) { for (i = 1; i < multiecho_max_size; ++i) { length[index + i] = 0; } } if (intensity) { for (i = 1; i < multiecho_max_size; ++i) { intensity[index + i] = 0; } } } // 距離データの格納 if (length) { length[index] = urg_scip_decode(p, 3); } p += 3; // 強度データの格納 if (is_intensity) { if (intensity) { intensity[index] = (unsigned short)urg_scip_decode(p, 3); } p += 3; } ++step_filled; line_filled -= data_size; } // 次に処理する文字を退避 memmove(buffer, p, line_filled); } while (n > 0); return step_filled; } //! 距離データの取得 static int receive_data(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { urg_measurement_type_t type; char buffer[BUFFER_SIZE]; int ret = 0; int n; int extended_timeout = urg->timeout + 2 * (urg->scan_usec * (urg->scanning_skip_scan) / 1000); // エコーバックの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, extended_timeout); if (n <= 0) { return set_errno_and_return(urg, URG_NO_RESPONSE); } // エコーバックの解析 type = parse_distance_echoback(urg, buffer); // 応答の取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 3) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } if (buffer[n - 1] != scip_checksum(buffer, n - 1)) { // チェックサムの評価 ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } if (type == URG_STOP) { // QT 応答の場合には、最後の改行を読み捨て、正常応答として処理する n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n == 0) { return 0; } else { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } if (urg->specified_scan_times != 1) { if (!strncmp(buffer, "00", 2)) { // "00" 応答の場合は、エコーバック応答とみなし、 // 最後の空行を読み捨て、次からのデータを返す n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 0) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } } } if (((urg->specified_scan_times == 1) && (strncmp(buffer, "00", 2))) || ((urg->specified_scan_times != 1) && (strncmp(buffer, "99", 2)))) { if (urg->error_handler) { type = urg->error_handler(buffer, urg); } if (type == URG_UNKNOWN) { // Gx, Hx のときは 00P が返されたときがデータ // Mx, Nx のときは 99b が返されたときがデータ ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } // タイムスタンプの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n > 0) { if (time_stamp) { *time_stamp = urg_scip_decode(buffer, 4); } if (system_time_stamp) { urg_get_walltime(system_time_stamp); } } // データの取得 switch (type) { case URG_DISTANCE: case URG_MULTIECHO: ret = receive_length_data(urg, data, NULL, type, buffer); break; case URG_DISTANCE_INTENSITY: case URG_MULTIECHO_INTENSITY: ret = receive_length_data(urg, data, intensity, type, buffer); break; case URG_STOP: case URG_UNKNOWN: ret = 0; break; } // specified_scan_times == 1 のときは Gx 系コマンドが使われるため // データを明示的に停止しなくてよい if ((urg->specified_scan_times > 1) && (urg->scanning_remain_times > 0)) { if (--urg->scanning_remain_times <= 0) { // データの停止のみを行う urg_stop_measurement(urg); } } return ret; } int urg_open(urg_t *urg, urg_connection_type_t connection_type, const char *device_or_address, long baudrate_or_port) { int ret; long baudrate = baudrate_or_port; urg->is_active = URG_FALSE; urg->is_sending = URG_TRUE; urg->last_errno = URG_NOT_CONNECTED; urg->timeout = MAX_TIMEOUT; urg->scanning_skip_scan = 0; urg->error_handler = NULL; // デバイスへの接続 ret = connection_open(&urg->connection, connection_type, device_or_address, baudrate_or_port); if (ret < 0) { switch (connection_type) { case URG_SERIAL: urg->last_errno = URG_SERIAL_OPEN_ERROR; break; case URG_ETHERNET: urg->last_errno = URG_ETHERNET_OPEN_ERROR; break; default: urg->last_errno = URG_INVALID_RESPONSE; break; } return urg->last_errno; } // 指定したボーレートで URG と通信できるように調整 if (connection_type == URG_ETHERNET) { // Ethernet のときは仮の通信速度を指定しておく baudrate = 115200; } if (connect_urg_device(urg, baudrate) != URG_NO_ERROR) { return set_errno_and_return(urg, ret); } urg->is_sending = URG_FALSE; // 変数の初期化 urg->last_errno = URG_NO_ERROR; urg->range_data_byte = URG_COMMUNICATION_3_BYTE; urg->specified_scan_times = 0; urg->scanning_remain_times = 0; urg->is_laser_on = URG_FALSE; // パラメータ情報を取得 ret = receive_parameter(urg); if (ret == URG_NO_ERROR) { urg->is_active = URG_TRUE; } return ret; } void urg_close(urg_t *urg) { if (urg->is_active) { ignore_receive_data_with_qt(urg, urg->timeout); } connection_close(&urg->connection); urg->is_active = URG_FALSE; } void urg_set_timeout_msec(urg_t *urg, int msec) { urg->timeout = msec; } int urg_start_time_stamp_mode(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM0 を発行する n = scip_response(urg, "TM0\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } long urg_time_stamp(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; char buffer[BUFFER_SIZE]; char *p; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } ret = scip_response(urg, "TM1\n", expected, urg->timeout, buffer, BUFFER_SIZE); if (ret < 0) { return ret; } // buffer からタイムスタンプを取得し、デコードして返す if (strcmp(buffer, "00P")) { // 最初の応答が "00P" でなければ戻る return set_errno_and_return(urg, URG_RECEIVE_ERROR); } p = buffer + 4; if (strlen(p) != 5) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (p[5] == scip_checksum(p, 4)) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } return urg_scip_decode(p, 4); } int urg_stop_time_stamp_mode(urg_t *urg) { int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM2 を発行する n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } static int send_distance_command(urg_t *urg, int scan_times, int skip_scan, char single_scan_ch, char continuous_scan_ch, char scan_type_ch) { char buffer[BUFFER_SIZE]; int write_size = 0; int front_index = urg->front_data_index; int n; urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times; urg->scanning_remain_times = urg->specified_scan_times; urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan; if (scan_times >= 100) { // 計測回数が 99 を越える場合は、無限回のスキャンを行う urg->specified_scan_times = 0; } if (urg->scanning_remain_times == 1) { // レーザ発光を指示 urg_laser_on(urg); write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n", single_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step); } else { write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n", continuous_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step, skip_scan, urg->specified_scan_times); urg->is_sending = URG_TRUE; } n = connection_write(&urg->connection, buffer, write_size); if (n != write_size) { return set_errno_and_return(urg, URG_SEND_ERROR); } return 0; } int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan) { char range_byte_ch; int ret = 0; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((skip_scan < 0) || (skip_scan > 9)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // !!! Mx 系, Nx 系の計測中のときは、QT を発行してから // !!! 計測開始コマンドを送信するようにする // !!! ただし、MD 計測中に MD を発行するように、同じコマンドの場合は // !!! Mx 系, Nx 系の計測は上書きすることができるようにする // 指定されたタイプのパケットを生成し、送信する switch (type) { case URG_DISTANCE: range_byte_ch = (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D'; ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', range_byte_ch); break; case URG_DISTANCE_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', 'E'); break; case URG_MULTIECHO: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'D'); break; case URG_MULTIECHO_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'E'); break; case URG_STOP: case URG_UNKNOWN: default: ignore_receive_data_with_qt(urg, urg->timeout); urg->last_errno = URG_INVALID_PARAMETER; ret = urg->last_errno; break; } return ret; } int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, NULL, time_stamp, system_time_stamp); } int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp); } int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp); } int urg_stop_measurement(urg_t *urg) { enum { MAX_READ_TIMES = 3 }; int ret = URG_INVALID_RESPONSE; int n; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // QT を発行する n = connection_write(&urg->connection, "QT\n", 3); if (n != 3) { return set_errno_and_return(urg, URG_SEND_ERROR); } for (i = 0; i < MAX_READ_TIMES; ++i) { // QT の応答が返されるまで、距離データを読み捨てる ret = receive_data(urg, NULL, NULL, NULL, NULL); if (ret == URG_NO_ERROR) { // 正常応答 urg->is_laser_on = URG_FALSE; urg->is_sending = URG_FALSE; return set_errno_and_return(urg, URG_NO_ERROR); } } return ret; } int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step) { // 設定の範囲外を指定したときは、エラーを返す if (((skip_step < 0) || (skip_step >= 100)) || (first_step > last_step) || (first_step < -urg->front_data_index) || (last_step > (urg->last_data_index - urg->front_data_index))) { return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR); } urg->scanning_first_step = first_step; urg->scanning_last_step = last_step; urg->scanning_skip_step = skip_step; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_set_connection_data_size(urg_t *urg, urg_range_data_byte_t data_byte) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((data_byte != URG_COMMUNICATION_3_BYTE) || (data_byte != URG_COMMUNICATION_2_BYTE)) { return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR); } urg->range_data_byte = data_byte; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_laser_on(urg_t *urg) { int expected[] = { 0, 2, EXPECTED_END }; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if (urg->is_laser_on != URG_FALSE) { // 既にレーザが発光しているときは、コマンドを送信しないようにする urg->last_errno = 0; return urg->last_errno; } ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0); if (ret >= 0) { urg->is_laser_on = URG_TRUE; ret = 0; } return ret; } int urg_laser_off(urg_t *urg) { return urg_stop_measurement(urg); } int urg_reboot(urg_t *urg) { int expected[] = { 0, 1, EXPECTED_END }; int ret; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // ２回目の RB 送信後、接続を切断する for (i = 0; i < 2; ++i) { ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0); if (ret < 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } urg->is_active = URG_FALSE; urg_close(urg); urg->last_errno = 0; return urg->last_errno; } void urg_sleep(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = 4 }; int sl_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE]; if (urg_stop_measurement(urg) != URG_NO_ERROR) { return; } scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); } void urg_wakeup(urg_t *urg) { urg_stop_measurement(urg); } int urg_is_stable(urg_t *urg) { const char *stat = urg_sensor_status(urg); return strncmp("Stable", stat, 6) ? 0 : 1; } static char *copy_token(char *dest, char *receive_buffer, const char *start_str, const char *end_ch, int lines) { size_t start_str_len = strlen(start_str); size_t end_ch_len = strlen(end_ch); int i; size_t j; for (j = 0; j < end_ch_len; ++j) { const char *p = receive_buffer; for (i = 0; i < lines; ++i) { if (!strncmp(p, start_str, start_str_len)) { char *last_p = strchr(p + start_str_len, end_ch[j]); if (last_p) { *last_p = '\0'; memcpy(dest, p + start_str_len, last_p - (p + start_str_len) + 1); return dest; } } p += strlen(p) + 1; } } return NULL; } static const char *receive_command_response(urg_t *urg, char *buffer, int buffer_size, const char* command, int response_lines) { const int vv_expected[] = { 0, EXPECTED_END }; int ret; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = scip_response(urg, command, vv_expected, urg->timeout, buffer, buffer_size); if (ret < response_lines) { return RECEIVE_ERROR_MESSAGE; } return NULL; } const char *urg_sensor_product_type(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROD:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_serial_id(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "SERI:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_vendor(urg_t *urg){ enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "VEND:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", "(", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_date(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } // Get the firmware version and append a '(', this will be what's before the date char firmware_version[50]; strcpy(firmware_version, urg_sensor_firmware_version(urg)); strcat(firmware_version, "("); ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } // Strip out the actual date from between the '(' and ')' char *date; p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", ";", VV_RESPONSE_LINES); date = copy_token(urg->return_buffer, p, firmware_version, ")", 1); return (date) ? date : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_protocol_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROT:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_status(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "STAT:", ";", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_state(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "MESM:", " (", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } void urg_set_error_handler(urg_t *urg, urg_error_handler handler) { urg->error_handler = handler; }
521  ignore_receive_data_with_qt(urg, urg->timeout);
522  return set_errno_and_return(urg, URG_RECEIVE_ERROR);
523  }
524 
525 
526  if (is_multiecho && (multiecho_index == 0)) {
527  // マルチエコーのデータ格納先をダミーデータで埋める int i; if (length) { for (i = 1; i < multiecho_max_size; ++i) { length[index + i] = 0; } } if (intensity) { for (i = 1; i < multiecho_max_size; ++i) { intensity[index + i] = 0; } } } // 距離データの格納 if (length) { length[index] = urg_scip_decode(p, 3); } p += 3; // 強度データの格納 if (is_intensity) { if (intensity) { intensity[index] = (unsigned short)urg_scip_decode(p, 3); } p += 3; } ++step_filled; line_filled -= data_size; } // 次に処理する文字を退避 memmove(buffer, p, line_filled); } while (n > 0); return step_filled; } //! 距離データの取得 static int receive_data(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { urg_measurement_type_t type; char buffer[BUFFER_SIZE]; int ret = 0; int n; int extended_timeout = urg->timeout + 2 * (urg->scan_usec * (urg->scanning_skip_scan) / 1000); // エコーバックの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, extended_timeout); if (n <= 0) { return set_errno_and_return(urg, URG_NO_RESPONSE); } // エコーバックの解析 type = parse_distance_echoback(urg, buffer); // 応答の取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 3) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } if (buffer[n - 1] != scip_checksum(buffer, n - 1)) { // チェックサムの評価 ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } if (type == URG_STOP) { // QT 応答の場合には、最後の改行を読み捨て、正常応答として処理する n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n == 0) { return 0; } else { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } if (urg->specified_scan_times != 1) { if (!strncmp(buffer, "00", 2)) { // "00" 応答の場合は、エコーバック応答とみなし、 // 最後の空行を読み捨て、次からのデータを返す n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 0) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } } } if (((urg->specified_scan_times == 1) && (strncmp(buffer, "00", 2))) || ((urg->specified_scan_times != 1) && (strncmp(buffer, "99", 2)))) { if (urg->error_handler) { type = urg->error_handler(buffer, urg); } if (type == URG_UNKNOWN) { // Gx, Hx のときは 00P が返されたときがデータ // Mx, Nx のときは 99b が返されたときがデータ ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } // タイムスタンプの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n > 0) { if (time_stamp) { *time_stamp = urg_scip_decode(buffer, 4); } if (system_time_stamp) { urg_get_walltime(system_time_stamp); } } // データの取得 switch (type) { case URG_DISTANCE: case URG_MULTIECHO: ret = receive_length_data(urg, data, NULL, type, buffer); break; case URG_DISTANCE_INTENSITY: case URG_MULTIECHO_INTENSITY: ret = receive_length_data(urg, data, intensity, type, buffer); break; case URG_STOP: case URG_UNKNOWN: ret = 0; break; } // specified_scan_times == 1 のときは Gx 系コマンドが使われるため // データを明示的に停止しなくてよい if ((urg->specified_scan_times > 1) && (urg->scanning_remain_times > 0)) { if (--urg->scanning_remain_times <= 0) { // データの停止のみを行う urg_stop_measurement(urg); } } return ret; } int urg_open(urg_t *urg, urg_connection_type_t connection_type, const char *device_or_address, long baudrate_or_port) { int ret; long baudrate = baudrate_or_port; urg->is_active = URG_FALSE; urg->is_sending = URG_TRUE; urg->last_errno = URG_NOT_CONNECTED; urg->timeout = MAX_TIMEOUT; urg->scanning_skip_scan = 0; urg->error_handler = NULL; // デバイスへの接続 ret = connection_open(&urg->connection, connection_type, device_or_address, baudrate_or_port); if (ret < 0) { switch (connection_type) { case URG_SERIAL: urg->last_errno = URG_SERIAL_OPEN_ERROR; break; case URG_ETHERNET: urg->last_errno = URG_ETHERNET_OPEN_ERROR; break; default: urg->last_errno = URG_INVALID_RESPONSE; break; } return urg->last_errno; } // 指定したボーレートで URG と通信できるように調整 if (connection_type == URG_ETHERNET) { // Ethernet のときは仮の通信速度を指定しておく baudrate = 115200; } if (connect_urg_device(urg, baudrate) != URG_NO_ERROR) { return set_errno_and_return(urg, ret); } urg->is_sending = URG_FALSE; // 変数の初期化 urg->last_errno = URG_NO_ERROR; urg->range_data_byte = URG_COMMUNICATION_3_BYTE; urg->specified_scan_times = 0; urg->scanning_remain_times = 0; urg->is_laser_on = URG_FALSE; // パラメータ情報を取得 ret = receive_parameter(urg); if (ret == URG_NO_ERROR) { urg->is_active = URG_TRUE; } return ret; } void urg_close(urg_t *urg) { if (urg->is_active) { ignore_receive_data_with_qt(urg, urg->timeout); } connection_close(&urg->connection); urg->is_active = URG_FALSE; } void urg_set_timeout_msec(urg_t *urg, int msec) { urg->timeout = msec; } int urg_start_time_stamp_mode(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM0 を発行する n = scip_response(urg, "TM0\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } long urg_time_stamp(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; char buffer[BUFFER_SIZE]; char *p; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } ret = scip_response(urg, "TM1\n", expected, urg->timeout, buffer, BUFFER_SIZE); if (ret < 0) { return ret; } // buffer からタイムスタンプを取得し、デコードして返す if (strcmp(buffer, "00P")) { // 最初の応答が "00P" でなければ戻る return set_errno_and_return(urg, URG_RECEIVE_ERROR); } p = buffer + 4; if (strlen(p) != 5) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (p[5] == scip_checksum(p, 4)) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } return urg_scip_decode(p, 4); } int urg_stop_time_stamp_mode(urg_t *urg) { int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM2 を発行する n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } static int send_distance_command(urg_t *urg, int scan_times, int skip_scan, char single_scan_ch, char continuous_scan_ch, char scan_type_ch) { char buffer[BUFFER_SIZE]; int write_size = 0; int front_index = urg->front_data_index; int n; urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times; urg->scanning_remain_times = urg->specified_scan_times; urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan; if (scan_times >= 100) { // 計測回数が 99 を越える場合は、無限回のスキャンを行う urg->specified_scan_times = 0; } if (urg->scanning_remain_times == 1) { // レーザ発光を指示 urg_laser_on(urg); write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n", single_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step); } else { write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n", continuous_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step, skip_scan, urg->specified_scan_times); urg->is_sending = URG_TRUE; } n = connection_write(&urg->connection, buffer, write_size); if (n != write_size) { return set_errno_and_return(urg, URG_SEND_ERROR); } return 0; } int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan) { char range_byte_ch; int ret = 0; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((skip_scan < 0) || (skip_scan > 9)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // !!! Mx 系, Nx 系の計測中のときは、QT を発行してから // !!! 計測開始コマンドを送信するようにする // !!! ただし、MD 計測中に MD を発行するように、同じコマンドの場合は // !!! Mx 系, Nx 系の計測は上書きすることができるようにする // 指定されたタイプのパケットを生成し、送信する switch (type) { case URG_DISTANCE: range_byte_ch = (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D'; ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', range_byte_ch); break; case URG_DISTANCE_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', 'E'); break; case URG_MULTIECHO: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'D'); break; case URG_MULTIECHO_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'E'); break; case URG_STOP: case URG_UNKNOWN: default: ignore_receive_data_with_qt(urg, urg->timeout); urg->last_errno = URG_INVALID_PARAMETER; ret = urg->last_errno; break; } return ret; } int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, NULL, time_stamp, system_time_stamp); } int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp); } int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp); } int urg_stop_measurement(urg_t *urg) { enum { MAX_READ_TIMES = 3 }; int ret = URG_INVALID_RESPONSE; int n; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // QT を発行する n = connection_write(&urg->connection, "QT\n", 3); if (n != 3) { return set_errno_and_return(urg, URG_SEND_ERROR); } for (i = 0; i < MAX_READ_TIMES; ++i) { // QT の応答が返されるまで、距離データを読み捨てる ret = receive_data(urg, NULL, NULL, NULL, NULL); if (ret == URG_NO_ERROR) { // 正常応答 urg->is_laser_on = URG_FALSE; urg->is_sending = URG_FALSE; return set_errno_and_return(urg, URG_NO_ERROR); } } return ret; } int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step) { // 設定の範囲外を指定したときは、エラーを返す if (((skip_step < 0) || (skip_step >= 100)) || (first_step > last_step) || (first_step < -urg->front_data_index) || (last_step > (urg->last_data_index - urg->front_data_index))) { return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR); } urg->scanning_first_step = first_step; urg->scanning_last_step = last_step; urg->scanning_skip_step = skip_step; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_set_connection_data_size(urg_t *urg, urg_range_data_byte_t data_byte) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((data_byte != URG_COMMUNICATION_3_BYTE) || (data_byte != URG_COMMUNICATION_2_BYTE)) { return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR); } urg->range_data_byte = data_byte; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_laser_on(urg_t *urg) { int expected[] = { 0, 2, EXPECTED_END }; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if (urg->is_laser_on != URG_FALSE) { // 既にレーザが発光しているときは、コマンドを送信しないようにする urg->last_errno = 0; return urg->last_errno; } ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0); if (ret >= 0) { urg->is_laser_on = URG_TRUE; ret = 0; } return ret; } int urg_laser_off(urg_t *urg) { return urg_stop_measurement(urg); } int urg_reboot(urg_t *urg) { int expected[] = { 0, 1, EXPECTED_END }; int ret; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // ２回目の RB 送信後、接続を切断する for (i = 0; i < 2; ++i) { ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0); if (ret < 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } urg->is_active = URG_FALSE; urg_close(urg); urg->last_errno = 0; return urg->last_errno; } void urg_sleep(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = 4 }; int sl_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE]; if (urg_stop_measurement(urg) != URG_NO_ERROR) { return; } scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); } void urg_wakeup(urg_t *urg) { urg_stop_measurement(urg); } int urg_is_stable(urg_t *urg) { const char *stat = urg_sensor_status(urg); return strncmp("Stable", stat, 6) ? 0 : 1; } static char *copy_token(char *dest, char *receive_buffer, const char *start_str, const char *end_ch, int lines) { size_t start_str_len = strlen(start_str); size_t end_ch_len = strlen(end_ch); int i; size_t j; for (j = 0; j < end_ch_len; ++j) { const char *p = receive_buffer; for (i = 0; i < lines; ++i) { if (!strncmp(p, start_str, start_str_len)) { char *last_p = strchr(p + start_str_len, end_ch[j]); if (last_p) { *last_p = '\0'; memcpy(dest, p + start_str_len, last_p - (p + start_str_len) + 1); return dest; } } p += strlen(p) + 1; } } return NULL; } static const char *receive_command_response(urg_t *urg, char *buffer, int buffer_size, const char* command, int response_lines) { const int vv_expected[] = { 0, EXPECTED_END }; int ret; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = scip_response(urg, command, vv_expected, urg->timeout, buffer, buffer_size); if (ret < response_lines) { return RECEIVE_ERROR_MESSAGE; } return NULL; } const char *urg_sensor_product_type(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROD:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_serial_id(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "SERI:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_vendor(urg_t *urg){ enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "VEND:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", "(", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_date(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } // Get the firmware version and append a '(', this will be what's before the date char firmware_version[50]; strcpy(firmware_version, urg_sensor_firmware_version(urg)); strcat(firmware_version, "("); ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } // Strip out the actual date from between the '(' and ')' char *date; p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", ";", VV_RESPONSE_LINES); date = copy_token(urg->return_buffer, p, firmware_version, ")", 1); return (date) ? date : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_protocol_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROT:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_status(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "STAT:", ";", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_state(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "MESM:", " (", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } void urg_set_error_handler(urg_t *urg, urg_error_handler handler) { urg->error_handler = handler; }
528  int i;
529  if (length) {
530  for (i = 1; i < multiecho_max_size; ++i) {
531  length[index + i] = 0;
532  }
533  }
534  if (intensity) {
535  for (i = 1; i < multiecho_max_size; ++i) {
536  intensity[index + i] = 0;
537  }
538  }
539  }
540 
541  // 距離データの格納
542  if (length) {
543  length[index] = urg_scip_decode(p, 3);
544  }
545  p += 3;
546 
547  // 強度データの格納
548  if (is_intensity) {
549  if (intensity) {
550  intensity[index] = (unsigned short)urg_scip_decode(p, 3);
551  }
552  p += 3;
553  }
554 
555  ++step_filled;
556  line_filled -= data_size;
557  }
558 
559  // 次に処理する文字を退避 memmove(buffer, p, line_filled); } while (n > 0); return step_filled; } //! 距離データの取得 static int receive_data(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { urg_measurement_type_t type; char buffer[BUFFER_SIZE]; int ret = 0; int n; int extended_timeout = urg->timeout + 2 * (urg->scan_usec * (urg->scanning_skip_scan) / 1000); // エコーバックの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, extended_timeout); if (n <= 0) { return set_errno_and_return(urg, URG_NO_RESPONSE); } // エコーバックの解析 type = parse_distance_echoback(urg, buffer); // 応答の取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 3) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } if (buffer[n - 1] != scip_checksum(buffer, n - 1)) { // チェックサムの評価 ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } if (type == URG_STOP) { // QT 応答の場合には、最後の改行を読み捨て、正常応答として処理する n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n == 0) { return 0; } else { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } if (urg->specified_scan_times != 1) { if (!strncmp(buffer, "00", 2)) { // "00" 応答の場合は、エコーバック応答とみなし、 // 最後の空行を読み捨て、次からのデータを返す n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 0) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } } } if (((urg->specified_scan_times == 1) && (strncmp(buffer, "00", 2))) || ((urg->specified_scan_times != 1) && (strncmp(buffer, "99", 2)))) { if (urg->error_handler) { type = urg->error_handler(buffer, urg); } if (type == URG_UNKNOWN) { // Gx, Hx のときは 00P が返されたときがデータ // Mx, Nx のときは 99b が返されたときがデータ ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } // タイムスタンプの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n > 0) { if (time_stamp) { *time_stamp = urg_scip_decode(buffer, 4); } if (system_time_stamp) { urg_get_walltime(system_time_stamp); } } // データの取得 switch (type) { case URG_DISTANCE: case URG_MULTIECHO: ret = receive_length_data(urg, data, NULL, type, buffer); break; case URG_DISTANCE_INTENSITY: case URG_MULTIECHO_INTENSITY: ret = receive_length_data(urg, data, intensity, type, buffer); break; case URG_STOP: case URG_UNKNOWN: ret = 0; break; } // specified_scan_times == 1 のときは Gx 系コマンドが使われるため // データを明示的に停止しなくてよい if ((urg->specified_scan_times > 1) && (urg->scanning_remain_times > 0)) { if (--urg->scanning_remain_times <= 0) { // データの停止のみを行う urg_stop_measurement(urg); } } return ret; } int urg_open(urg_t *urg, urg_connection_type_t connection_type, const char *device_or_address, long baudrate_or_port) { int ret; long baudrate = baudrate_or_port; urg->is_active = URG_FALSE; urg->is_sending = URG_TRUE; urg->last_errno = URG_NOT_CONNECTED; urg->timeout = MAX_TIMEOUT; urg->scanning_skip_scan = 0; urg->error_handler = NULL; // デバイスへの接続 ret = connection_open(&urg->connection, connection_type, device_or_address, baudrate_or_port); if (ret < 0) { switch (connection_type) { case URG_SERIAL: urg->last_errno = URG_SERIAL_OPEN_ERROR; break; case URG_ETHERNET: urg->last_errno = URG_ETHERNET_OPEN_ERROR; break; default: urg->last_errno = URG_INVALID_RESPONSE; break; } return urg->last_errno; } // 指定したボーレートで URG と通信できるように調整 if (connection_type == URG_ETHERNET) { // Ethernet のときは仮の通信速度を指定しておく baudrate = 115200; } if (connect_urg_device(urg, baudrate) != URG_NO_ERROR) { return set_errno_and_return(urg, ret); } urg->is_sending = URG_FALSE; // 変数の初期化 urg->last_errno = URG_NO_ERROR; urg->range_data_byte = URG_COMMUNICATION_3_BYTE; urg->specified_scan_times = 0; urg->scanning_remain_times = 0; urg->is_laser_on = URG_FALSE; // パラメータ情報を取得 ret = receive_parameter(urg); if (ret == URG_NO_ERROR) { urg->is_active = URG_TRUE; } return ret; } void urg_close(urg_t *urg) { if (urg->is_active) { ignore_receive_data_with_qt(urg, urg->timeout); } connection_close(&urg->connection); urg->is_active = URG_FALSE; } void urg_set_timeout_msec(urg_t *urg, int msec) { urg->timeout = msec; } int urg_start_time_stamp_mode(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM0 を発行する n = scip_response(urg, "TM0\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } long urg_time_stamp(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; char buffer[BUFFER_SIZE]; char *p; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } ret = scip_response(urg, "TM1\n", expected, urg->timeout, buffer, BUFFER_SIZE); if (ret < 0) { return ret; } // buffer からタイムスタンプを取得し、デコードして返す if (strcmp(buffer, "00P")) { // 最初の応答が "00P" でなければ戻る return set_errno_and_return(urg, URG_RECEIVE_ERROR); } p = buffer + 4; if (strlen(p) != 5) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (p[5] == scip_checksum(p, 4)) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } return urg_scip_decode(p, 4); } int urg_stop_time_stamp_mode(urg_t *urg) { int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM2 を発行する n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } static int send_distance_command(urg_t *urg, int scan_times, int skip_scan, char single_scan_ch, char continuous_scan_ch, char scan_type_ch) { char buffer[BUFFER_SIZE]; int write_size = 0; int front_index = urg->front_data_index; int n; urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times; urg->scanning_remain_times = urg->specified_scan_times; urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan; if (scan_times >= 100) { // 計測回数が 99 を越える場合は、無限回のスキャンを行う urg->specified_scan_times = 0; } if (urg->scanning_remain_times == 1) { // レーザ発光を指示 urg_laser_on(urg); write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n", single_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step); } else { write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n", continuous_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step, skip_scan, urg->specified_scan_times); urg->is_sending = URG_TRUE; } n = connection_write(&urg->connection, buffer, write_size); if (n != write_size) { return set_errno_and_return(urg, URG_SEND_ERROR); } return 0; } int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan) { char range_byte_ch; int ret = 0; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((skip_scan < 0) || (skip_scan > 9)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // !!! Mx 系, Nx 系の計測中のときは、QT を発行してから // !!! 計測開始コマンドを送信するようにする // !!! ただし、MD 計測中に MD を発行するように、同じコマンドの場合は // !!! Mx 系, Nx 系の計測は上書きすることができるようにする // 指定されたタイプのパケットを生成し、送信する switch (type) { case URG_DISTANCE: range_byte_ch = (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D'; ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', range_byte_ch); break; case URG_DISTANCE_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', 'E'); break; case URG_MULTIECHO: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'D'); break; case URG_MULTIECHO_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'E'); break; case URG_STOP: case URG_UNKNOWN: default: ignore_receive_data_with_qt(urg, urg->timeout); urg->last_errno = URG_INVALID_PARAMETER; ret = urg->last_errno; break; } return ret; } int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, NULL, time_stamp, system_time_stamp); } int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp); } int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp); } int urg_stop_measurement(urg_t *urg) { enum { MAX_READ_TIMES = 3 }; int ret = URG_INVALID_RESPONSE; int n; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // QT を発行する n = connection_write(&urg->connection, "QT\n", 3); if (n != 3) { return set_errno_and_return(urg, URG_SEND_ERROR); } for (i = 0; i < MAX_READ_TIMES; ++i) { // QT の応答が返されるまで、距離データを読み捨てる ret = receive_data(urg, NULL, NULL, NULL, NULL); if (ret == URG_NO_ERROR) { // 正常応答 urg->is_laser_on = URG_FALSE; urg->is_sending = URG_FALSE; return set_errno_and_return(urg, URG_NO_ERROR); } } return ret; } int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step) { // 設定の範囲外を指定したときは、エラーを返す if (((skip_step < 0) || (skip_step >= 100)) || (first_step > last_step) || (first_step < -urg->front_data_index) || (last_step > (urg->last_data_index - urg->front_data_index))) { return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR); } urg->scanning_first_step = first_step; urg->scanning_last_step = last_step; urg->scanning_skip_step = skip_step; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_set_connection_data_size(urg_t *urg, urg_range_data_byte_t data_byte) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((data_byte != URG_COMMUNICATION_3_BYTE) || (data_byte != URG_COMMUNICATION_2_BYTE)) { return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR); } urg->range_data_byte = data_byte; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_laser_on(urg_t *urg) { int expected[] = { 0, 2, EXPECTED_END }; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if (urg->is_laser_on != URG_FALSE) { // 既にレーザが発光しているときは、コマンドを送信しないようにする urg->last_errno = 0; return urg->last_errno; } ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0); if (ret >= 0) { urg->is_laser_on = URG_TRUE; ret = 0; } return ret; } int urg_laser_off(urg_t *urg) { return urg_stop_measurement(urg); } int urg_reboot(urg_t *urg) { int expected[] = { 0, 1, EXPECTED_END }; int ret; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // ２回目の RB 送信後、接続を切断する for (i = 0; i < 2; ++i) { ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0); if (ret < 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } urg->is_active = URG_FALSE; urg_close(urg); urg->last_errno = 0; return urg->last_errno; } void urg_sleep(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = 4 }; int sl_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE]; if (urg_stop_measurement(urg) != URG_NO_ERROR) { return; } scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); } void urg_wakeup(urg_t *urg) { urg_stop_measurement(urg); } int urg_is_stable(urg_t *urg) { const char *stat = urg_sensor_status(urg); return strncmp("Stable", stat, 6) ? 0 : 1; } static char *copy_token(char *dest, char *receive_buffer, const char *start_str, const char *end_ch, int lines) { size_t start_str_len = strlen(start_str); size_t end_ch_len = strlen(end_ch); int i; size_t j; for (j = 0; j < end_ch_len; ++j) { const char *p = receive_buffer; for (i = 0; i < lines; ++i) { if (!strncmp(p, start_str, start_str_len)) { char *last_p = strchr(p + start_str_len, end_ch[j]); if (last_p) { *last_p = '\0'; memcpy(dest, p + start_str_len, last_p - (p + start_str_len) + 1); return dest; } } p += strlen(p) + 1; } } return NULL; } static const char *receive_command_response(urg_t *urg, char *buffer, int buffer_size, const char* command, int response_lines) { const int vv_expected[] = { 0, EXPECTED_END }; int ret; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = scip_response(urg, command, vv_expected, urg->timeout, buffer, buffer_size); if (ret < response_lines) { return RECEIVE_ERROR_MESSAGE; } return NULL; } const char *urg_sensor_product_type(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROD:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_serial_id(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "SERI:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_vendor(urg_t *urg){ enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "VEND:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", "(", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_date(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } // Get the firmware version and append a '(', this will be what's before the date char firmware_version[50]; strcpy(firmware_version, urg_sensor_firmware_version(urg)); strcat(firmware_version, "("); ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } // Strip out the actual date from between the '(' and ')' char *date; p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", ";", VV_RESPONSE_LINES); date = copy_token(urg->return_buffer, p, firmware_version, ")", 1); return (date) ? date : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_protocol_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROT:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_status(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "STAT:", ";", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_state(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "MESM:", " (", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } void urg_set_error_handler(urg_t *urg, urg_error_handler handler) { urg->error_handler = handler; }
560  memmove(buffer, p, line_filled);
561  } while (n > 0);
562 
563  return step_filled;
564 }
565 
566 
568 static int receive_data(urg_t *urg, long data[], unsigned short intensity[],
569  long *time_stamp, unsigned long long *system_time_stamp)
570 {
571  urg_measurement_type_t type;
572  char buffer[BUFFER_SIZE];
573  int ret = 0;
574  int n;
575  int extended_timeout = urg->timeout
576  + 2 * (urg->scan_usec * (urg->scanning_skip_scan) / 1000);
577 
578  // エコーバックの取得
579  n = connection_readline(&urg->connection,
580  buffer, BUFFER_SIZE, extended_timeout);
581  if (n <= 0) {
582  return set_errno_and_return(urg, URG_NO_RESPONSE);
583  }
584  // エコーバックの解析 type = parse_distance_echoback(urg, buffer); // 応答の取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 3) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } if (buffer[n - 1] != scip_checksum(buffer, n - 1)) { // チェックサムの評価 ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } if (type == URG_STOP) { // QT 応答の場合には、最後の改行を読み捨て、正常応答として処理する n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n == 0) { return 0; } else { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } if (urg->specified_scan_times != 1) { if (!strncmp(buffer, "00", 2)) { // "00" 応答の場合は、エコーバック応答とみなし、 // 最後の空行を読み捨て、次からのデータを返す n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 0) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } } } if (((urg->specified_scan_times == 1) && (strncmp(buffer, "00", 2))) || ((urg->specified_scan_times != 1) && (strncmp(buffer, "99", 2)))) { if (urg->error_handler) { type = urg->error_handler(buffer, urg); } if (type == URG_UNKNOWN) { // Gx, Hx のときは 00P が返されたときがデータ // Mx, Nx のときは 99b が返されたときがデータ ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } // タイムスタンプの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n > 0) { if (time_stamp) { *time_stamp = urg_scip_decode(buffer, 4); } if (system_time_stamp) { urg_get_walltime(system_time_stamp); } } // データの取得 switch (type) { case URG_DISTANCE: case URG_MULTIECHO: ret = receive_length_data(urg, data, NULL, type, buffer); break; case URG_DISTANCE_INTENSITY: case URG_MULTIECHO_INTENSITY: ret = receive_length_data(urg, data, intensity, type, buffer); break; case URG_STOP: case URG_UNKNOWN: ret = 0; break; } // specified_scan_times == 1 のときは Gx 系コマンドが使われるため // データを明示的に停止しなくてよい if ((urg->specified_scan_times > 1) && (urg->scanning_remain_times > 0)) { if (--urg->scanning_remain_times <= 0) { // データの停止のみを行う urg_stop_measurement(urg); } } return ret; } int urg_open(urg_t *urg, urg_connection_type_t connection_type, const char *device_or_address, long baudrate_or_port) { int ret; long baudrate = baudrate_or_port; urg->is_active = URG_FALSE; urg->is_sending = URG_TRUE; urg->last_errno = URG_NOT_CONNECTED; urg->timeout = MAX_TIMEOUT; urg->scanning_skip_scan = 0; urg->error_handler = NULL; // デバイスへの接続 ret = connection_open(&urg->connection, connection_type, device_or_address, baudrate_or_port); if (ret < 0) { switch (connection_type) { case URG_SERIAL: urg->last_errno = URG_SERIAL_OPEN_ERROR; break; case URG_ETHERNET: urg->last_errno = URG_ETHERNET_OPEN_ERROR; break; default: urg->last_errno = URG_INVALID_RESPONSE; break; } return urg->last_errno; } // 指定したボーレートで URG と通信できるように調整 if (connection_type == URG_ETHERNET) { // Ethernet のときは仮の通信速度を指定しておく baudrate = 115200; } if (connect_urg_device(urg, baudrate) != URG_NO_ERROR) { return set_errno_and_return(urg, ret); } urg->is_sending = URG_FALSE; // 変数の初期化 urg->last_errno = URG_NO_ERROR; urg->range_data_byte = URG_COMMUNICATION_3_BYTE; urg->specified_scan_times = 0; urg->scanning_remain_times = 0; urg->is_laser_on = URG_FALSE; // パラメータ情報を取得 ret = receive_parameter(urg); if (ret == URG_NO_ERROR) { urg->is_active = URG_TRUE; } return ret; } void urg_close(urg_t *urg) { if (urg->is_active) { ignore_receive_data_with_qt(urg, urg->timeout); } connection_close(&urg->connection); urg->is_active = URG_FALSE; } void urg_set_timeout_msec(urg_t *urg, int msec) { urg->timeout = msec; } int urg_start_time_stamp_mode(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM0 を発行する n = scip_response(urg, "TM0\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } long urg_time_stamp(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; char buffer[BUFFER_SIZE]; char *p; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } ret = scip_response(urg, "TM1\n", expected, urg->timeout, buffer, BUFFER_SIZE); if (ret < 0) { return ret; } // buffer からタイムスタンプを取得し、デコードして返す if (strcmp(buffer, "00P")) { // 最初の応答が "00P" でなければ戻る return set_errno_and_return(urg, URG_RECEIVE_ERROR); } p = buffer + 4; if (strlen(p) != 5) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (p[5] == scip_checksum(p, 4)) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } return urg_scip_decode(p, 4); } int urg_stop_time_stamp_mode(urg_t *urg) { int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM2 を発行する n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } static int send_distance_command(urg_t *urg, int scan_times, int skip_scan, char single_scan_ch, char continuous_scan_ch, char scan_type_ch) { char buffer[BUFFER_SIZE]; int write_size = 0; int front_index = urg->front_data_index; int n; urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times; urg->scanning_remain_times = urg->specified_scan_times; urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan; if (scan_times >= 100) { // 計測回数が 99 を越える場合は、無限回のスキャンを行う urg->specified_scan_times = 0; } if (urg->scanning_remain_times == 1) { // レーザ発光を指示 urg_laser_on(urg); write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n", single_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step); } else { write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n", continuous_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step, skip_scan, urg->specified_scan_times); urg->is_sending = URG_TRUE; } n = connection_write(&urg->connection, buffer, write_size); if (n != write_size) { return set_errno_and_return(urg, URG_SEND_ERROR); } return 0; } int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan) { char range_byte_ch; int ret = 0; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((skip_scan < 0) || (skip_scan > 9)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // !!! Mx 系, Nx 系の計測中のときは、QT を発行してから // !!! 計測開始コマンドを送信するようにする // !!! ただし、MD 計測中に MD を発行するように、同じコマンドの場合は // !!! Mx 系, Nx 系の計測は上書きすることができるようにする // 指定されたタイプのパケットを生成し、送信する switch (type) { case URG_DISTANCE: range_byte_ch = (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D'; ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', range_byte_ch); break; case URG_DISTANCE_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', 'E'); break; case URG_MULTIECHO: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'D'); break; case URG_MULTIECHO_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'E'); break; case URG_STOP: case URG_UNKNOWN: default: ignore_receive_data_with_qt(urg, urg->timeout); urg->last_errno = URG_INVALID_PARAMETER; ret = urg->last_errno; break; } return ret; } int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, NULL, time_stamp, system_time_stamp); } int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp); } int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp); } int urg_stop_measurement(urg_t *urg) { enum { MAX_READ_TIMES = 3 }; int ret = URG_INVALID_RESPONSE; int n; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // QT を発行する n = connection_write(&urg->connection, "QT\n", 3); if (n != 3) { return set_errno_and_return(urg, URG_SEND_ERROR); } for (i = 0; i < MAX_READ_TIMES; ++i) { // QT の応答が返されるまで、距離データを読み捨てる ret = receive_data(urg, NULL, NULL, NULL, NULL); if (ret == URG_NO_ERROR) { // 正常応答 urg->is_laser_on = URG_FALSE; urg->is_sending = URG_FALSE; return set_errno_and_return(urg, URG_NO_ERROR); } } return ret; } int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step) { // 設定の範囲外を指定したときは、エラーを返す if (((skip_step < 0) || (skip_step >= 100)) || (first_step > last_step) || (first_step < -urg->front_data_index) || (last_step > (urg->last_data_index - urg->front_data_index))) { return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR); } urg->scanning_first_step = first_step; urg->scanning_last_step = last_step; urg->scanning_skip_step = skip_step; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_set_connection_data_size(urg_t *urg, urg_range_data_byte_t data_byte) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((data_byte != URG_COMMUNICATION_3_BYTE) || (data_byte != URG_COMMUNICATION_2_BYTE)) { return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR); } urg->range_data_byte = data_byte; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_laser_on(urg_t *urg) { int expected[] = { 0, 2, EXPECTED_END }; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if (urg->is_laser_on != URG_FALSE) { // 既にレーザが発光しているときは、コマンドを送信しないようにする urg->last_errno = 0; return urg->last_errno; } ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0); if (ret >= 0) { urg->is_laser_on = URG_TRUE; ret = 0; } return ret; } int urg_laser_off(urg_t *urg) { return urg_stop_measurement(urg); } int urg_reboot(urg_t *urg) { int expected[] = { 0, 1, EXPECTED_END }; int ret; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // ２回目の RB 送信後、接続を切断する for (i = 0; i < 2; ++i) { ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0); if (ret < 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } urg->is_active = URG_FALSE; urg_close(urg); urg->last_errno = 0; return urg->last_errno; } void urg_sleep(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = 4 }; int sl_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE]; if (urg_stop_measurement(urg) != URG_NO_ERROR) { return; } scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); } void urg_wakeup(urg_t *urg) { urg_stop_measurement(urg); } int urg_is_stable(urg_t *urg) { const char *stat = urg_sensor_status(urg); return strncmp("Stable", stat, 6) ? 0 : 1; } static char *copy_token(char *dest, char *receive_buffer, const char *start_str, const char *end_ch, int lines) { size_t start_str_len = strlen(start_str); size_t end_ch_len = strlen(end_ch); int i; size_t j; for (j = 0; j < end_ch_len; ++j) { const char *p = receive_buffer; for (i = 0; i < lines; ++i) { if (!strncmp(p, start_str, start_str_len)) { char *last_p = strchr(p + start_str_len, end_ch[j]); if (last_p) { *last_p = '\0'; memcpy(dest, p + start_str_len, last_p - (p + start_str_len) + 1); return dest; } } p += strlen(p) + 1; } } return NULL; } static const char *receive_command_response(urg_t *urg, char *buffer, int buffer_size, const char* command, int response_lines) { const int vv_expected[] = { 0, EXPECTED_END }; int ret; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = scip_response(urg, command, vv_expected, urg->timeout, buffer, buffer_size); if (ret < response_lines) { return RECEIVE_ERROR_MESSAGE; } return NULL; } const char *urg_sensor_product_type(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROD:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_serial_id(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "SERI:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_vendor(urg_t *urg){ enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "VEND:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", "(", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_date(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } // Get the firmware version and append a '(', this will be what's before the date char firmware_version[50]; strcpy(firmware_version, urg_sensor_firmware_version(urg)); strcat(firmware_version, "("); ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } // Strip out the actual date from between the '(' and ')' char *date; p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", ";", VV_RESPONSE_LINES); date = copy_token(urg->return_buffer, p, firmware_version, ")", 1); return (date) ? date : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_protocol_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROT:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_status(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "STAT:", ";", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_state(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "MESM:", " (", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } void urg_set_error_handler(urg_t *urg, urg_error_handler handler) { urg->error_handler = handler; }
585  type = parse_distance_echoback(urg, buffer);
586 
587  // 応答の取得
588  n = connection_readline(&urg->connection,
589  buffer, BUFFER_SIZE, urg->timeout);
590  if (n != 3) {
591  ignore_receive_data_with_qt(urg, urg->timeout);
592  return set_errno_and_return(urg, URG_INVALID_RESPONSE);
593  }
594 
595  if (buffer[n - 1] != scip_checksum(buffer, n - 1)) {
596  // チェックサムの評価
597  ignore_receive_data_with_qt(urg, urg->timeout);
598  return set_errno_and_return(urg, URG_CHECKSUM_ERROR);
599  }
600 
601  if (type == URG_STOP) {
602  // QT 応答の場合には、最後の改行を読み捨て、正常応答として処理する n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n == 0) { return 0; } else { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } if (urg->specified_scan_times != 1) { if (!strncmp(buffer, "00", 2)) { // "00" 応答の場合は、エコーバック応答とみなし、 // 最後の空行を読み捨て、次からのデータを返す n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n != 0) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } } } if (((urg->specified_scan_times == 1) && (strncmp(buffer, "00", 2))) || ((urg->specified_scan_times != 1) && (strncmp(buffer, "99", 2)))) { if (urg->error_handler) { type = urg->error_handler(buffer, urg); } if (type == URG_UNKNOWN) { // Gx, Hx のときは 00P が返されたときがデータ // Mx, Nx のときは 99b が返されたときがデータ ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } // タイムスタンプの取得 n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, urg->timeout); if (n > 0) { if (time_stamp) { *time_stamp = urg_scip_decode(buffer, 4); } if (system_time_stamp) { urg_get_walltime(system_time_stamp); } } // データの取得 switch (type) { case URG_DISTANCE: case URG_MULTIECHO: ret = receive_length_data(urg, data, NULL, type, buffer); break; case URG_DISTANCE_INTENSITY: case URG_MULTIECHO_INTENSITY: ret = receive_length_data(urg, data, intensity, type, buffer); break; case URG_STOP: case URG_UNKNOWN: ret = 0; break; } // specified_scan_times == 1 のときは Gx 系コマンドが使われるため // データを明示的に停止しなくてよい if ((urg->specified_scan_times > 1) && (urg->scanning_remain_times > 0)) { if (--urg->scanning_remain_times <= 0) { // データの停止のみを行う urg_stop_measurement(urg); } } return ret; } int urg_open(urg_t *urg, urg_connection_type_t connection_type, const char *device_or_address, long baudrate_or_port) { int ret; long baudrate = baudrate_or_port; urg->is_active = URG_FALSE; urg->is_sending = URG_TRUE; urg->last_errno = URG_NOT_CONNECTED; urg->timeout = MAX_TIMEOUT; urg->scanning_skip_scan = 0; urg->error_handler = NULL; // デバイスへの接続 ret = connection_open(&urg->connection, connection_type, device_or_address, baudrate_or_port); if (ret < 0) { switch (connection_type) { case URG_SERIAL: urg->last_errno = URG_SERIAL_OPEN_ERROR; break; case URG_ETHERNET: urg->last_errno = URG_ETHERNET_OPEN_ERROR; break; default: urg->last_errno = URG_INVALID_RESPONSE; break; } return urg->last_errno; } // 指定したボーレートで URG と通信できるように調整 if (connection_type == URG_ETHERNET) { // Ethernet のときは仮の通信速度を指定しておく baudrate = 115200; } if (connect_urg_device(urg, baudrate) != URG_NO_ERROR) { return set_errno_and_return(urg, ret); } urg->is_sending = URG_FALSE; // 変数の初期化 urg->last_errno = URG_NO_ERROR; urg->range_data_byte = URG_COMMUNICATION_3_BYTE; urg->specified_scan_times = 0; urg->scanning_remain_times = 0; urg->is_laser_on = URG_FALSE; // パラメータ情報を取得 ret = receive_parameter(urg); if (ret == URG_NO_ERROR) { urg->is_active = URG_TRUE; } return ret; } void urg_close(urg_t *urg) { if (urg->is_active) { ignore_receive_data_with_qt(urg, urg->timeout); } connection_close(&urg->connection); urg->is_active = URG_FALSE; } void urg_set_timeout_msec(urg_t *urg, int msec) { urg->timeout = msec; } int urg_start_time_stamp_mode(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM0 を発行する n = scip_response(urg, "TM0\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } long urg_time_stamp(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; char buffer[BUFFER_SIZE]; char *p; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } ret = scip_response(urg, "TM1\n", expected, urg->timeout, buffer, BUFFER_SIZE); if (ret < 0) { return ret; } // buffer からタイムスタンプを取得し、デコードして返す if (strcmp(buffer, "00P")) { // 最初の応答が "00P" でなければ戻る return set_errno_and_return(urg, URG_RECEIVE_ERROR); } p = buffer + 4; if (strlen(p) != 5) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (p[5] == scip_checksum(p, 4)) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } return urg_scip_decode(p, 4); } int urg_stop_time_stamp_mode(urg_t *urg) { int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM2 を発行する n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } static int send_distance_command(urg_t *urg, int scan_times, int skip_scan, char single_scan_ch, char continuous_scan_ch, char scan_type_ch) { char buffer[BUFFER_SIZE]; int write_size = 0; int front_index = urg->front_data_index; int n; urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times; urg->scanning_remain_times = urg->specified_scan_times; urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan; if (scan_times >= 100) { // 計測回数が 99 を越える場合は、無限回のスキャンを行う urg->specified_scan_times = 0; } if (urg->scanning_remain_times == 1) { // レーザ発光を指示 urg_laser_on(urg); write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n", single_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step); } else { write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n", continuous_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step, skip_scan, urg->specified_scan_times); urg->is_sending = URG_TRUE; } n = connection_write(&urg->connection, buffer, write_size); if (n != write_size) { return set_errno_and_return(urg, URG_SEND_ERROR); } return 0; } int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan) { char range_byte_ch; int ret = 0; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((skip_scan < 0) || (skip_scan > 9)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // !!! Mx 系, Nx 系の計測中のときは、QT を発行してから // !!! 計測開始コマンドを送信するようにする // !!! ただし、MD 計測中に MD を発行するように、同じコマンドの場合は // !!! Mx 系, Nx 系の計測は上書きすることができるようにする // 指定されたタイプのパケットを生成し、送信する switch (type) { case URG_DISTANCE: range_byte_ch = (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D'; ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', range_byte_ch); break; case URG_DISTANCE_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', 'E'); break; case URG_MULTIECHO: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'D'); break; case URG_MULTIECHO_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'E'); break; case URG_STOP: case URG_UNKNOWN: default: ignore_receive_data_with_qt(urg, urg->timeout); urg->last_errno = URG_INVALID_PARAMETER; ret = urg->last_errno; break; } return ret; } int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, NULL, time_stamp, system_time_stamp); } int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp); } int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp); } int urg_stop_measurement(urg_t *urg) { enum { MAX_READ_TIMES = 3 }; int ret = URG_INVALID_RESPONSE; int n; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // QT を発行する n = connection_write(&urg->connection, "QT\n", 3); if (n != 3) { return set_errno_and_return(urg, URG_SEND_ERROR); } for (i = 0; i < MAX_READ_TIMES; ++i) { // QT の応答が返されるまで、距離データを読み捨てる ret = receive_data(urg, NULL, NULL, NULL, NULL); if (ret == URG_NO_ERROR) { // 正常応答 urg->is_laser_on = URG_FALSE; urg->is_sending = URG_FALSE; return set_errno_and_return(urg, URG_NO_ERROR); } } return ret; } int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step) { // 設定の範囲外を指定したときは、エラーを返す if (((skip_step < 0) || (skip_step >= 100)) || (first_step > last_step) || (first_step < -urg->front_data_index) || (last_step > (urg->last_data_index - urg->front_data_index))) { return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR); } urg->scanning_first_step = first_step; urg->scanning_last_step = last_step; urg->scanning_skip_step = skip_step; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_set_connection_data_size(urg_t *urg, urg_range_data_byte_t data_byte) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((data_byte != URG_COMMUNICATION_3_BYTE) || (data_byte != URG_COMMUNICATION_2_BYTE)) { return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR); } urg->range_data_byte = data_byte; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_laser_on(urg_t *urg) { int expected[] = { 0, 2, EXPECTED_END }; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if (urg->is_laser_on != URG_FALSE) { // 既にレーザが発光しているときは、コマンドを送信しないようにする urg->last_errno = 0; return urg->last_errno; } ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0); if (ret >= 0) { urg->is_laser_on = URG_TRUE; ret = 0; } return ret; } int urg_laser_off(urg_t *urg) { return urg_stop_measurement(urg); } int urg_reboot(urg_t *urg) { int expected[] = { 0, 1, EXPECTED_END }; int ret; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // ２回目の RB 送信後、接続を切断する for (i = 0; i < 2; ++i) { ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0); if (ret < 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } urg->is_active = URG_FALSE; urg_close(urg); urg->last_errno = 0; return urg->last_errno; } void urg_sleep(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = 4 }; int sl_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE]; if (urg_stop_measurement(urg) != URG_NO_ERROR) { return; } scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); } void urg_wakeup(urg_t *urg) { urg_stop_measurement(urg); } int urg_is_stable(urg_t *urg) { const char *stat = urg_sensor_status(urg); return strncmp("Stable", stat, 6) ? 0 : 1; } static char *copy_token(char *dest, char *receive_buffer, const char *start_str, const char *end_ch, int lines) { size_t start_str_len = strlen(start_str); size_t end_ch_len = strlen(end_ch); int i; size_t j; for (j = 0; j < end_ch_len; ++j) { const char *p = receive_buffer; for (i = 0; i < lines; ++i) { if (!strncmp(p, start_str, start_str_len)) { char *last_p = strchr(p + start_str_len, end_ch[j]); if (last_p) { *last_p = '\0'; memcpy(dest, p + start_str_len, last_p - (p + start_str_len) + 1); return dest; } } p += strlen(p) + 1; } } return NULL; } static const char *receive_command_response(urg_t *urg, char *buffer, int buffer_size, const char* command, int response_lines) { const int vv_expected[] = { 0, EXPECTED_END }; int ret; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = scip_response(urg, command, vv_expected, urg->timeout, buffer, buffer_size); if (ret < response_lines) { return RECEIVE_ERROR_MESSAGE; } return NULL; } const char *urg_sensor_product_type(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROD:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_serial_id(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "SERI:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_vendor(urg_t *urg){ enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "VEND:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", "(", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_date(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } // Get the firmware version and append a '(', this will be what's before the date char firmware_version[50]; strcpy(firmware_version, urg_sensor_firmware_version(urg)); strcat(firmware_version, "("); ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } // Strip out the actual date from between the '(' and ')' char *date; p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", ";", VV_RESPONSE_LINES); date = copy_token(urg->return_buffer, p, firmware_version, ")", 1); return (date) ? date : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_protocol_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROT:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_status(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "STAT:", ";", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_state(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "MESM:", " (", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } void urg_set_error_handler(urg_t *urg, urg_error_handler handler) { urg->error_handler = handler; }
603  n = connection_readline(&urg->connection,
604  buffer, BUFFER_SIZE, urg->timeout);
605  if (n == 0) {
606  return 0;
607  } else {
608  return set_errno_and_return(urg, URG_INVALID_RESPONSE);
609  }
610  }
611 
612  if (urg->specified_scan_times != 1) {
613  if (!strncmp(buffer, "00", 2)) {
614  // "00" 応答の場合は、エコーバック応答とみなし、
615  // 最後の空行を読み捨て、次からのデータを返す
616  n = connection_readline(&urg->connection,
617  buffer, BUFFER_SIZE, urg->timeout);
618 
619  if (n != 0) {
620  ignore_receive_data_with_qt(urg, urg->timeout);
621  return set_errno_and_return(urg, URG_INVALID_RESPONSE);
622  } else {
623  return receive_data(urg, data, intensity, time_stamp, system_time_stamp);
624  }
625  }
626  }
627 
628  if (((urg->specified_scan_times == 1) && (strncmp(buffer, "00", 2))) ||
629  ((urg->specified_scan_times != 1) && (strncmp(buffer, "99", 2)))) {
630  if (urg->error_handler) {
631  type = urg->error_handler(buffer, urg);
632  }
633 
634  if (type == URG_UNKNOWN) {
635  // Gx, Hx のときは 00P が返されたときがデータ
636  // Mx, Nx のときは 99b が返されたときがデータ
637  ignore_receive_data_with_qt(urg, urg->timeout);
638  return set_errno_and_return(urg, URG_INVALID_RESPONSE);
639  }
640  }
641 
642  // タイムスタンプの取得
643  n = connection_readline(&urg->connection,
644  buffer, BUFFER_SIZE, urg->timeout);
645  if (n > 0) {
646  if (time_stamp) {
647  *time_stamp = urg_scip_decode(buffer, 4);
648  }
649  if (system_time_stamp) {
650  urg_get_walltime(system_time_stamp);
651  }
652  }
653 
654  // データの取得
655  switch (type) {
656  case URG_DISTANCE:
657  case URG_MULTIECHO:
658  ret = receive_length_data(urg, data, NULL, type, buffer);
659  break;
660 
661  case URG_DISTANCE_INTENSITY:
662  case URG_MULTIECHO_INTENSITY:
663  ret = receive_length_data(urg, data, intensity, type, buffer);
664  break;
665 
666  case URG_STOP:
667  case URG_UNKNOWN:
668  ret = 0;
669  break;
670  }
671 
672  // specified_scan_times == 1 のときは Gx 系コマンドが使われるため // データを明示的に停止しなくてよい if ((urg->specified_scan_times > 1) && (urg->scanning_remain_times > 0)) { if (--urg->scanning_remain_times <= 0) { // データの停止のみを行う urg_stop_measurement(urg); } } return ret; } int urg_open(urg_t *urg, urg_connection_type_t connection_type, const char *device_or_address, long baudrate_or_port) { int ret; long baudrate = baudrate_or_port; urg->is_active = URG_FALSE; urg->is_sending = URG_TRUE; urg->last_errno = URG_NOT_CONNECTED; urg->timeout = MAX_TIMEOUT; urg->scanning_skip_scan = 0; urg->error_handler = NULL; // デバイスへの接続 ret = connection_open(&urg->connection, connection_type, device_or_address, baudrate_or_port); if (ret < 0) { switch (connection_type) { case URG_SERIAL: urg->last_errno = URG_SERIAL_OPEN_ERROR; break; case URG_ETHERNET: urg->last_errno = URG_ETHERNET_OPEN_ERROR; break; default: urg->last_errno = URG_INVALID_RESPONSE; break; } return urg->last_errno; } // 指定したボーレートで URG と通信できるように調整 if (connection_type == URG_ETHERNET) { // Ethernet のときは仮の通信速度を指定しておく baudrate = 115200; } if (connect_urg_device(urg, baudrate) != URG_NO_ERROR) { return set_errno_and_return(urg, ret); } urg->is_sending = URG_FALSE; // 変数の初期化 urg->last_errno = URG_NO_ERROR; urg->range_data_byte = URG_COMMUNICATION_3_BYTE; urg->specified_scan_times = 0; urg->scanning_remain_times = 0; urg->is_laser_on = URG_FALSE; // パラメータ情報を取得 ret = receive_parameter(urg); if (ret == URG_NO_ERROR) { urg->is_active = URG_TRUE; } return ret; } void urg_close(urg_t *urg) { if (urg->is_active) { ignore_receive_data_with_qt(urg, urg->timeout); } connection_close(&urg->connection); urg->is_active = URG_FALSE; } void urg_set_timeout_msec(urg_t *urg, int msec) { urg->timeout = msec; } int urg_start_time_stamp_mode(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM0 を発行する n = scip_response(urg, "TM0\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } long urg_time_stamp(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; char buffer[BUFFER_SIZE]; char *p; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } ret = scip_response(urg, "TM1\n", expected, urg->timeout, buffer, BUFFER_SIZE); if (ret < 0) { return ret; } // buffer からタイムスタンプを取得し、デコードして返す if (strcmp(buffer, "00P")) { // 最初の応答が "00P" でなければ戻る return set_errno_and_return(urg, URG_RECEIVE_ERROR); } p = buffer + 4; if (strlen(p) != 5) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (p[5] == scip_checksum(p, 4)) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } return urg_scip_decode(p, 4); } int urg_stop_time_stamp_mode(urg_t *urg) { int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM2 を発行する n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } static int send_distance_command(urg_t *urg, int scan_times, int skip_scan, char single_scan_ch, char continuous_scan_ch, char scan_type_ch) { char buffer[BUFFER_SIZE]; int write_size = 0; int front_index = urg->front_data_index; int n; urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times; urg->scanning_remain_times = urg->specified_scan_times; urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan; if (scan_times >= 100) { // 計測回数が 99 を越える場合は、無限回のスキャンを行う urg->specified_scan_times = 0; } if (urg->scanning_remain_times == 1) { // レーザ発光を指示 urg_laser_on(urg); write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n", single_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step); } else { write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n", continuous_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step, skip_scan, urg->specified_scan_times); urg->is_sending = URG_TRUE; } n = connection_write(&urg->connection, buffer, write_size); if (n != write_size) { return set_errno_and_return(urg, URG_SEND_ERROR); } return 0; } int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan) { char range_byte_ch; int ret = 0; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((skip_scan < 0) || (skip_scan > 9)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // !!! Mx 系, Nx 系の計測中のときは、QT を発行してから // !!! 計測開始コマンドを送信するようにする // !!! ただし、MD 計測中に MD を発行するように、同じコマンドの場合は // !!! Mx 系, Nx 系の計測は上書きすることができるようにする // 指定されたタイプのパケットを生成し、送信する switch (type) { case URG_DISTANCE: range_byte_ch = (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D'; ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', range_byte_ch); break; case URG_DISTANCE_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', 'E'); break; case URG_MULTIECHO: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'D'); break; case URG_MULTIECHO_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'E'); break; case URG_STOP: case URG_UNKNOWN: default: ignore_receive_data_with_qt(urg, urg->timeout); urg->last_errno = URG_INVALID_PARAMETER; ret = urg->last_errno; break; } return ret; } int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, NULL, time_stamp, system_time_stamp); } int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp); } int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp); } int urg_stop_measurement(urg_t *urg) { enum { MAX_READ_TIMES = 3 }; int ret = URG_INVALID_RESPONSE; int n; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // QT を発行する n = connection_write(&urg->connection, "QT\n", 3); if (n != 3) { return set_errno_and_return(urg, URG_SEND_ERROR); } for (i = 0; i < MAX_READ_TIMES; ++i) { // QT の応答が返されるまで、距離データを読み捨てる ret = receive_data(urg, NULL, NULL, NULL, NULL); if (ret == URG_NO_ERROR) { // 正常応答 urg->is_laser_on = URG_FALSE; urg->is_sending = URG_FALSE; return set_errno_and_return(urg, URG_NO_ERROR); } } return ret; } int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step) { // 設定の範囲外を指定したときは、エラーを返す if (((skip_step < 0) || (skip_step >= 100)) || (first_step > last_step) || (first_step < -urg->front_data_index) || (last_step > (urg->last_data_index - urg->front_data_index))) { return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR); } urg->scanning_first_step = first_step; urg->scanning_last_step = last_step; urg->scanning_skip_step = skip_step; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_set_connection_data_size(urg_t *urg, urg_range_data_byte_t data_byte) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((data_byte != URG_COMMUNICATION_3_BYTE) || (data_byte != URG_COMMUNICATION_2_BYTE)) { return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR); } urg->range_data_byte = data_byte; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_laser_on(urg_t *urg) { int expected[] = { 0, 2, EXPECTED_END }; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if (urg->is_laser_on != URG_FALSE) { // 既にレーザが発光しているときは、コマンドを送信しないようにする urg->last_errno = 0; return urg->last_errno; } ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0); if (ret >= 0) { urg->is_laser_on = URG_TRUE; ret = 0; } return ret; } int urg_laser_off(urg_t *urg) { return urg_stop_measurement(urg); } int urg_reboot(urg_t *urg) { int expected[] = { 0, 1, EXPECTED_END }; int ret; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // ２回目の RB 送信後、接続を切断する for (i = 0; i < 2; ++i) { ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0); if (ret < 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } urg->is_active = URG_FALSE; urg_close(urg); urg->last_errno = 0; return urg->last_errno; } void urg_sleep(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = 4 }; int sl_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE]; if (urg_stop_measurement(urg) != URG_NO_ERROR) { return; } scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); } void urg_wakeup(urg_t *urg) { urg_stop_measurement(urg); } int urg_is_stable(urg_t *urg) { const char *stat = urg_sensor_status(urg); return strncmp("Stable", stat, 6) ? 0 : 1; } static char *copy_token(char *dest, char *receive_buffer, const char *start_str, const char *end_ch, int lines) { size_t start_str_len = strlen(start_str); size_t end_ch_len = strlen(end_ch); int i; size_t j; for (j = 0; j < end_ch_len; ++j) { const char *p = receive_buffer; for (i = 0; i < lines; ++i) { if (!strncmp(p, start_str, start_str_len)) { char *last_p = strchr(p + start_str_len, end_ch[j]); if (last_p) { *last_p = '\0'; memcpy(dest, p + start_str_len, last_p - (p + start_str_len) + 1); return dest; } } p += strlen(p) + 1; } } return NULL; } static const char *receive_command_response(urg_t *urg, char *buffer, int buffer_size, const char* command, int response_lines) { const int vv_expected[] = { 0, EXPECTED_END }; int ret; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = scip_response(urg, command, vv_expected, urg->timeout, buffer, buffer_size); if (ret < response_lines) { return RECEIVE_ERROR_MESSAGE; } return NULL; } const char *urg_sensor_product_type(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROD:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_serial_id(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "SERI:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_vendor(urg_t *urg){ enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "VEND:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", "(", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_date(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } // Get the firmware version and append a '(', this will be what's before the date char firmware_version[50]; strcpy(firmware_version, urg_sensor_firmware_version(urg)); strcat(firmware_version, "("); ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } // Strip out the actual date from between the '(' and ')' char *date; p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", ";", VV_RESPONSE_LINES); date = copy_token(urg->return_buffer, p, firmware_version, ")", 1); return (date) ? date : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_protocol_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROT:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_status(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "STAT:", ";", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_state(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "MESM:", " (", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } void urg_set_error_handler(urg_t *urg, urg_error_handler handler) { urg->error_handler = handler; }
673  // データを明示的に停止しなくてよい
674  if ((urg->specified_scan_times > 1) && (urg->scanning_remain_times > 0)) {
675  if (--urg->scanning_remain_times <= 0) {
676  // データの停止のみを行う
677  urg_stop_measurement(urg);
678  }
679  }
680  return ret;
681 }
682 
683 
684 int urg_open(urg_t *urg, urg_connection_type_t connection_type,
685  const char *device_or_address, long baudrate_or_port)
686 {
687  int ret;
688  long baudrate = baudrate_or_port;
689 
690  urg->is_active = URG_FALSE;
691  urg->is_sending = URG_TRUE;
692  urg->last_errno = URG_NOT_CONNECTED;
693  urg->timeout = MAX_TIMEOUT;
694  urg->scanning_skip_scan = 0;
695  urg->error_handler = NULL;
696 
697  // デバイスへの接続
698  ret = connection_open(&urg->connection, connection_type,
699  device_or_address, baudrate_or_port);
700 
701  if (ret < 0) {
702  switch (connection_type) {
703  case URG_SERIAL:
704  urg->last_errno = URG_SERIAL_OPEN_ERROR;
705  break;
706 
707  case URG_ETHERNET:
708  urg->last_errno = URG_ETHERNET_OPEN_ERROR;
709  break;
710 
711  default:
712  urg->last_errno = URG_INVALID_RESPONSE;
713  break;
714  }
715  return urg->last_errno;
716  }
717 
718  // 指定したボーレートで URG と通信できるように調整
719  if (connection_type == URG_ETHERNET) {
720  // Ethernet のときは仮の通信速度を指定しておく
721  baudrate = 115200;
722  }
723 
724  if (connect_urg_device(urg, baudrate) != URG_NO_ERROR) {
725  return set_errno_and_return(urg, ret);
726  }
727  urg->is_sending = URG_FALSE;
728 
729  // 変数の初期化
730  urg->last_errno = URG_NO_ERROR;
731  urg->range_data_byte = URG_COMMUNICATION_3_BYTE;
732  urg->specified_scan_times = 0;
733  urg->scanning_remain_times = 0;
734  urg->is_laser_on = URG_FALSE;
735 
736  // パラメータ情報を取得
737  ret = receive_parameter(urg);
738  if (ret == URG_NO_ERROR) {
739  urg->is_active = URG_TRUE;
740  }
741  return ret;
742 }
743 
744 
745 void urg_close(urg_t *urg)
746 {
747  if (urg->is_active) {
748  ignore_receive_data_with_qt(urg, urg->timeout);
749  }
750  connection_close(&urg->connection);
751  urg->is_active = URG_FALSE;
752 }
753 
754 
755 void urg_set_timeout_msec(urg_t *urg, int msec)
756 {
757  urg->timeout = msec;
758 }
759 
760 
761 int urg_start_time_stamp_mode(urg_t *urg)
762 {
763  const int expected[] = { 0, EXPECTED_END };
764  int n;
765 
766  if (!urg->is_active) {
767  return set_errno_and_return(urg, URG_NOT_CONNECTED);
768  }
769 
770  // TM0 を発行する n = scip_response(urg, "TM0\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } long urg_time_stamp(urg_t *urg) { const int expected[] = { 0, EXPECTED_END }; char buffer[BUFFER_SIZE]; char *p; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } ret = scip_response(urg, "TM1\n", expected, urg->timeout, buffer, BUFFER_SIZE); if (ret < 0) { return ret; } // buffer からタイムスタンプを取得し、デコードして返す if (strcmp(buffer, "00P")) { // 最初の応答が "00P" でなければ戻る return set_errno_and_return(urg, URG_RECEIVE_ERROR); } p = buffer + 4; if (strlen(p) != 5) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (p[5] == scip_checksum(p, 4)) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } return urg_scip_decode(p, 4); } int urg_stop_time_stamp_mode(urg_t *urg) { int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM2 を発行する n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } static int send_distance_command(urg_t *urg, int scan_times, int skip_scan, char single_scan_ch, char continuous_scan_ch, char scan_type_ch) { char buffer[BUFFER_SIZE]; int write_size = 0; int front_index = urg->front_data_index; int n; urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times; urg->scanning_remain_times = urg->specified_scan_times; urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan; if (scan_times >= 100) { // 計測回数が 99 を越える場合は、無限回のスキャンを行う urg->specified_scan_times = 0; } if (urg->scanning_remain_times == 1) { // レーザ発光を指示 urg_laser_on(urg); write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n", single_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step); } else { write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n", continuous_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step, skip_scan, urg->specified_scan_times); urg->is_sending = URG_TRUE; } n = connection_write(&urg->connection, buffer, write_size); if (n != write_size) { return set_errno_and_return(urg, URG_SEND_ERROR); } return 0; } int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan) { char range_byte_ch; int ret = 0; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((skip_scan < 0) || (skip_scan > 9)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // !!! Mx 系, Nx 系の計測中のときは、QT を発行してから // !!! 計測開始コマンドを送信するようにする // !!! ただし、MD 計測中に MD を発行するように、同じコマンドの場合は // !!! Mx 系, Nx 系の計測は上書きすることができるようにする // 指定されたタイプのパケットを生成し、送信する switch (type) { case URG_DISTANCE: range_byte_ch = (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D'; ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', range_byte_ch); break; case URG_DISTANCE_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', 'E'); break; case URG_MULTIECHO: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'D'); break; case URG_MULTIECHO_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'E'); break; case URG_STOP: case URG_UNKNOWN: default: ignore_receive_data_with_qt(urg, urg->timeout); urg->last_errno = URG_INVALID_PARAMETER; ret = urg->last_errno; break; } return ret; } int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, NULL, time_stamp, system_time_stamp); } int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp); } int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp); } int urg_stop_measurement(urg_t *urg) { enum { MAX_READ_TIMES = 3 }; int ret = URG_INVALID_RESPONSE; int n; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // QT を発行する n = connection_write(&urg->connection, "QT\n", 3); if (n != 3) { return set_errno_and_return(urg, URG_SEND_ERROR); } for (i = 0; i < MAX_READ_TIMES; ++i) { // QT の応答が返されるまで、距離データを読み捨てる ret = receive_data(urg, NULL, NULL, NULL, NULL); if (ret == URG_NO_ERROR) { // 正常応答 urg->is_laser_on = URG_FALSE; urg->is_sending = URG_FALSE; return set_errno_and_return(urg, URG_NO_ERROR); } } return ret; } int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step) { // 設定の範囲外を指定したときは、エラーを返す if (((skip_step < 0) || (skip_step >= 100)) || (first_step > last_step) || (first_step < -urg->front_data_index) || (last_step > (urg->last_data_index - urg->front_data_index))) { return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR); } urg->scanning_first_step = first_step; urg->scanning_last_step = last_step; urg->scanning_skip_step = skip_step; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_set_connection_data_size(urg_t *urg, urg_range_data_byte_t data_byte) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((data_byte != URG_COMMUNICATION_3_BYTE) || (data_byte != URG_COMMUNICATION_2_BYTE)) { return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR); } urg->range_data_byte = data_byte; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_laser_on(urg_t *urg) { int expected[] = { 0, 2, EXPECTED_END }; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if (urg->is_laser_on != URG_FALSE) { // 既にレーザが発光しているときは、コマンドを送信しないようにする urg->last_errno = 0; return urg->last_errno; } ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0); if (ret >= 0) { urg->is_laser_on = URG_TRUE; ret = 0; } return ret; } int urg_laser_off(urg_t *urg) { return urg_stop_measurement(urg); } int urg_reboot(urg_t *urg) { int expected[] = { 0, 1, EXPECTED_END }; int ret; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // ２回目の RB 送信後、接続を切断する for (i = 0; i < 2; ++i) { ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0); if (ret < 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } urg->is_active = URG_FALSE; urg_close(urg); urg->last_errno = 0; return urg->last_errno; } void urg_sleep(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = 4 }; int sl_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE]; if (urg_stop_measurement(urg) != URG_NO_ERROR) { return; } scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); } void urg_wakeup(urg_t *urg) { urg_stop_measurement(urg); } int urg_is_stable(urg_t *urg) { const char *stat = urg_sensor_status(urg); return strncmp("Stable", stat, 6) ? 0 : 1; } static char *copy_token(char *dest, char *receive_buffer, const char *start_str, const char *end_ch, int lines) { size_t start_str_len = strlen(start_str); size_t end_ch_len = strlen(end_ch); int i; size_t j; for (j = 0; j < end_ch_len; ++j) { const char *p = receive_buffer; for (i = 0; i < lines; ++i) { if (!strncmp(p, start_str, start_str_len)) { char *last_p = strchr(p + start_str_len, end_ch[j]); if (last_p) { *last_p = '\0'; memcpy(dest, p + start_str_len, last_p - (p + start_str_len) + 1); return dest; } } p += strlen(p) + 1; } } return NULL; } static const char *receive_command_response(urg_t *urg, char *buffer, int buffer_size, const char* command, int response_lines) { const int vv_expected[] = { 0, EXPECTED_END }; int ret; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = scip_response(urg, command, vv_expected, urg->timeout, buffer, buffer_size); if (ret < response_lines) { return RECEIVE_ERROR_MESSAGE; } return NULL; } const char *urg_sensor_product_type(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROD:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_serial_id(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "SERI:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_vendor(urg_t *urg){ enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "VEND:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", "(", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_date(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } // Get the firmware version and append a '(', this will be what's before the date char firmware_version[50]; strcpy(firmware_version, urg_sensor_firmware_version(urg)); strcat(firmware_version, "("); ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } // Strip out the actual date from between the '(' and ')' char *date; p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", ";", VV_RESPONSE_LINES); date = copy_token(urg->return_buffer, p, firmware_version, ")", 1); return (date) ? date : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_protocol_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROT:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_status(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "STAT:", ";", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_state(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "MESM:", " (", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } void urg_set_error_handler(urg_t *urg, urg_error_handler handler) { urg->error_handler = handler; }
771  n = scip_response(urg, "TM0\n", expected, urg->timeout, NULL, 0);
772  if (n <= 0) {
773  return set_errno_and_return(urg, URG_INVALID_RESPONSE);
774  } else {
775  return 0;
776  }
777 }
778 
779 
780 long urg_time_stamp(urg_t *urg)
781 {
782  const int expected[] = { 0, EXPECTED_END };
783  char buffer[BUFFER_SIZE];
784  char *p;
785  int ret;
786 
787  if (!urg->is_active) {
788  return set_errno_and_return(urg, URG_NOT_CONNECTED);
789  }
790 
791  ret = scip_response(urg, "TM1\n", expected,
792  urg->timeout, buffer, BUFFER_SIZE);
793  if (ret < 0) {
794  return ret;
795  }
796 
797  // buffer からタイムスタンプを取得し、デコードして返す
798  if (strcmp(buffer, "00P")) {
799  // 最初の応答が "00P" でなければ戻る return set_errno_and_return(urg, URG_RECEIVE_ERROR); } p = buffer + 4; if (strlen(p) != 5) { return set_errno_and_return(urg, URG_RECEIVE_ERROR); } if (p[5] == scip_checksum(p, 4)) { return set_errno_and_return(urg, URG_CHECKSUM_ERROR); } return urg_scip_decode(p, 4); } int urg_stop_time_stamp_mode(urg_t *urg) { int expected[] = { 0, EXPECTED_END }; int n; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // TM2 を発行する n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } static int send_distance_command(urg_t *urg, int scan_times, int skip_scan, char single_scan_ch, char continuous_scan_ch, char scan_type_ch) { char buffer[BUFFER_SIZE]; int write_size = 0; int front_index = urg->front_data_index; int n; urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times; urg->scanning_remain_times = urg->specified_scan_times; urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan; if (scan_times >= 100) { // 計測回数が 99 を越える場合は、無限回のスキャンを行う urg->specified_scan_times = 0; } if (urg->scanning_remain_times == 1) { // レーザ発光を指示 urg_laser_on(urg); write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n", single_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step); } else { write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n", continuous_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step, skip_scan, urg->specified_scan_times); urg->is_sending = URG_TRUE; } n = connection_write(&urg->connection, buffer, write_size); if (n != write_size) { return set_errno_and_return(urg, URG_SEND_ERROR); } return 0; } int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan) { char range_byte_ch; int ret = 0; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((skip_scan < 0) || (skip_scan > 9)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // !!! Mx 系, Nx 系の計測中のときは、QT を発行してから // !!! 計測開始コマンドを送信するようにする // !!! ただし、MD 計測中に MD を発行するように、同じコマンドの場合は // !!! Mx 系, Nx 系の計測は上書きすることができるようにする // 指定されたタイプのパケットを生成し、送信する switch (type) { case URG_DISTANCE: range_byte_ch = (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D'; ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', range_byte_ch); break; case URG_DISTANCE_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', 'E'); break; case URG_MULTIECHO: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'D'); break; case URG_MULTIECHO_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'E'); break; case URG_STOP: case URG_UNKNOWN: default: ignore_receive_data_with_qt(urg, urg->timeout); urg->last_errno = URG_INVALID_PARAMETER; ret = urg->last_errno; break; } return ret; } int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, NULL, time_stamp, system_time_stamp); } int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp); } int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp); } int urg_stop_measurement(urg_t *urg) { enum { MAX_READ_TIMES = 3 }; int ret = URG_INVALID_RESPONSE; int n; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // QT を発行する n = connection_write(&urg->connection, "QT\n", 3); if (n != 3) { return set_errno_and_return(urg, URG_SEND_ERROR); } for (i = 0; i < MAX_READ_TIMES; ++i) { // QT の応答が返されるまで、距離データを読み捨てる ret = receive_data(urg, NULL, NULL, NULL, NULL); if (ret == URG_NO_ERROR) { // 正常応答 urg->is_laser_on = URG_FALSE; urg->is_sending = URG_FALSE; return set_errno_and_return(urg, URG_NO_ERROR); } } return ret; } int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step) { // 設定の範囲外を指定したときは、エラーを返す if (((skip_step < 0) || (skip_step >= 100)) || (first_step > last_step) || (first_step < -urg->front_data_index) || (last_step > (urg->last_data_index - urg->front_data_index))) { return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR); } urg->scanning_first_step = first_step; urg->scanning_last_step = last_step; urg->scanning_skip_step = skip_step; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_set_connection_data_size(urg_t *urg, urg_range_data_byte_t data_byte) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((data_byte != URG_COMMUNICATION_3_BYTE) || (data_byte != URG_COMMUNICATION_2_BYTE)) { return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR); } urg->range_data_byte = data_byte; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_laser_on(urg_t *urg) { int expected[] = { 0, 2, EXPECTED_END }; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if (urg->is_laser_on != URG_FALSE) { // 既にレーザが発光しているときは、コマンドを送信しないようにする urg->last_errno = 0; return urg->last_errno; } ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0); if (ret >= 0) { urg->is_laser_on = URG_TRUE; ret = 0; } return ret; } int urg_laser_off(urg_t *urg) { return urg_stop_measurement(urg); } int urg_reboot(urg_t *urg) { int expected[] = { 0, 1, EXPECTED_END }; int ret; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // ２回目の RB 送信後、接続を切断する for (i = 0; i < 2; ++i) { ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0); if (ret < 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } urg->is_active = URG_FALSE; urg_close(urg); urg->last_errno = 0; return urg->last_errno; } void urg_sleep(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = 4 }; int sl_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE]; if (urg_stop_measurement(urg) != URG_NO_ERROR) { return; } scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); } void urg_wakeup(urg_t *urg) { urg_stop_measurement(urg); } int urg_is_stable(urg_t *urg) { const char *stat = urg_sensor_status(urg); return strncmp("Stable", stat, 6) ? 0 : 1; } static char *copy_token(char *dest, char *receive_buffer, const char *start_str, const char *end_ch, int lines) { size_t start_str_len = strlen(start_str); size_t end_ch_len = strlen(end_ch); int i; size_t j; for (j = 0; j < end_ch_len; ++j) { const char *p = receive_buffer; for (i = 0; i < lines; ++i) { if (!strncmp(p, start_str, start_str_len)) { char *last_p = strchr(p + start_str_len, end_ch[j]); if (last_p) { *last_p = '\0'; memcpy(dest, p + start_str_len, last_p - (p + start_str_len) + 1); return dest; } } p += strlen(p) + 1; } } return NULL; } static const char *receive_command_response(urg_t *urg, char *buffer, int buffer_size, const char* command, int response_lines) { const int vv_expected[] = { 0, EXPECTED_END }; int ret; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = scip_response(urg, command, vv_expected, urg->timeout, buffer, buffer_size); if (ret < response_lines) { return RECEIVE_ERROR_MESSAGE; } return NULL; } const char *urg_sensor_product_type(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROD:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_serial_id(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "SERI:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_vendor(urg_t *urg){ enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "VEND:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", "(", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_firmware_date(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } // Get the firmware version and append a '(', this will be what's before the date char firmware_version[50]; strcpy(firmware_version, urg_sensor_firmware_version(urg)); strcat(firmware_version, "("); ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } // Strip out the actual date from between the '(' and ')' char *date; p = copy_token(urg->return_buffer, receive_buffer, "FIRM:", ";", VV_RESPONSE_LINES); date = copy_token(urg->return_buffer, p, firmware_version, ")", 1); return (date) ? date : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_protocol_version(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "VV\n", VV_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "PROT:", ";", VV_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_status(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "STAT:", ";", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } const char *urg_sensor_state(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES, }; char receive_buffer[RECEIVE_BUFFER_SIZE]; const char *ret; char *p; if (!urg->is_active) { return NOT_CONNECTED_MESSAGE; } ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE, "II\n", II_RESPONSE_LINES); if (ret) { return ret; } p = copy_token(urg->return_buffer, receive_buffer, "MESM:", " (", II_RESPONSE_LINES); return (p) ? p : RECEIVE_ERROR_MESSAGE; } void urg_set_error_handler(urg_t *urg, urg_error_handler handler) { urg->error_handler = handler; }
800  return set_errno_and_return(urg, URG_RECEIVE_ERROR);
801  }
802  p = buffer + 4;
803  if (strlen(p) != 5) {
804  return set_errno_and_return(urg, URG_RECEIVE_ERROR);
805  }
806  if (p[5] == scip_checksum(p, 4)) {
807  return set_errno_and_return(urg, URG_CHECKSUM_ERROR);
808  }
809  return urg_scip_decode(p, 4);
810 }
811 
812 
813 int urg_stop_time_stamp_mode(urg_t *urg)
814 {
815  int expected[] = { 0, EXPECTED_END };
816  int n;
817 
818  if (!urg->is_active) {
819  return set_errno_and_return(urg, URG_NOT_CONNECTED);
820  }
821 
822  // TM2 を発行する n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0); if (n <= 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } else { return 0; } } static int send_distance_command(urg_t *urg, int scan_times, int skip_scan, char single_scan_ch, char continuous_scan_ch, char scan_type_ch) { char buffer[BUFFER_SIZE]; int write_size = 0; int front_index = urg->front_data_index; int n; urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times; urg->scanning_remain_times = urg->specified_scan_times; urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan; if (scan_times >= 100) { // 計測回数が 99 を越える場合は、無限回のスキャンを行う urg->specified_scan_times = 0; } if (urg->scanning_remain_times == 1) { // レーザ発光を指示 urg_laser_on(urg); write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n", single_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step); } else { write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n", continuous_scan_ch, scan_type_ch, urg->scanning_first_step + front_index, urg->scanning_last_step + front_index, urg->scanning_skip_step, skip_scan, urg->specified_scan_times); urg->is_sending = URG_TRUE; } n = connection_write(&urg->connection, buffer, write_size); if (n != write_size) { return set_errno_and_return(urg, URG_SEND_ERROR); } return 0; } int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan) { char range_byte_ch; int ret = 0; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((skip_scan < 0) || (skip_scan > 9)) { ignore_receive_data_with_qt(urg, urg->timeout); return set_errno_and_return(urg, URG_INVALID_PARAMETER); } // !!! Mx 系, Nx 系の計測中のときは、QT を発行してから // !!! 計測開始コマンドを送信するようにする // !!! ただし、MD 計測中に MD を発行するように、同じコマンドの場合は // !!! Mx 系, Nx 系の計測は上書きすることができるようにする // 指定されたタイプのパケットを生成し、送信する switch (type) { case URG_DISTANCE: range_byte_ch = (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D'; ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', range_byte_ch); break; case URG_DISTANCE_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'G', 'M', 'E'); break; case URG_MULTIECHO: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'D'); break; case URG_MULTIECHO_INTENSITY: ret = send_distance_command(urg, scan_times, skip_scan, 'H', 'N', 'E'); break; case URG_STOP: case URG_UNKNOWN: default: ignore_receive_data_with_qt(urg, urg->timeout); urg->last_errno = URG_INVALID_PARAMETER; ret = urg->last_errno; break; } return ret; } int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, NULL, time_stamp, system_time_stamp); } int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data, intensity, time_stamp, system_time_stamp); } int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp); } int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp); } int urg_stop_measurement(urg_t *urg) { enum { MAX_READ_TIMES = 3 }; int ret = URG_INVALID_RESPONSE; int n; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // QT を発行する n = connection_write(&urg->connection, "QT\n", 3); if (n != 3) { return set_errno_and_return(urg, URG_SEND_ERROR); } for (i = 0; i < MAX_READ_TIMES; ++i) { // QT の応答が返されるまで、距離データを読み捨てる ret = receive_data(urg, NULL, NULL, NULL, NULL); if (ret == URG_NO_ERROR) { // 正常応答 urg->is_laser_on = URG_FALSE; urg->is_sending = URG_FALSE; return set_errno_and_return(urg, URG_NO_ERROR); } } return ret; } int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step) { // 設定の範囲外を指定したときは、エラーを返す if (((skip_step < 0) || (skip_step >= 100)) || (first_step > last_step) || (first_step < -urg->front_data_index) || (last_step > (urg->last_data_index - urg->front_data_index))) { return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR); } urg->scanning_first_step = first_step; urg->scanning_last_step = last_step; urg->scanning_skip_step = skip_step; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_set_connection_data_size(urg_t *urg, urg_range_data_byte_t data_byte) { if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if ((data_byte != URG_COMMUNICATION_3_BYTE) || (data_byte != URG_COMMUNICATION_2_BYTE)) { return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR); } urg->range_data_byte = data_byte; return set_errno_and_return(urg, URG_NO_ERROR); } int urg_laser_on(urg_t *urg) { int expected[] = { 0, 2, EXPECTED_END }; int ret; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } if (urg->is_laser_on != URG_FALSE) { // 既にレーザが発光しているときは、コマンドを送信しないようにする urg->last_errno = 0; return urg->last_errno; } ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0); if (ret >= 0) { urg->is_laser_on = URG_TRUE; ret = 0; } return ret; } int urg_laser_off(urg_t *urg) { return urg_stop_measurement(urg); } int urg_reboot(urg_t *urg) { int expected[] = { 0, 1, EXPECTED_END }; int ret; int i; if (!urg->is_active) { return set_errno_and_return(urg, URG_NOT_CONNECTED); } // ２回目の RB 送信後、接続を切断する for (i = 0; i < 2; ++i) { ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0); if (ret < 0) { return set_errno_and_return(urg, URG_INVALID_RESPONSE); } } urg->is_active = URG_FALSE; urg_close(urg); urg->last_errno = 0; return urg->last_errno; } void urg_sleep(urg_t *urg) { enum { RECEIVE_BUFFER_SIZE = 4 }; int sl_expected[] = { 0, EXPECTED_END }; char receive_buffer[RECEIVE_BUFFER_SIZE]; if (urg_stop_measurement(urg) != URG_NO_ERROR) { return; } scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT, receive_buffer, RECEIVE_BUFFER_SIZE); } void urg_wakeup(urg_t *urg) { urg_stop_measurement(urg); } int urg_is_stable(urg_t *urg) { const char *stat = urg_sensor_status(urg); return strncmp("Stable", stat, 6) ? 0 : 1; } static char *copy_token(char *dest, char *receive_buffer, const char *start_str, const char *end_ch, int lines) { size_t start_str_len = strlen(start_str); size_t end_ch_len = strlen(end_ch); int i; size_t j; for (j = 0; j < end_ch_len; ++j) {