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) { 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; }
823  n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0);
824  if (n <= 0) {
825  return set_errno_and_return(urg, URG_INVALID_RESPONSE);
826  } else {
827  return 0;
828  }
829 }
830 
831 
832 static int send_distance_command(urg_t *urg, int scan_times, int skip_scan,
833  char single_scan_ch, char continuous_scan_ch,
834  char scan_type_ch)
835 {
836  char buffer[BUFFER_SIZE];
837  int write_size = 0;
838  int front_index = urg->front_data_index;
839  int n;
840 
841  urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times;
842  urg->scanning_remain_times = urg->specified_scan_times;
843  urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan;
844  if (scan_times >= 100) {
845  // 計測回数が 99 を越える場合は、無限回のスキャンを行う
846  urg->specified_scan_times = 0;
847  }
848 
849  if (urg->scanning_remain_times == 1) {
850  // レーザ発光を指示
851  urg_laser_on(urg);
852 
853  write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n",
854  single_scan_ch, scan_type_ch,
855  urg->scanning_first_step + front_index,
856  urg->scanning_last_step + front_index,
857  urg->scanning_skip_step);
858  } else {
859  write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n",
860  continuous_scan_ch, scan_type_ch,
861  urg->scanning_first_step + front_index,
862  urg->scanning_last_step + front_index,
863  urg->scanning_skip_step,
864  skip_scan, urg->specified_scan_times);
865  urg->is_sending = URG_TRUE;
866  }
867 
868  n = connection_write(&urg->connection, buffer, write_size);
869  if (n != write_size) {
870  return set_errno_and_return(urg, URG_SEND_ERROR);
871  }
872 
873  return 0;
874 }
875 
876 
877 int urg_start_measurement(urg_t *urg, urg_measurement_type_t type,
878  int scan_times, int skip_scan)
879 {
880  char range_byte_ch;
881  int ret = 0;
882 
883  if (!urg->is_active) {
884  return set_errno_and_return(urg, URG_NOT_CONNECTED);
885  }
886 
887  if ((skip_scan < 0) || (skip_scan > 9)) {
888  ignore_receive_data_with_qt(urg, urg->timeout);
889  return set_errno_and_return(urg, URG_INVALID_PARAMETER);
890  }
891 
892  // !!! 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; }
893  // !!! 計測開始コマンドを送信するようにする // !!! ただし、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; }
894  // !!! ただし、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; }
895  // !!! 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; }
896 
897  // 指定されたタイプのパケットを生成し、送信する 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; }
898  switch (type) {
899  case URG_DISTANCE:
900  range_byte_ch =
901  (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D';
902  ret = send_distance_command(urg, scan_times, skip_scan,
903  'G', 'M', range_byte_ch);
904  break;
905 
906  case URG_DISTANCE_INTENSITY:
907  ret = send_distance_command(urg, scan_times, skip_scan,
908  'G', 'M', 'E');
909  break;
910 
911  case URG_MULTIECHO:
912  ret = send_distance_command(urg, scan_times, skip_scan,
913  'H', 'N', 'D');
914  break;
915 
916  case URG_MULTIECHO_INTENSITY:
917  ret = send_distance_command(urg, scan_times, skip_scan,
918  'H', 'N', 'E');
919  break;
920 
921  case URG_STOP:
922  case URG_UNKNOWN:
923  default:
924  ignore_receive_data_with_qt(urg, urg->timeout);
925  urg->last_errno = URG_INVALID_PARAMETER;
926  ret = urg->last_errno;
927  break;
928  }
929 
930  return ret;
931 }
932 
933 
934 int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp)
935 {
936  if (!urg->is_active) {
937  return set_errno_and_return(urg, URG_NOT_CONNECTED);
938  }
939  return receive_data(urg, data, NULL, time_stamp, system_time_stamp);
940 }
941 
942 
943 int urg_get_distance_intensity(urg_t *urg,
944  long data[], unsigned short intensity[],
945  long *time_stamp, unsigned long long *system_time_stamp)
946 {
947  if (!urg->is_active) {
948  return set_errno_and_return(urg, URG_NOT_CONNECTED);
949  }
950 
951  return receive_data(urg, data, intensity, time_stamp, system_time_stamp);
952 }
953 
954 
955 int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp)
956 {
957  if (!urg->is_active) {
958  return set_errno_and_return(urg, URG_NOT_CONNECTED);
959  }
960 
961  return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp);
962 }
963 
964 
965 int urg_get_multiecho_intensity(urg_t *urg,
966  long data_multi[],
967  unsigned short intensity_multi[],
968  long *time_stamp, unsigned long long *system_time_stamp)
969 {
970  if (!urg->is_active) {
971  return set_errno_and_return(urg, URG_NOT_CONNECTED);
972  }
973 
974  return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp);
975 }
976 
977 
978 int urg_stop_measurement(urg_t *urg)
979 {
980  enum { MAX_READ_TIMES = 3 };
981  int ret = URG_INVALID_RESPONSE;
982  int n;
983  int i;
984 
985  if (!urg->is_active) {
986  return set_errno_and_return(urg, URG_NOT_CONNECTED);
987  }
988 
989  // 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; }
990  n = connection_write(&urg->connection, "QT\n", 3);
991  if (n != 3) {
992  return set_errno_and_return(urg, URG_SEND_ERROR);
993  }
994 
995  for (i = 0; i < MAX_READ_TIMES; ++i) {
996  // 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; }
997  ret = receive_data(urg, NULL, NULL, NULL, NULL);
998  if (ret == URG_NO_ERROR) {
999  // 正常応答
1000  urg->is_laser_on = URG_FALSE;
1001  urg->is_sending = URG_FALSE;
1002  return set_errno_and_return(urg, URG_NO_ERROR);
1003  }
1004  }
1005  return ret;
1006 }
1007 
1008 
1009 int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step,
1010  int skip_step)
1011 {
1012  // 設定の範囲外を指定したときは、エラーを返す
1013  if (((skip_step < 0) || (skip_step >= 100)) ||
1014  (first_step > last_step) ||
1015  (first_step < -urg->front_data_index) ||
1016  (last_step > (urg->last_data_index - urg->front_data_index))) {
1017  return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR);
1018  }
1019 
1020  urg->scanning_first_step = first_step;
1021  urg->scanning_last_step = last_step;
1022  urg->scanning_skip_step = skip_step;
1023 
1024  return set_errno_and_return(urg, URG_NO_ERROR);
1025 }
1026 
1027 
1028 int urg_set_connection_data_size(urg_t *urg,
1029  urg_range_data_byte_t data_byte)
1030 {
1031  if (!urg->is_active) {
1032  return set_errno_and_return(urg, URG_NOT_CONNECTED);
1033  }
1034 
1035  if ((data_byte != URG_COMMUNICATION_3_BYTE) ||
1036  (data_byte != URG_COMMUNICATION_2_BYTE)) {
1037  return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR);
1038  }
1039 
1040  urg->range_data_byte = data_byte;
1041 
1042  return set_errno_and_return(urg, URG_NO_ERROR);
1043 }
1044 
1045 
1046 int urg_laser_on(urg_t *urg)
1047 {
1048  int expected[] = { 0, 2, EXPECTED_END };
1049  int ret;
1050 
1051  if (!urg->is_active) {
1052  return set_errno_and_return(urg, URG_NOT_CONNECTED);
1053  }
1054 
1055  if (urg->is_laser_on != URG_FALSE) {
1056  // 既にレーザが発光しているときは、コマンドを送信しないようにする 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; }
1057  urg->last_errno = 0;
1058  return urg->last_errno;
1059  }
1060 
1061  ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0);
1062  if (ret >= 0) {
1063  urg->is_laser_on = URG_TRUE;
1064  ret = 0;
1065  }
1066  return ret;
1067 }
1068 
1069 
1070 int urg_laser_off(urg_t *urg)
1071 {
1072  return urg_stop_measurement(urg);
1073 }
1074 
1075 
1076 int urg_reboot(urg_t *urg)
1077 {
1078  int expected[] = { 0, 1, EXPECTED_END };
1079  int ret;
1080  int i;
1081 
1082  if (!urg->is_active) {
1083  return set_errno_and_return(urg, URG_NOT_CONNECTED);
1084  }
1085 
1086  // ２回目の 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; }
1087  for (i = 0; i < 2; ++i) {
1088  ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0);
1089  if (ret < 0) {
1090  return set_errno_and_return(urg, URG_INVALID_RESPONSE);
1091  }
1092  }
1093  urg->is_active = URG_FALSE;
1094  urg_close(urg);
1095 
1096  urg->last_errno = 0;
1097  return urg->last_errno;
1098 }
1099 
1100 
1101 void urg_sleep(urg_t *urg)
1102 {
1103  enum { RECEIVE_BUFFER_SIZE = 4 };
1104  int sl_expected[] = { 0, EXPECTED_END };
1105  char receive_buffer[RECEIVE_BUFFER_SIZE];
1106 
1107  if (urg_stop_measurement(urg) != URG_NO_ERROR) {
1108  return;
1109  }
1110 
1111  scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT,
1112  receive_buffer, RECEIVE_BUFFER_SIZE);
1113 }
1114 
1115 
1116 void urg_wakeup(urg_t *urg)
1117 {
1118  urg_stop_measurement(urg);
1119 }
1120 
1121 
1122 int urg_is_stable(urg_t *urg)
1123 {
1124  const char *stat = urg_sensor_status(urg);
1125  return strncmp("Stable", stat, 6) ? 0 : 1;
1126 }
1127 
1128 
1129 static char *copy_token(char *dest, char *receive_buffer,
1130  const char *start_str, const char *end_ch, int lines)
1131 {
1132  size_t start_str_len = strlen(start_str);
1133  size_t end_ch_len = strlen(end_ch);
1134  int i;
1135  size_t j;
1136 
1137  for (j = 0; j < end_ch_len; ++j) {
1138  const char *p = receive_buffer;
1139 
1140  for (i = 0; i < lines; ++i) {
1141  if (!strncmp(p, start_str, start_str_len)) {
1142 
1143  char *last_p = strchr(p + start_str_len, end_ch[j]);
1144  if (last_p) {
1145  *last_p = '\0';
1146  memcpy(dest, p + start_str_len,
1147  last_p - (p + start_str_len) + 1);
1148  return dest;
1149  }
1150  }
1151  p += strlen(p) + 1;
1152  }
1153  }
1154  return NULL;
1155 }
1156 
1157 
1158 static const char *receive_command_response(urg_t *urg,
1159  char *buffer, int buffer_size,
1160  const char* command,
1161  int response_lines)
1162 {
1163  const int vv_expected[] = { 0, EXPECTED_END };
1164  int ret;
1165 
1166  if (!urg->is_active) {
1167  return NOT_CONNECTED_MESSAGE;
1168  }
1169 
1170  ret = scip_response(urg, command, vv_expected, urg->timeout,
1171  buffer, buffer_size);
1172  if (ret < response_lines) {
1173  return RECEIVE_ERROR_MESSAGE;
1174  }
1175 
1176  return NULL;
1177 }
1178 
1179 
1180 const char *urg_sensor_product_type(urg_t *urg)
1181 {
1182  enum {
1183  RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES,
1184  };
1185  char receive_buffer[RECEIVE_BUFFER_SIZE];
1186  const char *ret;
1187  char *p;
1188 
1189  ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE,
1190  "VV\n", VV_RESPONSE_LINES);
1191  if (ret) {
1192  return ret;
1193  }
1194 
1195  p = copy_token(urg->return_buffer,
1196  receive_buffer, "PROD:", ";", VV_RESPONSE_LINES);
1197  return (p) ? p : RECEIVE_ERROR_MESSAGE;
1198 }
1199 
1200 
1201 const char *urg_sensor_serial_id(urg_t *urg)
1202 {
1203  enum {
1204  RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES,
1205  };
1206  char receive_buffer[RECEIVE_BUFFER_SIZE];
1207  const char *ret;
1208  char *p;
1209 
1210  ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE,
1211  "VV\n", VV_RESPONSE_LINES);
1212  if (ret) {
1213  return ret;
1214  }
1215 
1216  p = copy_token(urg->return_buffer,
1217  receive_buffer, "SERI:", ";", VV_RESPONSE_LINES);
1218  return (p) ? p : RECEIVE_ERROR_MESSAGE;
1219 }
1220 
1221 const char *urg_sensor_vendor(urg_t *urg){
1222  enum {
1223  RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES,
1224  };
1225  char receive_buffer[RECEIVE_BUFFER_SIZE];
1226  const char *ret;
1227  char *p;
1228 
1229  if (!urg->is_active) {
1230  return NOT_CONNECTED_MESSAGE;
1231  }
1232 
1233  ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE,
1234  "VV\n", VV_RESPONSE_LINES);
1235  if (ret) {
1236  return ret;
1237  }
1238 
1239  p = copy_token(urg->return_buffer,
1240  receive_buffer, "VEND:", ";", VV_RESPONSE_LINES);
1241  return (p) ? p : RECEIVE_ERROR_MESSAGE;
1242 }
1243 
1244 const char *urg_sensor_firmware_version(urg_t *urg)
1245 {
1246  enum {
1247  RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES,
1248  };
1249  char receive_buffer[RECEIVE_BUFFER_SIZE];
1250  const char *ret;
1251  char *p;
1252 
1253  if (!urg->is_active) {
1254  return NOT_CONNECTED_MESSAGE;
1255  }
1256 
1257  ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE,
1258  "VV\n", VV_RESPONSE_LINES);
1259  if (ret) {
1260  return ret;
1261  }
1262 
1263  p = copy_token(urg->return_buffer,
1264  receive_buffer, "FIRM:", "(", VV_RESPONSE_LINES);
1265  return (p) ? p : RECEIVE_ERROR_MESSAGE;
1266 }
1267 
1268 const char *urg_sensor_firmware_date(urg_t *urg)
1269 {
1270  enum {
1271  RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES,
1272  };
1273  char receive_buffer[RECEIVE_BUFFER_SIZE];
1274  const char *ret;
1275  char *p;
1276 
1277  if (!urg->is_active) {
1278  return NOT_CONNECTED_MESSAGE;
1279  }
1280 
1281  // Get the firmware version and append a '(', this will be what's before the date
1282  char firmware_version[50];
1283  strcpy(firmware_version, urg_sensor_firmware_version(urg));
1284  strcat(firmware_version, "(");
1285 
1286  ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE,
1287  "VV\n", VV_RESPONSE_LINES);
1288  if (ret) {
1289  return ret;
1290  }
1291 
1292  // Strip out the actual date from between the '(' and ')'
1293  char *date;
1294  p = copy_token(urg->return_buffer,
1295  receive_buffer, "FIRM:", ";", VV_RESPONSE_LINES);
1296  date = copy_token(urg->return_buffer, p, firmware_version, ")", 1);
1297  return (date) ? date : RECEIVE_ERROR_MESSAGE;
1298 }
1299 
1300 const char *urg_sensor_protocol_version(urg_t *urg)
1301 {
1302  enum {
1303  RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES,
1304  };
1305  char receive_buffer[RECEIVE_BUFFER_SIZE];
1306  const char *ret;
1307  char *p;
1308 
1309  if (!urg->is_active) {
1310  return NOT_CONNECTED_MESSAGE;
1311  }
1312 
1313  ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE,
1314  "VV\n", VV_RESPONSE_LINES);
1315  if (ret) {
1316  return ret;
1317  }
1318 
1319  p = copy_token(urg->return_buffer,
1320  receive_buffer, "PROT:", ";", VV_RESPONSE_LINES);
1321  return (p) ? p : RECEIVE_ERROR_MESSAGE;
1322 }
1323 
1324 
1325 const char *urg_sensor_status(urg_t *urg)
1326 {
1327  enum {
1328  RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES,
1329  };
1330  char receive_buffer[RECEIVE_BUFFER_SIZE];
1331  const char *ret;
1332  char *p;
1333 
1334  if (!urg->is_active) {
1335  return NOT_CONNECTED_MESSAGE;
1336  }
1337 
1338  ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE,
1339  "II\n", II_RESPONSE_LINES);
1340  if (ret) {
1341  return ret;
1342  }
1343 
1344  p = copy_token(urg->return_buffer,
1345  receive_buffer, "STAT:", ";", II_RESPONSE_LINES);
1346  return (p) ? p : RECEIVE_ERROR_MESSAGE;
1347 }
1348 
1349 
1350 const char *urg_sensor_state(urg_t *urg)
1351 {
1352  enum {
1353  RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES,
1354  };
1355  char receive_buffer[RECEIVE_BUFFER_SIZE];
1356  const char *ret;
1357  char *p;
1358 
1359  if (!urg->is_active) {
1360  return NOT_CONNECTED_MESSAGE;
1361  }
1362 
1363  ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE,
1364  "II\n", II_RESPONSE_LINES);
1365  if (ret) {
1366  return ret;
1367  }
1368 
1369  p = copy_token(urg->return_buffer,
1370  receive_buffer, "MESM:", " (", II_RESPONSE_LINES);
1371  return (p) ? p : RECEIVE_ERROR_MESSAGE;
1372 }
1373 
1374 
1375 void urg_set_error_handler(urg_t *urg, urg_error_handler handler)
1376 {
1377  urg->error_handler = handler;
1378 }
urg_t::received_last_index
int received_last_index
Definition: urg_sensor.h:97
urg_start_time_stamp_mode
int urg_start_time_stamp_mode(urg_t *urg)
Definition: urg_sensor.c:761
URG_SEND_ERROR
Definition: urg_errno.h:21
urg_sensor_vendor
const char * urg_sensor_vendor(urg_t *urg)
returns the vendor name
Definition: urg_sensor.c:1221
scip_checksum
static char scip_checksum(const char buffer[], int size)
チェックサムの計算
Definition: urg_sensor.c:47
urg_t::is_active
int is_active
Definition: urg_sensor.h:74
urg_t::is_sending
int is_sending
Definition: urg_sensor.h:100
urg_get_multiecho
int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp)
Definition: urg_sensor.c:955
parse_parameter
static int parse_parameter(const char *parameter, int size)
Definition: urg_sensor.c:379
URG_FALSE
Definition: urg_sensor.c:24
urg_t::scanning_skip_scan
int scanning_skip_scan
Definition: urg_sensor.h:88
parse_distance_parameter
static urg_measurement_type_t parse_distance_parameter(urg_t *urg, const char echoback[])
Definition: urg_sensor.c:390
urg_t::error_handler
urg_error_handler error_handler
Definition: urg_sensor.h:102
urg_t::max_distance
int max_distance
Definition: urg_sensor.h:84
urg_scip_decode
long urg_scip_decode(const char data[], int size)
SCIP 文字列のデコード.
Definition: urg_sensor.c:364
connection_close
void connection_close(urg_connection_t *connection)
切断
Definition: urg_connection.c:33
urg_reboot
int urg_reboot(urg_t *urg)
Definition: urg_sensor.c:1076
urg_stop_time_stamp_mode
int urg_stop_time_stamp_mode(urg_t *urg)
Definition: urg_sensor.c:813
URG_ETHERNET_OPEN_ERROR
Definition: urg_errno.h:30
urg_sleep
void urg_sleep(urg_t *urg)
Definition: urg_sensor.c:1101
NOT_CONNECTED_MESSAGE
static const char NOT_CONNECTED_MESSAGE[]
Definition: urg_sensor.c:42
urg_t::scanning_remain_times
int scanning_remain_times
Definition: urg_sensor.h:93
connection_readline
int connection_readline(urg_connection_t *connection, char *data, int max_size, int timeout)
改行文字までの受信
Definition: urg_connection.c:95
ignore_receive_data_with_qt
static void ignore_receive_data_with_qt(urg_t *urg, int timeout)
Definition: urg_sensor.c:164
VV_RESPONSE_LINES
Definition: urg_sensor.c:35
receive_data
static int receive_data(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp)
距離データの取得
Definition: urg_sensor.c:568
PP_RESPONSE_LINES
Definition: urg_sensor.c:34
URG_STOP
Definition: urg_sensor.h:40
urg_t::connection
urg_connection_t connection
Definition: urg_sensor.h:76
scip_response
static int scip_response(urg_t *urg, const char *command, const int expected_ret[], int timeout, char *receive_buffer, int receive_buffer_max_size)
Definition: urg_sensor.c:69
URG_COMMUNICATION_2_BYTE
Definition: urg_sensor.h:50
urg_t::is_laser_on
int is_laser_on
Definition: urg_sensor.h:94
URG_INVALID_PARAMETER
Definition: urg_errno.h:24
RECEIVE_DATA_COMPLETE
Definition: urg_sensor.c:32
URG_UNKNOWN
Definition: urg_sensor.h:41
connect_urg_device
static int connect_urg_device(urg_t *urg, long baudrate)
Definition: urg_sensor.c:213
urg_t::front_data_index
int front_data_index
Definition: urg_sensor.h:80
set_errno_and_return
static int set_errno_and_return(urg_t *urg, int urg_errno)
Definition: urg_sensor.c:61
urg_t
URG sensor.
Definition: urg_sensor.h:72
urg_start_measurement
int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan)
Definition: urg_sensor.c:877
RECEIVE_DATA_TIMEOUT
Definition: urg_sensor.c:31
II_RESPONSE_LINES
Definition: urg_sensor.c:36
change_sensor_baudrate
static int change_sensor_baudrate(urg_t *urg, long current_baudrate, long next_baudrate)
Definition: urg_sensor.c:177
URG_NOT_CONNECTED
Definition: urg_errno.h:16
urg_sensor.h
URG sensor.
urg_set_error_handler
void urg_set_error_handler(urg_t *urg, urg_error_handler handler)
Definition: urg_sensor.c:1375
EXPECTED_END
Definition: urg_sensor.c:29
urg_measurement_type_t
urg_measurement_type_t
Definition: urg_sensor.h:35
copy_token
static char * copy_token(char *dest, char *receive_buffer, const char *start_str, const char *end_ch, int lines)
Definition: urg_sensor.c:1129
URG_INVALID_RESPONSE
Definition: urg_errno.h:18
urg_t::return_buffer
char return_buffer[80]
Definition: urg_sensor.h:104
URG_ETHERNET
イーサーネット接続
Definition: urg_connection.h:34
urg_errno.h
URG ライブラリのエラー定義.
urg_connection_type_t
urg_connection_type_t
通信タイプ
Definition: urg_connection.h:32
urg_stop_measurement
int urg_stop_measurement(urg_t *urg)
Definition: urg_sensor.c:978
urg_get_multiecho_intensity
int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp)
Definition: urg_sensor.c:965
URG_MAX_ECHO
Definition: urg_sensor.h:56
urg_range_data_byte_t
urg_range_data_byte_t
Definition: urg_sensor.h:48
receive_length_data
static int receive_length_data(urg_t *urg, long length[], unsigned short intensity[], urg_measurement_type_t type, char buffer[])
Definition: urg_sensor.c:448
urg_t::range_data_byte
urg_range_data_byte_t range_data_byte
Definition: urg_sensor.h:89
receive_parameter
static int receive_parameter(urg_t *urg)
Definition: urg_sensor.c:294
urg_get_walltime
void urg_get_walltime(unsigned long long *nsecs)
Definition: urg_time.c:84
urg_t::area_resolution
int area_resolution
Definition: urg_sensor.h:81
urg_t::last_errno
int last_errno
Definition: urg_sensor.h:75
connection_write
int connection_write(urg_connection_t *connection, const char *data, int size)
送信
Definition: urg_connection.c:65
URG_TRUE
Definition: urg_sensor.c:25
URG_COMMUNICATION_3_BYTE
Definition: urg_sensor.h:49
urg_time_stamp
long urg_time_stamp(urg_t *urg)
Definition: urg_sensor.c:780
urg_set_scanning_parameter
int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step)
Definition: urg_sensor.c:1009
urg_sensor_firmware_date
const char * urg_sensor_firmware_date(urg_t *urg)
Definition: urg_sensor.c:1268
urg_t::received_first_index
int received_first_index
Definition: urg_sensor.h:96
URG_NO_RESPONSE
Definition: urg_errno.h:19
urg_error_handler
urg_measurement_type_t(* urg_error_handler)(const char *status, void *urg)
Definition: urg_sensor.h:62
ignore_receive_data
static void ignore_receive_data(urg_t *urg, int timeout)
Definition: urg_sensor.c:146
urg_laser_off
int urg_laser_off(urg_t *urg)
Definition: urg_sensor.c:1070
urg_close
void urg_close(urg_t *urg)
Definition: urg_sensor.c:745
urg_sensor_status
const char * urg_sensor_status(urg_t *urg)
Definition: urg_sensor.c:1325
urg_open
int urg_open(urg_t *urg, urg_connection_type_t connection_type, const char *device_or_address, long baudrate_or_port)
Definition: urg_sensor.c:684
urg_t::first_data_index
int first_data_index
Definition: urg_sensor.h:78
connection_set_baudrate
int connection_set_baudrate(urg_connection_t *connection, long baudrate)
Definition: urg_connection.c:47
urg_t::scanning_last_step
int scanning_last_step
Definition: urg_sensor.h:86
URG_NOT_DETECT_BAUDRATE_ERROR
Definition: urg_errno.h:29
MAX_TIMEOUT
Definition: urg_sensor.c:38
urg_sensor_product_type
const char * urg_sensor_product_type(urg_t *urg)
Definition: urg_sensor.c:1180
urg_t::min_distance
int min_distance
Definition: urg_sensor.h:83
urg_t::scan_usec
long scan_usec
Definition: urg_sensor.h:82
receive_command_response
static const char * receive_command_response(urg_t *urg, char *buffer, int buffer_size, const char *command, int response_lines)
Definition: urg_sensor.c:1158
urg_sensor_firmware_version
const char * urg_sensor_firmware_version(urg_t *urg)
Definition: urg_sensor.c:1244
URG_MULTIECHO_INTENSITY
Definition: urg_sensor.h:39
urg_t::scanning_skip_step
int scanning_skip_step
Definition: urg_sensor.h:87
RECEIVE_ERROR_MESSAGE
static const char RECEIVE_ERROR_MESSAGE[]
Definition: urg_sensor.c:43
urg_get_distance
int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp)
Definition: urg_sensor.c:934
connection_open
int connection_open(urg_connection_t *connection, urg_connection_type_t connection_type, const char *device, long baudrate_or_port)
接続
Definition: urg_connection.c:13
urg_laser_on
int urg_laser_on(urg_t *urg)
Definition: urg_sensor.c:1046
urg_t::last_data_index
int last_data_index
Definition: urg_sensor.h:79
URG_UNKNOWN_ERROR
Definition: urg_errno.h:15
urg_set_timeout_msec
void urg_set_timeout_msec(urg_t *urg, int msec)
タイムアウト時間の設定
Definition: urg_sensor.c:755
URG_NO_ERROR
Definition: urg_errno.h:14
URG_SERIAL_OPEN_ERROR
Definition: urg_errno.h:28
URG_DISTANCE_INTENSITY
Definition: urg_sensor.h:37
URG_CHECKSUM_ERROR
Definition: urg_errno.h:23
BUFFER_SIZE
Definition: urg_sensor.c:27
urg_get_distance_intensity
int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp)
Definition: urg_sensor.c:943
send_distance_command
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)
Definition: urg_sensor.c:832
URG_RECEIVE_ERROR
Definition: urg_errno.h:22
URG_DISTANCE
Definition: urg_sensor.h:36
urg_wakeup
void urg_wakeup(urg_t *urg)
Definition: urg_sensor.c:1116
urg_t::scanning_first_step
int scanning_first_step
Definition: urg_sensor.h:85
urg_t::received_range_data_byte
urg_range_data_byte_t received_range_data_byte
Definition: urg_sensor.h:99
urg_t::received_skip_step
int received_skip_step
Definition: urg_sensor.h:98
parse_distance_echoback
static urg_measurement_type_t parse_distance_echoback(urg_t *urg, const char echoback[])
Definition: urg_sensor.c:425
urg_sensor_serial_id
const char * urg_sensor_serial_id(urg_t *urg)
Definition: urg_sensor.c:1201
urg_t::timeout
int timeout
Definition: urg_sensor.h:91
urg_sensor_protocol_version
const char * urg_sensor_protocol_version(urg_t *urg)
returns the protocol version
Definition: urg_sensor.c:1300
URG_DATA_SIZE_PARAMETER_ERROR
Definition: urg_errno.h:32
urg_set_connection_data_size
int urg_set_connection_data_size(urg_t *urg, urg_range_data_byte_t data_byte)
Definition: urg_sensor.c:1028
urg_t::specified_scan_times
int specified_scan_times
Definition: urg_sensor.h:92
urg_is_stable
int urg_is_stable(urg_t *urg)
Definition: urg_sensor.c:1122
URG_MULTIECHO
Definition: urg_sensor.h:38
urg_sensor_state
const char * urg_sensor_state(urg_t *urg)
Definition: urg_sensor.c:1350
URG_SERIAL
シリアル, USB 接続
Definition: urg_connection.h:33
URG_SCANNING_PARAMETER_ERROR
Definition: urg_errno.h:31

urg_c
Author(s): Satofumi Kamimura , Katsumi Kimoto, Adrian Boeing
autogenerated on Wed Jun 10 2020 03:48:10