• R/O
  • SSH
  • HTTPS

qrobosdk: コミット


コミットメタ情報

リビジョン1970 (tree)
日時2011-11-09 15:53:29
作者satofumi

ログメッセージ

added Moving3D program

変更サマリ

差分

--- trunk/programs/Moving3D/Moving3DWindow.cpp (nonexistent)
+++ trunk/programs/Moving3D/Moving3DWindow.cpp (revision 1970)
@@ -0,0 +1,984 @@
1+/*!
2+ \file
3+ \brief メインフォーム管理
4+
5+ \author Satofumi KAMIMURA
6+
7+ $Id$
8+
9+ \todo 全体データを VRML 形式で保存する
10+ \todo スキャン範囲を config のプレビュー画面にアニメーション表示する
11+ \todo 描画のフェード時間を設定可能にする
12+ \todo 描画時のデータ操作が排他になるようにロックする
13+ \todo 判定エリアが未定義のときに、判定領域を描画しないようにする
14+*/
15+
16+#include "Moving3DWindow.h"
17+#include "Draw3DWidget.h"
18+#include "findUrgPorts.h"
19+//#include "FindComPorts.h"
20+#include "UrgCtrl.h"
21+//#include "UrgUsbCom.h"
22+#include "UrgLogHandler.h"
23+#include "convert2d.h"
24+//#include "AngleUtils.h"
25+#include "LineUtils.h"
26+#include "StopWatch.h"
27+#include "ticks.h"
28+#include "ConvertStdStringPath.h"
29+#include "split.h"
30+#include <QFileDialog>
31+#include <QGraphicsLineItem>
32+#include <QDateTime>
33+#include <QTimer>
34+#include <QShortcut>
35+#include <QMessageBox>
36+#include <QSettings>
37+#include <fstream>
38+#include <cerrno>
39+
40+using namespace qrk;
41+using namespace std;
42+
43+
44+namespace
45+{
46+ typedef enum {
47+ Recording, //!< 記録中
48+ Playing, //!< 再生中
49+ Pausing, //!< 再生のポーズ中
50+ Waiting, //!< 操作待ち
51+ } PlayState;
52+
53+ typedef vector<Point<long> > Points;
54+}
55+
56+
57+struct Moving3DWindow::pImpl
58+{
59+ enum {
60+ FirstSliderSecond = 10,
61+ DefaultSliderMaximum = 500,
62+ RedrawMsec = 50,
63+ PlayTimerUpdateMsec = 10,
64+ };
65+
66+ const static bool Start = true;
67+ const static bool Stop = false;
68+
69+ const static bool NoWarning = false;
70+ const static bool ShowWarning = true;
71+
72+ const static double MinRatio = 0.11;
73+ const static double MaxRatio = 6.19;
74+
75+
76+ Draw3DWidget draw_widget_;
77+
78+ UrgCtrl urg_;
79+ size_t data_max_;
80+ vector<long> capture_data_;
81+ size_t scan_msec_;
82+
83+ UrgLogHandler urg_logger_;
84+ UrgLogHandler obstacle_logger_;
85+ size_t first_timestamp_;
86+ size_t first_ticks_;
87+ QTimer capture_timer_;
88+ QTimer redraw_timer_;
89+
90+ size_t record_max_; // 初期スライダーの表示幅 [sec]
91+
92+ Points area_points_;
93+ QGraphicsScene scene_;
94+ vector<QGraphicsItem*> area_items_;
95+
96+ size_t last_timestamp_;
97+ Point3d<int> last_draw_position_;
98+
99+ StopWatch play_timer_; // 再生時間のタイマー
100+ QTimer playing_update_timer_; // 再生情報の更新を行うためのタイマー
101+ int play_period_max_;
102+
103+ double magnify_;
104+
105+ vector<long> detect_array_;
106+
107+ pImpl(void)
108+ : first_timestamp_(0), first_ticks_(0),
109+ record_max_(FirstSliderSecond), last_timestamp_(0), play_period_max_(0),
110+ magnify_(1.0)
111+ {
112+ }
113+
114+
115+ void initializeForm(Moving3DWindow* parent)
116+ {
117+ // 描画ウィンドウまわり
118+ parent->dummy_main_label_->hide();
119+ parent->main_layout_->addWidget(&draw_widget_);
120+
121+ // 接続ポートまわり
122+ parent->com_combobox_->setSizeAdjustPolicy(QComboBox::AdjustToContents);
123+ setRecordEnabled(parent, true);
124+ createComCombobox(parent);
125+
126+ // 閲覧まわり
127+ connect(parent->front_button_, SIGNAL(clicked()),
128+ parent, SLOT(front()));
129+ connect(parent->overview_button_, SIGNAL(clicked()),
130+ parent, SLOT(overview()));
131+
132+ // 記録まわり
133+ parent->progress_slider_->setMaximum(DefaultSliderMaximum);
134+ connect(parent->record_button_, SIGNAL(clicked()), parent, SLOT(record()));
135+ connect(parent->stop_button_, SIGNAL(clicked()), parent, SLOT(stop()));
136+ connect(parent->action_rescan_, SIGNAL(triggered()),
137+ parent, SLOT(rescanPort()));
138+ connect(&capture_timer_, SIGNAL(timeout()),
139+ parent, SLOT(recordCaptureData()));
140+
141+ // 描画まわり
142+ connect(&redraw_timer_, SIGNAL(timeout()), parent, SLOT(redraw()));
143+ connect(parent->afterimage_spinbox_, SIGNAL(valueChanged(double)),
144+ parent, SLOT(updateDrawPeriod(double)));
145+
146+ // 判定エリアまわり
147+ parent->area_graphics_->setScene(&scene_);
148+ connect(parent->rescan_button_, SIGNAL(clicked()),
149+ parent, SLOT(rescanArea()));
150+
151+ // 再生まわり
152+ connect(parent->play_button_, SIGNAL(clicked()),
153+ parent, SLOT(play()));
154+ connect(parent->pause_button_, SIGNAL(clicked()),
155+ parent, SLOT(pause()));
156+ connect(parent->play_stop_button_, SIGNAL(clicked()),
157+ parent, SLOT(play_stop()));
158+ connect(&playing_update_timer_, SIGNAL(timeout()),
159+ parent, SLOT(readPlayData()));
160+ connect(parent->play_file_path_button_, SIGNAL(clicked()),
161+ parent, SLOT(playFileLoad()));
162+
163+ // 障害物のバーまわり
164+ parent->dummy_bar_label_->hide();
165+
166+ // ズームまわり
167+ connect(parent->zoom_slider_, SIGNAL(valueChanged(int)),
168+ parent, SLOT(changeZoom(int)));
169+
170+ // 各種キーバインド
171+ (void) new QShortcut(Qt::Key_Space, parent, SLOT(pauseAndPlay()));
172+ (void) new QShortcut(Qt::Key_Greater, parent, SLOT(zoomIn()));
173+ (void) new QShortcut(Qt::Key_Period, parent, SLOT(zoomIn()));
174+ (void) new QShortcut(Qt::Key_Less, parent, SLOT(zoomOut()));
175+ (void) new QShortcut(Qt::Key_Comma, parent, SLOT(zoomOut()));
176+ (void) new QShortcut(Qt::Key_Up, parent, SLOT(viewUp()));
177+ (void) new QShortcut(Qt::Key_Down, parent, SLOT(viewDown()));
178+ (void) new QShortcut(Qt::Key_Left, parent, SLOT(viewLeft()));
179+ (void) new QShortcut(Qt::Key_Right, parent, SLOT(viewRight()));
180+
181+ // Ctrl-Q, Alt-F4, ESC で終了
182+ (void) new QShortcut(Qt::CTRL + Qt::Key_Q, parent, SLOT(close()));
183+ (void) new QShortcut(Qt::ALT + Qt::Key_F4, parent, SLOT(close()));
184+ (void) new QShortcut(Qt::Key_Escape, parent, SLOT(close()));
185+
186+ // アクションの初期化
187+ connect(parent->action_about_, SIGNAL(triggered()),
188+ parent, SLOT(aboutApplication()));
189+
190+ // フォーカスの初期化
191+ parent->record_button_->setFocus();
192+ }
193+
194+
195+ void updateInformation(Moving3DWindow* parent)
196+ {
197+ // 描画エリアの更新
198+ rescanArea(parent, NoWarning);
199+
200+ // !!! 記録時の判定領域を、ファイルに書き出しておいて利用すべき
201+ // !!! あと、以下の実装は適切に関数化すべき
202+ double h = getSensorHeight(parent);
203+ draw_widget_.setDrawLineData(area_points_, static_cast<int>(h));
204+
205+ // センサ位置設定の取得
206+ // !!!
207+
208+ // 描画開始
209+ redraw_timer_.start(RedrawMsec);
210+ }
211+
212+
213+ bool rescanArea(Moving3DWindow* parent, bool show_warning = true)
214+ {
215+ // 判定エリアの描画
216+ scene_.clear();
217+ area_items_.clear();
218+
219+ // 地面の描画
220+ QPen pen;
221+ pen.setStyle(Qt::DotLine);
222+ QGraphicsLineItem* ground_x = new QGraphicsLineItem(-200, 0, +200, 0);
223+ ground_x->setPen(pen);
224+ scene_.addItem(ground_x);
225+ QGraphicsLineItem* ground_y = new QGraphicsLineItem(0, -200, 0, +20);
226+ ground_y->setPen(pen);
227+ scene_.addItem(ground_y);
228+
229+ // センサ位置の描画
230+ const double ratio = 0.08;
231+ double h_mm = getSensorHeight(parent);
232+ QGraphicsRectItem* sensor_item =
233+ new QGraphicsRectItem(-25.0 * ratio, (-h_mm +25.0) * ratio,
234+ 50.0 * ratio, 50.0 * ratio);
235+ pen.setStyle(Qt::SolidLine);
236+ pen.setWidth(2);
237+ pen.setColor(QColor("darkorange"));
238+ sensor_item->setPen(pen);
239+ scene_.addItem(sensor_item);
240+
241+ // 判定エリア情報の取得
242+ QString file_name = parent->detect_area_edit_->text();
243+ string std_path = qrk::toStdStringPath(file_name);
244+
245+ ifstream fin(std_path.c_str());
246+ if (! fin.is_open()) {
247+ if (show_warning) {
248+ QMessageBox::warning(parent, "Area",
249+ strerror(errno), QMessageBox::Close);
250+ }
251+ return false;
252+ }
253+
254+ area_points_.clear();
255+ string line;
256+ while (getline(fin, line)) {
257+ vector<string> tokens;
258+ if (qrk::split(tokens, line, " \t,") == 2) {
259+ int x = static_cast<int>(atof(tokens[0].c_str()) * 1000);
260+ int y = static_cast<int>(atof(tokens[1].c_str()) * 1000);
261+ area_points_.push_back(Point<long>(x, y));
262+ }
263+ }
264+ if (area_points_.size() <= 2) {
265+ QMessageBox::warning(parent, tr("Area"),
266+ tr("required more than 3 points."),
267+ QMessageBox::Close);
268+ return false;
269+ }
270+
271+ // 判定領域の描画
272+ Points::iterator first = area_points_.begin();
273+ for (Points::iterator second = first + 1;
274+ second != area_points_.end(); ++second, ++first) {
275+
276+ QGraphicsLineItem* item =
277+ new QGraphicsLineItem(first->x * ratio, -first->y * ratio,
278+ second->x * ratio, -second->y * ratio);
279+ area_items_.push_back(item);
280+ }
281+ for (vector<QGraphicsItem*>::iterator it = area_items_.begin();
282+ it != area_items_.end(); ++it) {
283+ scene_.addItem(*it);
284+ }
285+
286+ return true;
287+ }
288+
289+
290+ void saveSettings(Moving3DWindow* parent)
291+ {
292+ QSettings settings("Hokuyo LTD.", "Moving3D");
293+
294+ // ウィンドウの位置と場所
295+ QRect position = settings.value("geometry", parent->geometry()).toRect();
296+ parent->setGeometry(position);
297+
298+ // フォーム設定
299+ settings.setValue("tab_index", parent->main_tab_->currentIndex());
300+ settings.setValue("last_logfile", parent->play_file_edit_->text());
301+ settings.setValue("area_file", parent->detect_area_edit_->text());
302+
303+ // 視点、拡大率
304+ Point3d<int> rotation = draw_widget_.viewRotation();
305+ settings.setValue("rotation_x", rotation.x);
306+ settings.setValue("rotation_y", rotation.y);
307+ settings.setValue("rotation_z", rotation.z);
308+ double zoom_ratio = draw_widget_.zoomRatio();
309+ settings.setValue("zoom_ratio", zoom_ratio);
310+
311+ // センサ高さ
312+ settings.setValue("sensor_height",
313+ parent->sensor_height_spinbox_->value());
314+ settings.setValue("ground_offset",
315+ parent->ground_offset_spinbox_->value());
316+
317+ // 移動速度
318+ settings.setValue("move_velocity",
319+ parent->m_sec_spinbox_->value());
320+ settings.setValue("afterimage_sec",
321+ parent->afterimage_spinbox_->value());
322+ }
323+
324+
325+ void loadSettings(Moving3DWindow* parent)
326+ {
327+ QSettings settings("Hokuyo LTD.", "Moving3D");
328+
329+ // ウィンドウの位置と場所
330+ QRect position = parent->geometry();
331+ QSize size = parent->size();
332+ settings.setValue("geometry", QRect(position.x(), position.y(),
333+ size.width(), size.height()));
334+
335+ // フォーム設定
336+ parent->main_tab_->setCurrentIndex(settings.value("tab_index", 0).toInt());
337+ parent->play_file_edit_->
338+ setText(settings.value("last_logfile", "").toString());
339+ // !!! ファイルの存在確認を行うべき
340+
341+ parent->detect_area_edit_->
342+ setText(settings.value("area_file", "detect_area.csv").toString());
343+
344+ // 視点、拡大率
345+ Point3d<int> rotation;
346+ rotation.x = settings.value("rotation_x", 0).toInt();
347+ rotation.y = settings.value("rotation_y", 0).toInt();
348+ rotation.z = settings.value("rotation_z", 0).toInt();
349+ draw_widget_.setViewRotation(rotation);
350+ double zoom_ratio = settings.value("zoom_ratio", 1.0).toDouble();
351+ draw_widget_.setViewZoom(zoom_ratio);
352+ updateZoomView(parent);
353+
354+ // センサ高さ
355+ parent->sensor_height_spinbox_->
356+ setValue(settings.value("sensor_height", 1.000).toDouble());
357+ parent->ground_offset_spinbox_->
358+ setValue(settings.value("ground_offset", 0.000).toDouble());
359+
360+ // 移動速度
361+ parent->m_sec_spinbox_->
362+ setValue(settings.value("move_velocity", 0.600).toDouble());
363+ double msec = settings.value("afterimage_sec", 8.0).toDouble();
364+ parent->afterimage_spinbox_->setValue(msec);
365+ parent->updateDrawPeriod(msec);
366+ }
367+
368+
369+ void createComCombobox(Moving3DWindow* parent)
370+ {
371+ //UrgUsbCom urg_usb;
372+ //FindComPorts urg_finder(&urg_usb);
373+ vector<string> ports;
374+ findUrgPorts(ports);
375+ //vector<string> ports = urg_finder.find();
376+
377+ // ポートがなければ、記録させない
378+ parent->record_button_->setEnabled(! ports.empty());
379+
380+ // 見付かったポートをコンボボックスに登録する
381+ parent->com_combobox_->clear();
382+ for (vector<string>::iterator it = ports.begin();
383+ it != ports.end(); ++it) {
384+ parent->com_combobox_->addItem(it->c_str());
385+ }
386+ }
387+
388+
389+ void record(Moving3DWindow* parent)
390+ {
391+ const string device =
392+ parent->com_combobox_->currentText().toStdString();
393+
394+ if (! urg_.connect(device.c_str())) {
395+ QMessageBox::warning(parent, "URG", urg_.what(), QMessageBox::Close);
396+ return;
397+ }
398+
399+ // タイムスタンプを取得してから、AutoCapture モードに設定する
400+ urg_.setCaptureMode(ManualCapture);
401+ loadUrgParameters();
402+ loadFirstTimestamp();
403+ urg_.setCaptureMode(AutoCapture);
404+
405+ // 障害物を検出するための準備
406+ createAreaDetectArray(parent);
407+
408+ setRecordStart(parent, Start);
409+ setRecordEnabled(parent, false);
410+
411+ parent->stop_button_->setFocus();
412+ }
413+
414+
415+ void createAreaDetectArray(Moving3DWindow* parent)
416+ {
417+ if (area_points_.size() <= 1) {
418+ return;
419+ }
420+
421+ double h = getSensorHeight(parent);
422+
423+ // !!! URG が上向きで取り付けられていることに依存している
424+ // !!! 判定のループは、内側と外側が逆の方がよい。いずれ作りなおすこと
425+ detect_array_.clear();
426+ int max_length = urg_.maxDistance();
427+ for (size_t i = 0; i < data_max_; ++i) {
428+ double radian = urg_.index2rad(i) + (M_PI / 2.0);
429+ double x = max_length * cos(radian);
430+ double y = max_length * sin(radian);
431+
432+ Point<double> a(0, h), b(x, y + h), p;
433+
434+ // 登録してある判定用の直線と交差判定を行う
435+ Points::iterator first = area_points_.begin();
436+ double cross_min_length = max_length + 1.0;
437+ for (Points::iterator it = first + 1;
438+ it != area_points_.end(); ++it, ++first) {
439+ Point<double> c(first->x, first->y), d(it->x, it->y);
440+ if (! isCrossLines<double>(p, Line<double>(a, b), Line<double>(c, d))) {
441+ continue;
442+ }
443+
444+ double cross_length = length<Point<double> >(a, p);
445+ if (cross_length < cross_min_length) {
446+ cross_min_length = cross_length;
447+ }
448+ }
449+ detect_array_.push_back(static_cast<int>(cross_min_length));
450+ }
451+ }
452+
453+
454+ void stop(Moving3DWindow* parent)
455+ {
456+ urg_.disconnect();
457+
458+ setRecordStart(parent, Stop);
459+ setRecordEnabled(parent, true);
460+ }
461+
462+
463+ void loadFirstTimestamp(void)
464+ {
465+ long timestamp = 0;
466+ int n = urg_.capture(capture_data_, &timestamp);
467+ first_timestamp_ = (n > 0) ? timestamp : 0;
468+ first_ticks_ = ticks();
469+ }
470+
471+
472+ void loadUrgParameters(void)
473+ {
474+ data_max_ = urg_.maxScanLines();
475+ scan_msec_ = urg_.scanMsec();
476+ }
477+
478+
479+ void setRecordEnabled(Moving3DWindow* parent, bool enable)
480+ {
481+ parent->record_button_->setEnabled(enable);
482+ parent->stop_button_->setEnabled(! enable);
483+ }
484+
485+
486+ void setRecordStart(Moving3DWindow* parent, bool isStart)
487+ {
488+ // 記録用のタイマーの動作設定
489+ if (isStart) {
490+ QDateTime date_time = QDateTime::currentDateTime();
491+ string log_file =
492+ "log_" + date_time.toString("yyMMdd_hhmmss").toStdString() + ".txt";
493+ if (! urg_logger_.create(log_file.c_str())) {
494+ QMessageBox::warning(parent, "Log File", strerror(errno),
495+ QMessageBox::Close);
496+ }
497+ parent->play_file_edit_->setText(log_file.c_str());
498+ setPlayEnabled(parent, Recording);
499+
500+ // 障害物の情報を記録
501+ // !!! エラーチェックをすべき
502+ string obstacle_file = "obstacle" + log_file;
503+ obstacle_logger_.create(obstacle_file.c_str());
504+
505+ record_max_ = FirstSliderSecond;
506+ parent->progress_slider_->setMaximum(DefaultSliderMaximum);
507+ capture_timer_.start(scan_msec_);
508+ } else {
509+ urg_logger_.close();
510+ obstacle_logger_.close();
511+ capture_timer_.stop();
512+
513+ // 再生可能にする
514+ setPlayEnabled(parent, Waiting);
515+ }
516+ }
517+
518+
519+ void setPlayEnabled(Moving3DWindow* parent, PlayState state)
520+ {
521+ bool playing = ((state == Waiting) || (state == Pausing)) ? true : false;
522+ parent->play_button_->setEnabled(playing);
523+ parent->pause_button_->setEnabled((state == Playing) ? true : false);
524+ parent->play_stop_button_->
525+ setEnabled(((state == Recording) || (state == Waiting)) ? false : true);
526+
527+ bool no_recording = (state == Recording) ? false : true;
528+ parent->play_file_label_->setEnabled(no_recording);
529+ parent->play_file_edit_->setEnabled((state == Waiting) ? true : false);
530+ parent->play_file_path_button_->setEnabled(no_recording);
531+ }
532+
533+
534+ // 再生処理
535+ void readPlayData(Moving3DWindow* parent)
536+ {
537+ long current_timestamp = play_timer_.ticks();
538+ if (updateTimeView(parent, current_timestamp)) {
539+ // 再生終了
540+ parent->play_stop();
541+ return;
542+ }
543+
544+ // データの読み出しと登録
545+ size_t timestamp = 0;
546+ Points points;
547+ if (urg_logger_.getData(timestamp, points, current_timestamp)) {
548+ vector<Point3d<int> > points_3d;
549+ createCurrentData(parent, points_3d, timestamp, points);
550+ draw_widget_.addData(timestamp, points_3d);
551+ last_timestamp_ = timestamp;
552+ }
553+
554+ Points obstacle_points;
555+ if (obstacle_logger_.getData(timestamp, obstacle_points,
556+ current_timestamp)) {
557+ parent->changeObstacleLabel(obstacle_points.empty() ? false : true);
558+
559+ vector<Point3d<int> > points_3d;
560+ createCurrentData(parent, points_3d, timestamp, obstacle_points);
561+ draw_widget_.addAdditionalData(timestamp, points_3d);
562+ }
563+ }
564+
565+
566+ void recordCaptureData(Moving3DWindow* parent)
567+ {
568+ long timestamp = 0;
569+ int n = urg_.capture(capture_data_, &timestamp);
570+ if (n <= 0) {
571+ return;
572+ }
573+
574+ Points points;
575+ Position<long> offset;
576+ convert2d(points, &urg_, capture_data_, offset);
577+ size_t record_msec = timestamp - first_timestamp_;
578+ //fprintf(stderr, "[%d]\n", timestamp);
579+ //fprintf(stderr, "<%ld, %d>\n", urg_.recentTimestamp(), first_timestamp_);
580+ urg_logger_.addData(record_msec, points);
581+
582+ // データの登録
583+ vector<Point3d<int> > points_3d;
584+ createCurrentData(parent, points_3d, record_msec, points);
585+ draw_widget_.addData(record_msec, points_3d);
586+
587+ // 判定領域の中に物体があるかの判定
588+ Points obstacle_points;
589+ createObstaclePoints(obstacle_points);
590+ parent->changeObstacleLabel(obstacle_points.empty() ? false : true);
591+ obstacle_logger_.addData(record_msec, obstacle_points);
592+
593+ vector<Point3d<int> > obstacle_points_3d;
594+ createCurrentData(parent, obstacle_points_3d, record_msec, obstacle_points);
595+ draw_widget_.addAdditionalData(record_msec, obstacle_points_3d);
596+
597+ // 記録時間の更新
598+ updateTimeView(parent, record_msec, true);
599+ }
600+
601+
602+ void createObstaclePoints(Points& points)
603+ {
604+ int min_distance = urg_.minDistance();
605+
606+ for (size_t i = 0; i < data_max_; ++i) {
607+ long distance = capture_data_[i];
608+ long judge_distance = detect_array_[i];
609+ if (! ((distance > min_distance) && (distance <= judge_distance))) {
610+ continue;
611+ }
612+ double radian = urg_.index2rad(i);
613+ int x = static_cast<int>(distance * cos(radian));
614+ int y = static_cast<int>(distance * sin(radian));
615+
616+ points.push_back(Point<long>(x, y));
617+ }
618+ }
619+
620+
621+ void createCurrentData(Moving3DWindow* parent,
622+ vector<Point3d<int> >& points_3d,
623+ int msec, Points& points)
624+ {
625+ int z = getCurrentPosition(parent, msec);
626+
627+ for (Points::iterator it = points.begin(); it != points.end(); ++it) {
628+ points_3d.push_back(Point3d<int>(it->y, it->x, z));
629+ }
630+ }
631+
632+
633+ bool updateTimeView(Moving3DWindow* parent, int msec, bool extendable = false)
634+ {
635+ size_t sec = msec / 1000;
636+ enum { BufferSize = 7 };
637+ char buffer[BufferSize];
638+ snprintf(buffer, BufferSize, "% d:%02d", sec / 60, sec % 60);
639+ parent->time_label_->setText(buffer);
640+
641+ double progress_percent = 100 * msec / (record_max_ * 1000.0);
642+ int maximum = parent->progress_slider_->maximum();
643+
644+ bool over = (progress_percent > 100.0) ? true : false;
645+ if (extendable && over) {
646+ // 記録時間のスライダが最大幅を越えたら、伸長する
647+ record_max_ *= 2;
648+ parent->progress_slider_->setMaximum(maximum * 2);
649+ }
650+ int value = static_cast<int>(maximum * progress_percent / 100.0);
651+ parent->progress_slider_->setValue(value);
652+
653+ return over;
654+ }
655+
656+
657+ int getCurrentPosition(Moving3DWindow* parent, int timestamp)
658+ {
659+ double velocity = parent->m_sec_spinbox_->value();
660+ int z = static_cast<int>(timestamp * velocity);
661+
662+ return z;
663+ }
664+
665+
666+ double getSensorHeight(Moving3DWindow* parent)
667+ {
668+ return 1000.0 * (parent->sensor_height_spinbox_->value() +
669+ parent->ground_offset_spinbox_->value());
670+ }
671+
672+
673+ void redraw(Moving3DWindow* parent)
674+ {
675+ // 記録時
676+ if (nowRecording(parent)) {
677+ last_timestamp_ = ticks() - first_ticks_;
678+
679+ } else if (nowPlaying()) {
680+ // 再生時は、スライダーと同期した位置を timestamp として利用する
681+ last_timestamp_ = play_timer_.ticks();
682+
683+ } else {
684+ // 待機時は、前回のタイムスタンプを用いる
685+ }
686+
687+ int z = getCurrentPosition(parent, last_timestamp_);
688+ last_draw_position_ = Point3d<int>(0, 0, z);
689+ draw_widget_.redraw(last_timestamp_, last_draw_position_);
690+ }
691+
692+
693+ bool nowRecording(Moving3DWindow* parent)
694+ {
695+ return parent->record_button_->isEnabled() ? false : true;
696+ }
697+
698+
699+ bool nowPlaying(void)
700+ {
701+ return (play_timer_.ticks() == 0) ? false : true;
702+ }
703+
704+
705+ void updateZoomView(Moving3DWindow* parent)
706+ {
707+ double ratio = draw_widget_.zoomRatio();
708+ int percent =
709+ static_cast<int>(100.0 * (ratio - MinRatio) / (MaxRatio - MinRatio));
710+ if (ratio < MinRatio) {
711+ percent = 0;
712+ } else if (ratio > MaxRatio) {
713+ percent = 100;
714+ }
715+ parent->zoom_slider_->setValue(percent);
716+ }
717+
718+
719+ void changeZoom(int percent)
720+ {
721+ double ratio = MinRatio + (MaxRatio - MinRatio) * percent / 100.0;
722+ draw_widget_.setViewZoom(ratio);
723+ }
724+};
725+
726+
727+Moving3DWindow::Moving3DWindow(void) : pimpl(new pImpl)
728+{
729+ setupUi(this);
730+
731+ pimpl->initializeForm(this);
732+ pimpl->loadSettings(this);
733+ pimpl->updateInformation(this);
734+}
735+
736+
737+Moving3DWindow::~Moving3DWindow(void)
738+{
739+ pimpl->saveSettings(this);
740+}
741+
742+
743+// 接続ポートの再スキャン
744+void Moving3DWindow::rescanPort(void)
745+{
746+ pimpl->createComCombobox(this);
747+}
748+
749+
750+// 記録の開始
751+void Moving3DWindow::record(void)
752+{
753+ pimpl->record(this);
754+}
755+
756+
757+// 記録の終了
758+void Moving3DWindow::stop(void)
759+{
760+ pimpl->stop(this);
761+}
762+
763+
764+// 取得データの記録
765+void Moving3DWindow::recordCaptureData(void)
766+{
767+ pimpl->recordCaptureData(this);
768+}
769+
770+
771+void Moving3DWindow::front(void)
772+{
773+ pimpl->draw_widget_.setViewRotation(Point3d<int>(0, 0, 0));
774+}
775+
776+
777+void Moving3DWindow::overview(void)
778+{
779+ pimpl->draw_widget_.setViewRotation(Point3d<int>(30, 325, 0));
780+}
781+
782+
783+void Moving3DWindow::rescanArea(void)
784+{
785+ pimpl->rescanArea(this);
786+}
787+
788+
789+void Moving3DWindow::redraw(void)
790+{
791+ pimpl->redraw(this);
792+}
793+
794+
795+void Moving3DWindow::play(void)
796+{
797+ if (! pimpl->nowPlaying()) {
798+ // 再生開始
799+
800+ if (pimpl->nowRecording(this)) {
801+ // 記録を停止し、その記録を再生する
802+ stop();
803+ }
804+
805+ QString file_name = play_file_edit_->text();
806+ string std_path = qrk::toStdStringPath(file_name);
807+ if (! pimpl->urg_logger_.load(std_path.c_str())) {
808+ if (pimpl->urg_logger_.getLastTimestamp() == 0) {
809+ QMessageBox::warning(this, tr("Log File"), tr("Invalid data file."),
810+ QMessageBox::Close);
811+ } else {
812+ QMessageBox::warning(this, tr("Log File"), strerror(errno),
813+ QMessageBox::Close);
814+ }
815+ return;
816+ }
817+
818+ // !!! ディレクトリ名を保持した上で、ファイル名の調整を行うべき
819+ QString obstacle_file_name = "obstacle" + play_file_edit_->text();
820+ string obstacle_std_path = qrk::toStdStringPath(obstacle_file_name);
821+ pimpl->obstacle_logger_.load(obstacle_std_path.c_str());
822+
823+ // 再生開始
824+ pimpl->draw_widget_.clear();
825+ pimpl->play_period_max_ = pimpl->urg_logger_.getLastTimestamp();
826+ pimpl->record_max_ = pimpl->play_period_max_ / 1000;
827+ progress_slider_->setMaximum(pImpl::DefaultSliderMaximum *
828+ pimpl->record_max_ / pImpl::FirstSliderSecond);
829+
830+ pimpl->play_timer_.start();
831+ pimpl->setPlayEnabled(this, Playing);
832+ pimpl->playing_update_timer_.start(pImpl::PlayTimerUpdateMsec);
833+
834+ } else {
835+ // 一時停止からの復帰
836+ pimpl->play_timer_.resume();
837+ pimpl->setPlayEnabled(this, Playing);
838+ }
839+}
840+
841+
842+void Moving3DWindow::pause(void)
843+{
844+ pimpl->play_timer_.pause();
845+ pimpl->setPlayEnabled(this, Pausing);
846+}
847+
848+
849+void Moving3DWindow::play_stop(void)
850+{
851+ pimpl->playing_update_timer_.stop();
852+ pimpl->play_timer_.stop();
853+ pimpl->setPlayEnabled(this, Waiting);
854+}
855+
856+
857+void Moving3DWindow::readPlayData(void)
858+{
859+ pimpl->readPlayData(this);
860+}
861+
862+
863+void Moving3DWindow::initializeView(void)
864+{
865+ front();
866+ initializeZoom();
867+}
868+
869+
870+void Moving3DWindow::pauseAndPlay(void)
871+{
872+ if (! pimpl->nowPlaying()) {
873+
874+ if (! pimpl->nowRecording(this)) {
875+ // 待機中ならば、再生を開始する
876+ play();
877+ }
878+ return;
879+ }
880+
881+ if (pimpl->play_timer_.isPause()) {
882+ play();
883+ } else {
884+ pause();
885+ }
886+}
887+
888+
889+void Moving3DWindow::initializeZoom(void)
890+{
891+ pimpl->draw_widget_.setViewZoom(1.0);
892+ pimpl->updateZoomView(this);
893+}
894+
895+
896+void Moving3DWindow::zoomIn(void)
897+{
898+ pimpl->draw_widget_.zoomIn();
899+ pimpl->updateZoomView(this);
900+}
901+
902+
903+void Moving3DWindow::zoomOut(void)
904+{
905+ pimpl->draw_widget_.zoomOut();
906+ pimpl->updateZoomView(this);
907+}
908+
909+
910+void Moving3DWindow::changeZoom(int value)
911+{
912+ int maximum = zoom_slider_->maximum();
913+ int percent = static_cast<int>(100.0 * value / maximum);
914+ pimpl->changeZoom(percent);
915+}
916+
917+
918+void Moving3DWindow::viewUp(void)
919+{
920+ pimpl->draw_widget_.viewUp();
921+}
922+
923+
924+void Moving3DWindow::viewDown(void)
925+{
926+ pimpl->draw_widget_.viewDown();
927+}
928+
929+
930+void Moving3DWindow::viewLeft(void)
931+{
932+ pimpl->draw_widget_.viewLeft();
933+}
934+
935+
936+void Moving3DWindow::viewRight(void)
937+{
938+ pimpl->draw_widget_.viewRight();
939+}
940+
941+
942+void Moving3DWindow::changeObstacleLabel(bool detected)
943+{
944+ if (detected) {
945+ detect_label_->setText(tr("Detected !!!"));
946+ } else {
947+ detect_label_->setText(tr("No Detection."));
948+ }
949+}
950+
951+
952+void Moving3DWindow::aboutApplication(void)
953+{
954+ QMessageBox::about(this, tr("About Moving3D"),
955+ tr("<h2>Moving3D</h2>"
956+ "<p>Demo Application for URG.</p>"
957+ "<p>Report bugs to "
958+ "&lt;satofumi@users.sourceforge.jp&gt;</p>"));
959+}
960+
961+
962+void Moving3DWindow::updateDrawPeriod(double value)
963+{
964+ // 値は毎回取得する
965+ int msec = static_cast<int>(value * 1000.0);
966+ pimpl->draw_widget_.setDrawPeriod(msec);
967+}
968+
969+
970+void Moving3DWindow::playFileLoad(void)
971+{
972+ QString fileName =
973+ QFileDialog::getOpenFileName(this, tr("Open log file."), ".",
974+ tr("log (*.txt)"));
975+
976+ if (! fileName.isEmpty()) {
977+ // 日本語を含むパスへの対処
978+ string std_path = qrk::toStdStringPath(fileName);
979+ QFileInfo fi(std_path.c_str());
980+ QString name = fi.fileName();
981+
982+ play_file_edit_->setText(name);
983+ }
984+}
Added: svn:keywords
## -0,0 +1 ##
+Id
\ No newline at end of property
--- trunk/programs/Moving3D/LineTypes.h (nonexistent)
+++ trunk/programs/Moving3D/LineTypes.h (revision 1970)
@@ -0,0 +1,44 @@
1+#ifndef LINE_TYPES_H
2+#define LINE_TYPES_H
3+
4+/*!
5+ \file
6+ \brief 直線の型定義
7+
8+ \author Satofumi KAMIMURA
9+
10+ $Id$
11+*/
12+
13+#include "Point.h"
14+#include "Angle.h"
15+
16+
17+namespace qrk
18+{
19+ /*!
20+ \brief 線分の型定義
21+ */
22+ template<class T>
23+ class Line
24+ {
25+ public:
26+ Point<T> start;
27+ Point<T> end;
28+
29+
30+ Line(const Point<T>& start_, const Point<T>& end_)
31+ : start(start_), end(end_)
32+ {
33+ }
34+
35+
36+ Line(void)
37+ {
38+ }
39+
40+ // !!!
41+ };
42+}
43+
44+#endif /* !LINE_TYPES_H */
Added: svn:keywords
## -0,0 +1 ##
+Id
\ No newline at end of property
--- trunk/programs/Moving3D/UrgLogHandler.h (nonexistent)
+++ trunk/programs/Moving3D/UrgLogHandler.h (revision 1970)
@@ -0,0 +1,53 @@
1+#ifndef URG_LOG_HANDLER_H
2+#define URG_LOG_HANDLER_H
3+
4+/*!
5+ \file
6+ \brief URG の出力ログ管理
7+
8+ \author Satofumi KAMIMURA
9+
10+ $Id$
11+
12+ \todo get*() 形式のメソッドを *() 形式に変更する
13+*/
14+
15+#include "Point.h"
16+#include <vector>
17+#include <memory>
18+#include <cstddef>
19+
20+
21+class UrgLogHandler
22+{
23+ UrgLogHandler(const UrgLogHandler& rhs);
24+ UrgLogHandler& operator = (const UrgLogHandler& rhs);
25+
26+ struct pImpl;
27+ std::auto_ptr<pImpl> pimpl;
28+
29+public:
30+ UrgLogHandler(void);
31+ ~UrgLogHandler(void);
32+
33+ //! ログ記録の開始
34+ bool create(const char* file_name);
35+
36+ //! データ追加
37+ void addData(size_t timestamp, const std::vector<qrk::Point<long> >& points);
38+
39+ //! 内容を書き出して終了
40+ void close();
41+
42+ //! 読み出すファイルを指定
43+ bool load(const char* file_name);
44+
45+ //! 最終タイムスタンプの取得
46+ int getLastTimestamp(void);
47+
48+ //! データの読み出し
49+ bool getData(size_t& timestamp, std::vector<qrk::Point<long> >& points,
50+ size_t current_timestamp);
51+};
52+
53+#endif /* !URG_LOG_HANDLER_H */
Added: svn:keywords
## -0,0 +1 ##
+Id
\ No newline at end of property
--- trunk/programs/Moving3D/Moving3DWindow.h (nonexistent)
+++ trunk/programs/Moving3D/Moving3DWindow.h (revision 1970)
@@ -0,0 +1,65 @@
1+#ifndef RAILS_3D_WINDOW_H
2+#define RAILS_3D_WINDOW_H
3+
4+/*!
5+ \file
6+ \brief メインフォーム管理
7+
8+ \author Satofumi KAMIMURA
9+
10+ $Id$
11+*/
12+
13+#include "ui_Moving3DWindowForm.h"
14+#include <memory>
15+
16+
17+class Moving3DWindow : public QMainWindow, private Ui::Moving3DWindowForm
18+{
19+ Q_OBJECT;
20+
21+ Moving3DWindow(const Moving3DWindow& rhs);
22+ Moving3DWindow& operator = (const Moving3DWindow& rhs);
23+
24+ struct pImpl;
25+ std::auto_ptr<pImpl> pimpl;
26+
27+ void changeObstacleLabel(bool detected);
28+
29+private slots:
30+ void rescanPort(void);
31+ void record(void);
32+ void stop(void);
33+ void recordCaptureData(void);
34+ void front(void);
35+ void overview(void);
36+ void rescanArea(void);
37+ void redraw(void);
38+ void play(void);
39+ void pause(void);
40+ void play_stop(void);
41+ void readPlayData(void);
42+
43+ void initializeView(void);
44+ void pauseAndPlay(void);
45+ void initializeZoom(void);
46+ void zoomIn(void);
47+ void zoomOut(void);
48+ void changeZoom(int value);
49+ void updateDrawPeriod(double value);
50+ void playFileLoad(void);
51+
52+ void viewUp(void);
53+ void viewDown(void);
54+ void viewLeft(void);
55+ void viewRight(void);
56+
57+ void aboutApplication(void);
58+
59+public:
60+ Moving3DWindow(void);
61+ ~Moving3DWindow(void);
62+};
63+
64+
65+#endif /* !RAILS_3D_WINDOW_H */
Added: svn:keywords
## -0,0 +1 ##
+Id
\ No newline at end of property
--- trunk/programs/Moving3D/main.cpp (nonexistent)
+++ trunk/programs/Moving3D/main.cpp (revision 1970)
@@ -0,0 +1,28 @@
1+/*!
2+ \file
3+ \brief 等速で移動して 3D 環境を取得するプログラム
4+
5+ \author Satofumi KAMIMURA
6+
7+ $Id$
8+*/
9+
10+#include "Moving3DWindow.h"
11+#include <QApplication>
12+#include <QTranslator>
13+
14+
15+int main(int argc, char *argv[]) {
16+
17+ QApplication app(argc, argv);
18+
19+ // ロケールの適用
20+ QString locale = QLocale::system().name();
21+ QTranslator translator;
22+ translator.load("Moving3D_" + locale);
23+ app.installTranslator(&translator);
24+
25+ Moving3DWindow window;
26+ window.show();
27+ return app.exec();
28+}
Added: svn:keywords
## -0,0 +1 ##
+Id
\ No newline at end of property
--- trunk/programs/Moving3D/Draw3DWidget.cpp (nonexistent)
+++ trunk/programs/Moving3D/Draw3DWidget.cpp (revision 1970)
@@ -0,0 +1,391 @@
1+/*!
2+ \file
3+ \brief URG データの表示ウィジット
4+
5+ \author Satofumi KAMIMURA
6+
7+ $Id$
8+
9+ \todo X, Y, Z 軸を描画する
10+*/
11+
12+#include "Draw3DWidget.h"
13+#include <QMouseEvent>
14+#include <deque>
15+#include <limits>
16+
17+#include <cstdio>
18+
19+using namespace qrk;
20+using namespace std;
21+
22+
23+namespace {
24+ class ScanData
25+ {
26+ public:
27+ size_t timestamp_;
28+ std::vector<qrk::Point3d<int> > points_;
29+
30+ ScanData(size_t timestamp, std::vector<qrk::Point3d<int> > points)
31+ : timestamp_(timestamp), points_(points)
32+ {
33+ }
34+ };
35+}
36+
37+
38+struct Draw3DWidget::pImpl
39+{
40+ enum {
41+ DefaultDrawPeriod = 8000, // [msec]
42+ };
43+
44+ QColor clear_color_;
45+ Point3d<int> rotation_;
46+
47+ std::deque<ScanData> plot_data_;
48+ std::deque<ScanData> additional_data_;
49+
50+ QPoint last_position_;
51+ Point3d<int> draw_position_;
52+ size_t draw_period_;
53+ size_t current_timestamp_;
54+
55+ double zoom_;
56+
57+ std::vector<Point<long> > draw_line_points_;
58+ int h_offset_;
59+
60+
61+ pImpl(void)
62+ : clear_color_(Qt::black), draw_period_(DefaultDrawPeriod),
63+ current_timestamp_(0), zoom_(1.0), h_offset_(0)
64+ {
65+ }
66+
67+
68+ void initializeGL(Draw3DWidget* parent)
69+ {
70+ parent->qglClearColor(clear_color_);
71+ glEnable(GL_DEPTH_TEST);
72+ glEnable(GL_CULL_FACE);
73+ glEnable(GL_TEXTURE_2D);
74+ }
75+
76+
77+ void paintGL(Draw3DWidget* parent)
78+ {
79+ parent->qglClearColor(clear_color_);
80+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
81+
82+ glLoadIdentity();
83+
84+ glRotated(rotation_.x, 1.0, 0.0, 0.0);
85+ glRotated(rotation_.y, 0.0, 1.0, 0.0);
86+ glRotated(rotation_.z, 0.0, 0.0, 1.0);
87+
88+ // 追加の直線を描画
89+ drawAdditionalLines();
90+
91+ // 最新データのレーザ範囲を描画
92+ if (! plot_data_.empty()) {
93+ drawLaserLine(plot_data_.front());
94+ }
95+
96+ // 取得データを時間経過に従って、薄くしながら描画
97+ glBegin(GL_POINTS);
98+ std::deque<ScanData>::iterator additional_it = additional_data_.begin();
99+ for (std::deque<ScanData>::iterator it = plot_data_.begin();
100+ it != plot_data_.end(); ++it, ++additional_it) {
101+ //fprintf(stderr, "%d, %d\n", current_timestamp_, it->timestamp_);
102+ size_t diff_time = current_timestamp_ - it->timestamp_;
103+ if (diff_time > draw_period_) {
104+ // 以降のデータを削除
105+ plot_data_.erase(it, plot_data_.end());
106+ additional_data_.erase(additional_it, additional_data_.end());
107+ break;
108+ }
109+
110+ // データの描画
111+ double alpha = 1.0 * (draw_period_ - diff_time) / draw_period_;
112+ drawScanData(*additional_it, alpha, 0.0, 0.0);
113+ drawScanData(*it, alpha, alpha, alpha);
114+ }
115+ glEnd();
116+ }
117+
118+
119+ void drawAdditionalLines(void)
120+ {
121+ if (draw_line_points_.size() <= 1) {
122+ return;
123+ }
124+ glColor3d(0.0, 0.5, 0.0);
125+
126+ std::vector<Point<long> >::iterator first = draw_line_points_.begin();
127+ for (std::vector<Point<long> >::iterator it = first + 1;
128+ it != draw_line_points_.end(); ++it, ++first) {
129+
130+ glBegin(GL_LINE_STRIP);
131+ glVertex3d(+(first->x - draw_position_.x) * zoom_,
132+ +(first->y - draw_position_.y - h_offset_) * zoom_, 0.0);
133+ glVertex3d(+(it->x - draw_position_.x) * zoom_,
134+ +(it->y - draw_position_.y - h_offset_) * zoom_, 0.0);
135+ glEnd();
136+
137+ }
138+ }
139+
140+
141+ void drawScanData(const ScanData& line_data, double r, double g, double b)
142+ {
143+ if (line_data.points_.empty()) {
144+ return;
145+ }
146+
147+ std::vector<Point3d<int> >::const_iterator it_end = line_data.points_.end();
148+ for (std::vector<Point3d<int> >::const_iterator it =
149+ line_data.points_.begin(); it != it_end; ++it) {
150+
151+ glColor3d(r, g, b);
152+ glVertex3d(+(it->x - draw_position_.x) * zoom_,
153+ +(it->y - draw_position_.y) * zoom_,
154+ -(it->z - draw_position_.z) * zoom_);
155+ }
156+ }
157+
158+
159+ void drawLaserLine(const ScanData& line_data)
160+ {
161+ glColor3d(0, 0, 0.4);
162+
163+ int index = 0;
164+ const int skip_line = 5;
165+
166+ std::vector<Point3d<int> >::const_iterator it_end = line_data.points_.end();
167+ for (std::vector<Point3d<int> >::const_iterator it =
168+ line_data.points_.begin(); it != it_end; ++it, ++index) {
169+ if ((index % skip_line) != 0) {
170+ continue;
171+ }
172+
173+ glBegin(GL_LINE_STRIP);
174+ glVertex3d(0.0, 0.0, 0.0);
175+ glVertex3d(+(it->x - draw_position_.x) * zoom_,
176+ +(it->y - draw_position_.y) * zoom_,
177+ -(it->z - draw_position_.z) * zoom_);
178+ glEnd();
179+ }
180+ }
181+
182+
183+ void setXRotation(int angle)
184+ {
185+ normalizeAngle(angle);
186+ if (angle != rotation_.x) {
187+ rotation_.x = angle;
188+ }
189+ }
190+
191+
192+ void setYRotation(int angle)
193+ {
194+ normalizeAngle(angle);
195+ if (angle != rotation_.y) {
196+ rotation_.y = angle;
197+ }
198+ }
199+
200+
201+ void setZRotation(int angle)
202+ {
203+ normalizeAngle(angle);
204+ if (angle != rotation_.z) {
205+ rotation_.z = angle;
206+ }
207+ }
208+
209+
210+ void normalizeAngle(int& angle)
211+ {
212+ while (angle < 0) {
213+ angle += 360 * 16;
214+ }
215+
216+ while (angle > 360 * 16) {
217+ angle -= 360 * 16;
218+ }
219+ }
220+};
221+
222+
223+Draw3DWidget::Draw3DWidget(QGLWidget* parent)
224+ : QGLWidget(parent), pimpl(new pImpl)
225+{
226+}
227+
228+
229+Draw3DWidget::~Draw3DWidget(void)
230+{
231+}
232+
233+
234+void Draw3DWidget::initializeGL(void)
235+{
236+ pimpl->initializeGL(this);
237+}
238+
239+
240+void Draw3DWidget::resizeGL(int width, int height)
241+{
242+ glViewport(0, 0, width, height);
243+
244+ glMatrixMode(GL_PROJECTION);
245+ glLoadIdentity();
246+
247+ double aspect = 1.0 * width / height;
248+ enum { DefaultViewLength = 5000 };
249+ glOrtho(-DefaultViewLength * aspect, +DefaultViewLength * aspect,
250+ -DefaultViewLength, DefaultViewLength,
251+ numeric_limits<int>::min(), numeric_limits<int>::max());
252+
253+ glMatrixMode(GL_MODELVIEW);
254+}
255+
256+
257+void Draw3DWidget::paintGL(void)
258+{
259+ pimpl->paintGL(this);
260+}
261+
262+
263+void Draw3DWidget::mousePressEvent(QMouseEvent *event)
264+{
265+ pimpl->last_position_ = event->pos();
266+}
267+
268+
269+void Draw3DWidget::mouseMoveEvent(QMouseEvent *event)
270+{
271+ int dx = (event->x() - pimpl->last_position_.x()) / 2;
272+ int dy = (event->y() - pimpl->last_position_.y()) / 2;
273+
274+ if (event->buttons() & Qt::LeftButton) {
275+ pimpl->setXRotation(pimpl->rotation_.x + dy);
276+ pimpl->setYRotation(pimpl->rotation_.y + dx);
277+
278+ } else if (event->buttons() & Qt::RightButton) {
279+ pimpl->setXRotation(pimpl->rotation_.x + dy);
280+ pimpl->setZRotation(pimpl->rotation_.z - dx);
281+ }
282+ pimpl->last_position_ = event->pos();
283+}
284+
285+
286+void Draw3DWidget::redraw(size_t timestamp, const Point3d<int>& draw_position)
287+{
288+ pimpl->current_timestamp_ = timestamp;
289+ pimpl->draw_position_ = draw_position;
290+ updateGL();
291+}
292+
293+
294+void Draw3DWidget::setDrawLineData(const std::vector<qrk::Point<long> >&
295+ draw_points, int h)
296+{
297+ pimpl->draw_line_points_ = draw_points;
298+ pimpl->h_offset_ = h;
299+}
300+
301+
302+void Draw3DWidget::clear(void)
303+{
304+ pimpl->plot_data_.clear();
305+ pimpl->additional_data_.clear();
306+}
307+
308+
309+void Draw3DWidget::addData(size_t timestamp,
310+ const std::vector<qrk::Point3d<int> >& points)
311+{
312+ pimpl->plot_data_.push_front(ScanData(timestamp, points));
313+}
314+
315+
316+void Draw3DWidget::addAdditionalData(size_t timestamp,
317+ const std::vector<qrk::Point3d<int> >&
318+ points)
319+{
320+ pimpl->additional_data_.push_front(ScanData(timestamp, points));
321+}
322+
323+
324+
325+void Draw3DWidget::setDrawPeriod(size_t msec)
326+{
327+ pimpl->draw_period_ = msec;
328+}
329+
330+
331+void Draw3DWidget::setViewRotation(const Point3d<int>& rotation)
332+{
333+ pimpl->rotation_ = rotation;
334+}
335+
336+
337+void Draw3DWidget::viewUp(void)
338+{
339+ pimpl->rotation_.x += 4;
340+}
341+
342+
343+void Draw3DWidget::viewDown(void)
344+{
345+ pimpl->rotation_.x -= 4;
346+}
347+
348+
349+void Draw3DWidget::viewLeft(void)
350+{
351+ pimpl->rotation_.y -= 4;
352+}
353+
354+
355+void Draw3DWidget::viewRight(void)
356+{
357+ pimpl->rotation_.y += 4;
358+}
359+
360+
361+qrk::Point3d<int> Draw3DWidget::viewRotation(void)
362+{
363+ return pimpl->rotation_;
364+}
365+
366+
367+void Draw3DWidget::setViewZoom(const double ratio)
368+{
369+ pimpl->zoom_ = ratio;
370+}
371+
372+
373+void Draw3DWidget::zoomIn(void)
374+{
375+ pimpl->zoom_ *= 1.2;
376+}
377+
378+
379+void Draw3DWidget::zoomOut(void)
380+{
381+ pimpl->zoom_ *= 0.8;
382+ if (pimpl->zoom_ < 0.0) {
383+ pimpl->zoom_ = 0.0;
384+ }
385+}
386+
387+
388+double Draw3DWidget::zoomRatio(void)
389+{
390+ return pimpl->zoom_;
391+}
Added: svn:keywords
## -0,0 +1 ##
+Id
\ No newline at end of property
--- trunk/programs/Moving3D/detect_area.csv (nonexistent)
+++ trunk/programs/Moving3D/detect_area.csv (revision 1970)
@@ -0,0 +1,6 @@
1+0.000, 0.000
2+-1.000, 0.000
3+-1.000, 2.000
4++1.000, 2.000
5++1.000, 0.000
6+0.000, 0.000
--- trunk/programs/Moving3D/img/Makefile (nonexistent)
+++ trunk/programs/Moving3D/img/Makefile (revision 1970)
@@ -0,0 +1,15 @@
1+# Makefile for Rails3D/img
2+# Satofumi KAMIMURA
3+# $Id$
4+
5+# Target
6+TARGET = position_abst.png
7+
8+all : $(TARGET)
9+
10+clean :
11+ $(RM) $(TARGET)
12+
13+.SUFFIXES: .png .obj
14+.obj.png:
15+ tgif -print -png $<
Added: svn:keywords
## -0,0 +1 ##
+Id
\ No newline at end of property
--- trunk/programs/Moving3D/LineUtils.h (nonexistent)
+++ trunk/programs/Moving3D/LineUtils.h (revision 1970)
@@ -0,0 +1,103 @@
1+#ifndef LINE_UTILS_H
2+#define LINE_UTILS_H
3+
4+/*!
5+ \file
6+ \brief 直線関連の関数
7+
8+ 「ゲームプログラミングのためのリアルタイム衝突判定」(ELSEVIER) より
9+
10+ \author Satofumi KAMIMURA
11+
12+ $Id$
13+*/
14+
15+#include "LineTypes.h"
16+#include "MathUtils.h"
17+
18+
19+namespace
20+{
21+ template <class T>
22+ double signed2DTriangleArea(const qrk::Point<T>& a,
23+ const qrk::Point<T>& b,
24+ const qrk::Point<T>& c)
25+ {
26+ return ((a.x - c.x) * (b.y - c.y)) - ((a.y - c.y) * (b.x - c.x));
27+ }
28+}
29+
30+
31+namespace qrk
32+{
33+ class Angle;
34+
35+
36+ /*!
37+ \brief 線分の交差判定
38+
39+ \param[in] p 線分の交差点
40+ \param[in] a 線分
41+ \param[in] b 線分
42+ */
43+ template <class T>
44+ bool isCrossLines(Point<T>& p, const Line<T>& a, const Line<T>& b)
45+ {
46+ double a1 = signed2DTriangleArea(a.start, a.end, b.end);
47+ double a2 = signed2DTriangleArea(a.start, a.end, b.start);
48+
49+ double a_1_2 = a1 * a2;
50+ if ((a_1_2) <= 0.0) {
51+ double a3 = signed2DTriangleArea(b.start, b.end, a.start);
52+ double a4 = a3 + a2 - a1;
53+
54+ double a_3_4 = a3 * a4;
55+ if (a_3_4 <= 0.0) {
56+
57+ double diff = a3 - a4;
58+ if (diff == 0.0) {
59+ // 直線が重なっているときは、a 開始位置を交差点として返す
60+ p = a.start;
61+ } else {
62+ double t = 1.0 * a3 / diff;
63+ Point<double> diff = a.end - a.start;
64+ p = (t * diff) + a.start;
65+ }
66+ return true;
67+ }
68+ }
69+ return false;
70+ }
71+
72+
73+ template <class T>
74+ double norm(const T& a)
75+ {
76+ return sqrt((a.x * a.x) + (a.y * a.y));
77+ }
78+
79+
80+ /*!
81+ \brief 線分の距離計算
82+
83+ \return 線分の距離
84+ */
85+ template <class T>
86+ double length(const T& a, const T& b)
87+ {
88+ double x_diff = a.x - b.x;
89+ double y_diff = a.y - b.y;
90+
91+ return sqrt((x_diff * x_diff) + (y_diff * y_diff));
92+ }
93+
94+
95+ template <class T>
96+ Angle lineAngle(const Point<T>& a, const Point<T>& b)
97+ {
98+ double angle_radian = atan2(a.y - b.y, a.x - b.x);
99+ return rad(angle_radian);
100+ }
101+}
102+
103+#endif /* !LINE_UTILS_H */
Added: svn:keywords
## -0,0 +1 ##
+Id
\ No newline at end of property
--- trunk/programs/Moving3D/UrgLogHandler.cpp (nonexistent)
+++ trunk/programs/Moving3D/UrgLogHandler.cpp (revision 1970)
@@ -0,0 +1,175 @@
1+/*!
2+ \file
3+ \brief URG の出力ログ管理
4+
5+ \author Satofumi KAMIMURA
6+
7+ $Id$
8+*/
9+
10+#include "UrgLogHandler.h"
11+#include "split.h"
12+#include <fstream>
13+#include <cstdlib>
14+
15+using namespace qrk;
16+using namespace std;
17+
18+
19+namespace {
20+ enum {
21+ InvalidTimestamp = -1,
22+ };
23+}
24+
25+struct UrgLogHandler::pImpl
26+{
27+ ofstream fout_;
28+ ifstream* fin_;
29+
30+ int last_timestamp_;
31+ int cached_timestamp_;
32+ vector<qrk::Point<long> > cached_points_;
33+
34+
35+ pImpl(void)
36+ : fin_(NULL), last_timestamp_(0), cached_timestamp_(InvalidTimestamp)
37+ {
38+ }
39+
40+
41+ void addData(size_t timestamp, const vector<qrk::Point<long> >& points)
42+ {
43+ last_timestamp_ = timestamp;
44+
45+ fout_ << timestamp << ',' << points.size() << ',';
46+
47+ for (vector<qrk::Point<long> >::const_iterator it = points.begin();
48+ it != points.end(); ++it) {
49+ fout_ << it->x << ',' << it->y << ',';
50+ }
51+ fout_ << endl;
52+ }
53+
54+
55+ bool load(const char* file_name)
56+ {
57+ fin_ = new ifstream(file_name);
58+ if (! fin_->is_open()) {
59+ delete fin_;
60+ fin_ = NULL;
61+ return false;
62+ }
63+
64+ // 最後の行を読み出し、最大のタイムスタンプ値を取得する
65+ string line;
66+ while (getline(*fin_, line)) {
67+ last_timestamp_ = atoi(line.c_str());
68+ }
69+ if (last_timestamp_ == 0) {
70+ delete fin_;
71+ fin_ = NULL;
72+ return false;
73+ }
74+
75+ cached_timestamp_ = InvalidTimestamp;
76+
77+ // ifstream::seekg(0) が動作しなかったため、再 open() する
78+ delete fin_;
79+ fin_ = new ifstream(file_name);
80+
81+ return true;
82+ }
83+
84+
85+ bool getData(size_t& timestamp, vector<qrk::Point<long> >& points,
86+ size_t current_timestamp)
87+ {
88+ if (cached_timestamp_ == InvalidTimestamp) {
89+ // データの読み出し
90+ string line;
91+ if (! getline(*fin_, line)) {
92+ return false;
93+ }
94+
95+ vector<string> tokens;
96+ if (qrk::split(tokens, line, " \t,") <= 1) {
97+ // タイムスタンプ、データ数、だけのトークンがない
98+ return false;
99+ }
100+
101+ cached_timestamp_ = atoi(tokens[0].c_str());
102+ int n = atoi(tokens[1].c_str());
103+
104+ cached_points_.clear();
105+ for (int i = 0; i < n; ++i) {
106+ int x = atoi(tokens[2 + (2 * i)].c_str());
107+ int y = atoi(tokens[2 + (2 * i) + 1].c_str());
108+
109+ cached_points_.push_back(Point<long>(x, y));
110+ }
111+ }
112+
113+ // 保持しているデータが指定タイムスタンプよりも古ければ、データを返す
114+ if (static_cast<size_t>(cached_timestamp_) > current_timestamp) {
115+ return false;
116+ }
117+ timestamp = cached_timestamp_;
118+ points = cached_points_;
119+
120+ cached_timestamp_ = InvalidTimestamp;
121+
122+ return true;
123+ }
124+};
125+
126+
127+UrgLogHandler::UrgLogHandler(void) : pimpl(new pImpl)
128+{
129+}
130+
131+
132+UrgLogHandler::~UrgLogHandler(void)
133+{
134+}
135+
136+
137+bool UrgLogHandler::create(const char* file_name) {
138+
139+ pimpl->fout_.open(file_name);
140+
141+ return (! pimpl->fout_.is_open()) ? false : true;
142+}
143+
144+
145+void UrgLogHandler::addData(size_t timestamp,
146+ const vector<qrk::Point<long> >& points)
147+{
148+ pimpl->addData(timestamp, points);
149+}
150+
151+
152+void UrgLogHandler::close()
153+{
154+ pimpl->fout_.close();
155+}
156+
157+
158+bool UrgLogHandler::load(const char* file_name)
159+{
160+ return pimpl->load(file_name);
161+}
162+
163+
164+int UrgLogHandler::getLastTimestamp(void)
165+{
166+ return pimpl->last_timestamp_;
167+}
168+
169+
170+bool UrgLogHandler::getData(size_t& timestamp,
171+ vector<qrk::Point<long> >& points,
172+ size_t current_timestamp)
173+{
174+ return pimpl->getData(timestamp, points, current_timestamp);
175+}
Added: svn:keywords
## -0,0 +1 ##
+Id
\ No newline at end of property
--- trunk/programs/Moving3D/Draw3DWidget.h (nonexistent)
+++ trunk/programs/Moving3D/Draw3DWidget.h (revision 1970)
@@ -0,0 +1,72 @@
1+#ifndef DRAW_3D_WIDGET_H
2+#define DRAW_3D_WIDGET_H
3+
4+/*!
5+ \file
6+ \brief URG データの表示ウィジット
7+
8+ \author Satofumi KAMIMURA
9+
10+ $Id$
11+*/
12+
13+#include "Point3d.h"
14+#include "Point.h"
15+#include <QGLWidget>
16+#include <memory>
17+
18+
19+class Draw3DWidget : public QGLWidget {
20+ Q_OBJECT;
21+
22+ Draw3DWidget(const Draw3DWidget& rhs);
23+ Draw3DWidget& operator = (const Draw3DWidget& rhs);
24+
25+ struct pImpl;
26+ std::auto_ptr<pImpl> pimpl;
27+
28+ protected:
29+ void initializeGL(void);
30+ void resizeGL(int width, int height);
31+ void paintGL(void);
32+ void mousePressEvent(QMouseEvent *event);
33+ void mouseMoveEvent(QMouseEvent *event);
34+
35+public:
36+ Draw3DWidget(QGLWidget* parent = 0);
37+ ~Draw3DWidget(void);
38+
39+ //! 再描画
40+ void redraw(size_t timestamp, const qrk::Point3d<int>& draw_position);
41+
42+ void setDrawLineData(const std::vector<qrk::Point<long> >& draw_points, int h);
43+
44+ //! 表示データのクリア
45+ void clear(void);
46+
47+ //! 表示データの追加
48+ void addData(size_t timestamp,
49+ const std::vector<qrk::Point3d<int> >& points);
50+
51+ void addAdditionalData(size_t timestamp,
52+ const std::vector<qrk::Point3d<int> >& points);
53+
54+ //! データを表示する期間の設定
55+ void setDrawPeriod(size_t msec);
56+
57+ //! データ表示角度の指定
58+ void setViewRotation(const qrk::Point3d<int>& rotation);
59+ void viewUp(void);
60+ void viewDown(void);
61+ void viewLeft(void);
62+ void viewRight(void);
63+ qrk::Point3d<int> viewRotation(void);
64+
65+ //! 拡大率の指定
66+ void setViewZoom(const double ratio);
67+ void zoomIn(void);
68+ void zoomOut(void);
69+ double zoomRatio(void);
70+};
71+
72+#endif /* !DRAW_3D_WIDGET_H */
Added: svn:keywords
## -0,0 +1 ##
+Id
\ No newline at end of property
旧リポジトリブラウザで表示