• R/O
  • SSH
  • HTTPS

qrobosdk: コミット


コミットメタ情報

リビジョン1930 (tree)
日時2010-09-30 12:26:52
作者satofumi

ログメッセージ

get_distance.cpp の動作を確認

変更サマリ

差分

--- branches/tsukuba_2010/libs/lidar/samples/Connection_information.cpp (nonexistent)
+++ branches/tsukuba_2010/libs/lidar/samples/Connection_information.cpp (revision 1930)
@@ -0,0 +1,83 @@
1+/*!
2+ \file
3+ \brief 接続情報の管理
4+
5+ \author Satofumi KAMIMURA
6+
7+ $Id$
8+*/
9+
10+#include "Connection_information.h"
11+#include "urg_detect_os.h"
12+#include <string>
13+#include <cstring>
14+
15+using namespace std;
16+using namespace qrk;
17+
18+
19+struct Connection_information::pImpl
20+{
21+ Urg_driver::connection_type_t connection_type_;
22+ string device_or_ip_name_;
23+ long baudrate_or_port_number_;
24+
25+
26+ void set_serial_connection(void)
27+ {
28+ connection_type_ = Urg_driver::Serial;
29+#if defined(URG_WINDOWS_OS)
30+ device_or_ip_name_ = "COM3";
31+#elif defined(URG_LINUX_OS)
32+ device_or_ip_name_ = "/dev/ttyACM0";
33+#else
34+#endif
35+ baudrate_or_port_number_ = 115200;
36+ }
37+
38+
39+ void set_ethernet_connection(void)
40+ {
41+ connection_type_ = Urg_driver::Ethernet;
42+ device_or_ip_name_ = "192.168.0.10";
43+ baudrate_or_port_number_ = 10940;
44+ }
45+};
46+
47+
48+Connection_information::Connection_information(int argc,
49+ const char*const argv[])
50+ : pimpl(new pImpl)
51+{
52+ for (int i = 1; i < argc; ++i) {
53+ if (!strcmp(argv[i], "-e")) {
54+ pimpl->set_ethernet_connection();
55+ return;
56+ }
57+ }
58+ pimpl->set_serial_connection();
59+}
60+
61+
62+Connection_information::~Connection_information(void)
63+{
64+}
65+
66+
67+Urg_driver::connection_type_t
68+Connection_information::connection_type(void) const
69+{
70+ return pimpl->connection_type_;
71+}
72+
73+
74+const char* Connection_information::device_or_ip_name(void) const
75+{
76+ return pimpl->device_or_ip_name_.c_str();
77+}
78+
79+
80+long Connection_information::baudrate_or_port_number(void) const
81+{
82+ return pimpl->baudrate_or_port_number_;
83+}
Added: svn:keywords
## -0,0 +1 ##
+Id
\ No newline at end of property
--- branches/tsukuba_2010/libs/lidar/samples/get_distance.cpp (revision 1929)
+++ branches/tsukuba_2010/libs/lidar/samples/get_distance.cpp (revision 1930)
@@ -1,7 +1,86 @@
1+/*!
2+ \example get_distance.c 距離データを取得する
3+
4+ \author Satofumi KAMIMURA
5+
6+ $Id$
7+*/
8+
9+#include "Urg_driver.h"
10+#include "Connection_information.h"
11+#include "math_utilities.h"
12+#include <iostream>
13+
14+using namespace qrk;
15+using namespace std;
16+
17+
18+namespace
19+{
20+ void print_data(Urg_driver& urg, const vector<long>& data, long time_stamp)
21+ {
22+#if 1
23+ // 前方のデータのみを表示
24+ int front_index = urg.step2index(0);
25+ cout << data[front_index] << " [mm], ("
26+ << time_stamp << " [msec])" << endl;
27+
28+#else
29+ // 全てのデータの X-Y の位置を表示
30+ long min_distance = urg.min_distance();
31+ long max_distance = urg.max_distance();
32+ size_t data_n = data.size();
33+ for (size_t i = 0; i < data_n; ++i) {
34+ long l = data[i];
35+ if ((l <= min_distance) || (l >= max_distance)) {
36+ continue;
37+ }
38+
39+ double radian = urg.index2rad(i);
40+ long x = static_cast<long>(l * cos(radian));
41+ long y = static_cast<long>(l * sin(radian));
42+ cout << "(" << x << ", " << y << ")" << endl;
43+ }
44+ cout << endl;
45+#endif
46+ }
47+}
48+
49+
150 int main(int argc, char *argv[])
251 {
3- (void)argc;
4- (void)argv;
5- // !!!
52+ Connection_information information(argc, argv);
53+
54+ // 接続
55+ Urg_driver urg;
56+ if (!urg.open(information.device_or_ip_name(),
57+ information.baudrate_or_port_number(),
58+ information.connection_type())) {
59+ cout << "Urg_driver::open(): "
60+ << information.device_or_ip_name() << ": " << urg.what() << endl;
61+ return 1;
62+ }
63+
64+ // データ取得
65+#if 1
66+ // データの取得範囲を変更する場合
67+ urg.set_scanning_parameter(urg.deg2step(-90), urg.deg2step(+90), 0);
68+#endif
69+ enum { Capture_times = 10 };
70+ urg.start_measurement(Urg_driver::Distance, Capture_times, 0);
71+ for (int i = 0; i < Capture_times; ++i) {
72+ vector<long> data;
73+ long time_stamp = 0;
74+
75+ if (!urg.get_distance(data, &time_stamp)) {
76+ cout << "Urg_driver::get_distance(): " << urg.what() << endl;
77+ return 1;
78+ }
79+ print_data(urg, data, time_stamp);
80+ }
81+
82+#if defined(URG_MSC)
83+ getchar();
84+#endif
685 return 0;
786 }
--- branches/tsukuba_2010/libs/lidar/samples/sensor_parameter.cpp (revision 1929)
+++ branches/tsukuba_2010/libs/lidar/samples/sensor_parameter.cpp (revision 1930)
@@ -7,71 +7,28 @@
77 */
88
99 #include "Urg_driver.h"
10+#include "Connection_information.h"
1011 #include <iostream>
11-#include <string>
12-#include <cstring>
1312
1413 using namespace qrk;
1514 using namespace std;
1615
1716
18-namespace
19-{
20- typedef struct
21- {
22- Urg_driver::connection_type_t connection_type;
23- string device_or_ip;
24- long baudrate_or_port;
25- } connection_information_t;
26-
27-
28- void set_serial_connection(connection_information_t& information)
29- {
30- information.connection_type = Urg_driver::Serial;
31-#if defined(URG_WINDOWS_OS)
32- information.device_or_ip = "COM3";
33-#elif defined(URG_LINUX_OS)
34- information.device_or_ip = "/dev/ttyACM0";
35-#else
36-#endif
37- information.baudrate_or_port = 115200;
38- }
39-
40-
41- void set_ethernet_connection(connection_information_t& information)
42- {
43- information.connection_type = Urg_driver::Ethernet;
44- information.device_or_ip = "192.168.0.10";
45- information.baudrate_or_port = 10940;
46- }
47-
48-
49- void parse_connection_setting(connection_information_t& information,
50- int argc, char *argv[])
51- {
52- for (int i = 1; i < argc; ++i) {
53- if (!strcmp(argv[i], "-e")) {
54- set_ethernet_connection(information);
55- return;
56- }
57- }
58- set_serial_connection(information);
59- }
60-}
61-
62-
6317 int main(int argc, char *argv[])
6418 {
65- connection_information_t information;
66- parse_connection_setting(information, argc, argv);
19+ Connection_information information(argc, argv);
6720
21+ // 接続
6822 Urg_driver urg;
69- if (urg.open(information.device_or_ip.c_str(), information.baudrate_or_port,
70- information.connection_type)) {
71- cout << "Urg_driver::open():" << urg.what() << endl;
23+ if (!urg.open(information.device_or_ip_name(),
24+ information.baudrate_or_port_number(),
25+ information.connection_type())) {
26+ cout << "Urg_driver::open():"
27+ << information.device_or_ip_name() << ": " << urg.what() << endl;
7228 return 1;
7329 }
7430
31+ // パラメータ情報の表示
7532 printf("Sensor firmware version: %s\n", urg.version());
7633 printf("Sensor serial ID: %s\n", urg.serial_id());
7734 printf("Sensor status: %s\n", urg.status());
--- branches/tsukuba_2010/libs/lidar/samples/Connection_information.h (nonexistent)
+++ branches/tsukuba_2010/libs/lidar/samples/Connection_information.h (revision 1930)
@@ -0,0 +1,36 @@
1+#ifndef QRK_CONNECTION_INFORMATION_H
2+#define QRK_CONNECTION_INFORMATION_H
3+
4+/*!
5+ \file
6+ \brief 接続情報の管理
7+
8+ \author Satofumi KAMIMURA
9+
10+ $Id$
11+*/
12+
13+#include "Urg_driver.h"
14+#include <memory>
15+
16+
17+namespace qrk
18+{
19+ class Connection_information
20+ {
21+ public:
22+ Connection_information(int argc, const char*const argv[]);
23+ ~Connection_information(void);
24+
25+ Urg_driver::connection_type_t connection_type() const;
26+ const char* device_or_ip_name(void) const;
27+ long baudrate_or_port_number(void) const;
28+
29+ private:
30+ Connection_information(void);
31+ struct pImpl;
32+ std::auto_ptr<pImpl> pimpl;
33+ };
34+}
35+
36+#endif /* !QRK_CONNECTION_INFORMATION_H */
Added: svn:keywords
## -0,0 +1 ##
+Id
\ No newline at end of property
--- branches/tsukuba_2010/libs/lidar/samples/Makefile (revision 1929)
+++ branches/tsukuba_2010/libs/lidar/samples/Makefile (revision 1930)
@@ -1,8 +1,8 @@
11 # lidar/samples
22
33 CC = $(CXX)
4-CXXFLAGS = -g -O0 -Wall -Werror $(INCLUDES)
5-INCLUDES = -I..
4+CXXFLAGS = -g -O0 -Wall -Werror $(INCLUDES) `urg_c-config --cflags`
5+INCLUDES = -I.. -I../../math -I../../system
66 LDFLAGS =
77 LDLIBS = `urg_c-config --libs`
88
@@ -30,6 +30,10 @@
3030 $(REQUIRE_LIBS) :
3131 cd $(@D)/ && $(MAKE) $(@F)
3232
33-$(TARGET) : $(REQUIRE_LIBS)
33+$(TARGET) : Connection_information.o $(REQUIRE_LIBS)
3434
3535 # DO NOT DELETE
36+
37+Connection_information.o: Connection_information.h
38+calculate_xy.o: Connection_information.h
39+sensor_parameter.o: Connection_information.h
--- branches/tsukuba_2010/libs/lidar/samples/calculate_xy.cpp (revision 1929)
+++ branches/tsukuba_2010/libs/lidar/samples/calculate_xy.cpp (revision 1930)
@@ -1,7 +1,68 @@
1+/*!
2+ \example calculate_xy.c X-Y 座標系での位置を計算する
3+
4+ センサ前方が X 軸の方向とみなした直行座標上で、距離データを位置を出力する。
5+
6+ \author Satofumi KAMIMURA
7+
8+ $Id$
9+*/
10+
11+#include "Urg_driver.h"
12+#include "Connection_information.h"
13+#include "math_utilities.h"
14+#include <iostream>
15+
16+using namespace qrk;
17+using namespace std;
18+
19+
120 int main(int argc, char *argv[])
221 {
3- (void)argc;
4- (void)argv;
5- // !!!
22+ Connection_information information(argc, argv);
23+
24+ // 接続
25+ Urg_driver urg;
26+ if (!urg.open(information.device_or_ip_name(),
27+ information.baudrate_or_port_number(),
28+ information.connection_type())) {
29+ cout << "Urg_driver::open(): "
30+ << information.device_or_ip_name() << ": " << urg.what() << endl;
31+ return 1;
32+ }
33+
34+ // データ取得
35+ if (!urg.start_measurement(Urg_driver::Distance, 1)) {
36+ cout << "Urg_driver::start_measurement(): " << urg.what() << endl;
37+ return 1;
38+ }
39+
40+ vector<long> length_data;
41+ if (!urg.get_distance(length_data)) {
42+ cout << "Urg_driver::get_distance(): " << urg.what() << endl;
43+ return 1;
44+ }
45+
46+ // 座標系の値を出力
47+ long min_distance = urg.min_distance();
48+ long max_distance = urg.max_distance();
49+ size_t n = length_data.size();
50+ for (size_t i = 0; i < n; ++i) {
51+ long distance = length_data[i];
52+ if ((distance < min_distance) || (distance > max_distance)) {
53+ continue;
54+ }
55+
56+ double radian = urg.index2rad(i);
57+ long x = static_cast<long>(distance * cos(radian));
58+ long y = static_cast<long>(distance * sin(radian));
59+
60+ cout << x << ", " << y << endl;
61+ }
62+ cout << endl;
63+
64+#if defined(URG_MSC)
65+ getchar();
66+#endif
667 return 0;
768 }
--- branches/tsukuba_2010/libs/lidar/Urg_driver.cpp (revision 1929)
+++ branches/tsukuba_2010/libs/lidar/Urg_driver.cpp (revision 1930)
@@ -11,8 +11,8 @@
1111 extern "C" {
1212 #include "urg_sensor.h"
1313 #include "urg_utils.h"
14+#include "urg_errno.h"
1415 }
15-#include <string>
1616
1717 using namespace qrk;
1818 using namespace std;
@@ -21,10 +21,11 @@
2121 struct Urg_driver::pImpl
2222 {
2323 urg_t urg_;
24- string error_message_;
24+ bool is_opened_;
25+ measurement_type_t last_measure_type_;
2526
2627
27- pImpl(void) : error_message_("no error.")
28+ pImpl(void) : is_opened_(false), last_measure_type_(Distance)
2829 {
2930 }
3031 };
@@ -37,12 +38,15 @@
3738
3839 Urg_driver::~Urg_driver(void)
3940 {
41+ if (pimpl->is_opened_) {
42+ close();
43+ }
4044 }
4145
4246
4347 const char* Urg_driver::what(void) const
4448 {
45- return pimpl->error_message_.c_str();
49+ return urg_error(&pimpl->urg_);
4650 }
4751
4852
@@ -49,11 +53,12 @@
4953 bool Urg_driver::open(const char* device_name, long baudrate,
5054 connection_type_t type)
5155 {
56+ pimpl->is_opened_ = true;
57+
5258 urg_connection_type_t connection_type =
5359 (type == Ethernet) ? URG_ETHERNET : URG_SERIAL;
5460 int ret = urg_open(&pimpl->urg_, connection_type, device_name, baudrate);
5561 if (ret < 0) {
56- pimpl->error_message_ = urg_error(&pimpl->urg_);
5762 return false;
5863 }
5964 return true;
@@ -69,31 +74,87 @@
6974 bool Urg_driver::start_measurement(measurement_type_t type,
7075 int scan_times, int skip_scan)
7176 {
72- (void)type;
73- (void)scan_times;
74- (void)skip_scan;
75- // !!!
77+ typedef struct {
78+ urg_measurement_type_t c_type;
79+ measurement_type_t type;
80+ } type_table_t;
7681
77- return -1;
82+ type_table_t type_table[] = {
83+ { URG_DISTANCE, Distance },
84+ { URG_DISTANCE_INTENSITY, Distance_intensity },
85+ { URG_MULTIECHO, Multiecho },
86+ { URG_MULTIECHO_INTENSITY, Multiecho_intensity },
87+ };
88+
89+ size_t n = sizeof(type_table) / sizeof(type_table[0]);
90+ for (size_t i = 0; i < n; ++i) {
91+ const type_table_t* p = &type_table[i];
92+ if (type == p->type) {
93+ int ret = urg_start_measurement(&pimpl->urg_,
94+ p->c_type, scan_times, skip_scan);
95+ if (ret < 0) {
96+ return false;
97+ } else {
98+ pimpl->last_measure_type_ = type;
99+ return true;
100+ }
101+ return (ret < 0) ? false : true;
102+ }
103+ }
104+ return false;
78105 }
79106
80107
81108 bool Urg_driver::get_distance(std::vector<long>& data, long* time_stamp)
82109 {
110+ if (pimpl->last_measure_type_ != Distance) {
111+ pimpl->urg_.last_errno = URG_MEASUREMENT_TYPE_MISMATCH;
112+ return false;
113+ }
114+
115+ // 最大サイズを確保し、そこにデータを格納する
116+ data.resize(max_data_size());
117+ int ret = urg_get_distance(&pimpl->urg_, &data[0], time_stamp);
118+ if (ret > 0) {
119+ data.resize(ret);
120+ }
121+
122+ return (ret < 0) ? false : true;
123+}
124+
125+
126+bool Urg_driver::get_distance_intensity(std::vector<long>& data,
127+ std::vector<unsigned short>& intensity,
128+ long* time_stamp)
129+{
130+ if (pimpl->last_measure_type_ != Distance_intensity) {
131+ pimpl->urg_.last_errno = URG_MEASUREMENT_TYPE_MISMATCH;
132+ return false;
133+ }
134+
83135 (void)data;
136+ (void)intensity;
84137 (void)time_stamp;
85138 // !!!
86-
87139 return false;
88140 }
89141
90142
91-void Urg_driver::urg_stop_measurement(void)
143+bool Urg_driver::set_scanning_parameter(int first_step, int last_step,
144+ int skip_step)
92145 {
93- // !!!
146+ int ret = urg_set_scanning_parameter(&pimpl->urg_,
147+ first_step, last_step, skip_step);
148+ return (ret < 0) ? false : true;
94149 }
95150
96151
152+void Urg_driver::stop_measurement(void)
153+{
154+ urg_stop_measurement(&pimpl->urg_);
155+}
156+
157+
97158 bool Urg_driver::set_sensor_timestamp(long timestamp)
98159 {
99160 (void)timestamp;
@@ -103,6 +164,60 @@
103164 }
104165
105166
167+double Urg_driver::index2rad(int index) const
168+{
169+ return urg_index2rad(&pimpl->urg_, index);
170+}
171+
172+
173+double Urg_driver::index2deg(int index) const
174+{
175+ return urg_index2deg(&pimpl->urg_, index);
176+}
177+
178+
179+int Urg_driver::rad2index(double radian) const
180+{
181+ return urg_rad2index(&pimpl->urg_, radian);
182+}
183+
184+
185+int Urg_driver::deg2index(double degree) const
186+{
187+ return urg_deg2index(&pimpl->urg_, degree);
188+}
189+
190+
191+int Urg_driver::rad2step(double radian) const
192+{
193+ return urg_rad2step(&pimpl->urg_, radian);
194+}
195+
196+
197+int Urg_driver::deg2step(double degree) const
198+{
199+ return urg_deg2step(&pimpl->urg_, degree);
200+}
201+
202+
203+double Urg_driver::step2rad(int step) const
204+{
205+ return urg_step2rad(&pimpl->urg_, step);
206+}
207+
208+
209+double Urg_driver::step2deg(int step) const
210+{
211+ return urg_step2deg(&pimpl->urg_, step);
212+}
213+
214+
215+int Urg_driver::step2index(int step) const
216+{
217+ return urg_step2index(&pimpl->urg_, step);
218+}
219+
220+
106221 int Urg_driver::min_step(void) const
107222 {
108223 int min_step;
--- branches/tsukuba_2010/libs/lidar/Urg_driver.h (revision 1929)
+++ branches/tsukuba_2010/libs/lidar/Urg_driver.h (revision 1930)
@@ -58,10 +58,23 @@
5858
5959 // !!! 受信データの受け取り
6060 bool get_distance(std::vector<long>& data, long* time_stamp = NULL);
61+ bool get_distance_intensity(std::vector<long>& data,
62+ std::vector<unsigned short>& intensity,
63+ long* time_stamp = NULL);
6164
65+ //extern int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp);
6266
67+ //extern int urg_get_multiecho_intensity(urg_t *urg, long data_multi[],
68+ //unsigned short intensity_multi[],
69+ //long *time_stamp);
70+
71+
72+ bool set_scanning_parameter(int first_step, int last_step,
73+ int skip_step);
74+
75+
6376 // !!! データ取得の中断
64- void urg_stop_measurement(void);
77+ void stop_measurement(void);
6578
6679
6780 // !!! タイムスタンプの同期
@@ -68,9 +81,15 @@
6881 bool set_sensor_timestamp(long timestamp);
6982
7083 // !!! 角度変換
71- // !!! 最小距離、最大距離の取得
72- // !!! 設定可能範囲の取得
73- // !!! 1周あたりのスキャン時間の取得
84+ double index2rad(int index) const;
85+ double index2deg(int index) const;
86+ int rad2index(double radian) const;
87+ int deg2index(double degree) const;
88+ int rad2step(double radian) const;
89+ int deg2step(double degree) const;
90+ double step2rad(int step) const;
91+ double step2deg(int step) const;
92+ int step2index(int step) const;
7493
7594 int min_step(void) const;
7695 int max_step(void) const;
旧リポジトリブラウザで表示