• R/O
  • SSH
  • HTTPS

qrobosdk: コミット


コミットメタ情報

リビジョン1977 (tree)
日時2012-04-05 17:04:51
作者satofumi

ログメッセージ

change line feed.

変更サマリ

差分

--- trunk/widgets/SerialConnectionWidget/SerialConnectionWidget.cpp (revision 1976)
+++ trunk/widgets/SerialConnectionWidget/SerialConnectionWidget.cpp (revision 1977)
@@ -1,142 +1,142 @@
1-/*!
2- \file
3- \brief 接続管理ウィジット
4-
5- \author Satofumi KAMIMURA
6-
7- $Id$
8-*/
9-
10-#include "SerialConnectionWidget.h"
11-
12-using namespace qrk;
13-using namespace std;
14-
15-
16-struct SerialConnectionWidget::pImpl
17-{
18- SerialConnectionWidget* widget_;
19- bool is_empty_;
20-
21-
22- pImpl(SerialConnectionWidget* widget) : widget_(widget), is_empty_(true)
23- {
24- }
25-
26-
27- void initializeForm(void)
28- {
29- widget_->device_combobox_->setSizeAdjustPolicy(QComboBox::AdjustToContents);
30-
31- connect(widget_->connect_button_, SIGNAL(clicked(bool)),
32- widget_, SLOT(connectPressed(bool)));
33- connect(widget_->rescan_button_, SIGNAL(clicked(bool)),
34- widget_, SLOT(rescanPressed()));
35- }
36-};
37-
38-
39-SerialConnectionWidget::SerialConnectionWidget(QWidget* widget)
40- : QWidget(widget), pimpl(new pImpl(this))
41-{
42- setupUi(this);
43- pimpl->initializeForm();
44-
45- vector<string> empty_devices;
46- setDevices(empty_devices);
47-}
48-
49-
50-SerialConnectionWidget::~SerialConnectionWidget(void)
51-{
52-}
53-
54-
55-void SerialConnectionWidget::connectPressed(bool checked)
56-{
57- // 接続ボタンが押下されたとき、setConnected() が呼ばれるまでボタンを無効にする
58- connect_button_->setEnabled(false);
59- string device_name = device_combobox_->currentText().toStdString();
60- string device = device_name.substr(0, device_name.find(' '));
61- emit connectRequest(checked, device);
62-}
63-
64-
65-void SerialConnectionWidget::rescanPressed(void)
66-{
67- emit rescanRequest();
68-}
69-
70-
71-void SerialConnectionWidget::setConnected(bool connected)
72-{
73- connect_button_->setEnabled(true);
74- connect_button_->setChecked(connected);
75-
76- if (connected) {
77- connect_button_->setText(tr("Disconnect"));
78- } else {
79- connect_button_->setText(tr("Connect"));
80- }
81- device_combobox_->setEnabled(! connected);
82- rescan_button_->setEnabled(! connected);
83-
84- connect_button_->setFocus();
85-}
86-
87-
88-bool SerialConnectionWidget::isConnected(void) const
89-{
90- return (connect_button_->isEnabled() && connect_button_->isChecked())
91- ? true : false;
92-}
93-
94-
95-void SerialConnectionWidget::setDevices(const vector<string>& devices)
96-{
97- device_combobox_->clear();
98- for (vector<string>::const_iterator it = devices.begin();
99- it != devices.end(); ++it) {
100- device_combobox_->addItem(it->c_str());
101- }
102-
103- bool connectable = (device_combobox_->count() == 0) ? false : true;
104- device_combobox_->setEnabled(connectable);
105- connect_button_->setEnabled(connectable);
106-
107- pimpl->is_empty_ = devices.empty();
108- if (pimpl->is_empty_) {
109- device_combobox_->addItem(tr("-- press rescan --"));
110- }
111-}
112-
113-
114-vector<string> SerialConnectionWidget::devices(void)
115-{
116- vector<string> devices;
117-
118- if (! pimpl->is_empty_) {
119- size_t n = device_combobox_->count();
120- for (size_t i = 0; i < n; ++n) {
121- devices.push_back(device_combobox_->itemText(n).toStdString());
122- }
123- }
124-
125- return devices;
126-}
127-
128-
129-void SerialConnectionWidget::setEnabled(bool enable)
130-{
131- QWidget::setEnabled(enable);
132-}
133-
134-
135-void SerialConnectionWidget::setFocus(void)
136-{
137- if (! connect_button_->isEnabled()) {
138- rescan_button_->setFocus();
139- } else {
140- connect_button_->setFocus();
141- }
142-}
1+/*!
2+ \file
3+ \brief 接続管理ウィジット
4+
5+ \author Satofumi KAMIMURA
6+
7+ $Id$
8+*/
9+
10+#include "SerialConnectionWidget.h"
11+
12+using namespace qrk;
13+using namespace std;
14+
15+
16+struct SerialConnectionWidget::pImpl
17+{
18+ SerialConnectionWidget* widget_;
19+ bool is_empty_;
20+
21+
22+ pImpl(SerialConnectionWidget* widget) : widget_(widget), is_empty_(true)
23+ {
24+ }
25+
26+
27+ void initializeForm(void)
28+ {
29+ widget_->device_combobox_->setSizeAdjustPolicy(QComboBox::AdjustToContents);
30+
31+ connect(widget_->connect_button_, SIGNAL(clicked(bool)),
32+ widget_, SLOT(connectPressed(bool)));
33+ connect(widget_->rescan_button_, SIGNAL(clicked(bool)),
34+ widget_, SLOT(rescanPressed()));
35+ }
36+};
37+
38+
39+SerialConnectionWidget::SerialConnectionWidget(QWidget* widget)
40+ : QWidget(widget), pimpl(new pImpl(this))
41+{
42+ setupUi(this);
43+ pimpl->initializeForm();
44+
45+ vector<string> empty_devices;
46+ setDevices(empty_devices);
47+}
48+
49+
50+SerialConnectionWidget::~SerialConnectionWidget(void)
51+{
52+}
53+
54+
55+void SerialConnectionWidget::connectPressed(bool checked)
56+{
57+ // 接続ボタンが押下されたとき、setConnected() が呼ばれるまでボタンを無効にする
58+ connect_button_->setEnabled(false);
59+ string device_name = device_combobox_->currentText().toStdString();
60+ string device = device_name.substr(0, device_name.find(' '));
61+ emit connectRequest(checked, device);
62+}
63+
64+
65+void SerialConnectionWidget::rescanPressed(void)
66+{
67+ emit rescanRequest();
68+}
69+
70+
71+void SerialConnectionWidget::setConnected(bool connected)
72+{
73+ connect_button_->setEnabled(true);
74+ connect_button_->setChecked(connected);
75+
76+ if (connected) {
77+ connect_button_->setText(tr("Disconnect"));
78+ } else {
79+ connect_button_->setText(tr("Connect"));
80+ }
81+ device_combobox_->setEnabled(! connected);
82+ rescan_button_->setEnabled(! connected);
83+
84+ connect_button_->setFocus();
85+}
86+
87+
88+bool SerialConnectionWidget::isConnected(void) const
89+{
90+ return (connect_button_->isEnabled() && connect_button_->isChecked())
91+ ? true : false;
92+}
93+
94+
95+void SerialConnectionWidget::setDevices(const vector<string>& devices)
96+{
97+ device_combobox_->clear();
98+ for (vector<string>::const_iterator it = devices.begin();
99+ it != devices.end(); ++it) {
100+ device_combobox_->addItem(it->c_str());
101+ }
102+
103+ bool connectable = (device_combobox_->count() == 0) ? false : true;
104+ device_combobox_->setEnabled(connectable);
105+ connect_button_->setEnabled(connectable);
106+
107+ pimpl->is_empty_ = devices.empty();
108+ if (pimpl->is_empty_) {
109+ device_combobox_->addItem(tr("-- press rescan --"));
110+ }
111+}
112+
113+
114+vector<string> SerialConnectionWidget::devices(void)
115+{
116+ vector<string> devices;
117+
118+ if (! pimpl->is_empty_) {
119+ size_t n = device_combobox_->count();
120+ for (size_t i = 0; i < n; ++n) {
121+ devices.push_back(device_combobox_->itemText(n).toStdString());
122+ }
123+ }
124+
125+ return devices;
126+}
127+
128+
129+void SerialConnectionWidget::setEnabled(bool enable)
130+{
131+ QWidget::setEnabled(enable);
132+}
133+
134+
135+void SerialConnectionWidget::setFocus(void)
136+{
137+ if (! connect_button_->isEnabled()) {
138+ rescan_button_->setFocus();
139+ } else {
140+ connect_button_->setFocus();
141+ }
142+}
--- trunk/widgets/SerialConnectionWidget/SerialConnectionWidget.h (revision 1976)
+++ trunk/widgets/SerialConnectionWidget/SerialConnectionWidget.h (revision 1977)
@@ -1,54 +1,54 @@
1-#ifndef QRK_SERIAL_CONNECTION_WIDGET_H
2-#define QRK_SERIAL_CONNECTION_WIDGET_H
3-
4-/*!
5- \file
6- \brief 接続管理ウィジット
7-
8- \author Satofumi KAMIMURA
9-
10- $Id$
11-*/
12-
13-#include "ui_SerialConnectionWidgetForm.h"
14-#include <memory>
15-
16-
17-namespace qrk
18-{
19- class SerialConnectionWidget :
20- public QWidget, private Ui::SerialConnectionWidgetForm
21- {
22- Q_OBJECT;
23-
24- public:
25- SerialConnectionWidget(QWidget* parent = 0);
26- ~SerialConnectionWidget(void);
27-
28- void setConnected(bool connected);
29- bool isConnected(void) const;
30-
31- void setDevices(const std::vector<std::string>& devices);
32- std::vector<std::string> devices(void);
33-
34- void setEnabled(bool enable);
35- void setFocus(void);
36-
37- signals:
38- void connectRequest(bool connection, const std::string& device);
39- void rescanRequest(void);
40-
41- private slots:
42- void connectPressed(bool checked);
43- void rescanPressed(void);
44-
45- private:
46- SerialConnectionWidget(const SerialConnectionWidget& rhs);
47- SerialConnectionWidget& operator = (const SerialConnectionWidget& rhs);
48-
49- struct pImpl;
50- std::auto_ptr<pImpl> pimpl;
51- };
52-}
53-
54-#endif /*! QRK_SERIAL_CONNECTION_WIDGET_H */
1+#ifndef QRK_SERIAL_CONNECTION_WIDGET_H
2+#define QRK_SERIAL_CONNECTION_WIDGET_H
3+
4+/*!
5+ \file
6+ \brief 接続管理ウィジット
7+
8+ \author Satofumi KAMIMURA
9+
10+ $Id$
11+*/
12+
13+#include "ui_SerialConnectionWidgetForm.h"
14+#include <memory>
15+
16+
17+namespace qrk
18+{
19+ class SerialConnectionWidget :
20+ public QWidget, private Ui::SerialConnectionWidgetForm
21+ {
22+ Q_OBJECT;
23+
24+ public:
25+ SerialConnectionWidget(QWidget* parent = 0);
26+ ~SerialConnectionWidget(void);
27+
28+ void setConnected(bool connected);
29+ bool isConnected(void) const;
30+
31+ void setDevices(const std::vector<std::string>& devices);
32+ std::vector<std::string> devices(void);
33+
34+ void setEnabled(bool enable);
35+ void setFocus(void);
36+
37+ signals:
38+ void connectRequest(bool connection, const std::string& device);
39+ void rescanRequest(void);
40+
41+ private slots:
42+ void connectPressed(bool checked);
43+ void rescanPressed(void);
44+
45+ private:
46+ SerialConnectionWidget(const SerialConnectionWidget& rhs);
47+ SerialConnectionWidget& operator = (const SerialConnectionWidget& rhs);
48+
49+ struct pImpl;
50+ std::auto_ptr<pImpl> pimpl;
51+ };
52+}
53+
54+#endif /*! QRK_SERIAL_CONNECTION_WIDGET_H */
--- trunk/widgets/SerialConnectionWidget/SerialConnectionWidgetSample.h (revision 1976)
+++ trunk/widgets/SerialConnectionWidget/SerialConnectionWidgetSample.h (revision 1977)
@@ -1,39 +1,39 @@
1-#ifndef QRK_SERIAL_CONNECTION_WIDGET_SAMPLE_H
2-#define QRK_SERIAL_CONNECTION_WIDGET_SAMPLE_H
3-
4-/*!
5- \file
6- \brief SerialConnectionWidget の動作確認ウィジット
7-
8- \author Satofumi KAMIMURA
9-
10- $Id$
11-*/
12-
13-#include "ui_SerialConnectionWidgetSampleForm.h"
14-#include <memory>
15-
16-
17-class SerialConnectionWidgetSample
18- : public QWidget, private Ui::SerialConnectionWidgetSampleForm
19-{
20- Q_OBJECT;
21-
22-public:
23- SerialConnectionWidgetSample(QWidget* parent = 0);
24- ~SerialConnectionWidgetSample(void);
25-
26-private slots:
27- void connectPressed(bool connection, const std::string& device);
28- void rescanPressed(void);
29-
30-private:
31- SerialConnectionWidgetSample(const SerialConnectionWidgetSample& rhs);
32- SerialConnectionWidgetSample& operator
33- = (const SerialConnectionWidgetSample& rhs);
34-
35- struct pImpl;
36- std::auto_ptr<pImpl> pimpl;
37-};
38-
39-#endif /* !QRK_SERIAL_CONNECTION_WIDGET_SAMPLE_H */
1+#ifndef QRK_SERIAL_CONNECTION_WIDGET_SAMPLE_H
2+#define QRK_SERIAL_CONNECTION_WIDGET_SAMPLE_H
3+
4+/*!
5+ \file
6+ \brief SerialConnectionWidget の動作確認ウィジット
7+
8+ \author Satofumi KAMIMURA
9+
10+ $Id$
11+*/
12+
13+#include "ui_SerialConnectionWidgetSampleForm.h"
14+#include <memory>
15+
16+
17+class SerialConnectionWidgetSample
18+ : public QWidget, private Ui::SerialConnectionWidgetSampleForm
19+{
20+ Q_OBJECT;
21+
22+public:
23+ SerialConnectionWidgetSample(QWidget* parent = 0);
24+ ~SerialConnectionWidgetSample(void);
25+
26+private slots:
27+ void connectPressed(bool connection, const std::string& device);
28+ void rescanPressed(void);
29+
30+private:
31+ SerialConnectionWidgetSample(const SerialConnectionWidgetSample& rhs);
32+ SerialConnectionWidgetSample& operator
33+ = (const SerialConnectionWidgetSample& rhs);
34+
35+ struct pImpl;
36+ std::auto_ptr<pImpl> pimpl;
37+};
38+
39+#endif /* !QRK_SERIAL_CONNECTION_WIDGET_SAMPLE_H */
--- trunk/libs/system/qt/Thread.cpp (revision 1976)
+++ trunk/libs/system/qt/Thread.cpp (revision 1977)
@@ -1,105 +1,105 @@
1-/*!
2- \file
3- \brief スレッド処理のラッパー
4-
5- \author Satofumi KAMIMURA
6-
7- $Id$
8-*/
9-
10-#include "Thread.h"
11-#include <QThread>
12-#include <limits>
13-
14-using namespace qrk;
15-using namespace std;
16-
17-
18-namespace
19-{
20- class ThreadWrapper : public QThread
21- {
22- int (*function_)(void *);
23- void* args_;
24- int times_;
25- int return_value_;
26-
27-
28- public:
29-
30- ThreadWrapper(int (*function)(void *), void* args)
31- : function_(function), args_(args), times_(1), return_value_(-1)
32- {
33- }
34-
35-
36- void setTimes(int times)
37- {
38- times_ = times;
39- }
40-
41-
42- void run(void)
43- {
44- for (int i = 0; i < times_; ++i) {
45- return_value_ = function_(args_);
46- usleep(1);
47- }
48- }
49-
50-
51- int returnValue(void)
52- {
53- return return_value_;
54- }
55- };
56-}
57-
58-
59-struct Thread::pImpl
60-{
61- ThreadWrapper thread_;
62-
63-
64- pImpl(int (*fn)(void *), void* args) : thread_(fn, args)
65- {
66- }
67-};
68-
69-
70-Thread::Thread(int (*fn)(void *), void* args) : pimpl(new pImpl(fn, args))
71-{
72-}
73-
74-
75-Thread::~Thread(void)
76-{
77- pimpl->thread_.wait();
78-}
79-
80-
81-void Thread::run(int times)
82-{
83- pimpl->thread_.setTimes((times == Infinity)
84- ? numeric_limits<int>::max() : times);
85- pimpl->thread_.start();
86-}
87-
88-
89-void Thread::stop(void)
90-{
91- pimpl->thread_.terminate();
92-}
93-
94-
95-int Thread::wait(void)
96-{
97- pimpl->thread_.wait();
98- return pimpl->thread_.returnValue();
99-}
100-
101-
102-bool Thread::isRunning(void) const
103-{
104- return pimpl->thread_.isRunning();
105-}
1+/*!
2+ \file
3+ \brief スレッド処理のラッパー
4+
5+ \author Satofumi KAMIMURA
6+
7+ $Id$
8+*/
9+
10+#include "Thread.h"
11+#include <QThread>
12+#include <limits>
13+
14+using namespace qrk;
15+using namespace std;
16+
17+
18+namespace
19+{
20+ class ThreadWrapper : public QThread
21+ {
22+ int (*function_)(void *);
23+ void* args_;
24+ int times_;
25+ int return_value_;
26+
27+
28+ public:
29+
30+ ThreadWrapper(int (*function)(void *), void* args)
31+ : function_(function), args_(args), times_(1), return_value_(-1)
32+ {
33+ }
34+
35+
36+ void setTimes(int times)
37+ {
38+ times_ = times;
39+ }
40+
41+
42+ void run(void)
43+ {
44+ for (int i = 0; i < times_; ++i) {
45+ return_value_ = function_(args_);
46+ usleep(1);
47+ }
48+ }
49+
50+
51+ int returnValue(void)
52+ {
53+ return return_value_;
54+ }
55+ };
56+}
57+
58+
59+struct Thread::pImpl
60+{
61+ ThreadWrapper thread_;
62+
63+
64+ pImpl(int (*fn)(void *), void* args) : thread_(fn, args)
65+ {
66+ }
67+};
68+
69+
70+Thread::Thread(int (*fn)(void *), void* args) : pimpl(new pImpl(fn, args))
71+{
72+}
73+
74+
75+Thread::~Thread(void)
76+{
77+ pimpl->thread_.wait();
78+}
79+
80+
81+void Thread::run(int times)
82+{
83+ pimpl->thread_.setTimes((times == Infinity)
84+ ? numeric_limits<int>::max() : times);
85+ pimpl->thread_.start();
86+}
87+
88+
89+void Thread::stop(void)
90+{
91+ pimpl->thread_.terminate();
92+}
93+
94+
95+int Thread::wait(void)
96+{
97+ pimpl->thread_.wait();
98+ return pimpl->thread_.returnValue();
99+}
100+
101+
102+bool Thread::isRunning(void) const
103+{
104+ return pimpl->thread_.isRunning();
105+}
--- trunk/libs/system/Thread.h (revision 1976)
+++ trunk/libs/system/Thread.h (revision 1977)
@@ -1,74 +1,74 @@
1-#ifndef QRK_THREAD_H
2-#define QRK_THREAD_H
3-
4-/*!
5- \file
6- \brief スレッド処理のラッパー
7-
8- \author Satofumi KAMIMURA
9-
10- $Id$
11-*/
12-
13-#include <memory>
14-
15-
16-namespace qrk
17-{
18- //! スレッド処理
19- class Thread
20- {
21- public:
22- enum {
23- Infinity = -1,
24- };
25-
26- /*!
27- \brief コンストラクタ
28-
29- \param[in,out] fn 関数ポインタ
30- \param[in,out] args スレッド関数への引数
31- */
32- explicit Thread(int (*fn)(void *), void* args);
33- ~Thread(void);
34-
35-
36- /*!
37- \brief 処理回数を指定してスレッドを起動
38-
39- \param[in] times 処理回数
40- */
41- void run(int times = 1);
42-
43-
44- //! スレッドを停止
45- void stop(void);
46-
47-
48- /*!
49- \brief スレッドの終了を待つ
50-
51- \return スレッドの戻り値
52- */
53- int wait(void);
54-
55-
56- /*!
57- \brief スレッドが動作中かを返す
58-
59- \retval true 動作中
60- \retval false 停止中
61- */
62- bool isRunning(void) const;
63-
64- private:
65- Thread(void);
66- Thread(const Thread& rhs);
67- Thread& operator = (const Thread& rhs);
68-
69- struct pImpl;
70- const std::auto_ptr<pImpl> pimpl;
71- };
72-}
73-
74-#endif /* !QRK_THREAD_H */
1+#ifndef QRK_THREAD_H
2+#define QRK_THREAD_H
3+
4+/*!
5+ \file
6+ \brief スレッド処理のラッパー
7+
8+ \author Satofumi KAMIMURA
9+
10+ $Id$
11+*/
12+
13+#include <memory>
14+
15+
16+namespace qrk
17+{
18+ //! スレッド処理
19+ class Thread
20+ {
21+ public:
22+ enum {
23+ Infinity = -1,
24+ };
25+
26+ /*!
27+ \brief コンストラクタ
28+
29+ \param[in,out] fn 関数ポインタ
30+ \param[in,out] args スレッド関数への引数
31+ */
32+ explicit Thread(int (*fn)(void *), void* args);
33+ ~Thread(void);
34+
35+
36+ /*!
37+ \brief 処理回数を指定してスレッドを起動
38+
39+ \param[in] times 処理回数
40+ */
41+ void run(int times = 1);
42+
43+
44+ //! スレッドを停止
45+ void stop(void);
46+
47+
48+ /*!
49+ \brief スレッドの終了を待つ
50+
51+ \return スレッドの戻り値
52+ */
53+ int wait(void);
54+
55+
56+ /*!
57+ \brief スレッドが動作中かを返す
58+
59+ \retval true 動作中
60+ \retval false 停止中
61+ */
62+ bool isRunning(void) const;
63+
64+ private:
65+ Thread(void);
66+ Thread(const Thread& rhs);
67+ Thread& operator = (const Thread& rhs);
68+
69+ struct pImpl;
70+ const std::auto_ptr<pImpl> pimpl;
71+ };
72+}
73+
74+#endif /* !QRK_THREAD_H */
--- trunk/libs/range_sensor/UrgDevice.cpp (revision 1976)
+++ trunk/libs/range_sensor/UrgDevice.cpp (revision 1977)
@@ -1,1020 +1,1020 @@
1-/*!
2- \file
3- \brief URG センサ制御
4-
5- \author Satofumi KAMIMURA
6-
7- $Id$
8-
9- \todo remain_times の 100 をマクロで置き換える
10-*/
11-
12-#include "UrgDevice.h"
13-#include "SerialDevice.h"
14-#include "ScipHandler.h"
15-#include "RangeSensorParameter.h"
16-#include "ticks.h"
17-#include "Thread.h"
18-#include "LockGuard.h"
19-#include "Lock.h"
20-#include <deque>
21-#include <limits>
22-#include <cstring>
23-#include <cstdio>
24-
25-#ifdef MSC
26-#define snprintf _snprintf
27-#endif
28-
29-using namespace qrk;
30-using namespace std;
31-
32-
33-namespace
34-{
35- enum {
36- MdScansMax = 100, // [times]
37- };
38-}
39-
40-
41-struct UrgDevice::pImpl
42-{
43- struct ScanData
44- {
45- vector<long> length_data;
46- long timestamp;
47- CaptureSettings settings;
48-
49- ScanData(void) : timestamp(-1)
50- {
51- }
52- };
53-
54-
55- class Capture
56- {
57- public:
58- virtual ~Capture(void)
59- {
60- }
61-
62- virtual string createCaptureCommand(void) = 0;
63- virtual int capture(vector<long>& data, long* timestamp) = 0;
64- virtual void setCapturesSize(size_t size) = 0;
65- virtual size_t capturesSize(void) = 0;
66- virtual size_t remainCaptureTimes(void) = 0;
67- };
68-
69-
70- class RawManualCapture : public Capture
71- {
72- pImpl* pimpl_;
73-
74- public:
75- RawManualCapture(pImpl* pimpl) : pimpl_(pimpl)
76- {
77- }
78-
79-
80- ~RawManualCapture(void)
81- {
82- }
83-
84-
85- string createCaptureCommand(void)
86- {
87- // !!! parameter 受信直後に、送受信パラメータへの代入を行う
88- // !!! ここでの送受信には、送受信パラメータの内容を用いる
89-
90- char buffer[] = "GDbbbbeeeegg\n";
91- snprintf(buffer, strlen(buffer) + 1, "GD%04d%04d%02u\n",
92- pimpl_->capture_begin_, pimpl_->capture_end_,
93- pimpl_->capture_skip_lines_);
94-
95- return buffer;
96- }
97-
98-
99- int capture(vector<long>& data, long* timestamp)
100- {
101- pimpl_->retry_times_ = 0;
102-
103- // レーザを点灯させておく
104- pimpl_->scip_.setLaserOutput(ScipHandler::On);
105-
106- // データ取得コマンドの送信
107- string command = createCaptureCommand();
108- int n = pimpl_->scip_.send(command.c_str(),
109- static_cast<int>(command.size()));
110- if (n != static_cast<int>(command.size())) {
111- pimpl_->error_message_ = "Send command:" + command + " fail.";
112- return -1;
113- }
114-
115- CaptureSettings settings;
116- pimpl_->scip_.receiveCaptureData(data, settings, timestamp, NULL);
117- return static_cast<int>(data.size());
118- }
119-
120-
121- void setCapturesSize(size_t size)
122- {
123- static_cast<void>(size);
124-
125- // 何もしない
126- }
127-
128-
129- size_t capturesSize(void)
130- {
131- return 1;
132- }
133-
134-
135- size_t remainCaptureTimes(void)
136- {
137- return 0;
138- }
139- };
140-
141-
142- class RawAutoCapture : public Capture
143- {
144- pImpl* pimpl_;
145- size_t captures_size_;
146-
147-
148- public:
149- RawAutoCapture(pImpl* pimpl) : pimpl_(pimpl), captures_size_(1)
150- {
151- }
152-
153-
154- ~RawAutoCapture(void)
155- {
156- }
157-
158-
159- string createCaptureCommand(void)
160- {
161- char buffer[] = "MDbbbbeeeeggstt\n";
162- snprintf(buffer, strlen(buffer) + 1, "MD%04d%04d%02u%01u%02u\n",
163- pimpl_->capture_begin_, pimpl_->capture_end_,
164- pimpl_->capture_skip_lines_,
165- pimpl_->capture_frame_interval_,
166- (pimpl_->capture_times_ > 99) ? 0 : pimpl_->capture_times_);
167-
168- pimpl_->remain_times_ = pimpl_->capture_times_;
169-
170- return buffer;
171- }
172-
173-
174- int capture(vector<long>& data, long* timestamp)
175- {
176- if (pimpl_->retry_times_ > pimpl_->max_retry_times_) {
177- return -1;
178- }
179-
180- // スレッドを起動
181- LockGuard guard(pimpl_->mutex_);
182- if ((! pimpl_->thread_.isRunning()) && pimpl_->data_buffer_.empty()) {
183- pimpl_->thread_.run(1);
184- }
185-
186- // 取得済みデータがなければ、戻る
187- if (pimpl_->data_buffer_.empty()) {
188- if (pimpl_->invalid_packet_) {
189- return -1;
190- }
191- return 0;
192- }
193-
194- swap(data, pimpl_->data_buffer_.front().length_data);
195- if (timestamp) {
196- *timestamp = pimpl_->data_buffer_.front().timestamp;
197- //fprintf(stderr, "MD: %ld, %ld\n", ticks(), *timestamp);
198- }
199- pimpl_->data_buffer_.pop_front();
200-
201- return static_cast<int>(data.size());
202- }
203-
204-
205- void setCapturesSize(size_t size)
206- {
207- captures_size_ = size;
208- }
209-
210-
211- size_t capturesSize(void)
212- {
213- return captures_size_;
214- }
215-
216-
217- size_t remainCaptureTimes(void)
218- {
219- if (pimpl_->capture_times_ == 0) {
220- return numeric_limits<size_t>::max();
221- }
222-
223- LockGuard guard(pimpl_->mutex_);
224- return pimpl_->remain_times_;
225- }
226- };
227-
228-
229- class RawIntensityCapture : public Capture
230- {
231- pImpl* pimpl_;
232- size_t captures_size_;
233-
234-
235- public:
236- RawIntensityCapture(pImpl* pimpl) : pimpl_(pimpl), captures_size_(1)
237- {
238- }
239-
240-
241- ~RawIntensityCapture(void)
242- {
243- }
244-
245-
246- string createCaptureCommand(void)
247- {
248- char buffer[] = "MEbbbbeeeeggstt\n";
249- if ((! pimpl_->urg_type_.compare("URG-04LX")) ||
250- (! pimpl_->urg_type_.compare("URG-04LX-UG01")) ||
251- (! pimpl_->urg_type_.compare("UBG-04LX-F01"))) {
252- // URG-04LX ならば、FF 形式でデータ送信を行う
253- snprintf(buffer, strlen(buffer) + 1, "MD%04d%04dFF%01u%02u\n",
254- pimpl_->capture_begin_, pimpl_->capture_end_,
255- pimpl_->capture_frame_interval_,
256- (pimpl_->capture_times_ > 99) ? 0 : pimpl_->capture_times_);
257-
258- } else {
259- // UTM-30LX ならば、ME コマンドを用いる
260- snprintf(buffer, strlen(buffer) + 1, "ME%04d%04d%02u%01u%02u\n",
261- pimpl_->capture_begin_, pimpl_->capture_end_,
262- pimpl_->capture_skip_lines_,
263- pimpl_->capture_frame_interval_,
264- (pimpl_->capture_times_ > 99) ? 0 : pimpl_->capture_times_);
265- }
266- pimpl_->remain_times_ = pimpl_->capture_times_;
267-
268- return buffer;
269- }
270-
271-
272- int capture(vector<long>& data, long* timestamp)
273- {
274- if (pimpl_->retry_times_ > pimpl_->max_retry_times_) {
275- return -1;
276- }
277-
278- LockGuard guard(pimpl_->mutex_);
279- // 取得済みデータがなければ、スレッドを起動
280- if ((! pimpl_->thread_.isRunning()) && pimpl_->data_buffer_.empty()) {
281- pimpl_->thread_.run(1);
282- }
283-
284- // 取得済みデータがなければ、戻る
285- if (pimpl_->data_buffer_.empty()) {
286- if (pimpl_->invalid_packet_) {
287- return -1;
288- }
289- return 0;
290- }
291-
292- swap(data, pimpl_->data_buffer_.front().length_data);
293- if (timestamp) {
294- *timestamp = pimpl_->data_buffer_.front().timestamp;
295- }
296- CaptureSettings settings = pimpl_->data_buffer_.front().settings;
297- pimpl_->data_buffer_.pop_front();
298- pimpl_->intensity_data_.timestamp = *timestamp;
299- pimpl_->intensity_data_.length_data.clear();
300-
301- if (pimpl_->urg_type_.compare("URG-04LX") &&
302- pimpl_->urg_type_.compare("URG-04LX-UG01") &&
303- pimpl_->urg_type_.compare("UBG-04LX-F01")) {
304- // UTM-30LX のとき
305- int skip_lines = settings.skip_lines;
306- int min_length = pimpl_->parameters_.distance_min;
307- int n = data.size() / skip_lines / 2;
308- pimpl_->intensity_data_.length_data.reserve(n * skip_lines);
309-
310- int filled = 0;
311- for (int i = 0; i < n; ++i) {
312- size_t index = 2 * skip_lines * i;
313- long length = data[index];
314- long intensity_value =
315- (length < min_length) ? 0 : data[index + skip_lines];
316- for (int j = 0; j < skip_lines; ++j) {
317- pimpl_->intensity_data_.length_data.push_back(intensity_value);
318- data[filled++] = length;
319- }
320- }
321- vector<long>::iterator first_it = data.begin() + (data.size() / 2);
322- data.erase(first_it, data.end());
323-
324- } else if ((! pimpl_->urg_type_.compare("URG-04LX")) ||
325- (! pimpl_->urg_type_.compare("URG-04LX-UG01")) ||
326- (! pimpl_->urg_type_.compare("UBG-04LX-F01"))) {
327- // URG-04LX のとき
328- size_t n = data.size();
329- int min_length = pimpl_->parameters_.distance_min;
330- pimpl_->intensity_data_.length_data.reserve(data.size());
331- size_t offset = pimpl_->capture_begin_ & 0x1;
332- for (size_t i = 0; i < n; i += 2) {
333- size_t index = i + offset;
334- long length = data[index];
335- if ((index + 1) < n) {
336- long intensity_value = (length < min_length) ? 0 : data[index + 1];
337- pimpl_->intensity_data_.length_data.push_back(intensity_value);
338- pimpl_->intensity_data_.length_data.push_back(intensity_value);
339- data[index + 1] = length;
340- }
341- }
342- } else {
343- // !!! エラー処理
344- }
345-
346- return static_cast<int>(data.size());
347- }
348-
349-
350- void setCapturesSize(size_t size)
351- {
352- captures_size_ = size;
353- }
354-
355-
356- size_t capturesSize(void)
357- {
358- return captures_size_;
359- }
360-
361- size_t remainCaptureTimes(void)
362- {
363- if (pimpl_->capture_times_ == 0) {
364- return numeric_limits<size_t>::max();
365- }
366-
367- LockGuard guard(pimpl_->mutex_);
368- return pimpl_->remain_times_;
369- }
370- };
371-
372-
373- string error_message_;
374- UrgDevice* parent_;
375- Connection* con_;
376- SerialDevice* serial_; //!< 有効な接続オブジェクトがないときに用いる
377- ScipHandler scip_;
378- RangeSensorParameter parameters_;
379- string urg_type_;
380- long recent_timestamp_;
381- int timestamp_offset_;
382-
383- RangeCaptureMode capture_mode_;
384- RawManualCapture manual_capture_;
385- RawAutoCapture auto_capture_;
386- RawIntensityCapture intensity_capture_;
387- Capture* capture_;
388- Thread thread_;
389- Lock mutex_;
390-
391- deque<ScanData> data_buffer_;
392- ScanData intensity_data_;
393-
394- int capture_begin_;
395- int capture_end_;
396- size_t capture_skip_lines_;
397- int capture_skip_frames_;
398-
399- size_t capture_frame_interval_;
400- size_t capture_times_;
401- size_t remain_times_;
402- bool invalid_packet_;
403- size_t max_retry_times_;
404- size_t retry_times_;
405-
406- long base_timestamp_;
407- long pre_timestamp_;
408-
409-
410- pImpl(UrgDevice* parent)
411- : error_message_("no error."), parent_(parent),
412- con_(NULL), serial_(NULL), urg_type_(""),
413- recent_timestamp_(0), timestamp_offset_(0),
414- capture_mode_(ManualCapture),
415- manual_capture_(this), auto_capture_(this),
416- intensity_capture_(this), capture_(&manual_capture_),
417- thread_(&capture_thread, this),
418- capture_begin_(0), capture_end_(0),
419- capture_skip_lines_(1), capture_skip_frames_(0),
420- capture_frame_interval_(0), capture_times_(0),
421- remain_times_(0), invalid_packet_(false),
422- max_retry_times_(DefaultRetryTimes), retry_times_(0),
423- base_timestamp_(0), pre_timestamp_(0)
424- {
425- }
426-
427-
428- ~pImpl(void)
429- {
430- disconnect();
431-
432- // con_ は外部からセットされる可能性があるため、このクラスでは解放しない
433- // serial_ のみ解放する
434- delete serial_;
435- }
436-
437-
438- void initializeSerial(void)
439- {
440- if (! serial_) {
441- serial_ = new SerialDevice;
442- }
443- con_ = serial_;
444- }
445-
446-
447- bool connect(const char* device, long baudrate)
448- {
449- disconnect();
450- if (! con_) {
451- initializeSerial();
452- }
453- scip_.setConnection(con_);
454-
455- // ボーレートを検出した上でのデバイスとの接続
456- if (! scip_.connect(device, baudrate)) {
457- error_message_ = scip_.what();
458- return false;
459- }
460-
461- if (! loadParameter()) {
462- return false;
463- }
464- updateCaptureParameters();
465-
466- return true;
467- }
468-
469-
470- bool loadParameter(void)
471- {
472- // URG パラメータの取得
473- RangeSensorParameter parameters;
474- if (! scip_.loadParameter(parameters)) {
475- error_message_ = scip_.what();
476- return false;
477- }
478- swap(parameters_, parameters);
479-
480- size_t type_length = min(parameters_.model.find('('),
481- parameters_.model.find('['));
482- urg_type_ = parameters_.model.substr(0, type_length);
483-
484- // !!! capture_begin_, capture_end_ との調整をすべき
485- // !!! min(), max() を使うこと
486-
487- return true;
488- }
489-
490-
491- void disconnect(void)
492- {
493- stop();
494- if (con_) {
495- con_->disconnect();
496- }
497- }
498-
499-
500- void updateCaptureParameters(void)
501- {
502- capture_begin_ = parameters_.area_min;
503- capture_end_ = parameters_.area_max;
504- }
505-
506-
507- // AutoCapture, IntensityCapture のデータ取得を行う
508- static int capture_thread(void* args)
509- {
510- pImpl* obj = static_cast<pImpl*>(args);
511-
512- // 設定に基づいて、データ受信コマンドを作成して発行
513- string capture_command = obj->capture_->createCaptureCommand();
514- if (capture_command.empty()) {
515- // ここのエラーが IntensityCapture のときのみ発生するのに依存した実装
516- obj->error_message_ = "This urg is not support intensity capture.";
517- return -1;
518- }
519- int n = obj->scip_.send(capture_command.c_str(),
520- static_cast<int>(capture_command.size()));
521- if (n != static_cast<int>(capture_command.size())) {
522- obj->error_message_ = capture_command + " fail.";
523- ++obj->retry_times_;
524- return -1;
525- }
526-
527- // 受信待ち
528- ScanData data;
529- int remain_times = MdScansMax;
530- int total_times = 0;
531- while (1) {
532- // 受信完了、およびエラーで抜ける
533- obj->invalid_packet_ = false;
534- CaptureType type =
535- obj->scip_.receiveCaptureData(data.length_data, data.settings,
536- &data.timestamp,
537- &remain_times, &total_times);
538- if (type == Mx_Reply) {
539- // MS/MD の応答パケットの場合、次のデータを待つ
540- continue;
541- }
542-
543- if (! ((type == MD) || (type == MS) || (type == ME))) {
544- // 受信データでなければ、スレッド処理を中断する
545- obj->invalid_packet_ = true;
546- ++obj->retry_times_;
547- break;
548- }
549- obj->retry_times_ = 0;
550-
551- // タイムスタンプが 24 bit しかないため、1巡することへの対処
552- if ((data.timestamp >= 0) && (data.timestamp < obj->pre_timestamp_)) {
553- obj->base_timestamp_ += 1 << 24;
554- }
555- obj->pre_timestamp_ = data.timestamp;
556- data.timestamp += obj->base_timestamp_;
557-
558- LockGuard guard(obj->mutex_);
559- deque<ScanData>& data_buffer = obj->data_buffer_;
560-
561- // 古くなったデータを取り除く
562- int current_size = static_cast<int>(data_buffer.size());
563- int erase_size =
564- current_size - static_cast<int>(obj->capture_->capturesSize());
565- if (erase_size > 0) {
566- data_buffer.erase(data_buffer.begin(),
567- data_buffer.begin() + erase_size);
568- }
569-
570- // 今回のデータを追加
571- ScanData dummy_data;
572- data_buffer.push_back(dummy_data);
573- swap(data_buffer.back(), data);
574-
575- if ((obj->capture_times_ > 0) && (remain_times < MdScansMax)) {
576- obj->remain_times_ = remain_times;
577- } else {
578- --obj->remain_times_;
579- if (obj->remain_times_ == 0) {
580- obj->scip_.setLaserOutput(Off);
581- }
582- }
583- obj->parent_->captureReceived();
584-
585- if ((total_times > 0) && (remain_times == 0)) {
586- // 取得が終了したら、抜ける
587- break;
588- }
589- }
590-
591- return 0;
592- }
593-
594-
595- int capture(vector<long>& data, long* timestamp)
596- {
597- long raw_timestamp = 0;
598- int n = capture_->capture(data, &raw_timestamp);
599- if (n < 0) {
600- error_message_ = scip_.what();
601- return n;
602- }
603-
604- recent_timestamp_ = raw_timestamp - timestamp_offset_;
605- if (timestamp) {
606- *timestamp = recent_timestamp_;
607- }
608- return n;
609- }
610-
611-
612- int captureWithIntensity(vector<long>& data,
613- vector<long>& intensity_data,
614- long* timestamp)
615- {
616- int n = capture(data, timestamp);
617- if (n < 0) {
618- return n;
619- }
620- return captureIntensity(intensity_data, NULL);
621- }
622-
623-
624- int captureIntensity(vector<long>& intensity_data, int* timestamp)
625- {
626- if (capture_mode_ != IntensityCapture) {
627- error_message_ = "please call setCaptureMode(IntensityCapture).";
628- return -1;
629- }
630-
631- if (intensity_data_.length_data.empty()) {
632- return 0;
633- }
634-
635- if (timestamp) {
636- *timestamp = intensity_data_.timestamp;
637- recent_timestamp_ = *timestamp;
638- }
639- swap(intensity_data, intensity_data_.length_data);
640- intensity_data_.length_data.clear();
641-
642- return static_cast<int>(intensity_data.size());
643- }
644-
645-
646- bool setTimestamp(int timestamp, int* response_msec, int* force_delay_msec)
647- {
648- long prepare_ticks = ticks();
649-
650- // TM0 モードに遷移
651- // !!! true, false をキーワードで置換すること
652- if (! scip_.setRawTimestampMode(true)) {
653- return false;
654- }
655-
656- // TM1 のタイムスタンプを取得し、通信遅延と URG のタイムスタンプを取得する
657- int urg_timestamp = 0;
658- int first_ticks = ticks();
659- if (scip_.rawTimestamp(&urg_timestamp)) {
660-
661- //fprintf(stderr, "raw: %d, %d\n", timestamp, urg_timestamp);
662-
663- int delay = ticks() - first_ticks;
664- if (force_delay_msec) {
665- delay = *force_delay_msec;
666- }
667- if (response_msec) {
668- *response_msec = delay;
669- }
670- timestamp_offset_ =
671- (urg_timestamp + (delay / 2))
672- - (timestamp + (first_ticks - prepare_ticks));
673- }
674-
675- // URG タイムスタンプ用のオフセット時間を計算し、TM2 で抜ける
676- if (! scip_.setRawTimestampMode(false)) {
677- return false;
678- }
679-
680- return true;
681- }
682-
683-
684- int rad2index(const double radian) const
685- {
686- int area_total = parameters_.area_total;
687- int index =
688- static_cast<int>(floor(((radian * area_total) / (2.0 * M_PI)) + 0.5)
689- + parameters_.area_front);
690-
691- if (index < 0) {
692- index = 0;
693- } else if (index > parameters_.area_max) {
694- index = parameters_.area_max;
695- }
696- return index;
697- }
698-
699-
700- bool isConnected(void) const
701- {
702- return (con_ == NULL) ? false : con_->isConnected();
703- }
704-
705-
706- void stop(void)
707- {
708- if (! isConnected()) {
709- return;
710- }
711-
712- if (capture_mode_ == ManualCapture) {
713- return;
714- }
715-
716- // QT コマンドの発行
717- scip_.setLaserOutput(ScipHandler::Off);
718-
719- // 応答を待つ
720- if (thread_.isRunning()) {
721- thread_.wait();
722- }
723- }
724-
725-
726- void clear(void)
727- {
728- LockGuard guard(mutex_);
729- data_buffer_.clear();
730- intensity_data_.length_data.clear();
731- }
732-};
733-
734-
735-UrgDevice::UrgDevice(void) : pimpl(new pImpl(this))
736-{
737-}
738-
739-
740-UrgDevice::~UrgDevice(void)
741-{
742-}
743-
744-
745-void UrgDevice::captureReceived(void)
746-{
747- // 受信完了を継承先のクラスが利用するためのメソッド
748-}
749-
750-
751-const char* UrgDevice::what(void) const
752-{
753- return pimpl->error_message_.c_str();
754-}
755-
756-
757-bool UrgDevice::connect(const char* device, long baudrate)
758-{
759- return pimpl->connect(device, baudrate);
760-}
761-
762-
763-void UrgDevice::setConnection(Connection* con)
764-{
765- pimpl->serial_ = NULL;
766- pimpl->con_ = con;
767- pimpl->scip_.setConnection(con);
768-}
769-
770-
771-Connection* UrgDevice::connection(void)
772-{
773- return pimpl->con_;
774-}
775-
776-
777-void UrgDevice::disconnect(void)
778-{
779- pimpl->disconnect();
780-}
781-
782-
783-bool UrgDevice::isConnected(void) const
784-{
785- return pimpl->isConnected();
786-}
787-
788-
789-long UrgDevice::minDistance(void) const
790-{
791- return pimpl->parameters_.distance_min;
792-}
793-
794-
795-long UrgDevice::maxDistance(void) const
796-{
797- return pimpl->parameters_.distance_max;
798-}
799-
800-
801-int UrgDevice::maxScanLines(void) const
802-{
803- // +1 は、parameters_ が未初期化のときに new long [0] しないための処置
804- return pimpl->parameters_.area_max + 1;
805-}
806-
807-
808-void UrgDevice::setRetryTimes(size_t times)
809-{
810- pimpl->max_retry_times_ = times;
811-}
812-
813-
814-void UrgDevice::setCapturesSize(size_t size)
815-{
816- pimpl->capture_->setCapturesSize(size);
817-}
818-
819-
820-size_t UrgDevice::remainCaptureTimes(void)
821-{
822- return pimpl->capture_->remainCaptureTimes();
823-}
824-
825-
826-int UrgDevice::scanMsec(void) const
827-{
828- int scan_rpm = pimpl->parameters_.scan_rpm;
829- return (scan_rpm <= 0) ? 1 : (1000 * 60 / scan_rpm);
830-}
831-
832-
833-void UrgDevice::setCaptureMode(RangeCaptureMode mode)
834-{
835- // capture を停止する。capture の再開は行わない
836- stop();
837- pimpl->clear();
838-
839- if (mode == ManualCapture) {
840- pimpl->capture_ = &pimpl->manual_capture_;
841-
842- } else if (mode == AutoCapture) {
843- pimpl->capture_ = &pimpl->auto_capture_;
844-
845- } else if (mode == IntensityCapture) {
846- pimpl->capture_ = &pimpl->intensity_capture_;
847- }
848-
849- pimpl->capture_mode_ = mode;
850-}
851-
852-
853-RangeCaptureMode UrgDevice::captureMode(void)
854-{
855- return pimpl->capture_mode_;
856-}
857-
858-
859-void UrgDevice::setCaptureRange(int begin_index, int end_index)
860-{
861- // !!! 排他制御
862-
863- // capture を停止する。capture の再開は行わない
864- stop();
865- pimpl->clear();
866-
867- pimpl->capture_begin_ = begin_index;
868- pimpl->capture_end_ = end_index;
869-}
870-
871-
872-void UrgDevice::setCaptureFrameInterval(size_t interval)
873-{
874- // !!! 排他制御
875-
876- // capture を停止する。capture の再開は行わない
877- stop();
878- pimpl->clear();
879-
880- pimpl->capture_frame_interval_ = interval;
881-}
882-
883-
884-void UrgDevice::setCaptureTimes(size_t times)
885-{
886- // !!! 排他制御
887-
888- // capture を停止する。capture の再開は行わない
889- stop();
890- pimpl->clear();
891-
892- // !!! 範囲制限の判定処理
893- pimpl->capture_times_ = times;
894-}
895-
896-
897-
898-void UrgDevice::setCaptureSkipLines(size_t skip_lines)
899-{
900- // capture を停止する。capture の再開は行わない
901- stop();
902- pimpl->clear();
903-
904- pimpl->capture_skip_lines_ = skip_lines;
905-}
906-
907-
908-int UrgDevice::capture(vector<long>& data, long* timestamp)
909-{
910- // !!! 未接続ならば、戻す
911-
912- return pimpl->capture(data, timestamp);
913-}
914-
915-
916-int UrgDevice::captureWithIntensity(vector<long>& data,
917- vector<long>& intensity_data,
918- long* timestamp)
919-{
920- // !!! 未接続ならば、戻す
921- return pimpl->captureWithIntensity(data, intensity_data, timestamp);
922-}
923-
924-
925-void UrgDevice::stop(void)
926-{
927- pimpl->stop();
928-}
929-
930-
931-bool UrgDevice::setTimestamp(int ticks, int* response_msec,
932- int* force_delay_msec)
933-{
934- // !!! 未接続ならば、戻す
935-
936- return pimpl->setTimestamp(ticks, response_msec, force_delay_msec);
937-}
938-
939-
940-long UrgDevice::recentTimestamp(void) const
941-{
942- return pimpl->recent_timestamp_;
943-}
944-
945-
946-bool UrgDevice::setLaserOutput(bool on)
947-{
948- // !!! 未接続ならば、戻す
949-
950- return pimpl->scip_.setLaserOutput(on, ScipHandler::Force);
951-}
952-
953-
954-double UrgDevice::index2rad(const int index) const
955-{
956- int index_from_front = index - pimpl->parameters_.area_front;
957- return index_from_front * (2.0 * M_PI) / pimpl->parameters_.area_total;
958-}
959-
960-
961-int UrgDevice::rad2index(const double radian) const
962-{
963- return pimpl->rad2index(radian);
964-}
965-
966-
967-void UrgDevice::setParameter(const RangeSensorParameter& parameter)
968-{
969- pimpl->parameters_ = parameter;
970- pimpl->updateCaptureParameters();
971-}
972-
973-
974-RangeSensorParameter UrgDevice::parameter(void) const
975-{
976- return pimpl->parameters_;
977-}
978-
979-
980-bool UrgDevice::loadParameter(void)
981-{
982- return pimpl->loadParameter();
983-}
984-
985-
986-bool UrgDevice::versionLines(vector<string>& lines)
987-{
988- if (! isConnected()) {
989- return false;
990- }
991-
992- return pimpl->scip_.versionLines(lines);
993-}
994-
995-
996-bool UrgDevice::reboot(void)
997-{
998- UrgDevice::setLaserOutput(Off);
999-
1000- // send "RB" twice.
1001- for (int i = 0; i < 2; ++i) {
1002- pimpl->scip_.send("RB\n", 3);
1003-
1004- enum {
1005- RB_Timeout = 1000, // [msec]
1006- };
1007- char recv_buffer[3];
1008- int send_n = pimpl->scip_.recv(recv_buffer, 3, RB_Timeout);
1009- if (send_n != 3) {
1010- return false;
1011- }
1012- if (strncmp(recv_buffer, "RB\n", 3)) {
1013- return false;
1014- }
1015- }
1016-
1017- UrgDevice::disconnect();
1018-
1019- return true;
1020-}
1+/*!
2+ \file
3+ \brief URG センサ制御
4+
5+ \author Satofumi KAMIMURA
6+
7+ $Id$
8+
9+ \todo remain_times の 100 をマクロで置き換える
10+*/
11+
12+#include "UrgDevice.h"
13+#include "SerialDevice.h"
14+#include "ScipHandler.h"
15+#include "RangeSensorParameter.h"
16+#include "ticks.h"
17+#include "Thread.h"
18+#include "LockGuard.h"
19+#include "Lock.h"
20+#include <deque>
21+#include <limits>
22+#include <cstring>
23+#include <cstdio>
24+
25+#ifdef MSC
26+#define snprintf _snprintf
27+#endif
28+
29+using namespace qrk;
30+using namespace std;
31+
32+
33+namespace
34+{
35+ enum {
36+ MdScansMax = 100, // [times]
37+ };
38+}
39+
40+
41+struct UrgDevice::pImpl
42+{
43+ struct ScanData
44+ {
45+ vector<long> length_data;
46+ long timestamp;
47+ CaptureSettings settings;
48+
49+ ScanData(void) : timestamp(-1)
50+ {
51+ }
52+ };
53+
54+
55+ class Capture
56+ {
57+ public:
58+ virtual ~Capture(void)
59+ {
60+ }
61+
62+ virtual string createCaptureCommand(void) = 0;
63+ virtual int capture(vector<long>& data, long* timestamp) = 0;
64+ virtual void setCapturesSize(size_t size) = 0;
65+ virtual size_t capturesSize(void) = 0;
66+ virtual size_t remainCaptureTimes(void) = 0;
67+ };
68+
69+
70+ class RawManualCapture : public Capture
71+ {
72+ pImpl* pimpl_;
73+
74+ public:
75+ RawManualCapture(pImpl* pimpl) : pimpl_(pimpl)
76+ {
77+ }
78+
79+
80+ ~RawManualCapture(void)
81+ {
82+ }
83+
84+
85+ string createCaptureCommand(void)
86+ {
87+ // !!! parameter 受信直後に、送受信パラメータへの代入を行う
88+ // !!! ここでの送受信には、送受信パラメータの内容を用いる
89+
90+ char buffer[] = "GDbbbbeeeegg\n";
91+ snprintf(buffer, strlen(buffer) + 1, "GD%04d%04d%02u\n",
92+ pimpl_->capture_begin_, pimpl_->capture_end_,
93+ pimpl_->capture_skip_lines_);
94+
95+ return buffer;
96+ }
97+
98+
99+ int capture(vector<long>& data, long* timestamp)
100+ {
101+ pimpl_->retry_times_ = 0;
102+
103+ // レーザを点灯させておく
104+ pimpl_->scip_.setLaserOutput(ScipHandler::On);
105+
106+ // データ取得コマンドの送信
107+ string command = createCaptureCommand();
108+ int n = pimpl_->scip_.send(command.c_str(),
109+ static_cast<int>(command.size()));
110+ if (n != static_cast<int>(command.size())) {
111+ pimpl_->error_message_ = "Send command:" + command + " fail.";
112+ return -1;
113+ }
114+
115+ CaptureSettings settings;
116+ pimpl_->scip_.receiveCaptureData(data, settings, timestamp, NULL);
117+ return static_cast<int>(data.size());
118+ }
119+
120+
121+ void setCapturesSize(size_t size)
122+ {
123+ static_cast<void>(size);
124+
125+ // 何もしない
126+ }
127+
128+
129+ size_t capturesSize(void)
130+ {
131+ return 1;
132+ }
133+
134+
135+ size_t remainCaptureTimes(void)
136+ {
137+ return 0;
138+ }
139+ };
140+
141+
142+ class RawAutoCapture : public Capture
143+ {
144+ pImpl* pimpl_;
145+ size_t captures_size_;
146+
147+
148+ public:
149+ RawAutoCapture(pImpl* pimpl) : pimpl_(pimpl), captures_size_(1)
150+ {
151+ }
152+
153+
154+ ~RawAutoCapture(void)
155+ {
156+ }
157+
158+
159+ string createCaptureCommand(void)
160+ {
161+ char buffer[] = "MDbbbbeeeeggstt\n";
162+ snprintf(buffer, strlen(buffer) + 1, "MD%04d%04d%02u%01u%02u\n",
163+ pimpl_->capture_begin_, pimpl_->capture_end_,
164+ pimpl_->capture_skip_lines_,
165+ pimpl_->capture_frame_interval_,
166+ (pimpl_->capture_times_ > 99) ? 0 : pimpl_->capture_times_);
167+
168+ pimpl_->remain_times_ = pimpl_->capture_times_;
169+
170+ return buffer;
171+ }
172+
173+
174+ int capture(vector<long>& data, long* timestamp)
175+ {
176+ if (pimpl_->retry_times_ > pimpl_->max_retry_times_) {
177+ return -1;
178+ }
179+
180+ // スレッドを起動
181+ LockGuard guard(pimpl_->mutex_);
182+ if ((! pimpl_->thread_.isRunning()) && pimpl_->data_buffer_.empty()) {
183+ pimpl_->thread_.run(1);
184+ }
185+
186+ // 取得済みデータがなければ、戻る
187+ if (pimpl_->data_buffer_.empty()) {
188+ if (pimpl_->invalid_packet_) {
189+ return -1;
190+ }
191+ return 0;
192+ }
193+
194+ swap(data, pimpl_->data_buffer_.front().length_data);
195+ if (timestamp) {
196+ *timestamp = pimpl_->data_buffer_.front().timestamp;
197+ //fprintf(stderr, "MD: %ld, %ld\n", ticks(), *timestamp);
198+ }
199+ pimpl_->data_buffer_.pop_front();
200+
201+ return static_cast<int>(data.size());
202+ }
203+
204+
205+ void setCapturesSize(size_t size)
206+ {
207+ captures_size_ = size;
208+ }
209+
210+
211+ size_t capturesSize(void)
212+ {
213+ return captures_size_;
214+ }
215+
216+
217+ size_t remainCaptureTimes(void)
218+ {
219+ if (pimpl_->capture_times_ == 0) {
220+ return numeric_limits<size_t>::max();
221+ }
222+
223+ LockGuard guard(pimpl_->mutex_);
224+ return pimpl_->remain_times_;
225+ }
226+ };
227+
228+
229+ class RawIntensityCapture : public Capture
230+ {
231+ pImpl* pimpl_;
232+ size_t captures_size_;
233+
234+
235+ public:
236+ RawIntensityCapture(pImpl* pimpl) : pimpl_(pimpl), captures_size_(1)
237+ {
238+ }
239+
240+
241+ ~RawIntensityCapture(void)
242+ {
243+ }
244+
245+
246+ string createCaptureCommand(void)
247+ {
248+ char buffer[] = "MEbbbbeeeeggstt\n";
249+ if ((! pimpl_->urg_type_.compare("URG-04LX")) ||
250+ (! pimpl_->urg_type_.compare("URG-04LX-UG01")) ||
251+ (! pimpl_->urg_type_.compare("UBG-04LX-F01"))) {
252+ // URG-04LX ならば、FF 形式でデータ送信を行う
253+ snprintf(buffer, strlen(buffer) + 1, "MD%04d%04dFF%01u%02u\n",
254+ pimpl_->capture_begin_, pimpl_->capture_end_,
255+ pimpl_->capture_frame_interval_,
256+ (pimpl_->capture_times_ > 99) ? 0 : pimpl_->capture_times_);
257+
258+ } else {
259+ // UTM-30LX ならば、ME コマンドを用いる
260+ snprintf(buffer, strlen(buffer) + 1, "ME%04d%04d%02u%01u%02u\n",
261+ pimpl_->capture_begin_, pimpl_->capture_end_,
262+ pimpl_->capture_skip_lines_,
263+ pimpl_->capture_frame_interval_,
264+ (pimpl_->capture_times_ > 99) ? 0 : pimpl_->capture_times_);
265+ }
266+ pimpl_->remain_times_ = pimpl_->capture_times_;
267+
268+ return buffer;
269+ }
270+
271+
272+ int capture(vector<long>& data, long* timestamp)
273+ {
274+ if (pimpl_->retry_times_ > pimpl_->max_retry_times_) {
275+ return -1;
276+ }
277+
278+ LockGuard guard(pimpl_->mutex_);
279+ // 取得済みデータがなければ、スレッドを起動
280+ if ((! pimpl_->thread_.isRunning()) && pimpl_->data_buffer_.empty()) {
281+ pimpl_->thread_.run(1);
282+ }
283+
284+ // 取得済みデータがなければ、戻る
285+ if (pimpl_->data_buffer_.empty()) {
286+ if (pimpl_->invalid_packet_) {
287+ return -1;
288+ }
289+ return 0;
290+ }
291+
292+ swap(data, pimpl_->data_buffer_.front().length_data);
293+ if (timestamp) {
294+ *timestamp = pimpl_->data_buffer_.front().timestamp;
295+ }
296+ CaptureSettings settings = pimpl_->data_buffer_.front().settings;
297+ pimpl_->data_buffer_.pop_front();
298+ pimpl_->intensity_data_.timestamp = *timestamp;
299+ pimpl_->intensity_data_.length_data.clear();
300+
301+ if (pimpl_->urg_type_.compare("URG-04LX") &&
302+ pimpl_->urg_type_.compare("URG-04LX-UG01") &&
303+ pimpl_->urg_type_.compare("UBG-04LX-F01")) {
304+ // UTM-30LX のとき
305+ int skip_lines = settings.skip_lines;
306+ int min_length = pimpl_->parameters_.distance_min;
307+ int n = data.size() / skip_lines / 2;
308+ pimpl_->intensity_data_.length_data.reserve(n * skip_lines);
309+
310+ int filled = 0;
311+ for (int i = 0; i < n; ++i) {
312+ size_t index = 2 * skip_lines * i;
313+ long length = data[index];
314+ long intensity_value =
315+ (length < min_length) ? 0 : data[index + skip_lines];
316+ for (int j = 0; j < skip_lines; ++j) {
317+ pimpl_->intensity_data_.length_data.push_back(intensity_value);
318+ data[filled++] = length;
319+ }
320+ }
321+ vector<long>::iterator first_it = data.begin() + (data.size() / 2);
322+ data.erase(first_it, data.end());
323+
324+ } else if ((! pimpl_->urg_type_.compare("URG-04LX")) ||
325+ (! pimpl_->urg_type_.compare("URG-04LX-UG01")) ||
326+ (! pimpl_->urg_type_.compare("UBG-04LX-F01"))) {
327+ // URG-04LX のとき
328+ size_t n = data.size();
329+ int min_length = pimpl_->parameters_.distance_min;
330+ pimpl_->intensity_data_.length_data.reserve(data.size());
331+ size_t offset = pimpl_->capture_begin_ & 0x1;
332+ for (size_t i = 0; i < n; i += 2) {
333+ size_t index = i + offset;
334+ long length = data[index];
335+ if ((index + 1) < n) {
336+ long intensity_value = (length < min_length) ? 0 : data[index + 1];
337+ pimpl_->intensity_data_.length_data.push_back(intensity_value);
338+ pimpl_->intensity_data_.length_data.push_back(intensity_value);
339+ data[index + 1] = length;
340+ }
341+ }
342+ } else {
343+ // !!! エラー処理
344+ }
345+
346+ return static_cast<int>(data.size());
347+ }
348+
349+
350+ void setCapturesSize(size_t size)
351+ {
352+ captures_size_ = size;
353+ }
354+
355+
356+ size_t capturesSize(void)
357+ {
358+ return captures_size_;
359+ }
360+
361+ size_t remainCaptureTimes(void)
362+ {
363+ if (pimpl_->capture_times_ == 0) {
364+ return numeric_limits<size_t>::max();
365+ }
366+
367+ LockGuard guard(pimpl_->mutex_);
368+ return pimpl_->remain_times_;
369+ }
370+ };
371+
372+
373+ string error_message_;
374+ UrgDevice* parent_;
375+ Connection* con_;
376+ SerialDevice* serial_; //!< 有効な接続オブジェクトがないときに用いる
377+ ScipHandler scip_;
378+ RangeSensorParameter parameters_;
379+ string urg_type_;
380+ long recent_timestamp_;
381+ int timestamp_offset_;
382+
383+ RangeCaptureMode capture_mode_;
384+ RawManualCapture manual_capture_;
385+ RawAutoCapture auto_capture_;
386+ RawIntensityCapture intensity_capture_;
387+ Capture* capture_;
388+ Thread thread_;
389+ Lock mutex_;
390+
391+ deque<ScanData> data_buffer_;
392+ ScanData intensity_data_;
393+
394+ int capture_begin_;
395+ int capture_end_;
396+ size_t capture_skip_lines_;
397+ int capture_skip_frames_;
398+
399+ size_t capture_frame_interval_;
400+ size_t capture_times_;
401+ size_t remain_times_;
402+ bool invalid_packet_;
403+ size_t max_retry_times_;
404+ size_t retry_times_;
405+
406+ long base_timestamp_;
407+ long pre_timestamp_;
408+
409+
410+ pImpl(UrgDevice* parent)
411+ : error_message_("no error."), parent_(parent),
412+ con_(NULL), serial_(NULL), urg_type_(""),
413+ recent_timestamp_(0), timestamp_offset_(0),
414+ capture_mode_(ManualCapture),
415+ manual_capture_(this), auto_capture_(this),
416+ intensity_capture_(this), capture_(&manual_capture_),
417+ thread_(&capture_thread, this),
418+ capture_begin_(0), capture_end_(0),
419+ capture_skip_lines_(1), capture_skip_frames_(0),
420+ capture_frame_interval_(0), capture_times_(0),
421+ remain_times_(0), invalid_packet_(false),
422+ max_retry_times_(DefaultRetryTimes), retry_times_(0),
423+ base_timestamp_(0), pre_timestamp_(0)
424+ {
425+ }
426+
427+
428+ ~pImpl(void)
429+ {
430+ disconnect();
431+
432+ // con_ は外部からセットされる可能性があるため、このクラスでは解放しない
433+ // serial_ のみ解放する
434+ delete serial_;
435+ }
436+
437+
438+ void initializeSerial(void)
439+ {
440+ if (! serial_) {
441+ serial_ = new SerialDevice;
442+ }
443+ con_ = serial_;
444+ }
445+
446+
447+ bool connect(const char* device, long baudrate)
448+ {
449+ disconnect();
450+ if (! con_) {
451+ initializeSerial();
452+ }
453+ scip_.setConnection(con_);
454+
455+ // ボーレートを検出した上でのデバイスとの接続
456+ if (! scip_.connect(device, baudrate)) {
457+ error_message_ = scip_.what();
458+ return false;
459+ }
460+
461+ if (! loadParameter()) {
462+ return false;
463+ }
464+ updateCaptureParameters();
465+
466+ return true;
467+ }
468+
469+
470+ bool loadParameter(void)
471+ {
472+ // URG パラメータの取得
473+ RangeSensorParameter parameters;
474+ if (! scip_.loadParameter(parameters)) {
475+ error_message_ = scip_.what();
476+ return false;
477+ }
478+ swap(parameters_, parameters);
479+
480+ size_t type_length = min(parameters_.model.find('('),
481+ parameters_.model.find('['));
482+ urg_type_ = parameters_.model.substr(0, type_length);
483+
484+ // !!! capture_begin_, capture_end_ との調整をすべき
485+ // !!! min(), max() を使うこと
486+
487+ return true;
488+ }
489+
490+
491+ void disconnect(void)
492+ {
493+ stop();
494+ if (con_) {
495+ con_->disconnect();
496+ }
497+ }
498+
499+
500+ void updateCaptureParameters(void)
501+ {
502+ capture_begin_ = parameters_.area_min;
503+ capture_end_ = parameters_.area_max;
504+ }
505+
506+
507+ // AutoCapture, IntensityCapture のデータ取得を行う
508+ static int capture_thread(void* args)
509+ {
510+ pImpl* obj = static_cast<pImpl*>(args);
511+
512+ // 設定に基づいて、データ受信コマンドを作成して発行
513+ string capture_command = obj->capture_->createCaptureCommand();
514+ if (capture_command.empty()) {
515+ // ここのエラーが IntensityCapture のときのみ発生するのに依存した実装
516+ obj->error_message_ = "This urg is not support intensity capture.";
517+ return -1;
518+ }
519+ int n = obj->scip_.send(capture_command.c_str(),
520+ static_cast<int>(capture_command.size()));
521+ if (n != static_cast<int>(capture_command.size())) {
522+ obj->error_message_ = capture_command + " fail.";
523+ ++obj->retry_times_;
524+ return -1;
525+ }
526+
527+ // 受信待ち
528+ ScanData data;
529+ int remain_times = MdScansMax;
530+ int total_times = 0;
531+ while (1) {
532+ // 受信完了、およびエラーで抜ける
533+ obj->invalid_packet_ = false;
534+ CaptureType type =
535+ obj->scip_.receiveCaptureData(data.length_data, data.settings,
536+ &data.timestamp,
537+ &remain_times, &total_times);
538+ if (type == Mx_Reply) {
539+ // MS/MD の応答パケットの場合、次のデータを待つ
540+ continue;
541+ }
542+
543+ if (! ((type == MD) || (type == MS) || (type == ME))) {
544+ // 受信データでなければ、スレッド処理を中断する
545+ obj->invalid_packet_ = true;
546+ ++obj->retry_times_;
547+ break;
548+ }
549+ obj->retry_times_ = 0;
550+
551+ // タイムスタンプが 24 bit しかないため、1巡することへの対処
552+ if ((data.timestamp >= 0) && (data.timestamp < obj->pre_timestamp_)) {
553+ obj->base_timestamp_ += 1 << 24;
554+ }
555+ obj->pre_timestamp_ = data.timestamp;
556+ data.timestamp += obj->base_timestamp_;
557+
558+ LockGuard guard(obj->mutex_);
559+ deque<ScanData>& data_buffer = obj->data_buffer_;
560+
561+ // 古くなったデータを取り除く
562+ int current_size = static_cast<int>(data_buffer.size());
563+ int erase_size =
564+ current_size - static_cast<int>(obj->capture_->capturesSize());
565+ if (erase_size > 0) {
566+ data_buffer.erase(data_buffer.begin(),
567+ data_buffer.begin() + erase_size);
568+ }
569+
570+ // 今回のデータを追加
571+ ScanData dummy_data;
572+ data_buffer.push_back(dummy_data);
573+ swap(data_buffer.back(), data);
574+
575+ if ((obj->capture_times_ > 0) && (remain_times < MdScansMax)) {
576+ obj->remain_times_ = remain_times;
577+ } else {
578+ --obj->remain_times_;
579+ if (obj->remain_times_ == 0) {
580+ obj->scip_.setLaserOutput(Off);
581+ }
582+ }
583+ obj->parent_->captureReceived();
584+
585+ if ((total_times > 0) && (remain_times == 0)) {
586+ // 取得が終了したら、抜ける
587+ break;
588+ }
589+ }
590+
591+ return 0;
592+ }
593+
594+
595+ int capture(vector<long>& data, long* timestamp)
596+ {
597+ long raw_timestamp = 0;
598+ int n = capture_->capture(data, &raw_timestamp);
599+ if (n < 0) {
600+ error_message_ = scip_.what();
601+ return n;
602+ }
603+
604+ recent_timestamp_ = raw_timestamp - timestamp_offset_;
605+ if (timestamp) {
606+ *timestamp = recent_timestamp_;
607+ }
608+ return n;
609+ }
610+
611+
612+ int captureWithIntensity(vector<long>& data,
613+ vector<long>& intensity_data,
614+ long* timestamp)
615+ {
616+ int n = capture(data, timestamp);
617+ if (n < 0) {
618+ return n;
619+ }
620+ return captureIntensity(intensity_data, NULL);
621+ }
622+
623+
624+ int captureIntensity(vector<long>& intensity_data, int* timestamp)
625+ {
626+ if (capture_mode_ != IntensityCapture) {
627+ error_message_ = "please call setCaptureMode(IntensityCapture).";
628+ return -1;
629+ }
630+
631+ if (intensity_data_.length_data.empty()) {
632+ return 0;
633+ }
634+
635+ if (timestamp) {
636+ *timestamp = intensity_data_.timestamp;
637+ recent_timestamp_ = *timestamp;
638+ }
639+ swap(intensity_data, intensity_data_.length_data);
640+ intensity_data_.length_data.clear();
641+
642+ return static_cast<int>(intensity_data.size());
643+ }
644+
645+
646+ bool setTimestamp(int timestamp, int* response_msec, int* force_delay_msec)
647+ {
648+ long prepare_ticks = ticks();
649+
650+ // TM0 モードに遷移
651+ // !!! true, false をキーワードで置換すること
652+ if (! scip_.setRawTimestampMode(true)) {
653+ return false;
654+ }
655+
656+ // TM1 のタイムスタンプを取得し、通信遅延と URG のタイムスタンプを取得する
657+ int urg_timestamp = 0;
658+ int first_ticks = ticks();
659+ if (scip_.rawTimestamp(&urg_timestamp)) {
660+
661+ //fprintf(stderr, "raw: %d, %d\n", timestamp, urg_timestamp);
662+
663+ int delay = ticks() - first_ticks;
664+ if (force_delay_msec) {
665+ delay = *force_delay_msec;
666+ }
667+ if (response_msec) {
668+ *response_msec = delay;
669+ }
670+ timestamp_offset_ =
671+ (urg_timestamp + (delay / 2))
672+ - (timestamp + (first_ticks - prepare_ticks));
673+ }
674+
675+ // URG タイムスタンプ用のオフセット時間を計算し、TM2 で抜ける
676+ if (! scip_.setRawTimestampMode(false)) {
677+ return false;
678+ }
679+
680+ return true;
681+ }
682+
683+
684+ int rad2index(const double radian) const
685+ {
686+ int area_total = parameters_.area_total;
687+ int index =
688+ static_cast<int>(floor(((radian * area_total) / (2.0 * M_PI)) + 0.5)
689+ + parameters_.area_front);
690+
691+ if (index < 0) {
692+ index = 0;
693+ } else if (index > parameters_.area_max) {
694+ index = parameters_.area_max;
695+ }
696+ return index;
697+ }
698+
699+
700+ bool isConnected(void) const
701+ {
702+ return (con_ == NULL) ? false : con_->isConnected();
703+ }
704+
705+
706+ void stop(void)
707+ {
708+ if (! isConnected()) {
709+ return;
710+ }
711+
712+ if (capture_mode_ == ManualCapture) {
713+ return;
714+ }
715+
716+ // QT コマンドの発行
717+ scip_.setLaserOutput(ScipHandler::Off);
718+
719+ // 応答を待つ
720+ if (thread_.isRunning()) {
721+ thread_.wait();
722+ }
723+ }
724+
725+
726+ void clear(void)
727+ {
728+ LockGuard guard(mutex_);
729+ data_buffer_.clear();
730+ intensity_data_.length_data.clear();
731+ }
732+};
733+
734+
735+UrgDevice::UrgDevice(void) : pimpl(new pImpl(this))
736+{
737+}
738+
739+
740+UrgDevice::~UrgDevice(void)
741+{
742+}
743+
744+
745+void UrgDevice::captureReceived(void)
746+{
747+ // 受信完了を継承先のクラスが利用するためのメソッド
748+}
749+
750+
751+const char* UrgDevice::what(void) const
752+{
753+ return pimpl->error_message_.c_str();
754+}
755+
756+
757+bool UrgDevice::connect(const char* device, long baudrate)
758+{
759+ return pimpl->connect(device, baudrate);
760+}
761+
762+
763+void UrgDevice::setConnection(Connection* con)
764+{
765+ pimpl->serial_ = NULL;
766+ pimpl->con_ = con;
767+ pimpl->scip_.setConnection(con);
768+}
769+
770+
771+Connection* UrgDevice::connection(void)
772+{
773+ return pimpl->con_;
774+}
775+
776+
777+void UrgDevice::disconnect(void)
778+{
779+ pimpl->disconnect();
780+}
781+
782+
783+bool UrgDevice::isConnected(void) const
784+{
785+ return pimpl->isConnected();
786+}
787+
788+
789+long UrgDevice::minDistance(void) const
790+{
791+ return pimpl->parameters_.distance_min;
792+}
793+
794+
795+long UrgDevice::maxDistance(void) const
796+{
797+ return pimpl->parameters_.distance_max;
798+}
799+
800+
801+int UrgDevice::maxScanLines(void) const
802+{
803+ // +1 は、parameters_ が未初期化のときに new long [0] しないための処置
804+ return pimpl->parameters_.area_max + 1;
805+}
806+
807+
808+void UrgDevice::setRetryTimes(size_t times)
809+{
810+ pimpl->max_retry_times_ = times;
811+}
812+
813+
814+void UrgDevice::setCapturesSize(size_t size)
815+{
816+ pimpl->capture_->setCapturesSize(size);
817+}
818+
819+
820+size_t UrgDevice::remainCaptureTimes(void)
821+{
822+ return pimpl->capture_->remainCaptureTimes();
823+}
824+
825+
826+int UrgDevice::scanMsec(void) const
827+{
828+ int scan_rpm = pimpl->parameters_.scan_rpm;
829+ return (scan_rpm <= 0) ? 1 : (1000 * 60 / scan_rpm);
830+}
831+
832+
833+void UrgDevice::setCaptureMode(RangeCaptureMode mode)
834+{
835+ // capture を停止する。capture の再開は行わない
836+ stop();
837+ pimpl->clear();
838+
839+ if (mode == ManualCapture) {
840+ pimpl->capture_ = &pimpl->manual_capture_;
841+
842+ } else if (mode == AutoCapture) {
843+ pimpl->capture_ = &pimpl->auto_capture_;
844+
845+ } else if (mode == IntensityCapture) {
846+ pimpl->capture_ = &pimpl->intensity_capture_;
847+ }
848+
849+ pimpl->capture_mode_ = mode;
850+}
851+
852+
853+RangeCaptureMode UrgDevice::captureMode(void)
854+{
855+ return pimpl->capture_mode_;
856+}
857+
858+
859+void UrgDevice::setCaptureRange(int begin_index, int end_index)
860+{
861+ // !!! 排他制御
862+
863+ // capture を停止する。capture の再開は行わない
864+ stop();
865+ pimpl->clear();
866+
867+ pimpl->capture_begin_ = begin_index;
868+ pimpl->capture_end_ = end_index;
869+}
870+
871+
872+void UrgDevice::setCaptureFrameInterval(size_t interval)
873+{
874+ // !!! 排他制御
875+
876+ // capture を停止する。capture の再開は行わない
877+ stop();
878+ pimpl->clear();
879+
880+ pimpl->capture_frame_interval_ = interval;
881+}
882+
883+
884+void UrgDevice::setCaptureTimes(size_t times)
885+{
886+ // !!! 排他制御
887+
888+ // capture を停止する。capture の再開は行わない
889+ stop();
890+ pimpl->clear();
891+
892+ // !!! 範囲制限の判定処理
893+ pimpl->capture_times_ = times;
894+}
895+
896+
897+
898+void UrgDevice::setCaptureSkipLines(size_t skip_lines)
899+{
900+ // capture を停止する。capture の再開は行わない
901+ stop();
902+ pimpl->clear();
903+
904+ pimpl->capture_skip_lines_ = skip_lines;
905+}
906+
907+
908+int UrgDevice::capture(vector<long>& data, long* timestamp)
909+{
910+ // !!! 未接続ならば、戻す
911+
912+ return pimpl->capture(data, timestamp);
913+}
914+
915+
916+int UrgDevice::captureWithIntensity(vector<long>& data,
917+ vector<long>& intensity_data,
918+ long* timestamp)
919+{
920+ // !!! 未接続ならば、戻す
921+ return pimpl->captureWithIntensity(data, intensity_data, timestamp);
922+}
923+
924+
925+void UrgDevice::stop(void)
926+{
927+ pimpl->stop();
928+}
929+
930+
931+bool UrgDevice::setTimestamp(int ticks, int* response_msec,
932+ int* force_delay_msec)
933+{
934+ // !!! 未接続ならば、戻す
935+
936+ return pimpl->setTimestamp(ticks, response_msec, force_delay_msec);
937+}
938+
939+
940+long UrgDevice::recentTimestamp(void) const
941+{
942+ return pimpl->recent_timestamp_;
943+}
944+
945+
946+bool UrgDevice::setLaserOutput(bool on)
947+{
948+ // !!! 未接続ならば、戻す
949+
950+ return pimpl->scip_.setLaserOutput(on, ScipHandler::Force);
951+}
952+
953+
954+double UrgDevice::index2rad(const int index) const
955+{
956+ int index_from_front = index - pimpl->parameters_.area_front;
957+ return index_from_front * (2.0 * M_PI) / pimpl->parameters_.area_total;
958+}
959+
960+
961+int UrgDevice::rad2index(const double radian) const
962+{
963+ return pimpl->rad2index(radian);
964+}
965+
966+
967+void UrgDevice::setParameter(const RangeSensorParameter& parameter)
968+{
969+ pimpl->parameters_ = parameter;
970+ pimpl->updateCaptureParameters();
971+}
972+
973+
974+RangeSensorParameter UrgDevice::parameter(void) const
975+{
976+ return pimpl->parameters_;
977+}
978+
979+
980+bool UrgDevice::loadParameter(void)
981+{
982+ return pimpl->loadParameter();
983+}
984+
985+
986+bool UrgDevice::versionLines(vector<string>& lines)
987+{
988+ if (! isConnected()) {
989+ return false;
990+ }
991+
992+ return pimpl->scip_.versionLines(lines);
993+}
994+
995+
996+bool UrgDevice::reboot(void)
997+{
998+ UrgDevice::setLaserOutput(Off);
999+
1000+ // send "RB" twice.
1001+ for (int i = 0; i < 2; ++i) {
1002+ pimpl->scip_.send("RB\n", 3);
1003+
1004+ enum {
1005+ RB_Timeout = 1000, // [msec]
1006+ };
1007+ char recv_buffer[3];
1008+ int send_n = pimpl->scip_.recv(recv_buffer, 3, RB_Timeout);
1009+ if (send_n != 3) {
1010+ return false;
1011+ }
1012+ if (strncmp(recv_buffer, "RB\n", 3)) {
1013+ return false;
1014+ }
1015+ }
1016+
1017+ UrgDevice::disconnect();
1018+
1019+ return true;
1020+}
--- trunk/libs/range_sensor/ScipHandler.cpp (revision 1976)
+++ trunk/libs/range_sensor/ScipHandler.cpp (revision 1977)
@@ -1,842 +1,842 @@
1-/*!
2- \file
3- \brief SCIP 通信処理
4-
5- \author Satofumi KAMIMURA
6-
7- $Id$
8-*/
9-
10-#include "ScipHandler.h"
11-#include "RangeSensorParameter.h"
12-#include "Connection.h"
13-#include "ConnectionUtils.h"
14-#include "ticks.h"
15-#include "delay.h"
16-#include "DetectOS.h"
17-#include "log_printf.h"
18-#include <cstring>
19-#include <cstdio>
20-
21-#ifdef MSC
22-#define snprintf _snprintf
23-#endif
24-
25-using namespace qrk;
26-using namespace std;
27-
28-
29-namespace
30-{
31- typedef enum {
32- ProcessContinue,
33- ProcessBreak,
34- ProcessNormal,
35- } LoopProcess;
36-
37- void clearReceived(vector<long>& data, CaptureType& type,
38- int& line_count, int& timeout,
39- string& remain_string,
40- string& left_packet_data)
41- {
42- data.clear();
43- type = TypeUnknown;
44- line_count = 0;
45- timeout = 0;
46- remain_string.clear();
47- left_packet_data.clear();
48- }
49-}
50-
51-
52-struct ScipHandler::pImpl
53-{
54- enum {
55- TotalTimeout = 1000, // [msec]
56- ContinuousTimeout = 100, // [msec]
57- FirstTimeout = 1000, // [msec]
58-
59- BufferSize = 64 + 1 + 1, // データ長 + チェックサム + 改行
60-
61- ResponseTimeout = -1,
62- MismatchResponse = -2,
63- SendFail = -3,
64- ChecksumFail = -4,
65- Scip11Response = -14,
66-
67- InvalidRange = -1,
68- };
69-
70- typedef enum {
71- LaserUnknown = 0,
72- LaserOn,
73- LaserOff,
74- } LaserState;
75-
76- string error_message_;
77- Connection* con_;
78- LaserState laser_state_;
79- bool mx_capturing_;
80-
81- bool isPreCommand_QT_;
82-
83-
84- pImpl(void)
85- : error_message_("no error."), con_(NULL), laser_state_(LaserUnknown),
86- mx_capturing_(false), isPreCommand_QT_(false)
87- {
88- }
89-
90-
91- bool connect(const char* device, long baudrate)
92- {
93- if (! con_->connect(device, baudrate)) {
94- error_message_ = con_->what();
95- return false;
96- }
97-
98- long try_baudrates[] = { 115200, 19200, 38400, };
99- size_t try_size = sizeof(try_baudrates) / sizeof(try_baudrates[0]);
100-
101- // 接続したいボーレートを配列の先頭と入れ換える
102- for (size_t i = 1; i < try_size; ++i) {
103- if (baudrate == try_baudrates[i]) {
104- swap(try_baudrates[0], try_baudrates[i]);
105- break;
106- }
107- }
108-
109- // 指定のボーレートで接続し、応答が返されるかどうか試す
110- for (size_t i = 0; i < try_size; ++i) {
111-
112- // ホスト側のボーレートを変更
113- if (! con_->setBaudrate(try_baudrates[i])) {
114- error_message_ = con_->what();
115- return false;
116- }
117-
118- // 前回分の受信パケットを読み捨て
119- con_->clear();
120-
121- // QT の発行
122- int return_code = -1;
123- char qt_expected_response[] = { 0, -1 };
124- // return_code を使いたいため、setLaserOutput() を用いずに QT を送信する
125- if (response(return_code, "QT\n", qt_expected_response)) {
126- laser_state_ = LaserOff;
127- return changeBothBaudrate(baudrate);
128-
129- } else if (return_code == ResponseTimeout) {
130- // ボーレートが違っていて、通信できなかったとみなす
131- error_message_ = "baudrate is not detected.";
132- continue;
133-
134- } else if (return_code == MismatchResponse) {
135- // MD/MS の応答とみなし、受信データを読み飛ばす
136- con_->clear();
137- skip(con_, ContinuousTimeout);
138- return changeBothBaudrate(baudrate);
139-
140- } else if (return_code == Scip11Response) {
141- // SCIP1.1 プロトコルの場合のみ、SCIP2.0 を送信する
142- char scip20_expected_response[] = { 0, -1 };
143- if (! response(return_code, "SCIP2.0\n", scip20_expected_response)) {
144- error_message_ =
145- "SCIP1.1 protocol is not supported. Please update URG firmware, or reconnect after a few seconds because sensor is booting.";
146- return false;
147- }
148- laser_state_ = LaserOff;
149- return changeBothBaudrate(baudrate);
150-
151- } else if (return_code == 0xE) {
152- // TM モードとみなし、TM2 を発行する
153- char tm2_expected_response[] = { 0, -1 };
154- if (response(return_code, "TM2\n", tm2_expected_response)) {
155- laser_state_ = LaserOff;
156- return changeBothBaudrate(baudrate);
157- }
158- }
159- }
160-
161- con_->disconnect();
162- return false;
163- }
164-
165-
166- bool changeBothBaudrate(long baudrate)
167- {
168- // 既に目標対象のボーレート値ならば、成功とみなす
169- // この関数は、ScipHandler::connect() か、それ以後でないと呼ばれないため
170- if (con_->baudrate() == baudrate) {
171- return true;
172- }
173-
174- // URG 側のボーレートを変更
175- int pre_ticks = ticks();
176- if (! changeBaudrate(baudrate)) {
177- return false;
178- }
179-
180- // シリアル通信の場合、ボーレート変更後、1周分だけ待つ必要がある
181- int reply_msec = ticks() - pre_ticks;
182- delay((reply_msec * 4 / 3) + 10);
183-
184- // ホスト側のボーレートを変更
185- return con_->setBaudrate(baudrate);
186- }
187-
188-
189- bool changeBaudrate(long baudrate)
190- {
191-#if 0
192- // Tcpip 接続に対応するため、コメントアウト
193- if (! ((baudrate == 19200) || (baudrate == 38400) ||
194- (baudrate == 57600) || (baudrate == 115200))) {
195- error_message_ = "Invalid baudrate value.";
196- return false;
197- }
198-#endif
199-
200- // SS を送信し、URG 側のボーレートを変更する
201- char send_buffer[] = "SSxxxxxx\n";
202- snprintf(send_buffer, 10, "SS%06ld\n", baudrate);
203- int return_code = -1;
204- // !!! 既に設定対象のボーレート、の場合の戻り値を ss_expected... に追加する
205- char ss_expected_response[] = { 0, 0x3, 0x4, 0xf, -1 };
206- if (! response(return_code, send_buffer, ss_expected_response)) {
207- error_message_ = "Baudrate change fail.";
208- return false;
209- }
210-
211- return true;
212- }
213-
214-
215- bool loadParameter(RangeSensorParameter& parameters)
216- {
217- // PP の送信とデータの受信
218- int return_code = -1;
219- char pp_expected_response[] = { 0, -1 };
220- vector<string> lines;
221- if (! response(return_code, "PP\n", pp_expected_response, &lines)) {
222- error_message_ = "PP fail.";
223- return false;
224- }
225-
226- // PP 応答内容の格納
227- if (lines.size() != 8) {
228- error_message_ = "Invalid PP response.";
229- return false;
230- }
231-
232- // !!! チェックサムの評価を行うべき
233-
234- int modl_length =
235- static_cast<int>(lines[RangeSensorParameter::MODL].size());
236- // 最初のタグと、チェックサムを除いた文字列を返す
237- if (modl_length > (5 + 2)) {
238- modl_length -= (5 + 2);
239- }
240- parameters.model = lines[RangeSensorParameter::MODL].substr(5, modl_length);
241-
242- parameters.distance_min = substr2int(lines[RangeSensorParameter::DMIN], 5);
243- parameters.distance_max = substr2int(lines[RangeSensorParameter::DMAX], 5);
244- parameters.area_total = substr2int(lines[RangeSensorParameter::ARES], 5);
245- parameters.area_min = substr2int(lines[RangeSensorParameter::AMIN], 5);
246- parameters.area_max = substr2int(lines[RangeSensorParameter::AMAX], 5);
247- parameters.area_front = substr2int(lines[RangeSensorParameter::AFRT], 5);
248- parameters.scan_rpm = substr2int(lines[RangeSensorParameter::SCAN], 5);
249-
250- return true;
251- }
252-
253-
254- int substr2int(const string& line, int from_n, int length = string::npos)
255- {
256- return atoi(line.substr(from_n, length).c_str());
257- }
258-
259-
260- bool response(int& return_code, const char send_command[],
261- char expected_response[],
262- vector<string>* lines = NULL)
263- {
264- return_code = -1;
265- if (! con_) {
266- error_message_ = "no connection.";
267- return false;
268- }
269-
270- size_t send_size = strlen(send_command);
271- int actual_send_size = con_->send(send_command, send_size);
272- if (strncmp(send_command, "QT\n", send_size)) {
273- isPreCommand_QT_ = false;
274- }
275- if (actual_send_size != static_cast<int>(send_size)) {
276- return_code = SendFail;
277- return false;
278- }
279-
280- // エコーバックの受信
281- char buffer[BufferSize];
282- int recv_size = readline(con_, buffer, BufferSize, FirstTimeout);
283- if (recv_size < 0) {
284- error_message_ = "response timeout.";
285- return_code = ResponseTimeout;
286- return false;
287- }
288-
289- // シリアル接続でボーレート変更直後の 0x00 は、判定外とする
290- if (! ((recv_size == 1) && (buffer[0] == 0x00))) {
291- if ((recv_size != static_cast<int>(send_size - 1)) ||
292- (strncmp(buffer, send_command, recv_size))) {
293- error_message_ = "mismatch response: " + string(buffer);
294- return_code = MismatchResponse;
295- return false;
296- }
297- }
298-
299- // 応答の受信
300- // !!! 上記の処理となるべく共通にする
301- // !!! SCIP1.1 プロトコルの応答は、負号を付加した上で return_code に格納する
302- recv_size = readline(con_, buffer, BufferSize, ContinuousTimeout);
303- if (recv_size < 0) {
304- // !!! この処理をくくる
305- error_message_ = "response timeout.";
306- return_code = ResponseTimeout;
307- return false;
308- }
309- if (recv_size == 3) {
310- // 3文字ならば、SCIP2.0 とみなしてチェックサムを確認する
311- // !!! チェックサムの確認
312- if (! checkSum(buffer, recv_size - 1, buffer[recv_size - 1])) {
313- return_code = ChecksumFail;
314- return false;
315- }
316- buffer[2] = '\0';
317- return_code = strtol(buffer, NULL, 16);
318-
319- } else if (recv_size == 1) {
320- // 1文字ならば、SCIP1.1 とみなして 16進変換した値に負号をつけて返す
321- buffer[1] = '\0';
322- return_code = -strtol(buffer, NULL, 16);
323- }
324-
325- // データ領域の受信
326- // 1行読み出し、改行のみならば終了とみなす
327- do {
328- recv_size = readline(con_, buffer, BufferSize, ContinuousTimeout);
329- if (lines && (recv_size > 0)) {
330- lines->push_back(buffer);
331- }
332- } while (recv_size > 0);
333-
334- for (int i = 0; expected_response[i] != -1; ++i) {
335- if (return_code == expected_response[i]) {
336- return true;
337- }
338- }
339- return false;
340- }
341-
342-
343- bool setLaserOutput(bool on, bool force)
344- {
345- if (((on == true) && (laser_state_ == LaserOn)) ||
346- ((on == false) && (laser_state_ == LaserOff))) {
347- if (! force) {
348- // レーザ出力が現在の状態と同じならば戻る
349- // 強制設定フラグが true のときは戻らずに設定を行う
350- return true;
351- }
352- }
353- if ((!on) && isPreCommand_QT_) {
354- return false;
355- }
356-
357- if (on) {
358- int return_code = -1;
359- char expected_response[] = { 0, -1 };
360- if (! response(return_code, "BM\n", expected_response)) {
361- error_message_ = "BM fail.";
362- return false;
363- }
364- laser_state_ = LaserOn;
365- return true;
366-
367- } else {
368- // "QT"
369- if (! mx_capturing_) {
370- // 消灯するための QT では、応答を待つべき
371- int return_code = -1;
372- char qt_expected_response[] = { 0, -1 };
373- if (! response(return_code, "QT\n", qt_expected_response)) {
374- return false;
375- }
376- laser_state_ = LaserOff;
377- isPreCommand_QT_ = true;
378- return true;
379-
380- } else {
381- // MD を中断するための QT では、応答を待ってはならない
382- // 応答は、受信スレッド内で処理される
383- con_->send("QT\n", 3);
384- isPreCommand_QT_ = true;
385- }
386-
387- return true;
388- }
389- }
390-
391-
392- bool testChecksum(const char* buffer, int line_size,
393- vector<long>& data, CaptureType& type,
394- int line_count, int timeout,
395- string& remain_string, string& left_packet_data)
396- {
397- if (! checkSum(buffer, line_size - 1, buffer[line_size - 1])) {
398- log_printf("checksum error: %s\n", buffer);
399- // return InvalidData;
400- // !!! URG のパケットエラーがなくなったら、この実装に戻す
401-
402- // !!! 存在するだけのパケットを読み飛ばす
403- error_message_ = "invalid packet.";
404- clearReceived(data, type, line_count, timeout,
405- remain_string, left_packet_data);
406- return false;
407- }
408-
409- return true;
410- }
411-
412-
413- LoopProcess handleEchoback(const char* buffer, CaptureSettings& settings,
414- CaptureType& type, vector<long>& data,
415- int line_count, int timeout,
416- string& remain_string,
417- string& left_packet_data)
418- {
419- string line = buffer;
420- if ((! line.compare(0, 2, "GD")) || (! line.compare(0, 2, "GS"))) {
421- if (! parseGdEchoback(settings, line)) {
422- return ProcessBreak;
423- }
424- type = (line[1] = 'D') ? GD : GS;
425-
426- } else if ((! line.compare(0, 2, "MD")) ||
427- (! line.compare(0, 2, "MS"))) {
428- if (! parseMdEchoback(settings, line)) {
429- return ProcessBreak;
430- }
431- type = (line[1] = 'D') ? MD : MS;
432- laser_state_ = LaserOn;
433-
434- } else if (! line.compare(0, 2, "ME")) {
435- if (! parseMeEchoback(settings, line)) {
436- return ProcessBreak;
437- }
438- type = ME;
439- laser_state_ = LaserOn;
440-
441- } else if (! line.compare(0, 2, "QT")) {
442- settings.remain_times = 0;
443- laser_state_ = LaserOff;
444- mx_capturing_ = false;
445- return ProcessNormal;
446-
447- } else {
448- //return InvalidData;
449- // !!! URG が正常なパケットを返すようになったら、この実装に戻す
450-
451- clearReceived(data, type, line_count, timeout,
452- remain_string, left_packet_data);
453- //fprintf(stderr, "invalid data: %s\n", buffer);
454- return ProcessContinue;
455- }
456-
457- data.reserve(settings.capture_last + 1);
458- return ProcessNormal;
459- }
460-
461-
462- void handleReturnCode(char* buffer, CaptureSettings& settings, int timeout,
463- CaptureType& type, int* total_times)
464- {
465- // !!! 長さが 2 + 1 かのチェックをすべき
466- buffer[2] = '\0';
467- settings.error_code = atoi(buffer);
468-
469- if (settings.error_code == 10) {
470- // レーザ消灯を検出
471- laser_state_ = pImpl::LaserOff;
472- }
473-
474- // "0B" が返された場合、センサとホストの応答がずれている可能性があるので
475- // 続く応答を読み捨てる
476- if (! strncmp(buffer, "0B", 2)) {
477- skip(con_, TotalTimeout, timeout);
478- }
479-
480- // !!! "00P" との比較をすべき
481- if ((settings.error_code == 0) &&
482- ((type == MD) || (type == MS) || (type == ME))) {
483- if (total_times) {
484- *total_times = settings.remain_times;
485- }
486- type = Mx_Reply;
487- }
488- }
489-
490-
491- CaptureType receiveCaptureData(vector<long>& data,
492- CaptureSettings& settings, long* timestamp,
493- int* remain_times, int* total_times)
494- {
495- int line_count = 0;
496- data.clear();
497-
498- string remain_string;
499-
500- string left_packet_data;
501- char buffer[BufferSize];
502-
503- error_message_ = "no response.";
504-
505- CaptureType type = TypeUnknown;
506- int timeout = FirstTimeout;
507- int line_size = 0;
508- while ((line_size = readline(con_, buffer, BufferSize, timeout)) > 0) {
509- //fprintf(stderr, "%d: % 3d: %s\n", ticks(), line_count, buffer);
510-
511- // チェックサムの確認
512- if (line_count != 0) {
513- // エコーバックにはチェックサム文字列がないので、無視
514- if (! testChecksum(buffer, line_size, data, type, line_count, timeout,
515- remain_string, left_packet_data)) {
516- continue;
517- }
518- }
519-
520- if (line_count == 0) {
521- // エコーバック
522- LoopProcess loop_process =
523- handleEchoback(buffer, settings, type, data, line_count, timeout,
524- remain_string, left_packet_data);
525- if (loop_process == ProcessContinue) {
526- continue;
527-
528- } else if (loop_process == ProcessBreak) {
529- break;
530- }
531-
532- } else if (line_count == 1) {
533- // 応答コード
534- handleReturnCode(buffer, settings, timeout, type, total_times);
535-
536- } else if (line_count == 2) {
537- // タイムスタンプ
538- if (timestamp) {
539- *timestamp = decode(buffer, 4);
540- }
541- } else {
542- if (line_count == 3) {
543- // 受信データがない先頭からの領域を、ダミーデータで埋める
544- for (int i = 0; i < settings.capture_first; ++i) {
545- data.push_back(InvalidRange);
546- if (type == ME) {
547- // ME 受信のときは、強度データ分も埋める
548- data.push_back(InvalidRange);
549- }
550- }
551- }
552- // 距離データの格納
553- left_packet_data =
554- addLengthData(data, string(buffer), left_packet_data,
555- settings.data_byte, settings.skip_lines);
556- }
557- ++line_count;
558- timeout = ContinuousTimeout;
559- }
560-
561- // !!! type が距離データ取得のときは、正常に受信が完了したか、を確認すべき
562-
563- // ME で "まとめる数" 設定以上のデータが返されるバグに対処
564- size_t expected_n = settings.capture_last * ((type == ME) ? 2 : 1);
565- if (expected_n < data.size()) {
566- data.erase(data.begin() + expected_n, data.end());
567- }
568-
569- if (remain_times) {
570- *remain_times = settings.remain_times;
571- }
572- return type;
573- }
574-
575-
576- bool parseGdEchoback(CaptureSettings& settings, const string& line)
577- {
578- if (line.size() != 12) {
579- error_message_ = "Invalid Gx packet has arrived.";
580- return false;
581- }
582-
583- settings.capture_first = substr2int(line, 2, 4);
584- settings.capture_last = substr2int(line, 6, 4) + 1;
585- int skip_lines = substr2int(line, 10, 2);
586- settings.skip_lines = (skip_lines == 0) ? 1 : skip_lines;
587- settings.data_byte = (line[1] == 'D') ? 3 : 2;
588-
589- return true;
590- }
591-
592-
593- bool parseMdEchoback(CaptureSettings& settings, const string& line)
594- {
595- if (line.size() != 15) {
596- error_message_ = "Invalid Mx packet has arrived.";
597- return false;
598- }
599-
600- settings.capture_first = substr2int(line, 2, 4);
601- settings.capture_last = substr2int(line, 6, 4) + 1;
602- int skip_lines = substr2int(line, 10, 2);
603- settings.skip_lines = (skip_lines == 0) ? 1 : skip_lines;
604- settings.skip_frames = substr2int(line, 12, 1);
605- settings.remain_times = substr2int(line, 13, 2);
606- settings.data_byte = (line[1] == 'D') ? 3 : 2;
607-
608- if (settings.remain_times == 1) {
609- // 最後のデータ取得で、レーザを消灯扱いにする
610- // 本当は、次のデータ取得後にレーザは消灯されている
611- // 1 で判定すると、取得回数が 1 のときにも正常に動作するため
612- mx_capturing_ = false;
613-
614- } else {
615- if (settings.remain_times > 0) {
616- mx_capturing_ = true;
617- } else if (settings.remain_times == 0) {
618- settings.remain_times = 100;
619- }
620- }
621-
622- return true;
623- }
624-
625-
626- bool parseMeEchoback(CaptureSettings& settings, const string& line)
627- {
628- if (line.size() != 15) {
629- error_message_ = "Invalid ME packet has arrived.";
630- return false;
631- }
632-
633- settings.capture_first = substr2int(line, 2, 4);
634- settings.capture_last = substr2int(line, 6, 4) + 1;
635- int skip_lines = substr2int(line, 10, 2);
636- settings.skip_lines = (skip_lines == 0) ? 1 : skip_lines;
637- settings.skip_frames = substr2int(line, 12, 1);
638- settings.remain_times = substr2int(line, 13, 2);
639- settings.data_byte = 3;
640-
641- if (settings.remain_times == 1) {
642- mx_capturing_ = false;
643-
644- } else {
645- mx_capturing_ = true;
646- }
647-
648- return true;
649- }
650-
651-
652- string addLengthData(vector<long>& data,
653- const string& line,
654- const string& left_packet_data,
655- const size_t data_byte, const int skip_lines = 1)
656- {
657- if (line.empty()) {
658- // 空行の場合、戻る
659- return left_packet_data;
660- }
661-
662- // 端数。次回に処理する分
663- string left_byte = left_packet_data;
664-
665- size_t data_size = (left_byte.size() + (line.size() - 1)) / data_byte;
666- size_t n = data_size * data_byte - left_byte.size();
667- for (size_t i = 0; i < n; ++i) {
668- left_byte.push_back(line[i]);
669- if (left_byte.size() >= data_byte) {
670- // データを距離に変換して、格納
671- long length = decode(&left_byte[0], data_byte);
672- for (int j = 0; j < skip_lines; ++j) {
673- data.push_back(length);
674- }
675- left_byte.clear();
676- }
677- }
678- left_byte += line.substr(n, (line.size() - n) - 1);
679-
680- return left_byte;
681- }
682-};
683-
684-
685-ScipHandler::ScipHandler(void) : pimpl(new pImpl)
686-{
687-}
688-
689-
690-ScipHandler::~ScipHandler(void)
691-{
692-}
693-
694-
695-const char* ScipHandler::what(void) const
696-{
697- return pimpl->error_message_.c_str();
698-}
699-
700-
701-long ScipHandler::decode(const char* data, size_t size)
702-{
703- const char* p = data;
704- const char* last_p = p + size;
705-
706- int value = 0;
707- while (p < last_p) {
708- value <<= 6;
709- value &= ~0x3f;
710- value |= *p++ - 0x30;
711- }
712- return value;
713-}
714-
715-
716-bool ScipHandler::checkSum(const char* buffer, int size, char actual_sum)
717-{
718- const char* p = buffer;
719- const char* last_p = p + size;
720-
721- char expected_sum = 0x00;
722- while (p < last_p) {
723- expected_sum += *p++;
724- }
725- expected_sum = (expected_sum & 0x3f) + 0x30;
726-
727- return (expected_sum == actual_sum) ? true : false;
728-}
729-
730-
731-void ScipHandler::setConnection(Connection* con)
732-{
733- pimpl->con_ = con;
734-}
735-
736-
737-Connection* ScipHandler::connection(void)
738-{
739- return pimpl->con_;
740-}
741-
742-
743-bool ScipHandler::connect(const char* device, long baudrate)
744-{
745- return pimpl->connect(device, baudrate);
746-}
747-
748-
749-int ScipHandler::send(const char data[], int size)
750-{
751- if (size >= 2) {
752- // コマンド送信後の受信前だと、レーザ点灯中の判定ができないため、
753- // ここでレーザ点灯中の状態を判定する
754- if ((! strncmp("MD", data, 2)) || (! strncmp("MS", data, 2)) ||
755- (! strncmp("ME", data, 2))) {
756- pimpl->laser_state_ = pImpl::LaserOn;
757- pimpl->mx_capturing_ = true;
758- pimpl->isPreCommand_QT_ = false;
759- }
760- }
761- return pimpl->con_->send(data, size);
762-}
763-
764-
765-int ScipHandler::recv(char data[], int size, int timeout)
766-{
767- return pimpl->con_->receive(data, size, timeout);
768-}
769-
770-
771-bool ScipHandler::loadParameter(RangeSensorParameter& parameters)
772-{
773- return pimpl->loadParameter(parameters);
774-}
775-
776-
777-bool ScipHandler::versionLines(vector<string>& lines)
778-{
779- int return_code = -1;
780- char expected_response[] = { 0, -1 };
781- if (! pimpl->response(return_code, "VV\n", expected_response, &lines)) {
782- return false;
783- }
784- return true;
785-}
786-
787-
788-bool ScipHandler::setRawTimestampMode(bool on)
789-{
790- char send_command[] = "TMx\n";
791- send_command[2] = (on) ? '0' : '2';
792-
793- // TM0 or TM2 の送信
794- int return_code = -1;
795- char expected_response[] = { 0, -1 };
796- if (! pimpl->response(return_code, send_command, expected_response)) {
797- pimpl->error_message_ = (on) ? "TM0 fail." : "TM2 fail.";
798- return false;
799- }
800-
801- // TM1, TM2 の応答が正常ならば、レーザは消灯しているはず
802- pimpl->laser_state_ = pImpl::LaserOff;
803-
804- return true;
805-}
806-
807-
808-bool ScipHandler::rawTimestamp(int* timestamp)
809-{
810- // TM1 の値を返す
811- int return_code = -1;
812- char expected_response[] = { 0, -1 };
813- vector<string> lines;
814- if (! pimpl->response(return_code, "TM1\n", expected_response, &lines)) {
815- pimpl->error_message_ = "TM1 fail.";
816- return false;
817- }
818-
819- if ((lines.size() != 1) || (lines[0].size() != 5)) {
820- pimpl->error_message_ = "response mismatch.";
821- return false;
822- }
823-
824- *timestamp = decode(lines[0].c_str(), 4);
825- return true;
826-}
827-
828-
829-bool ScipHandler::setLaserOutput(bool on, bool force)
830-{
831- return pimpl->setLaserOutput(on, force);
832-}
833-
834-
835-CaptureType ScipHandler::receiveCaptureData(vector<long>& data,
836- CaptureSettings& settings,
837- long* timestamp, int* remain_times,
838- int* total_times)
839-{
840- return pimpl->receiveCaptureData(data, settings,
841- timestamp, remain_times, total_times);
842-}
1+/*!
2+ \file
3+ \brief SCIP 通信処理
4+
5+ \author Satofumi KAMIMURA
6+
7+ $Id$
8+*/
9+
10+#include "ScipHandler.h"
11+#include "RangeSensorParameter.h"
12+#include "Connection.h"
13+#include "ConnectionUtils.h"
14+#include "ticks.h"
15+#include "delay.h"
16+#include "DetectOS.h"
17+#include "log_printf.h"
18+#include <cstring>
19+#include <cstdio>
20+
21+#ifdef MSC
22+#define snprintf _snprintf
23+#endif
24+
25+using namespace qrk;
26+using namespace std;
27+
28+
29+namespace
30+{
31+ typedef enum {
32+ ProcessContinue,
33+ ProcessBreak,
34+ ProcessNormal,
35+ } LoopProcess;
36+
37+ void clearReceived(vector<long>& data, CaptureType& type,
38+ int& line_count, int& timeout,
39+ string& remain_string,
40+ string& left_packet_data)
41+ {
42+ data.clear();
43+ type = TypeUnknown;
44+ line_count = 0;
45+ timeout = 0;
46+ remain_string.clear();
47+ left_packet_data.clear();
48+ }
49+}
50+
51+
52+struct ScipHandler::pImpl
53+{
54+ enum {
55+ TotalTimeout = 1000, // [msec]
56+ ContinuousTimeout = 100, // [msec]
57+ FirstTimeout = 1000, // [msec]
58+
59+ BufferSize = 64 + 1 + 1, // データ長 + チェックサム + 改行
60+
61+ ResponseTimeout = -1,
62+ MismatchResponse = -2,
63+ SendFail = -3,
64+ ChecksumFail = -4,
65+ Scip11Response = -14,
66+
67+ InvalidRange = -1,
68+ };
69+
70+ typedef enum {
71+ LaserUnknown = 0,
72+ LaserOn,
73+ LaserOff,
74+ } LaserState;
75+
76+ string error_message_;
77+ Connection* con_;
78+ LaserState laser_state_;
79+ bool mx_capturing_;
80+
81+ bool isPreCommand_QT_;
82+
83+
84+ pImpl(void)
85+ : error_message_("no error."), con_(NULL), laser_state_(LaserUnknown),
86+ mx_capturing_(false), isPreCommand_QT_(false)
87+ {
88+ }
89+
90+
91+ bool connect(const char* device, long baudrate)
92+ {
93+ if (! con_->connect(device, baudrate)) {
94+ error_message_ = con_->what();
95+ return false;
96+ }
97+
98+ long try_baudrates[] = { 115200, 19200, 38400, };
99+ size_t try_size = sizeof(try_baudrates) / sizeof(try_baudrates[0]);
100+
101+ // 接続したいボーレートを配列の先頭と入れ換える
102+ for (size_t i = 1; i < try_size; ++i) {
103+ if (baudrate == try_baudrates[i]) {
104+ swap(try_baudrates[0], try_baudrates[i]);
105+ break;
106+ }
107+ }
108+
109+ // 指定のボーレートで接続し、応答が返されるかどうか試す
110+ for (size_t i = 0; i < try_size; ++i) {
111+
112+ // ホスト側のボーレートを変更
113+ if (! con_->setBaudrate(try_baudrates[i])) {
114+ error_message_ = con_->what();
115+ return false;
116+ }
117+
118+ // 前回分の受信パケットを読み捨て
119+ con_->clear();
120+
121+ // QT の発行
122+ int return_code = -1;
123+ char qt_expected_response[] = { 0, -1 };
124+ // return_code を使いたいため、setLaserOutput() を用いずに QT を送信する
125+ if (response(return_code, "QT\n", qt_expected_response)) {
126+ laser_state_ = LaserOff;
127+ return changeBothBaudrate(baudrate);
128+
129+ } else if (return_code == ResponseTimeout) {
130+ // ボーレートが違っていて、通信できなかったとみなす
131+ error_message_ = "baudrate is not detected.";
132+ continue;
133+
134+ } else if (return_code == MismatchResponse) {
135+ // MD/MS の応答とみなし、受信データを読み飛ばす
136+ con_->clear();
137+ skip(con_, ContinuousTimeout);
138+ return changeBothBaudrate(baudrate);
139+
140+ } else if (return_code == Scip11Response) {
141+ // SCIP1.1 プロトコルの場合のみ、SCIP2.0 を送信する
142+ char scip20_expected_response[] = { 0, -1 };
143+ if (! response(return_code, "SCIP2.0\n", scip20_expected_response)) {
144+ error_message_ =
145+ "SCIP1.1 protocol is not supported. Please update URG firmware, or reconnect after a few seconds because sensor is booting.";
146+ return false;
147+ }
148+ laser_state_ = LaserOff;
149+ return changeBothBaudrate(baudrate);
150+
151+ } else if (return_code == 0xE) {
152+ // TM モードとみなし、TM2 を発行する
153+ char tm2_expected_response[] = { 0, -1 };
154+ if (response(return_code, "TM2\n", tm2_expected_response)) {
155+ laser_state_ = LaserOff;
156+ return changeBothBaudrate(baudrate);
157+ }
158+ }
159+ }
160+
161+ con_->disconnect();
162+ return false;
163+ }
164+
165+
166+ bool changeBothBaudrate(long baudrate)
167+ {
168+ // 既に目標対象のボーレート値ならば、成功とみなす
169+ // この関数は、ScipHandler::connect() か、それ以後でないと呼ばれないため
170+ if (con_->baudrate() == baudrate) {
171+ return true;
172+ }
173+
174+ // URG 側のボーレートを変更
175+ int pre_ticks = ticks();
176+ if (! changeBaudrate(baudrate)) {
177+ return false;
178+ }
179+
180+ // シリアル通信の場合、ボーレート変更後、1周分だけ待つ必要がある
181+ int reply_msec = ticks() - pre_ticks;
182+ delay((reply_msec * 4 / 3) + 10);
183+
184+ // ホスト側のボーレートを変更
185+ return con_->setBaudrate(baudrate);
186+ }
187+
188+
189+ bool changeBaudrate(long baudrate)
190+ {
191+#if 0
192+ // Tcpip 接続に対応するため、コメントアウト
193+ if (! ((baudrate == 19200) || (baudrate == 38400) ||
194+ (baudrate == 57600) || (baudrate == 115200))) {
195+ error_message_ = "Invalid baudrate value.";
196+ return false;
197+ }
198+#endif
199+
200+ // SS を送信し、URG 側のボーレートを変更する
201+ char send_buffer[] = "SSxxxxxx\n";
202+ snprintf(send_buffer, 10, "SS%06ld\n", baudrate);
203+ int return_code = -1;
204+ // !!! 既に設定対象のボーレート、の場合の戻り値を ss_expected... に追加する
205+ char ss_expected_response[] = { 0, 0x3, 0x4, 0xf, -1 };
206+ if (! response(return_code, send_buffer, ss_expected_response)) {
207+ error_message_ = "Baudrate change fail.";
208+ return false;
209+ }
210+
211+ return true;
212+ }
213+
214+
215+ bool loadParameter(RangeSensorParameter& parameters)
216+ {
217+ // PP の送信とデータの受信
218+ int return_code = -1;
219+ char pp_expected_response[] = { 0, -1 };
220+ vector<string> lines;
221+ if (! response(return_code, "PP\n", pp_expected_response, &lines)) {
222+ error_message_ = "PP fail.";
223+ return false;
224+ }
225+
226+ // PP 応答内容の格納
227+ if (lines.size() != 8) {
228+ error_message_ = "Invalid PP response.";
229+ return false;
230+ }
231+
232+ // !!! チェックサムの評価を行うべき
233+
234+ int modl_length =
235+ static_cast<int>(lines[RangeSensorParameter::MODL].size());
236+ // 最初のタグと、チェックサムを除いた文字列を返す
237+ if (modl_length > (5 + 2)) {
238+ modl_length -= (5 + 2);
239+ }
240+ parameters.model = lines[RangeSensorParameter::MODL].substr(5, modl_length);
241+
242+ parameters.distance_min = substr2int(lines[RangeSensorParameter::DMIN], 5);
243+ parameters.distance_max = substr2int(lines[RangeSensorParameter::DMAX], 5);
244+ parameters.area_total = substr2int(lines[RangeSensorParameter::ARES], 5);
245+ parameters.area_min = substr2int(lines[RangeSensorParameter::AMIN], 5);
246+ parameters.area_max = substr2int(lines[RangeSensorParameter::AMAX], 5);
247+ parameters.area_front = substr2int(lines[RangeSensorParameter::AFRT], 5);
248+ parameters.scan_rpm = substr2int(lines[RangeSensorParameter::SCAN], 5);
249+
250+ return true;
251+ }
252+
253+
254+ int substr2int(const string& line, int from_n, int length = string::npos)
255+ {
256+ return atoi(line.substr(from_n, length).c_str());
257+ }
258+
259+
260+ bool response(int& return_code, const char send_command[],
261+ char expected_response[],
262+ vector<string>* lines = NULL)
263+ {
264+ return_code = -1;
265+ if (! con_) {
266+ error_message_ = "no connection.";
267+ return false;
268+ }
269+
270+ size_t send_size = strlen(send_command);
271+ int actual_send_size = con_->send(send_command, send_size);
272+ if (strncmp(send_command, "QT\n", send_size)) {
273+ isPreCommand_QT_ = false;
274+ }
275+ if (actual_send_size != static_cast<int>(send_size)) {
276+ return_code = SendFail;
277+ return false;
278+ }
279+
280+ // エコーバックの受信
281+ char buffer[BufferSize];
282+ int recv_size = readline(con_, buffer, BufferSize, FirstTimeout);
283+ if (recv_size < 0) {
284+ error_message_ = "response timeout.";
285+ return_code = ResponseTimeout;
286+ return false;
287+ }
288+
289+ // シリアル接続でボーレート変更直後の 0x00 は、判定外とする
290+ if (! ((recv_size == 1) && (buffer[0] == 0x00))) {
291+ if ((recv_size != static_cast<int>(send_size - 1)) ||
292+ (strncmp(buffer, send_command, recv_size))) {
293+ error_message_ = "mismatch response: " + string(buffer);
294+ return_code = MismatchResponse;
295+ return false;
296+ }
297+ }
298+
299+ // 応答の受信
300+ // !!! 上記の処理となるべく共通にする
301+ // !!! SCIP1.1 プロトコルの応答は、負号を付加した上で return_code に格納する
302+ recv_size = readline(con_, buffer, BufferSize, ContinuousTimeout);
303+ if (recv_size < 0) {
304+ // !!! この処理をくくる
305+ error_message_ = "response timeout.";
306+ return_code = ResponseTimeout;
307+ return false;
308+ }
309+ if (recv_size == 3) {
310+ // 3文字ならば、SCIP2.0 とみなしてチェックサムを確認する
311+ // !!! チェックサムの確認
312+ if (! checkSum(buffer, recv_size - 1, buffer[recv_size - 1])) {
313+ return_code = ChecksumFail;
314+ return false;
315+ }
316+ buffer[2] = '\0';
317+ return_code = strtol(buffer, NULL, 16);
318+
319+ } else if (recv_size == 1) {
320+ // 1文字ならば、SCIP1.1 とみなして 16進変換した値に負号をつけて返す
321+ buffer[1] = '\0';
322+ return_code = -strtol(buffer, NULL, 16);
323+ }
324+
325+ // データ領域の受信
326+ // 1行読み出し、改行のみならば終了とみなす
327+ do {
328+ recv_size = readline(con_, buffer, BufferSize, ContinuousTimeout);
329+ if (lines && (recv_size > 0)) {
330+ lines->push_back(buffer);
331+ }
332+ } while (recv_size > 0);
333+
334+ for (int i = 0; expected_response[i] != -1; ++i) {
335+ if (return_code == expected_response[i]) {
336+ return true;
337+ }
338+ }
339+ return false;
340+ }
341+
342+
343+ bool setLaserOutput(bool on, bool force)
344+ {
345+ if (((on == true) && (laser_state_ == LaserOn)) ||
346+ ((on == false) && (laser_state_ == LaserOff))) {
347+ if (! force) {
348+ // レーザ出力が現在の状態と同じならば戻る
349+ // 強制設定フラグが true のときは戻らずに設定を行う
350+ return true;
351+ }
352+ }
353+ if ((!on) && isPreCommand_QT_) {
354+ return false;
355+ }
356+
357+ if (on) {
358+ int return_code = -1;
359+ char expected_response[] = { 0, -1 };
360+ if (! response(return_code, "BM\n", expected_response)) {
361+ error_message_ = "BM fail.";
362+ return false;
363+ }
364+ laser_state_ = LaserOn;
365+ return true;
366+
367+ } else {
368+ // "QT"
369+ if (! mx_capturing_) {
370+ // 消灯するための QT では、応答を待つべき
371+ int return_code = -1;
372+ char qt_expected_response[] = { 0, -1 };
373+ if (! response(return_code, "QT\n", qt_expected_response)) {
374+ return false;
375+ }
376+ laser_state_ = LaserOff;
377+ isPreCommand_QT_ = true;
378+ return true;
379+
380+ } else {
381+ // MD を中断するための QT では、応答を待ってはならない
382+ // 応答は、受信スレッド内で処理される
383+ con_->send("QT\n", 3);
384+ isPreCommand_QT_ = true;
385+ }
386+
387+ return true;
388+ }
389+ }
390+
391+
392+ bool testChecksum(const char* buffer, int line_size,
393+ vector<long>& data, CaptureType& type,
394+ int line_count, int timeout,
395+ string& remain_string, string& left_packet_data)
396+ {
397+ if (! checkSum(buffer, line_size - 1, buffer[line_size - 1])) {
398+ log_printf("checksum error: %s\n", buffer);
399+ // return InvalidData;
400+ // !!! URG のパケットエラーがなくなったら、この実装に戻す
401+
402+ // !!! 存在するだけのパケットを読み飛ばす
403+ error_message_ = "invalid packet.";
404+ clearReceived(data, type, line_count, timeout,
405+ remain_string, left_packet_data);
406+ return false;
407+ }
408+
409+ return true;
410+ }
411+
412+
413+ LoopProcess handleEchoback(const char* buffer, CaptureSettings& settings,
414+ CaptureType& type, vector<long>& data,
415+ int line_count, int timeout,
416+ string& remain_string,
417+ string& left_packet_data)
418+ {
419+ string line = buffer;
420+ if ((! line.compare(0, 2, "GD")) || (! line.compare(0, 2, "GS"))) {
421+ if (! parseGdEchoback(settings, line)) {
422+ return ProcessBreak;
423+ }
424+ type = (line[1] = 'D') ? GD : GS;
425+
426+ } else if ((! line.compare(0, 2, "MD")) ||
427+ (! line.compare(0, 2, "MS"))) {
428+ if (! parseMdEchoback(settings, line)) {
429+ return ProcessBreak;
430+ }
431+ type = (line[1] = 'D') ? MD : MS;
432+ laser_state_ = LaserOn;
433+
434+ } else if (! line.compare(0, 2, "ME")) {
435+ if (! parseMeEchoback(settings, line)) {
436+ return ProcessBreak;
437+ }
438+ type = ME;
439+ laser_state_ = LaserOn;
440+
441+ } else if (! line.compare(0, 2, "QT")) {
442+ settings.remain_times = 0;
443+ laser_state_ = LaserOff;
444+ mx_capturing_ = false;
445+ return ProcessNormal;
446+
447+ } else {
448+ //return InvalidData;
449+ // !!! URG が正常なパケットを返すようになったら、この実装に戻す
450+
451+ clearReceived(data, type, line_count, timeout,
452+ remain_string, left_packet_data);
453+ //fprintf(stderr, "invalid data: %s\n", buffer);
454+ return ProcessContinue;
455+ }
456+
457+ data.reserve(settings.capture_last + 1);
458+ return ProcessNormal;
459+ }
460+
461+
462+ void handleReturnCode(char* buffer, CaptureSettings& settings, int timeout,
463+ CaptureType& type, int* total_times)
464+ {
465+ // !!! 長さが 2 + 1 かのチェックをすべき
466+ buffer[2] = '\0';
467+ settings.error_code = atoi(buffer);
468+
469+ if (settings.error_code == 10) {
470+ // レーザ消灯を検出
471+ laser_state_ = pImpl::LaserOff;
472+ }
473+
474+ // "0B" が返された場合、センサとホストの応答がずれている可能性があるので
475+ // 続く応答を読み捨てる
476+ if (! strncmp(buffer, "0B", 2)) {
477+ skip(con_, TotalTimeout, timeout);
478+ }
479+
480+ // !!! "00P" との比較をすべき
481+ if ((settings.error_code == 0) &&
482+ ((type == MD) || (type == MS) || (type == ME))) {
483+ if (total_times) {
484+ *total_times = settings.remain_times;
485+ }
486+ type = Mx_Reply;
487+ }
488+ }
489+
490+
491+ CaptureType receiveCaptureData(vector<long>& data,
492+ CaptureSettings& settings, long* timestamp,
493+ int* remain_times, int* total_times)
494+ {
495+ int line_count = 0;
496+ data.clear();
497+
498+ string remain_string;
499+
500+ string left_packet_data;
501+ char buffer[BufferSize];
502+
503+ error_message_ = "no response.";
504+
505+ CaptureType type = TypeUnknown;
506+ int timeout = FirstTimeout;
507+ int line_size = 0;
508+ while ((line_size = readline(con_, buffer, BufferSize, timeout)) > 0) {
509+ //fprintf(stderr, "%d: % 3d: %s\n", ticks(), line_count, buffer);
510+
511+ // チェックサムの確認
512+ if (line_count != 0) {
513+ // エコーバックにはチェックサム文字列がないので、無視
514+ if (! testChecksum(buffer, line_size, data, type, line_count, timeout,
515+ remain_string, left_packet_data)) {
516+ continue;
517+ }
518+ }
519+
520+ if (line_count == 0) {
521+ // エコーバック
522+ LoopProcess loop_process =
523+ handleEchoback(buffer, settings, type, data, line_count, timeout,
524+ remain_string, left_packet_data);
525+ if (loop_process == ProcessContinue) {
526+ continue;
527+
528+ } else if (loop_process == ProcessBreak) {
529+ break;
530+ }
531+
532+ } else if (line_count == 1) {
533+ // 応答コード
534+ handleReturnCode(buffer, settings, timeout, type, total_times);
535+
536+ } else if (line_count == 2) {
537+ // タイムスタンプ
538+ if (timestamp) {
539+ *timestamp = decode(buffer, 4);
540+ }
541+ } else {
542+ if (line_count == 3) {
543+ // 受信データがない先頭からの領域を、ダミーデータで埋める
544+ for (int i = 0; i < settings.capture_first; ++i) {
545+ data.push_back(InvalidRange);
546+ if (type == ME) {
547+ // ME 受信のときは、強度データ分も埋める
548+ data.push_back(InvalidRange);
549+ }
550+ }
551+ }
552+ // 距離データの格納
553+ left_packet_data =
554+ addLengthData(data, string(buffer), left_packet_data,
555+ settings.data_byte, settings.skip_lines);
556+ }
557+ ++line_count;
558+ timeout = ContinuousTimeout;
559+ }
560+
561+ // !!! type が距離データ取得のときは、正常に受信が完了したか、を確認すべき
562+
563+ // ME で "まとめる数" 設定以上のデータが返されるバグに対処
564+ size_t expected_n = settings.capture_last * ((type == ME) ? 2 : 1);
565+ if (expected_n < data.size()) {
566+ data.erase(data.begin() + expected_n, data.end());
567+ }
568+
569+ if (remain_times) {
570+ *remain_times = settings.remain_times;
571+ }
572+ return type;
573+ }
574+
575+
576+ bool parseGdEchoback(CaptureSettings& settings, const string& line)
577+ {
578+ if (line.size() != 12) {
579+ error_message_ = "Invalid Gx packet has arrived.";
580+ return false;
581+ }
582+
583+ settings.capture_first = substr2int(line, 2, 4);
584+ settings.capture_last = substr2int(line, 6, 4) + 1;
585+ int skip_lines = substr2int(line, 10, 2);
586+ settings.skip_lines = (skip_lines == 0) ? 1 : skip_lines;
587+ settings.data_byte = (line[1] == 'D') ? 3 : 2;
588+
589+ return true;
590+ }
591+
592+
593+ bool parseMdEchoback(CaptureSettings& settings, const string& line)
594+ {
595+ if (line.size() != 15) {
596+ error_message_ = "Invalid Mx packet has arrived.";
597+ return false;
598+ }
599+
600+ settings.capture_first = substr2int(line, 2, 4);
601+ settings.capture_last = substr2int(line, 6, 4) + 1;
602+ int skip_lines = substr2int(line, 10, 2);
603+ settings.skip_lines = (skip_lines == 0) ? 1 : skip_lines;
604+ settings.skip_frames = substr2int(line, 12, 1);
605+ settings.remain_times = substr2int(line, 13, 2);
606+ settings.data_byte = (line[1] == 'D') ? 3 : 2;
607+
608+ if (settings.remain_times == 1) {
609+ // 最後のデータ取得で、レーザを消灯扱いにする
610+ // 本当は、次のデータ取得後にレーザは消灯されている
611+ // 1 で判定すると、取得回数が 1 のときにも正常に動作するため
612+ mx_capturing_ = false;
613+
614+ } else {
615+ if (settings.remain_times > 0) {
616+ mx_capturing_ = true;
617+ } else if (settings.remain_times == 0) {
618+ settings.remain_times = 100;
619+ }
620+ }
621+
622+ return true;
623+ }
624+
625+
626+ bool parseMeEchoback(CaptureSettings& settings, const string& line)
627+ {
628+ if (line.size() != 15) {
629+ error_message_ = "Invalid ME packet has arrived.";
630+ return false;
631+ }
632+
633+ settings.capture_first = substr2int(line, 2, 4);
634+ settings.capture_last = substr2int(line, 6, 4) + 1;
635+ int skip_lines = substr2int(line, 10, 2);
636+ settings.skip_lines = (skip_lines == 0) ? 1 : skip_lines;
637+ settings.skip_frames = substr2int(line, 12, 1);
638+ settings.remain_times = substr2int(line, 13, 2);
639+ settings.data_byte = 3;
640+
641+ if (settings.remain_times == 1) {
642+ mx_capturing_ = false;
643+
644+ } else {
645+ mx_capturing_ = true;
646+ }
647+
648+ return true;
649+ }
650+
651+
652+ string addLengthData(vector<long>& data,
653+ const string& line,
654+ const string& left_packet_data,
655+ const size_t data_byte, const int skip_lines = 1)
656+ {
657+ if (line.empty()) {
658+ // 空行の場合、戻る
659+ return left_packet_data;
660+ }
661+
662+ // 端数。次回に処理する分
663+ string left_byte = left_packet_data;
664+
665+ size_t data_size = (left_byte.size() + (line.size() - 1)) / data_byte;
666+ size_t n = data_size * data_byte - left_byte.size();
667+ for (size_t i = 0; i < n; ++i) {
668+ left_byte.push_back(line[i]);
669+ if (left_byte.size() >= data_byte) {
670+ // データを距離に変換して、格納
671+ long length = decode(&left_byte[0], data_byte);
672+ for (int j = 0; j < skip_lines; ++j) {
673+ data.push_back(length);
674+ }
675+ left_byte.clear();
676+ }
677+ }
678+ left_byte += line.substr(n, (line.size() - n) - 1);
679+
680+ return left_byte;
681+ }
682+};
683+
684+
685+ScipHandler::ScipHandler(void) : pimpl(new pImpl)
686+{
687+}
688+
689+
690+ScipHandler::~ScipHandler(void)
691+{
692+}
693+
694+
695+const char* ScipHandler::what(void) const
696+{
697+ return pimpl->error_message_.c_str();
698+}
699+
700+
701+long ScipHandler::decode(const char* data, size_t size)
702+{
703+ const char* p = data;
704+ const char* last_p = p + size;
705+
706+ int value = 0;
707+ while (p < last_p) {
708+ value <<= 6;
709+ value &= ~0x3f;
710+ value |= *p++ - 0x30;
711+ }
712+ return value;
713+}
714+
715+
716+bool ScipHandler::checkSum(const char* buffer, int size, char actual_sum)
717+{
718+ const char* p = buffer;
719+ const char* last_p = p + size;
720+
721+ char expected_sum = 0x00;
722+ while (p < last_p) {
723+ expected_sum += *p++;
724+ }
725+ expected_sum = (expected_sum & 0x3f) + 0x30;
726+
727+ return (expected_sum == actual_sum) ? true : false;
728+}
729+
730+
731+void ScipHandler::setConnection(Connection* con)
732+{
733+ pimpl->con_ = con;
734+}
735+
736+
737+Connection* ScipHandler::connection(void)
738+{
739+ return pimpl->con_;
740+}
741+
742+
743+bool ScipHandler::connect(const char* device, long baudrate)
744+{
745+ return pimpl->connect(device, baudrate);
746+}
747+
748+
749+int ScipHandler::send(const char data[], int size)
750+{
751+ if (size >= 2) {
752+ // コマンド送信後の受信前だと、レーザ点灯中の判定ができないため、
753+ // ここでレーザ点灯中の状態を判定する
754+ if ((! strncmp("MD", data, 2)) || (! strncmp("MS", data, 2)) ||
755+ (! strncmp("ME", data, 2))) {
756+ pimpl->laser_state_ = pImpl::LaserOn;
757+ pimpl->mx_capturing_ = true;
758+ pimpl->isPreCommand_QT_ = false;
759+ }
760+ }
761+ return pimpl->con_->send(data, size);
762+}
763+
764+
765+int ScipHandler::recv(char data[], int size, int timeout)
766+{
767+ return pimpl->con_->receive(data, size, timeout);
768+}
769+
770+
771+bool ScipHandler::loadParameter(RangeSensorParameter& parameters)
772+{
773+ return pimpl->loadParameter(parameters);
774+}
775+
776+
777+bool ScipHandler::versionLines(vector<string>& lines)
778+{
779+ int return_code = -1;
780+ char expected_response[] = { 0, -1 };
781+ if (! pimpl->response(return_code, "VV\n", expected_response, &lines)) {
782+ return false;
783+ }
784+ return true;
785+}
786+
787+
788+bool ScipHandler::setRawTimestampMode(bool on)
789+{
790+ char send_command[] = "TMx\n";
791+ send_command[2] = (on) ? '0' : '2';
792+
793+ // TM0 or TM2 の送信
794+ int return_code = -1;
795+ char expected_response[] = { 0, -1 };
796+ if (! pimpl->response(return_code, send_command, expected_response)) {
797+ pimpl->error_message_ = (on) ? "TM0 fail." : "TM2 fail.";
798+ return false;
799+ }
800+
801+ // TM1, TM2 の応答が正常ならば、レーザは消灯しているはず
802+ pimpl->laser_state_ = pImpl::LaserOff;
803+
804+ return true;
805+}
806+
807+
808+bool ScipHandler::rawTimestamp(int* timestamp)
809+{
810+ // TM1 の値を返す
811+ int return_code = -1;
812+ char expected_response[] = { 0, -1 };
813+ vector<string> lines;
814+ if (! pimpl->response(return_code, "TM1\n", expected_response, &lines)) {
815+ pimpl->error_message_ = "TM1 fail.";
816+ return false;
817+ }
818+
819+ if ((lines.size() != 1) || (lines[0].size() != 5)) {
820+ pimpl->error_message_ = "response mismatch.";
821+ return false;
822+ }
823+
824+ *timestamp = decode(lines[0].c_str(), 4);
825+ return true;
826+}
827+
828+
829+bool ScipHandler::setLaserOutput(bool on, bool force)
830+{
831+ return pimpl->setLaserOutput(on, force);
832+}
833+
834+
835+CaptureType ScipHandler::receiveCaptureData(vector<long>& data,
836+ CaptureSettings& settings,
837+ long* timestamp, int* remain_times,
838+ int* total_times)
839+{
840+ return pimpl->receiveCaptureData(data, settings,
841+ timestamp, remain_times, total_times);
842+}
--- trunk/libs/coordinate/Coordinate.cpp (revision 1976)
+++ trunk/libs/coordinate/Coordinate.cpp (revision 1977)
@@ -1,210 +1,210 @@
1-/*!
2- \file
3- \brief 座標系
4-
5- \author Satofumi KAMIMURA
6-
7- $Id$
8-*/
9-
10-#include "Coordinate.h"
11-#include "MathUtils.h"
12-#include <map>
13-#include <cstdio>
14-
15-using namespace qrk;
16-using namespace std;
17-
18-
19-namespace
20-{
21- class Child
22- {
23- public:
24- Coordinate* pointer_;
25- Position<long> offset_;
26-
27-
28- Child(void) : pointer_(NULL)
29- {
30- }
31-
32-
33- Child(Coordinate* pointer, const Position<long>& offset)
34- : pointer_(pointer), offset_(offset)
35- {
36- }
37- };
38- typedef map<Coordinate*, Child> Children;
39-
40-
41- // Rotate.h の関数を使うようにする
42- Position<long> rotate(const Position<long>& point, const double radian)
43- {
44- long x = static_cast<long>
45- (lrint((point.x * cos(radian)) - (point.y * sin(radian))));
46- long y = static_cast<long>
47- (lrint((point.x * sin(radian)) + (point.y * cos(radian))));
48-
49- return Position<long>(x, y, point.angle);
50- }
51-}
52-
53-
54-struct Coordinate::pImpl
55-{
56- Coordinate* parent_;
57- Children children_;
58-
59-
60- pImpl(Coordinate* parent)
61- : parent_(parent)
62- {
63- }
64-
65-
66- void eraseFromParent(Coordinate* coordinate)
67- {
68- parent_->pimpl->children_.erase(coordinate);
69- }
70-
71-
72- Position<long> positionOnChild(const Coordinate* child,
73- const Position<long>& position)
74- {
75- Coordinate* parent = child->parent();
76- Position<long> offset = parent->offset(child);
77-
78- // 子の座標系での位置 x, y を、子の offset の角度 t 向きに回転させる
79- // 計算後の x, y と t に 子の offset の x, y, t をそれぞれ加算する
80- Position<long> converted_position = rotate(position, offset.to_rad());
81- converted_position += offset;
82-
83- return converted_position;
84- }
85-
86-
87- Position<long> positionOnParent(const Coordinate* child,
88- const Position<long>& position)
89- {
90- Coordinate* parent = child->parent();
91- Position<long> offset = parent->offset(child);
92-
93- // 親の座標系での位置 x, y から offset の x, y, t を引く
94- // 計算後の x, y を offset の角度 -t 向きに回転させる
95- Position<long> converted_position = position - offset;
96- return rotate(converted_position, -offset.to_rad());
97- }
98-
99-
100- Position<long> positionOnRoot(const Coordinate* coordinate,
101- const Position<long>& position)
102- {
103- if (coordinate == root()) {
104- return position;
105- }
106-
107- return positionOnParent(coordinate,
108- positionOnRoot(coordinate->parent(), position));
109- }
110-};
111-
112-
113-// root() 専用のコンストラクタ
114-Coordinate::Coordinate(Coordinate* parent) : pimpl(new pImpl(parent))
115-{
116-}
117-
118-
119-Coordinate::Coordinate(void) : pimpl(new pImpl(root()))
120-{
121- // root の子の座標系として登録する
122- setOriginTo(root(), Position<long>(0, 0, deg(0)));
123-}
124-
125-
126-Coordinate::~Coordinate(void)
127-{
128- if (! pimpl->parent_) {
129- return;
130- }
131-
132- // 子の座標系を親座標系に割り振る
133- // !!!
134-
135- // 親から登録を削除
136- pimpl->eraseFromParent(this);
137-}
138-
139-
140-Coordinate* Coordinate::root(void)
141-{
142- static Coordinate root_coordinate(NULL);
143- return &root_coordinate;
144-}
145-
146-
147-void Coordinate::setOriginTo(Coordinate* parent,
148- const Position<long>& position)
149-{
150- pimpl->eraseFromParent(this);
151-
152- parent->pimpl->children_[this] = Child(this, position);
153- pimpl->parent_ = parent;
154-}
155-
156-
157-Coordinate* Coordinate::parent(void) const
158-{
159- return pimpl->parent_;
160-}
161-
162-
163-set<Coordinate*> Coordinate::children(void) const
164-{
165- set<Coordinate*> children;
166- for (Children::iterator it = pimpl->children_.begin();
167- it != pimpl->children_.end(); ++it) {
168- children.insert(it->first);
169- }
170- return children;
171-}
172-
173-
174-Position<long> Coordinate::offset(const Coordinate* child) const
175-{
176- Children::iterator it = pimpl->children_.find(const_cast<Coordinate*>(child));
177- if (it != pimpl->children_.end()) {
178- return it->second.offset_;
179- }
180-
181- // 例外を投げることを検討する
182- // !!!
183- fprintf(stderr, "Coordinte::offset(): no child.\n");
184-
185- Position<long> dummy;
186- return dummy;
187-}
188-
189-
190-Position<long> Coordinate::pointPosition(const Coordinate* coordinate,
191- const Position<long>& position) const
192-{
193- // 座標系に特有な処理を実行
194- const_cast<Coordinate*>(this)->beforeEvaluate();
195- if (coordinate) {
196- const_cast<Coordinate*>(coordinate)->beforeEvaluate();
197- }
198-
199- // coordinate のグローバル座標系での位置を求める
200- Position<long> root_position = position;
201- Coordinate* root_coordinate = root();
202- for (const Coordinate* p = coordinate;
203- p != root_coordinate; p = p->parent()) {
204- root_position = pimpl->positionOnChild(p, root_position);
205- //fprintf(stderr, "{%ld,%ld,%d}, ", root_position.x, root_position.y, root_position.deg());
206- }
207-
208- // この座標系での位置を求めて返す
209- return pimpl->positionOnRoot(this, root_position);
210-}
1+/*!
2+ \file
3+ \brief 座標系
4+
5+ \author Satofumi KAMIMURA
6+
7+ $Id$
8+*/
9+
10+#include "Coordinate.h"
11+#include "MathUtils.h"
12+#include <map>
13+#include <cstdio>
14+
15+using namespace qrk;
16+using namespace std;
17+
18+
19+namespace
20+{
21+ class Child
22+ {
23+ public:
24+ Coordinate* pointer_;
25+ Position<long> offset_;
26+
27+
28+ Child(void) : pointer_(NULL)
29+ {
30+ }
31+
32+
33+ Child(Coordinate* pointer, const Position<long>& offset)
34+ : pointer_(pointer), offset_(offset)
35+ {
36+ }
37+ };
38+ typedef map<Coordinate*, Child> Children;
39+
40+
41+ // Rotate.h の関数を使うようにする
42+ Position<long> rotate(const Position<long>& point, const double radian)
43+ {
44+ long x = static_cast<long>
45+ (lrint((point.x * cos(radian)) - (point.y * sin(radian))));
46+ long y = static_cast<long>
47+ (lrint((point.x * sin(radian)) + (point.y * cos(radian))));
48+
49+ return Position<long>(x, y, point.angle);
50+ }
51+}
52+
53+
54+struct Coordinate::pImpl
55+{
56+ Coordinate* parent_;
57+ Children children_;
58+
59+
60+ pImpl(Coordinate* parent)
61+ : parent_(parent)
62+ {
63+ }
64+
65+
66+ void eraseFromParent(Coordinate* coordinate)
67+ {
68+ parent_->pimpl->children_.erase(coordinate);
69+ }
70+
71+
72+ Position<long> positionOnChild(const Coordinate* child,
73+ const Position<long>& position)
74+ {
75+ Coordinate* parent = child->parent();
76+ Position<long> offset = parent->offset(child);
77+
78+ // 子の座標系での位置 x, y を、子の offset の角度 t 向きに回転させる
79+ // 計算後の x, y と t に 子の offset の x, y, t をそれぞれ加算する
80+ Position<long> converted_position = rotate(position, offset.to_rad());
81+ converted_position += offset;
82+
83+ return converted_position;
84+ }
85+
86+
87+ Position<long> positionOnParent(const Coordinate* child,
88+ const Position<long>& position)
89+ {
90+ Coordinate* parent = child->parent();
91+ Position<long> offset = parent->offset(child);
92+
93+ // 親の座標系での位置 x, y から offset の x, y, t を引く
94+ // 計算後の x, y を offset の角度 -t 向きに回転させる
95+ Position<long> converted_position = position - offset;
96+ return rotate(converted_position, -offset.to_rad());
97+ }
98+
99+
100+ Position<long> positionOnRoot(const Coordinate* coordinate,
101+ const Position<long>& position)
102+ {
103+ if (coordinate == root()) {
104+ return position;
105+ }
106+
107+ return positionOnParent(coordinate,
108+ positionOnRoot(coordinate->parent(), position));
109+ }
110+};
111+
112+
113+// root() 専用のコンストラクタ
114+Coordinate::Coordinate(Coordinate* parent) : pimpl(new pImpl(parent))
115+{
116+}
117+
118+
119+Coordinate::Coordinate(void) : pimpl(new pImpl(root()))
120+{
121+ // root の子の座標系として登録する
122+ setOriginTo(root(), Position<long>(0, 0, deg(0)));
123+}
124+
125+
126+Coordinate::~Coordinate(void)
127+{
128+ if (! pimpl->parent_) {
129+ return;
130+ }
131+
132+ // 子の座標系を親座標系に割り振る
133+ // !!!
134+
135+ // 親から登録を削除
136+ pimpl->eraseFromParent(this);
137+}
138+
139+
140+Coordinate* Coordinate::root(void)
141+{
142+ static Coordinate root_coordinate(NULL);
143+ return &root_coordinate;
144+}
145+
146+
147+void Coordinate::setOriginTo(Coordinate* parent,
148+ const Position<long>& position)
149+{
150+ pimpl->eraseFromParent(this);
151+
152+ parent->pimpl->children_[this] = Child(this, position);
153+ pimpl->parent_ = parent;
154+}
155+
156+
157+Coordinate* Coordinate::parent(void) const
158+{
159+ return pimpl->parent_;
160+}
161+
162+
163+set<Coordinate*> Coordinate::children(void) const
164+{
165+ set<Coordinate*> children;
166+ for (Children::iterator it = pimpl->children_.begin();
167+ it != pimpl->children_.end(); ++it) {
168+ children.insert(it->first);
169+ }
170+ return children;
171+}
172+
173+
174+Position<long> Coordinate::offset(const Coordinate* child) const
175+{
176+ Children::iterator it = pimpl->children_.find(const_cast<Coordinate*>(child));
177+ if (it != pimpl->children_.end()) {
178+ return it->second.offset_;
179+ }
180+
181+ // 例外を投げることを検討する
182+ // !!!
183+ fprintf(stderr, "Coordinte::offset(): no child.\n");
184+
185+ Position<long> dummy;
186+ return dummy;
187+}
188+
189+
190+Position<long> Coordinate::pointPosition(const Coordinate* coordinate,
191+ const Position<long>& position) const
192+{
193+ // 座標系に特有な処理を実行
194+ const_cast<Coordinate*>(this)->beforeEvaluate();
195+ if (coordinate) {
196+ const_cast<Coordinate*>(coordinate)->beforeEvaluate();
197+ }
198+
199+ // coordinate のグローバル座標系での位置を求める
200+ Position<long> root_position = position;
201+ Coordinate* root_coordinate = root();
202+ for (const Coordinate* p = coordinate;
203+ p != root_coordinate; p = p->parent()) {
204+ root_position = pimpl->positionOnChild(p, root_position);
205+ //fprintf(stderr, "{%ld,%ld,%d}, ", root_position.x, root_position.y, root_position.deg());
206+ }
207+
208+ // この座標系での位置を求めて返す
209+ return pimpl->positionOnRoot(this, root_position);
210+}
--- trunk/libs/connection/SerialDevice.cpp (revision 1976)
+++ trunk/libs/connection/SerialDevice.cpp (revision 1977)
@@ -1,214 +1,214 @@
1-/*!
2- \file
3- \brief シリアル通信
4-
5- \author Satofumi KAMIMURA
6-
7- $Id$
8-*/
9-
10-#include "SerialDevice.h"
11-#include "DetectOS.h"
12-#include "RingBuffer.h"
13-#include <string>
14-
15-using namespace qrk;
16-using namespace std;
17-
18-
19-#if defined(WINDOWS_OS)
20-#include "SerialDevice_win.cpp" // Windows (win32) 環境
21-#else
22-#include "SerialDevice_lin.cpp" // Linux, Mac 環境 (共通)
23-#endif
24-
25-
26-struct SerialDevice::pImpl
27-{
28- string error_message_;
29- long baudrate_;
30- RawSerialDevice raw_;
31- RingBuffer<char> ring_buffer_; //!< 受信バッファ
32-
33-
34- pImpl(void) : error_message_("no error"), baudrate_(0)
35- {
36- }
37-
38-
39- void updateRingBuffer(void)
40- {
41- enum { BufferSize = 2048 };
42- char buffer[BufferSize];
43-
44- int n = raw_.receive(buffer, BufferSize, 0);
45- if (n > 0) {
46- ring_buffer_.put(buffer, n);
47- }
48- }
49-
50-
51- int receive(char* data, size_t count, int timeout)
52- {
53- if (! isConnected()) {
54- error_message_ = "no connection.";
55- return -1;
56- }
57- if (count == 0) {
58- return 0;
59- }
60-
61- size_t filled = 0;
62-
63- size_t ring_filled = ring_buffer_.size();
64- if (ring_filled < count) {
65- updateRingBuffer();
66- }
67-
68- // バッファにデータがある場合、バッファからデータを格納する
69- size_t read_size = std::min(count, ring_buffer_.size());
70- ring_buffer_.get(data, read_size);
71- filled += read_size;
72-
73- // バッファが空の場合、残りのデータはシステムから直接読み込む
74- read_size = max(0, static_cast<int>(count - filled));
75- if (read_size > 0) {
76- int n = raw_.receive(&data[filled],
77- static_cast<int>(read_size), timeout);
78- if (n < 0) {
79- error_message_ = raw_.what();
80- return n;
81- }
82- filled += n;
83- }
84- return static_cast<int>(filled);
85- }
86-
87-
88- bool isConnected(void)
89- {
90- return raw_.isConnected();
91- }
92-};
93-
94-
95-SerialDevice::SerialDevice(void) : pimpl(new pImpl)
96-{
97-}
98-
99-
100-SerialDevice::~SerialDevice(void)
101-{
102- disconnect();
103-}
104-
105-
106-const char* SerialDevice::what(void) const
107-{
108- return pimpl->error_message_.c_str();
109-}
110-
111-
112-bool SerialDevice::connect(const char* device, long baudrate)
113-{
114- disconnect();
115- clear();
116- if (! pimpl->raw_.connect(device, baudrate)) {
117- pimpl->error_message_ = pimpl->raw_.what();
118- return false;
119- } else {
120- return true;
121- }
122-}
123-
124-
125-void SerialDevice::disconnect(void)
126-{
127- return pimpl->raw_.disconnect();
128-}
129-
130-
131-bool SerialDevice::setBaudrate(long baudrate)
132-{
133- if (! pimpl->raw_.setBaudrate(baudrate)) {
134- pimpl->error_message_ = pimpl->raw_.what();
135- pimpl->baudrate_ = 0;
136- return false;
137- }
138- pimpl->baudrate_ = baudrate;
139- return true;
140-}
141-
142-
143-long SerialDevice::baudrate(void) const
144-{
145- return pimpl->baudrate_;
146-}
147-
148-
149-bool SerialDevice::isConnected(void) const
150-{
151- return pimpl->isConnected();
152-}
153-
154-
155-int SerialDevice::send(const char* data, size_t count)
156-{
157- if (! isConnected()) {
158- pimpl->error_message_ = "no connection.";
159- return 0;
160- }
161-
162- int n = pimpl->raw_.send(data, static_cast<int>(count));
163- if (n < 0) {
164- pimpl->error_message_ = pimpl->raw_.what();
165- }
166- return n;
167-}
168-
169-
170-int SerialDevice::receive(char* data, size_t count, int timeout)
171-{
172- if (! isConnected()) {
173- pimpl->error_message_ = "no connection.";
174- return 0;
175- }
176-
177- return pimpl->receive(data, count, timeout);
178-}
179-
180-
181-size_t SerialDevice::size(void) const
182-{
183- pimpl->updateRingBuffer();
184- return pimpl->ring_buffer_.size();
185-}
186-
187-
188-void SerialDevice::flush(void)
189-{
190- if (! isConnected()) {
191- pimpl->error_message_ = "no connection.";
192- return;
193- }
194-
195- return pimpl->raw_.flush();
196-}
197-
198-
199-void SerialDevice::clear(void)
200-{
201- pimpl->raw_.flush();
202- pimpl->ring_buffer_.clear();
203-}
204-
205-
206-void SerialDevice::ungetc(const char ch)
207-{
208- if (! isConnected()) {
209- pimpl->error_message_ = "no connection.";
210- return;
211- }
212-
213- pimpl->ring_buffer_.ungetc(ch);
214-}
1+/*!
2+ \file
3+ \brief シリアル通信
4+
5+ \author Satofumi KAMIMURA
6+
7+ $Id$
8+*/
9+
10+#include "SerialDevice.h"
11+#include "DetectOS.h"
12+#include "RingBuffer.h"
13+#include <string>
14+
15+using namespace qrk;
16+using namespace std;
17+
18+
19+#if defined(WINDOWS_OS)
20+#include "SerialDevice_win.cpp" // Windows (win32) 環境
21+#else
22+#include "SerialDevice_lin.cpp" // Linux, Mac 環境 (共通)
23+#endif
24+
25+
26+struct SerialDevice::pImpl
27+{
28+ string error_message_;
29+ long baudrate_;
30+ RawSerialDevice raw_;
31+ RingBuffer<char> ring_buffer_; //!< 受信バッファ
32+
33+
34+ pImpl(void) : error_message_("no error"), baudrate_(0)
35+ {
36+ }
37+
38+
39+ void updateRingBuffer(void)
40+ {
41+ enum { BufferSize = 2048 };
42+ char buffer[BufferSize];
43+
44+ int n = raw_.receive(buffer, BufferSize, 0);
45+ if (n > 0) {
46+ ring_buffer_.put(buffer, n);
47+ }
48+ }
49+
50+
51+ int receive(char* data, size_t count, int timeout)
52+ {
53+ if (! isConnected()) {
54+ error_message_ = "no connection.";
55+ return -1;
56+ }
57+ if (count == 0) {
58+ return 0;
59+ }
60+
61+ size_t filled = 0;
62+
63+ size_t ring_filled = ring_buffer_.size();
64+ if (ring_filled < count) {
65+ updateRingBuffer();
66+ }
67+
68+ // バッファにデータがある場合、バッファからデータを格納する
69+ size_t read_size = std::min(count, ring_buffer_.size());
70+ ring_buffer_.get(data, read_size);
71+ filled += read_size;
72+
73+ // バッファが空の場合、残りのデータはシステムから直接読み込む
74+ read_size = max(0, static_cast<int>(count - filled));
75+ if (read_size > 0) {
76+ int n = raw_.receive(&data[filled],
77+ static_cast<int>(read_size), timeout);
78+ if (n < 0) {
79+ error_message_ = raw_.what();
80+ return n;
81+ }
82+ filled += n;
83+ }
84+ return static_cast<int>(filled);
85+ }
86+
87+
88+ bool isConnected(void)
89+ {
90+ return raw_.isConnected();
91+ }
92+};
93+
94+
95+SerialDevice::SerialDevice(void) : pimpl(new pImpl)
96+{
97+}
98+
99+
100+SerialDevice::~SerialDevice(void)
101+{
102+ disconnect();
103+}
104+
105+
106+const char* SerialDevice::what(void) const
107+{
108+ return pimpl->error_message_.c_str();
109+}
110+
111+
112+bool SerialDevice::connect(const char* device, long baudrate)
113+{
114+ disconnect();
115+ clear();
116+ if (! pimpl->raw_.connect(device, baudrate)) {
117+ pimpl->error_message_ = pimpl->raw_.what();
118+ return false;
119+ } else {
120+ return true;
121+ }
122+}
123+
124+
125+void SerialDevice::disconnect(void)
126+{
127+ return pimpl->raw_.disconnect();
128+}
129+
130+
131+bool SerialDevice::setBaudrate(long baudrate)
132+{
133+ if (! pimpl->raw_.setBaudrate(baudrate)) {
134+ pimpl->error_message_ = pimpl->raw_.what();
135+ pimpl->baudrate_ = 0;
136+ return false;
137+ }
138+ pimpl->baudrate_ = baudrate;
139+ return true;
140+}
141+
142+
143+long SerialDevice::baudrate(void) const
144+{
145+ return pimpl->baudrate_;
146+}
147+
148+
149+bool SerialDevice::isConnected(void) const
150+{
151+ return pimpl->isConnected();
152+}
153+
154+
155+int SerialDevice::send(const char* data, size_t count)
156+{
157+ if (! isConnected()) {
158+ pimpl->error_message_ = "no connection.";
159+ return 0;
160+ }
161+
162+ int n = pimpl->raw_.send(data, static_cast<int>(count));
163+ if (n < 0) {
164+ pimpl->error_message_ = pimpl->raw_.what();
165+ }
166+ return n;
167+}
168+
169+
170+int SerialDevice::receive(char* data, size_t count, int timeout)
171+{
172+ if (! isConnected()) {
173+ pimpl->error_message_ = "no connection.";
174+ return 0;
175+ }
176+
177+ return pimpl->receive(data, count, timeout);
178+}
179+
180+
181+size_t SerialDevice::size(void) const
182+{
183+ pimpl->updateRingBuffer();
184+ return pimpl->ring_buffer_.size();
185+}
186+
187+
188+void SerialDevice::flush(void)
189+{
190+ if (! isConnected()) {
191+ pimpl->error_message_ = "no connection.";
192+ return;
193+ }
194+
195+ return pimpl->raw_.flush();
196+}
197+
198+
199+void SerialDevice::clear(void)
200+{
201+ pimpl->raw_.flush();
202+ pimpl->ring_buffer_.clear();
203+}
204+
205+
206+void SerialDevice::ungetc(const char ch)
207+{
208+ if (! isConnected()) {
209+ pimpl->error_message_ = "no connection.";
210+ return;
211+ }
212+
213+ pimpl->ring_buffer_.ungetc(ch);
214+}
--- trunk/libs/connection/SerialDevice_lin.cpp (revision 1976)
+++ trunk/libs/connection/SerialDevice_lin.cpp (revision 1977)
@@ -1,205 +1,205 @@
1-/*!
2- \file
3- \brief シリアル通信の実処理 (Linux, Mac)
4-
5- \author Satofumi KAMIMURA
6-
7- $Id$
8-*/
9-
10-#include <fcntl.h>
11-#include <termios.h>
12-#include <cerrno>
13-#include <cstring>
14-#include <cstdio>
15-
16-
17-class RawSerialDevice
18-{
19- enum {
20- InvalidFd = -1,
21- };
22-
23- string error_message_;
24- int fd_;
25- struct termios sio_; //!< 通信ターミナル制御
26- fd_set rfds_; //!< タイムアウト制御
27-
28-
29- bool waitReceive(int timeout)
30- {
31- // タイムアウト設定
32- FD_ZERO(&rfds_);
33- FD_SET(fd_, &rfds_);
34-
35- struct timeval tv;
36- tv.tv_sec = timeout / 1000;
37- tv.tv_usec = (timeout % 1000) * 1000;
38-
39- if (select(fd_ + 1, &rfds_, NULL, NULL,
40- (timeout < 0) ? NULL : &tv) <= 0) {
41- /* タイムアウト発生 */
42- return false;
43- }
44- return true;
45- }
46-
47-
48-public:
49- RawSerialDevice(void) : error_message_("no error."), fd_(InvalidFd)
50- {
51- }
52-
53-
54- const char* what(void)
55- {
56- return error_message_.c_str();
57- }
58-
59-
60- bool connect(const char* device, long baudrate)
61- {
62-#ifndef MAC_OS
63- enum { O_EXLOCK = 0x0 }; // Linux では使えないのでダミーを作成しておく
64-#endif
65- fd_ = open(device, O_RDWR | O_EXLOCK | O_NONBLOCK | O_NOCTTY);
66- if (fd_ < 0) {
67- // 接続に失敗
68- error_message_ = string(device) + ": " + strerror(errno);
69- return false;
70- }
71- int flags = fcntl(fd_, F_GETFL, 0);
72- fcntl(fd_, F_SETFL, flags & ~O_NONBLOCK);
73-
74- // シリアル通信の初期化
75- tcgetattr(fd_, &sio_);
76- sio_.c_iflag = 0;
77- sio_.c_oflag = 0;
78- sio_.c_cflag &= ~(CSIZE | PARENB | CSTOPB);
79- sio_.c_cflag |= CS8 | CREAD | CLOCAL;
80- sio_.c_lflag &= ~(ICANON | ECHO | ISIG | IEXTEN);
81-
82- sio_.c_cc[VMIN] = 0;
83- sio_.c_cc[VTIME] = 0;
84-
85- // ボーレートの変更
86- if (! setBaudrate(baudrate)) {
87- return false;
88- }
89- return true;
90- }
91-
92-
93- void disconnect(void)
94- {
95- if (fd_ != InvalidFd) {
96- close(fd_);
97- fd_ = InvalidFd;
98- }
99- }
100-
101-
102- bool isConnected(void)
103- {
104- return (fd_ == InvalidFd) ? false : true;
105- }
106-
107-
108- bool setBaudrate(long baudrate)
109- {
110- long baudrate_value = -1;
111- enum { ErrorMessageSize = 256 };
112- char error_message[ErrorMessageSize];
113-
114- switch (baudrate) {
115-
116- case 4800:
117- baudrate_value = B4800;
118- break;
119-
120- case 9600:
121- baudrate_value = B9600;
122- break;
123-
124- case 19200:
125- baudrate_value = B19200;
126- break;
127-
128- case 38400:
129- baudrate_value = B38400;
130- break;
131-
132- case 57600:
133- baudrate_value = B57600;
134- break;
135-
136- case 115200:
137- baudrate_value = B115200;
138- break;
139-
140- default:
141- sprintf(error_message, "No handle baudrate value: %ld", baudrate);
142- error_message_ = string(error_message);
143- return false;
144- }
145-
146- /* ボーレート変更 */
147- cfsetospeed(&sio_, baudrate_value);
148- cfsetispeed(&sio_, baudrate_value);
149- tcsetattr(fd_, TCSANOW, &sio_);
150- flush();
151-
152- return true;
153- }
154-
155-
156- int send(const char* data, int count)
157- {
158- if (! isConnected()) {
159- error_message_ = "no connection.";
160- return 0;
161- }
162- return write(fd_, data, count);
163- }
164-
165-
166- int receive(char buffer[], int count, int timeout)
167- {
168- int filled = 0;
169-
170- // 指定サイズの読み出しを行う
171- int n = read(fd_, buffer, count);
172- if (n < 0) {
173- return n;
174- }
175- filled += n;
176-
177- // 受信が完了していたら、戻る
178- if (filled >= count) {
179- return filled;
180- }
181-
182- // タイムアウト付きで読み出しを行う
183- while (filled < count) {
184- if (! waitReceive(timeout)) {
185- break;
186- }
187-
188- int required_n = count - filled;
189- n = read(fd_, &buffer[filled], required_n);
190- if (n <= 0) {
191- /* 読み出しエラー。現在までの受信内容で戻る */
192- break;
193- }
194- filled += n;
195- }
196- return filled;
197- }
198-
199-
200- void flush(void)
201- {
202- tcdrain(fd_);
203- tcflush(fd_, TCIOFLUSH);
204- }
205-};
1+/*!
2+ \file
3+ \brief シリアル通信の実処理 (Linux, Mac)
4+
5+ \author Satofumi KAMIMURA
6+
7+ $Id$
8+*/
9+
10+#include <fcntl.h>
11+#include <termios.h>
12+#include <cerrno>
13+#include <cstring>
14+#include <cstdio>
15+
16+
17+class RawSerialDevice
18+{
19+ enum {
20+ InvalidFd = -1,
21+ };
22+
23+ string error_message_;
24+ int fd_;
25+ struct termios sio_; //!< 通信ターミナル制御
26+ fd_set rfds_; //!< タイムアウト制御
27+
28+
29+ bool waitReceive(int timeout)
30+ {
31+ // タイムアウト設定
32+ FD_ZERO(&rfds_);
33+ FD_SET(fd_, &rfds_);
34+
35+ struct timeval tv;
36+ tv.tv_sec = timeout / 1000;
37+ tv.tv_usec = (timeout % 1000) * 1000;
38+
39+ if (select(fd_ + 1, &rfds_, NULL, NULL,
40+ (timeout < 0) ? NULL : &tv) <= 0) {
41+ /* タイムアウト発生 */
42+ return false;
43+ }
44+ return true;
45+ }
46+
47+
48+public:
49+ RawSerialDevice(void) : error_message_("no error."), fd_(InvalidFd)
50+ {
51+ }
52+
53+
54+ const char* what(void)
55+ {
56+ return error_message_.c_str();
57+ }
58+
59+
60+ bool connect(const char* device, long baudrate)
61+ {
62+#ifndef MAC_OS
63+ enum { O_EXLOCK = 0x0 }; // Linux では使えないのでダミーを作成しておく
64+#endif
65+ fd_ = open(device, O_RDWR | O_EXLOCK | O_NONBLOCK | O_NOCTTY);
66+ if (fd_ < 0) {
67+ // 接続に失敗
68+ error_message_ = string(device) + ": " + strerror(errno);
69+ return false;
70+ }
71+ int flags = fcntl(fd_, F_GETFL, 0);
72+ fcntl(fd_, F_SETFL, flags & ~O_NONBLOCK);
73+
74+ // シリアル通信の初期化
75+ tcgetattr(fd_, &sio_);
76+ sio_.c_iflag = 0;
77+ sio_.c_oflag = 0;
78+ sio_.c_cflag &= ~(CSIZE | PARENB | CSTOPB);
79+ sio_.c_cflag |= CS8 | CREAD | CLOCAL;
80+ sio_.c_lflag &= ~(ICANON | ECHO | ISIG | IEXTEN);
81+
82+ sio_.c_cc[VMIN] = 0;
83+ sio_.c_cc[VTIME] = 0;
84+
85+ // ボーレートの変更
86+ if (! setBaudrate(baudrate)) {
87+ return false;
88+ }
89+ return true;
90+ }
91+
92+
93+ void disconnect(void)
94+ {
95+ if (fd_ != InvalidFd) {
96+ close(fd_);
97+ fd_ = InvalidFd;
98+ }
99+ }
100+
101+
102+ bool isConnected(void)
103+ {
104+ return (fd_ == InvalidFd) ? false : true;
105+ }
106+
107+
108+ bool setBaudrate(long baudrate)
109+ {
110+ long baudrate_value = -1;
111+ enum { ErrorMessageSize = 256 };
112+ char error_message[ErrorMessageSize];
113+
114+ switch (baudrate) {
115+
116+ case 4800:
117+ baudrate_value = B4800;
118+ break;
119+
120+ case 9600:
121+ baudrate_value = B9600;
122+ break;
123+
124+ case 19200:
125+ baudrate_value = B19200;
126+ break;
127+
128+ case 38400:
129+ baudrate_value = B38400;
130+ break;
131+
132+ case 57600:
133+ baudrate_value = B57600;
134+ break;
135+
136+ case 115200:
137+ baudrate_value = B115200;
138+ break;
139+
140+ default:
141+ sprintf(error_message, "No handle baudrate value: %ld", baudrate);
142+ error_message_ = string(error_message);
143+ return false;
144+ }
145+
146+ /* ボーレート変更 */
147+ cfsetospeed(&sio_, baudrate_value);
148+ cfsetispeed(&sio_, baudrate_value);
149+ tcsetattr(fd_, TCSANOW, &sio_);
150+ flush();
151+
152+ return true;
153+ }
154+
155+
156+ int send(const char* data, int count)
157+ {
158+ if (! isConnected()) {
159+ error_message_ = "no connection.";
160+ return 0;
161+ }
162+ return write(fd_, data, count);
163+ }
164+
165+
166+ int receive(char buffer[], int count, int timeout)
167+ {
168+ int filled = 0;
169+
170+ // 指定サイズの読み出しを行う
171+ int n = read(fd_, buffer, count);
172+ if (n < 0) {
173+ return n;
174+ }
175+ filled += n;
176+
177+ // 受信が完了していたら、戻る
178+ if (filled >= count) {
179+ return filled;
180+ }
181+
182+ // タイムアウト付きで読み出しを行う
183+ while (filled < count) {
184+ if (! waitReceive(timeout)) {
185+ break;
186+ }
187+
188+ int required_n = count - filled;
189+ n = read(fd_, &buffer[filled], required_n);
190+ if (n <= 0) {
191+ /* 読み出しエラー。現在までの受信内容で戻る */
192+ break;
193+ }
194+ filled += n;
195+ }
196+ return filled;
197+ }
198+
199+
200+ void flush(void)
201+ {
202+ tcdrain(fd_);
203+ tcflush(fd_, TCIOFLUSH);
204+ }
205+};
--- trunk/libs/connection/FindComPorts.cpp (revision 1976)
+++ trunk/libs/connection/FindComPorts.cpp (revision 1977)
@@ -1,287 +1,287 @@
1-/*!
2- \file
3- \brief COM ポート一覧の取得
4-
5- \author Satofumi KAMIMURA
6-
7- $Id$
8-*/
9-
10-#include "FindComPorts.h"
11-#include "DetectOS.h"
12-#ifdef WINDOWS_OS
13-#include <windows.h>
14-#include <setupapi.h>
15-#include <deque>
16-#include <algorithm>
17-
18-#if defined(MSC)
19-#pragma comment(lib, "setupapi.lib")
20-#endif
21-
22-#else
23-#include <deque>
24-#include <algorithm>
25-#include <cstring>
26-#include <dirent.h>
27-#include <sys/stat.h>
28-#endif
29-
30-using namespace qrk;
31-using namespace std;
32-
33-
34-struct FindComPorts::pImpl
35-{
36- vector<string> base_names_;
37- vector<string> driver_names_;
38-
39-
40- pImpl(void)
41- {
42- // Windows はレジストリ情報だけから COM 一覧の探索を行う
43- // よって、以降の設定は Linux, MacOS のみで使われる
44-#if defined(LINUX_OS)
45- base_names_.push_back("/dev/ttyUSB");
46- base_names_.push_back("/dev/usb/ttyUSB");
47-#elif defined(MAC_OS)
48- base_names_.push_back("/dev/tty.usbmodem");
49-#endif
50- }
51-
52-
53- void addBaseName(const char* base_name)
54- {
55- base_names_.insert(base_names_.begin(), base_name);
56- }
57-
58-
59- void addDriverName(const char* driver_name)
60- {
61- driver_names_.push_back(driver_name);
62- }
63-
64-
65- void orderByDriver(deque<string>& ports, deque<string>& drivers,
66- const char* driver_name,
67- bool all_ports)
68- {
69- if (ports.empty()) {
70- return;
71- }
72-
73- vector<string>::const_iterator it =
74- std::find(driver_names_.begin(), driver_names_.end(), driver_name);
75- if (it != driver_names_.end()) {
76- ports.push_front(ports.back());
77- ports.pop_back();
78-
79- drivers.push_front(drivers.back());
80- drivers.pop_back();
81-
82- } else {
83- if (! all_ports) {
84- ports.pop_back();
85- drivers.pop_back();
86- }
87- }
88- }
89-
90-
91- void addFoundPorts(vector<string>& found_ports,
92- const string& port)
93- {
94- for (vector<string>::iterator it = found_ports.begin();
95- it != found_ports.end(); ++it) {
96-
97- // !!! データ構造を map にすべきかも
98- // !!! 検索のアルゴリズムを用いるべき
99-
100- if (! port.compare(*it)) {
101- return;
102- }
103- }
104- found_ports.push_back(port);
105- }
106-};
107-
108-
109-FindComPorts::FindComPorts(void) : pimpl(new pImpl)
110-{
111-}
112-
113-
114-FindComPorts::~FindComPorts(void)
115-{
116-}
117-
118-
119-void FindComPorts::clearBaseNames(void)
120-{
121- pimpl->base_names_.clear();
122-}
123-
124-
125-void FindComPorts::addBaseName(const char* base_name)
126-{
127- pimpl->addBaseName(base_name);
128-}
129-
130-
131-vector<string> FindComPorts::baseNames(void)
132-{
133- return pimpl->base_names_;
134-}
135-
136-
137-void FindComPorts::addDriverName(const char* driver_name)
138-{
139- pimpl->addDriverName(driver_name);
140-}
141-
142-
143-#ifdef WINDOWS_OS
144-// Windows の場合
145-size_t FindComPorts::find(std::vector<std::string>& ports,
146- std::vector<std::string>& driver_names,
147- bool all_ports)
148-{
149- // デバイスマネージャの一覧から COM デバイスを探す
150- deque<string> found_ports;
151- deque<string> found_drivers;
152-
153- //4D36E978-E325-11CE-BFC1-08002BE10318
154- GUID GUID_DEVINTERFACE_COM_DEVICE =
155- {0x4D36E978L, 0xE325, 0x11CE,
156- {0xBF, 0xC1, 0x08, 0x00, 0x2B, 0xE1, 0x03, 0x18 }
157- };
158-
159- HDEVINFO hdi = SetupDiGetClassDevs(&GUID_DEVINTERFACE_COM_DEVICE, 0, 0,
160- DIGCF_PRESENT | DIGCF_INTERFACEDEVICE);
161- if (hdi == INVALID_HANDLE_VALUE) {
162- return 0;
163- }
164-
165- SP_DEVINFO_DATA sDevInfo;
166- sDevInfo.cbSize = sizeof(SP_DEVINFO_DATA);
167- for (DWORD i = 0; SetupDiEnumDeviceInfo(hdi, i, &sDevInfo); ++i){
168-
169- enum { BufferSize = 256 };
170- char buffer[BufferSize + 1];
171- DWORD dwRegType;
172- DWORD dwSize;
173-
174- // フレンドリーネームを取得して COM 番号を取り出す
175- SetupDiGetDeviceRegistryPropertyA(hdi, &sDevInfo, SPDRP_FRIENDLYNAME,
176- &dwRegType, (BYTE*)buffer, BufferSize,
177- &dwSize);
178- enum { ComNameLengthMax = 7 };
179- size_t n = strlen(buffer);
180- if (n < ComNameLengthMax) {
181- // COM 名が短過ぎた場合、処理しない
182- // 問題がある場合は、修正する
183- continue;
184- }
185-
186- // (COMx) の最後の括弧を探す
187- for (int j = n - 1; j > 0; --j) {
188- if (buffer[j] == ')') {
189- buffer[j] = '\0';
190- break;
191- }
192- }
193- char* p = strstr(&buffer[n - ComNameLengthMax], "COM");
194- if (! p) {
195- continue;
196- }
197- found_ports.push_back(p);
198-
199- // デバイス名を取得し、IsUsbCom の条件に一致したら、先頭に配置する
200- SetupDiGetDeviceRegistryPropertyA(hdi, &sDevInfo, SPDRP_DEVICEDESC,
201- &dwRegType, (BYTE*)buffer, BufferSize,
202- &dwSize);
203- found_drivers.push_back(buffer);
204- pimpl->orderByDriver(found_ports, found_drivers, buffer, all_ports);
205- }
206- SetupDiDestroyDeviceInfoList(hdi);
207-
208- ports.insert(ports.end(), found_ports.begin(), found_ports.end());
209- driver_names.insert(driver_names.end(),
210- found_drivers.begin(), found_drivers.end());
211-
212- return ports.size();
213-}
214-
215-
216-#else
217-
218-namespace
219-{
220- void searchFiles(vector<string>& ports, const string& dir_name)
221- {
222- DIR* dp = opendir(dir_name.c_str());
223- if (! dp) {
224- return;
225- }
226-
227- struct dirent* dir;
228- while ((dir = readdir(dp))) {
229- struct stat state;
230- stat(dir->d_name, &state);
231- if (S_ISDIR(state.st_mode)) {
232- string file = dir_name + dir->d_name;
233- ports.push_back(file);
234- }
235- }
236- }
237-}
238-
239-
240-// Linux, Mac の場合
241-size_t FindComPorts::find(std::vector<std::string>& ports,
242- std::vector<std::string>& driver_names,
243- bool all_ports)
244-{
245- static_cast<void>(all_ports);
246- static_cast<void>(driver_names);
247-
248- vector<string> found_ports;
249-
250- // 登録ベース名毎に、ファイル名がないかの探索を行う
251- for (vector<string>::iterator it = pimpl->base_names_.begin();
252- it != pimpl->base_names_.end(); ++it) {
253-
254- // ディレクトリ名とファイル名との切り分け
255- const char* path = it->c_str();
256- const char* last_slash = strrchr(path, '/') + 1;
257-
258- string dir_name = it->substr(0, last_slash - path);
259- vector<string> ports;
260- searchFiles(ports, dir_name);
261-
262- size_t n = it->size();
263- for (vector<string>::iterator fit = ports.begin();
264- fit != ports.end(); ++fit) {
265- if (! fit->compare(0, n, *it)) {
266- // マッチしたら、登録を行う
267- pimpl->addFoundPorts(found_ports, *fit);
268- }
269- }
270- }
271-
272- for (vector<string>::iterator it = found_ports.begin();
273- it != found_ports.end(); ++it) {
274- ports.push_back(*it);
275- driver_names.push_back("");
276- }
277-
278- return found_ports.size();
279-}
280-#endif
281-
282-
283-size_t FindComPorts::find(vector<string>& ports, bool all_ports)
284-{
285- vector<string> driver_names_dummy;
286- return find(ports, driver_names_dummy, all_ports);
287-}
1+/*!
2+ \file
3+ \brief COM ポート一覧の取得
4+
5+ \author Satofumi KAMIMURA
6+
7+ $Id$
8+*/
9+
10+#include "FindComPorts.h"
11+#include "DetectOS.h"
12+#ifdef WINDOWS_OS
13+#include <windows.h>
14+#include <setupapi.h>
15+#include <deque>
16+#include <algorithm>
17+
18+#if defined(MSC)
19+#pragma comment(lib, "setupapi.lib")
20+#endif
21+
22+#else
23+#include <deque>
24+#include <algorithm>
25+#include <cstring>
26+#include <dirent.h>
27+#include <sys/stat.h>
28+#endif
29+
30+using namespace qrk;
31+using namespace std;
32+
33+
34+struct FindComPorts::pImpl
35+{
36+ vector<string> base_names_;
37+ vector<string> driver_names_;
38+
39+
40+ pImpl(void)
41+ {
42+ // Windows はレジストリ情報だけから COM 一覧の探索を行う
43+ // よって、以降の設定は Linux, MacOS のみで使われる
44+#if defined(LINUX_OS)
45+ base_names_.push_back("/dev/ttyUSB");
46+ base_names_.push_back("/dev/usb/ttyUSB");
47+#elif defined(MAC_OS)
48+ base_names_.push_back("/dev/tty.usbmodem");
49+#endif
50+ }
51+
52+
53+ void addBaseName(const char* base_name)
54+ {
55+ base_names_.insert(base_names_.begin(), base_name);
56+ }
57+
58+
59+ void addDriverName(const char* driver_name)
60+ {
61+ driver_names_.push_back(driver_name);
62+ }
63+
64+
65+ void orderByDriver(deque<string>& ports, deque<string>& drivers,
66+ const char* driver_name,
67+ bool all_ports)
68+ {
69+ if (ports.empty()) {
70+ return;
71+ }
72+
73+ vector<string>::const_iterator it =
74+ std::find(driver_names_.begin(), driver_names_.end(), driver_name);
75+ if (it != driver_names_.end()) {
76+ ports.push_front(ports.back());
77+ ports.pop_back();
78+
79+ drivers.push_front(drivers.back());
80+ drivers.pop_back();
81+
82+ } else {
83+ if (! all_ports) {
84+ ports.pop_back();
85+ drivers.pop_back();
86+ }
87+ }
88+ }
89+
90+
91+ void addFoundPorts(vector<string>& found_ports,
92+ const string& port)
93+ {
94+ for (vector<string>::iterator it = found_ports.begin();
95+ it != found_ports.end(); ++it) {
96+
97+ // !!! データ構造を map にすべきかも
98+ // !!! 検索のアルゴリズムを用いるべき
99+
100+ if (! port.compare(*it)) {
101+ return;
102+ }
103+ }
104+ found_ports.push_back(port);
105+ }
106+};
107+
108+
109+FindComPorts::FindComPorts(void) : pimpl(new pImpl)
110+{
111+}
112+
113+
114+FindComPorts::~FindComPorts(void)
115+{
116+}
117+
118+
119+void FindComPorts::clearBaseNames(void)
120+{
121+ pimpl->base_names_.clear();
122+}
123+
124+
125+void FindComPorts::addBaseName(const char* base_name)
126+{
127+ pimpl->addBaseName(base_name);
128+}
129+
130+
131+vector<string> FindComPorts::baseNames(void)
132+{
133+ return pimpl->base_names_;
134+}
135+
136+
137+void FindComPorts::addDriverName(const char* driver_name)
138+{
139+ pimpl->addDriverName(driver_name);
140+}
141+
142+
143+#ifdef WINDOWS_OS
144+// Windows の場合
145+size_t FindComPorts::find(std::vector<std::string>& ports,
146+ std::vector<std::string>& driver_names,
147+ bool all_ports)
148+{
149+ // デバイスマネージャの一覧から COM デバイスを探す
150+ deque<string> found_ports;
151+ deque<string> found_drivers;
152+
153+ //4D36E978-E325-11CE-BFC1-08002BE10318
154+ GUID GUID_DEVINTERFACE_COM_DEVICE =
155+ {0x4D36E978L, 0xE325, 0x11CE,
156+ {0xBF, 0xC1, 0x08, 0x00, 0x2B, 0xE1, 0x03, 0x18 }
157+ };
158+
159+ HDEVINFO hdi = SetupDiGetClassDevs(&GUID_DEVINTERFACE_COM_DEVICE, 0, 0,
160+ DIGCF_PRESENT | DIGCF_INTERFACEDEVICE);
161+ if (hdi == INVALID_HANDLE_VALUE) {
162+ return 0;
163+ }
164+
165+ SP_DEVINFO_DATA sDevInfo;
166+ sDevInfo.cbSize = sizeof(SP_DEVINFO_DATA);
167+ for (DWORD i = 0; SetupDiEnumDeviceInfo(hdi, i, &sDevInfo); ++i){
168+
169+ enum { BufferSize = 256 };
170+ char buffer[BufferSize + 1];
171+ DWORD dwRegType;
172+ DWORD dwSize;
173+
174+ // フレンドリーネームを取得して COM 番号を取り出す
175+ SetupDiGetDeviceRegistryPropertyA(hdi, &sDevInfo, SPDRP_FRIENDLYNAME,
176+ &dwRegType, (BYTE*)buffer, BufferSize,
177+ &dwSize);
178+ enum { ComNameLengthMax = 7 };
179+ size_t n = strlen(buffer);
180+ if (n < ComNameLengthMax) {
181+ // COM 名が短過ぎた場合、処理しない
182+ // 問題がある場合は、修正する
183+ continue;
184+ }
185+
186+ // (COMx) の最後の括弧を探す
187+ for (int j = n - 1; j > 0; --j) {
188+ if (buffer[j] == ')') {
189+ buffer[j] = '\0';
190+ break;
191+ }
192+ }
193+ char* p = strstr(&buffer[n - ComNameLengthMax], "COM");
194+ if (! p) {
195+ continue;
196+ }
197+ found_ports.push_back(p);
198+
199+ // デバイス名を取得し、IsUsbCom の条件に一致したら、先頭に配置する
200+ SetupDiGetDeviceRegistryPropertyA(hdi, &sDevInfo, SPDRP_DEVICEDESC,
201+ &dwRegType, (BYTE*)buffer, BufferSize,
202+ &dwSize);
203+ found_drivers.push_back(buffer);
204+ pimpl->orderByDriver(found_ports, found_drivers, buffer, all_ports);
205+ }
206+ SetupDiDestroyDeviceInfoList(hdi);
207+
208+ ports.insert(ports.end(), found_ports.begin(), found_ports.end());
209+ driver_names.insert(driver_names.end(),
210+ found_drivers.begin(), found_drivers.end());
211+
212+ return ports.size();
213+}
214+
215+
216+#else
217+
218+namespace
219+{
220+ void searchFiles(vector<string>& ports, const string& dir_name)
221+ {
222+ DIR* dp = opendir(dir_name.c_str());
223+ if (! dp) {
224+ return;
225+ }
226+
227+ struct dirent* dir;
228+ while ((dir = readdir(dp))) {
229+ struct stat state;
230+ stat(dir->d_name, &state);
231+ if (S_ISDIR(state.st_mode)) {
232+ string file = dir_name + dir->d_name;
233+ ports.push_back(file);
234+ }
235+ }
236+ }
237+}
238+
239+
240+// Linux, Mac の場合
241+size_t FindComPorts::find(std::vector<std::string>& ports,
242+ std::vector<std::string>& driver_names,
243+ bool all_ports)
244+{
245+ static_cast<void>(all_ports);
246+ static_cast<void>(driver_names);
247+
248+ vector<string> found_ports;
249+
250+ // 登録ベース名毎に、ファイル名がないかの探索を行う
251+ for (vector<string>::iterator it = pimpl->base_names_.begin();
252+ it != pimpl->base_names_.end(); ++it) {
253+
254+ // ディレクトリ名とファイル名との切り分け
255+ const char* path = it->c_str();
256+ const char* last_slash = strrchr(path, '/') + 1;
257+
258+ string dir_name = it->substr(0, last_slash - path);
259+ vector<string> ports;
260+ searchFiles(ports, dir_name);
261+
262+ size_t n = it->size();
263+ for (vector<string>::iterator fit = ports.begin();
264+ fit != ports.end(); ++fit) {
265+ if (! fit->compare(0, n, *it)) {
266+ // マッチしたら、登録を行う
267+ pimpl->addFoundPorts(found_ports, *fit);
268+ }
269+ }
270+ }
271+
272+ for (vector<string>::iterator it = found_ports.begin();
273+ it != found_ports.end(); ++it) {
274+ ports.push_back(*it);
275+ driver_names.push_back("");
276+ }
277+
278+ return found_ports.size();
279+}
280+#endif
281+
282+
283+size_t FindComPorts::find(vector<string>& ports, bool all_ports)
284+{
285+ vector<string> driver_names_dummy;
286+ return find(ports, driver_names_dummy, all_ports);
287+}
--- trunk/programs/UrgRecorder/UrgRecorderWidget.cpp (revision 1976)
+++ trunk/programs/UrgRecorder/UrgRecorderWidget.cpp (revision 1977)
@@ -1,397 +1,397 @@
1-/*!
2- \file
3- \brief URG データの記録ウィジット
4-
5- \author Satofumi KAMIMURA
6-
7- $Id$
8-*/
9-
10-#include "UrgRecorderWidget.h"
11-#include "SerialConnectionWidget.h"
12-#include "RecordConnection.h"
13-#include "UrgDevice.h"
14-#include "UrgUsbCom.h"
15-#include "FindComPorts.h"
16-#include "DetectOS.h"
17-#include <QTimer>
18-#include <QDateTime>
19-#include <QDir>
20-#include <QTextStream>
21-#include <QMessageBox>
22-#include <QShortcut>
23-#include <QSettings>
24-
25-using namespace qrk;
26-using namespace std;
27-
28-#if defined(MSC)
29-#define snprintf _snprintf
30-#endif
31-
32-
33-namespace
34-{
35- const char* Organization = "Hokuyo LTD.";
36- const char* Application = "URG Recorder";
37-
38- enum {
39- DefaultCaptureTimes = 10,
40- };
41-}
42-
43-
44-struct UrgRecorderWidget::pImpl
45-{
46- UrgRecorderWidget* widget_;
47- SerialConnectionWidget connection_widget_;
48-
49- UrgUsbCom urg_usb_;
50- FindComPorts urg_finder_;
51- UrgDevice urg_;
52- Connection* serial_connection_;
53-
54- QTimer capture_timer_;
55- QString save_directory_;
56- size_t capture_max_;
57- size_t total_times_;
58- bool is_raw_record_;
59-
60- bool intensity_mode_;
61-
62-
63- pImpl(UrgRecorderWidget* widget)
64- : widget_(widget), serial_connection_(NULL),
65- capture_max_(1), total_times_(0), is_raw_record_(false),
66- intensity_mode_(false)
67- {
68- urg_finder_.addBaseName("/dev/ttyACM");
69- urg_finder_.addBaseName("/dev/tty.usbmodem");
70- urg_finder_.addDriverName("URG Series USB Device Driver");
71- urg_finder_.addDriverName("URG-X002 USB Device Driver");
72- }
73-
74-
75- void initializeForm(void)
76- {
77- // 位置と大きさを読み出す
78- loadSettings();
79-
80- // ConnectionWidget の配置
81- widget_->connection_layout_->addWidget(&connection_widget_);
82- widget_->connection_dummy_label_->hide();
83-
84- // イベントの接続
85- connect(&connection_widget_, SIGNAL(rescanRequest()),
86- widget_, SLOT(rescanPressed()));
87- connect(&connection_widget_,
88- SIGNAL(connectRequest(bool, const std::string&)),
89- widget_, SLOT(connectPressed(bool, const std::string&)));
90-
91- connect(widget_->times_spinbox_, SIGNAL(valueChanged(int)),
92- widget_, SLOT(timesChanged(int)));
93-
94- connect(widget_->record_button_, SIGNAL(clicked()),
95- widget_, SLOT(recordPressed()));
96- connect(widget_->cancel_button_, SIGNAL(clicked()),
97- widget_, SLOT(cancelPressed()));
98-
99- connect(widget_->raw_button_, SIGNAL(toggled(bool)),
100- widget_, SLOT(rawButtonChanged(bool)));
101-
102- connect(&capture_timer_, SIGNAL(timeout()),
103- widget_, SLOT(recordData()));
104- }
105-
106-
107- void loadSettings(void)
108- {
109- QSettings settings(Organization, Application);
110- widget_->restoreGeometry(settings.value("geometry").toByteArray());
111- int capture_times =
112- settings.value("capture_times", DefaultCaptureTimes).toInt();
113- widget_->times_spinbox_->setValue(capture_times);
114- widget_->times_progress_->setMaximum(capture_times);
115-
116- is_raw_record_ = settings.value("raw_button_checked", false).toBool();
117- widget_->raw_button_->setChecked(is_raw_record_);
118- rawButtonChanged(is_raw_record_);
119- }
120-
121-
122- void saveSettings(void)
123- {
124- QSettings settings(Organization, Application);
125- settings.setValue("geometry", widget_->saveGeometry());
126- settings.setValue("capture_times", widget_->times_spinbox_->value());
127- settings.setValue("raw_button_checked",
128- widget_->raw_button_->isChecked());
129- }
130-
131-
132- void setRecording(bool recording)
133- {
134- widget_->record_button_->setEnabled(! recording);
135- widget_->cancel_button_->setEnabled(recording);
136- widget_->output_group_->setEnabled(! recording);
137- widget_->times_group_->setEnabled(! recording);
138- }
139-
140-
141- void recordPressed(void)
142- {
143- setRecording(true);
144-
145- // 出力先ディレクトリの作成
146- save_directory_ =
147- QDateTime::currentDateTime().toString("yyyy-MM-dd_hh_mm_ss");
148- QDir dir;
149- dir.mkdir(save_directory_);
150-
151- // 取得モードの設定
152- is_raw_record_ = widget_->raw_button_->isChecked();
153- if (is_raw_record_) {
154- is_raw_record_ = true;
155- serial_connection_ = urg_.connection();
156- string send_file = save_directory_.toStdString() + "/send.txt";
157- string receive_file =
158- save_directory_.toStdString() + "/receive.txt";
159- urg_.setConnection(new RecordConnection(serial_connection_,
160- send_file.c_str(),
161- receive_file.c_str()));
162-
163- // ログに URG の情報を含めるための呼び出し
164- urg_.loadParameter();
165- }
166-
167- // データ取得の開始
168- capture_max_ = widget_->times_spinbox_->value();
169- total_times_ = 0;
170- if (intensity_mode_) {
171- urg_.setCaptureMode(IntensityCapture);
172- } else {
173- urg_.setCaptureMode(AutoCapture);
174- }
175- capture_timer_.setInterval(urg_.scanMsec() / 2);
176- capture_timer_.start();
177- }
178-
179-
180- void recordData(void)
181- {
182- vector<long> data;
183- vector<long> intensity_data;
184-
185- int n;
186- if (intensity_mode_) {
187- n = urg_.captureWithIntensity(data, intensity_data);
188- } else {
189- n = urg_.capture(data);
190- }
191-
192- if (n <= 0) {
193- return;
194- }
195- ++total_times_;
196-
197- if (! is_raw_record_) {
198-
199- // ファイルへのデータ書き出し
200- char buffer[] = "/data_xxxxxxxxxx.csv";
201- snprintf(buffer, sizeof(buffer), "/data_%09d.csv", total_times_);
202- QFile file(save_directory_ + QString(buffer));
203- if (! file.open(QIODevice::WriteOnly)) {
204- // !!! エラー表示
205- return;
206- }
207-
208- QTextStream fout(&file);
209- saveFile(fout, data, intensity_data);
210- }
211-
212- widget_->times_progress_->setValue(total_times_);
213- if (total_times_ >= capture_max_) {
214- stopRecording();
215- }
216- }
217-
218-
219- void stopRecording(void)
220- {
221- setRecording(false);
222- capture_timer_.stop();
223- widget_->times_progress_->setValue(0);
224-
225- urg_.stop();
226-
227- if (is_raw_record_) {
228- Connection* connection = urg_.connection();
229- if (connection != serial_connection_) {
230- delete connection;
231- urg_.setConnection(serial_connection_);
232- }
233- }
234- }
235-
236-
237- void saveFile(QTextStream& fout, const vector<long>& data,
238- vector<long>& intensity_data)
239- {
240- size_t n = data.size();
241- for (size_t i = 0; i < n; ++i) {
242- long length = data[i];
243- double radian = urg_.index2rad(i);
244- double x = length * cos(radian);
245- double y = length * sin(radian);
246- fout << i << ',' << length << ','
247- << radian << ',' << x << ',' << y;
248-
249- if (intensity_mode_) {
250- fout << ',' << intensity_data[i];
251- }
252-
253- fout << endl;
254- }
255- }
256-
257-
258- void rawButtonChanged(bool checked)
259- {
260- if (checked) {
261- widget_->sample_text_->clear();
262- widget_->sample_text_->
263- insertPlainText("# receive data sample\n"
264- "MD0044072501000\n"
265- "99b\n"
266- "...");
267- } else {
268- widget_->sample_text_->clear();
269- widget_->sample_text_->
270- insertPlainText("# index, length, radian, x, y\n"
271- "0,669,-2.08621,-329.749,-582.088\n"
272- "1,667,-2.08008,-325.196,-582.354\n"
273- "...");
274- }
275- }
276-};
277-
278-
279-UrgRecorderWidget::UrgRecorderWidget(QWidget* parent)
280- : QWidget(parent), pimpl(new pImpl(this))
281-{
282- setupUi(this);
283-
284- // フォームを初期化し、最初の表示を行う
285- pimpl->initializeForm();
286- rescanPressed();
287-
288- pimpl->connection_widget_.setFocus();
289-
290- // Ctrl-q, Alt-F4 で終了させる
291- (void) new QShortcut(Qt::CTRL + Qt::Key_Q, this, SLOT(close()));
292- (void) new QShortcut(Qt::ALT + Qt::Key_F4, this, SLOT(close()));
293-}
294-
295-
296-UrgRecorderWidget::~UrgRecorderWidget(void)
297-{
298-}
299-
300-
301-void UrgRecorderWidget::setIntensityMode(void)
302-{
303- if (! pimpl->intensity_mode_) {
304- pimpl->intensity_mode_ = true;
305- if (pimpl->intensity_mode_) {
306- setWindowTitle(windowTitle() + " " + tr("[intensity]"));
307- }
308- }
309-}
310-
311-
312-void UrgRecorderWidget::closeEvent(QCloseEvent* event)
313-{
314- static_cast<void>(event);
315-
316- pimpl->urg_.stop();
317- pimpl->saveSettings();
318-}
319-
320-
321-void UrgRecorderWidget::rescanPressed(void)
322-{
323- vector<string> devices;
324- pimpl->urg_finder_.find(devices);
325- for (vector<string>::iterator it = devices.begin();
326- it != devices.end(); ++it) {
327- if (pimpl->urg_usb_.isUsbCom(it->c_str())) {
328- *it = *it + " [URG]";
329- }
330- }
331- pimpl->connection_widget_.setDevices(devices);
332-}
333-
334-
335-void UrgRecorderWidget::connectPressed(bool connection,
336- const string& device)
337-{
338- // !!! 接続処理をスレッドで行うように調整する
339- bool connected = connection;
340-
341- if (connection) {
342- if (! pimpl->urg_.connect(device.c_str())) {
343- QMessageBox::warning(this, tr("Connection error"),
344- pimpl->urg_.what());
345- connected = false;
346- }
347- } else {
348- pimpl->stopRecording();
349- }
350-
351- record_group_->setEnabled(connected);
352- cancel_button_->setEnabled(false);
353-
354- pimpl->connection_widget_.setConnected(connected);
355-
356- if (connected) {
357- // フォーカスを Record ボタンに移動させる
358- record_button_->setFocus();
359- }
360-}
361-
362-
363-void UrgRecorderWidget::recordPressed(void)
364-{
365- if (pimpl->capture_timer_.isActive()) {
366- return;
367- }
368- pimpl->recordPressed();
369-}
370-
371-
372-void UrgRecorderWidget::cancelPressed(void)
373-{
374- pimpl->stopRecording();
375-}
376-
377-
378-void UrgRecorderWidget::timesChanged(int times)
379-{
380- times_progress_->setMaximum(times);
381-
382- // 再描画が行われるように、2回ほど値をセットしている
383- times_progress_->setValue(1);
384- times_progress_->setValue(0);
385-}
386-
387-
388-void UrgRecorderWidget::recordData(void)
389-{
390- pimpl->recordData();
391-}
392-
393-
394-void UrgRecorderWidget::rawButtonChanged(bool checked)
395-{
396- pimpl->rawButtonChanged(checked);
397-}
1+/*!
2+ \file
3+ \brief URG データの記録ウィジット
4+
5+ \author Satofumi KAMIMURA
6+
7+ $Id$
8+*/
9+
10+#include "UrgRecorderWidget.h"
11+#include "SerialConnectionWidget.h"
12+#include "RecordConnection.h"
13+#include "UrgDevice.h"
14+#include "UrgUsbCom.h"
15+#include "FindComPorts.h"
16+#include "DetectOS.h"
17+#include <QTimer>
18+#include <QDateTime>
19+#include <QDir>
20+#include <QTextStream>
21+#include <QMessageBox>
22+#include <QShortcut>
23+#include <QSettings>
24+
25+using namespace qrk;
26+using namespace std;
27+
28+#if defined(MSC)
29+#define snprintf _snprintf
30+#endif
31+
32+
33+namespace
34+{
35+ const char* Organization = "Hokuyo LTD.";
36+ const char* Application = "URG Recorder";
37+
38+ enum {
39+ DefaultCaptureTimes = 10,
40+ };
41+}
42+
43+
44+struct UrgRecorderWidget::pImpl
45+{
46+ UrgRecorderWidget* widget_;
47+ SerialConnectionWidget connection_widget_;
48+
49+ UrgUsbCom urg_usb_;
50+ FindComPorts urg_finder_;
51+ UrgDevice urg_;
52+ Connection* serial_connection_;
53+
54+ QTimer capture_timer_;
55+ QString save_directory_;
56+ size_t capture_max_;
57+ size_t total_times_;
58+ bool is_raw_record_;
59+
60+ bool intensity_mode_;
61+
62+
63+ pImpl(UrgRecorderWidget* widget)
64+ : widget_(widget), serial_connection_(NULL),
65+ capture_max_(1), total_times_(0), is_raw_record_(false),
66+ intensity_mode_(false)
67+ {
68+ urg_finder_.addBaseName("/dev/ttyACM");
69+ urg_finder_.addBaseName("/dev/tty.usbmodem");
70+ urg_finder_.addDriverName("URG Series USB Device Driver");
71+ urg_finder_.addDriverName("URG-X002 USB Device Driver");
72+ }
73+
74+
75+ void initializeForm(void)
76+ {
77+ // 位置と大きさを読み出す
78+ loadSettings();
79+
80+ // ConnectionWidget の配置
81+ widget_->connection_layout_->addWidget(&connection_widget_);
82+ widget_->connection_dummy_label_->hide();
83+
84+ // イベントの接続
85+ connect(&connection_widget_, SIGNAL(rescanRequest()),
86+ widget_, SLOT(rescanPressed()));
87+ connect(&connection_widget_,
88+ SIGNAL(connectRequest(bool, const std::string&)),
89+ widget_, SLOT(connectPressed(bool, const std::string&)));
90+
91+ connect(widget_->times_spinbox_, SIGNAL(valueChanged(int)),
92+ widget_, SLOT(timesChanged(int)));
93+
94+ connect(widget_->record_button_, SIGNAL(clicked()),
95+ widget_, SLOT(recordPressed()));
96+ connect(widget_->cancel_button_, SIGNAL(clicked()),
97+ widget_, SLOT(cancelPressed()));
98+
99+ connect(widget_->raw_button_, SIGNAL(toggled(bool)),
100+ widget_, SLOT(rawButtonChanged(bool)));
101+
102+ connect(&capture_timer_, SIGNAL(timeout()),
103+ widget_, SLOT(recordData()));
104+ }
105+
106+
107+ void loadSettings(void)
108+ {
109+ QSettings settings(Organization, Application);
110+ widget_->restoreGeometry(settings.value("geometry").toByteArray());
111+ int capture_times =
112+ settings.value("capture_times", DefaultCaptureTimes).toInt();
113+ widget_->times_spinbox_->setValue(capture_times);
114+ widget_->times_progress_->setMaximum(capture_times);
115+
116+ is_raw_record_ = settings.value("raw_button_checked", false).toBool();
117+ widget_->raw_button_->setChecked(is_raw_record_);
118+ rawButtonChanged(is_raw_record_);
119+ }
120+
121+
122+ void saveSettings(void)
123+ {
124+ QSettings settings(Organization, Application);
125+ settings.setValue("geometry", widget_->saveGeometry());
126+ settings.setValue("capture_times", widget_->times_spinbox_->value());
127+ settings.setValue("raw_button_checked",
128+ widget_->raw_button_->isChecked());
129+ }
130+
131+
132+ void setRecording(bool recording)
133+ {
134+ widget_->record_button_->setEnabled(! recording);
135+ widget_->cancel_button_->setEnabled(recording);
136+ widget_->output_group_->setEnabled(! recording);
137+ widget_->times_group_->setEnabled(! recording);
138+ }
139+
140+
141+ void recordPressed(void)
142+ {
143+ setRecording(true);
144+
145+ // 出力先ディレクトリの作成
146+ save_directory_ =
147+ QDateTime::currentDateTime().toString("yyyy-MM-dd_hh_mm_ss");
148+ QDir dir;
149+ dir.mkdir(save_directory_);
150+
151+ // 取得モードの設定
152+ is_raw_record_ = widget_->raw_button_->isChecked();
153+ if (is_raw_record_) {
154+ is_raw_record_ = true;
155+ serial_connection_ = urg_.connection();
156+ string send_file = save_directory_.toStdString() + "/send.txt";
157+ string receive_file =
158+ save_directory_.toStdString() + "/receive.txt";
159+ urg_.setConnection(new RecordConnection(serial_connection_,
160+ send_file.c_str(),
161+ receive_file.c_str()));
162+
163+ // ログに URG の情報を含めるための呼び出し
164+ urg_.loadParameter();
165+ }
166+
167+ // データ取得の開始
168+ capture_max_ = widget_->times_spinbox_->value();
169+ total_times_ = 0;
170+ if (intensity_mode_) {
171+ urg_.setCaptureMode(IntensityCapture);
172+ } else {
173+ urg_.setCaptureMode(AutoCapture);
174+ }
175+ capture_timer_.setInterval(urg_.scanMsec() / 2);
176+ capture_timer_.start();
177+ }
178+
179+
180+ void recordData(void)
181+ {
182+ vector<long> data;
183+ vector<long> intensity_data;
184+
185+ int n;
186+ if (intensity_mode_) {
187+ n = urg_.captureWithIntensity(data, intensity_data);
188+ } else {
189+ n = urg_.capture(data);
190+ }
191+
192+ if (n <= 0) {
193+ return;
194+ }
195+ ++total_times_;
196+
197+ if (! is_raw_record_) {
198+
199+ // ファイルへのデータ書き出し
200+ char buffer[] = "/data_xxxxxxxxxx.csv";
201+ snprintf(buffer, sizeof(buffer), "/data_%09d.csv", total_times_);
202+ QFile file(save_directory_ + QString(buffer));
203+ if (! file.open(QIODevice::WriteOnly)) {
204+ // !!! エラー表示
205+ return;
206+ }
207+
208+ QTextStream fout(&file);
209+ saveFile(fout, data, intensity_data);
210+ }
211+
212+ widget_->times_progress_->setValue(total_times_);
213+ if (total_times_ >= capture_max_) {
214+ stopRecording();
215+ }
216+ }
217+
218+
219+ void stopRecording(void)
220+ {
221+ setRecording(false);
222+ capture_timer_.stop();
223+ widget_->times_progress_->setValue(0);
224+
225+ urg_.stop();
226+
227+ if (is_raw_record_) {
228+ Connection* connection = urg_.connection();
229+ if (connection != serial_connection_) {
230+ delete connection;
231+ urg_.setConnection(serial_connection_);
232+ }
233+ }
234+ }
235+
236+
237+ void saveFile(QTextStream& fout, const vector<long>& data,
238+ vector<long>& intensity_data)
239+ {
240+ size_t n = data.size();
241+ for (size_t i = 0; i < n; ++i) {
242+ long length = data[i];
243+ double radian = urg_.index2rad(i);
244+ double x = length * cos(radian);
245+ double y = length * sin(radian);
246+ fout << i << ',' << length << ','
247+ << radian << ',' << x << ',' << y;
248+
249+ if (intensity_mode_) {
250+ fout << ',' << intensity_data[i];
251+ }
252+
253+ fout << endl;
254+ }
255+ }
256+
257+
258+ void rawButtonChanged(bool checked)
259+ {
260+ if (checked) {
261+ widget_->sample_text_->clear();
262+ widget_->sample_text_->
263+ insertPlainText("# receive data sample\n"
264+ "MD0044072501000\n"
265+ "99b\n"
266+ "...");
267+ } else {
268+ widget_->sample_text_->clear();
269+ widget_->sample_text_->
270+ insertPlainText("# index, length, radian, x, y\n"
271+ "0,669,-2.08621,-329.749,-582.088\n"
272+ "1,667,-2.08008,-325.196,-582.354\n"
273+ "...");
274+ }
275+ }
276+};
277+
278+
279+UrgRecorderWidget::UrgRecorderWidget(QWidget* parent)
280+ : QWidget(parent), pimpl(new pImpl(this))
281+{
282+ setupUi(this);
283+
284+ // フォームを初期化し、最初の表示を行う
285+ pimpl->initializeForm();
286+ rescanPressed();
287+
288+ pimpl->connection_widget_.setFocus();
289+
290+ // Ctrl-q, Alt-F4 で終了させる
291+ (void) new QShortcut(Qt::CTRL + Qt::Key_Q, this, SLOT(close()));
292+ (void) new QShortcut(Qt::ALT + Qt::Key_F4, this, SLOT(close()));
293+}
294+
295+
296+UrgRecorderWidget::~UrgRecorderWidget(void)
297+{
298+}
299+
300+
301+void UrgRecorderWidget::setIntensityMode(void)
302+{
303+ if (! pimpl->intensity_mode_) {
304+ pimpl->intensity_mode_ = true;
305+ if (pimpl->intensity_mode_) {
306+ setWindowTitle(windowTitle() + " " + tr("[intensity]"));
307+ }
308+ }
309+}
310+
311+
312+void UrgRecorderWidget::closeEvent(QCloseEvent* event)
313+{
314+ static_cast<void>(event);
315+
316+ pimpl->urg_.stop();
317+ pimpl->saveSettings();
318+}
319+
320+
321+void UrgRecorderWidget::rescanPressed(void)
322+{
323+ vector<string> devices;
324+ pimpl->urg_finder_.find(devices);
325+ for (vector<string>::iterator it = devices.begin();
326+ it != devices.end(); ++it) {
327+ if (pimpl->urg_usb_.isUsbCom(it->c_str())) {
328+ *it = *it + " [URG]";
329+ }
330+ }
331+ pimpl->connection_widget_.setDevices(devices);
332+}
333+
334+
335+void UrgRecorderWidget::connectPressed(bool connection,
336+ const string& device)
337+{
338+ // !!! 接続処理をスレッドで行うように調整する
339+ bool connected = connection;
340+
341+ if (connection) {
342+ if (! pimpl->urg_.connect(device.c_str())) {
343+ QMessageBox::warning(this, tr("Connection error"),
344+ pimpl->urg_.what());
345+ connected = false;
346+ }
347+ } else {
348+ pimpl->stopRecording();
349+ }
350+
351+ record_group_->setEnabled(connected);
352+ cancel_button_->setEnabled(false);
353+
354+ pimpl->connection_widget_.setConnected(connected);
355+
356+ if (connected) {
357+ // フォーカスを Record ボタンに移動させる
358+ record_button_->setFocus();
359+ }
360+}
361+
362+
363+void UrgRecorderWidget::recordPressed(void)
364+{
365+ if (pimpl->capture_timer_.isActive()) {
366+ return;
367+ }
368+ pimpl->recordPressed();
369+}
370+
371+
372+void UrgRecorderWidget::cancelPressed(void)
373+{
374+ pimpl->stopRecording();
375+}
376+
377+
378+void UrgRecorderWidget::timesChanged(int times)
379+{
380+ times_progress_->setMaximum(times);
381+
382+ // 再描画が行われるように、2回ほど値をセットしている
383+ times_progress_->setValue(1);
384+ times_progress_->setValue(0);
385+}
386+
387+
388+void UrgRecorderWidget::recordData(void)
389+{
390+ pimpl->recordData();
391+}
392+
393+
394+void UrgRecorderWidget::rawButtonChanged(bool checked)
395+{
396+ pimpl->rawButtonChanged(checked);
397+}
旧リポジトリブラウザで表示