• R/O
  • SSH
  • HTTPS

qrobosdk: コミット


コミットメタ情報

リビジョン248 (tree)
日時2008-10-01 21:12:49
作者satofumi

ログメッセージ

draw grid

変更サマリ

差分

--- trunk/libs/range_sensor/convert2D.cpp (revision 247)
+++ trunk/libs/range_sensor/convert2D.cpp (revision 248)
@@ -19,7 +19,7 @@
1919 int min_distance = sensor->minDistance();
2020 int max_distance = sensor->maxDistance();
2121
22- double offset_radian = -offset.to_rad();
22+ double offset_radian = offset.to_rad();
2323
2424 int index = 0;
2525 for (std::vector<long>::const_iterator it = data.begin();
--- trunk/widgets/UrgDrawWidget/UrgDrawWidget.cpp (revision 247)
+++ trunk/widgets/UrgDrawWidget/UrgDrawWidget.cpp (revision 248)
@@ -6,20 +6,17 @@
66
77 $Id$
88
9- \todo URG データを描画
10- \todo ホイールによる拡大縮小機能を付加
11- \todo ドラッグによる注視点の変更機能を付加
12- \todo リターンキーによる、始点の初期化機能を付加
13- \todo 強度データの表示コンボボックスを付加
14- \todo データの表示方法を複数追加
15- \todo グリッドを描画
16- \todo グリッドに数値と単位系を描画
9+ \todo 強度データ用の表示切替えコンボボックスを付加
10+ \todo ポリゴン形式での表示方法を実装する
11+ \todo グリッドに数値と単位系を描画。画面左端と、画面下
12+ \todo 描画する色を変更できるようにする
1713 */
1814
1915 #include "UrgDrawWidget.h"
2016 #include "RangeSensor.h"
2117 #include "convert2D.h"
22-#include "getTicks.h"
18+#include "MathUtils.h"
19+#include <QMouseEvent>
2320 #include <deque>
2421
2522 using namespace qrk;
@@ -27,7 +24,7 @@
2724
2825 namespace
2926 {
30- const double DefaultPixelPerMm = 10.0;
27+ const double DefaultPixelPerMm = 20.0;
3128
3229 struct DrawData
3330 {
@@ -49,11 +46,13 @@
4946 enum {
5047 MinimumWidth = 100,
5148 MinimumHeight = 100,
49+
50+ GridLength = 30 * 1000, // [mm]
5251 };
5352
5453 UrgDrawWidget* parent_;
5554
56- Grid<long> view_center_;
55+ Grid<double> view_center_;
5756 double pixel_per_mm_;
5857 DrawMode draw_mode_;
5958 size_t draw_period_;
@@ -60,11 +59,24 @@
6059
6160 typedef std::deque<DrawData> DataArray;
6261 DataArray draw_data_;
62+ int last_timestamp_;
63+ int current_timestamp_;
64+ size_t width_;
65+ size_t height_;
6366
67+ QColor clear_color_;
68+ Position<long> rotate_offset_;
69+ QPoint clicked_position_;
70+ bool now_pressed_;
6471
72+
6573 pImpl(UrgDrawWidget* parent)
6674 : parent_(parent),
67- pixel_per_mm_(DefaultPixelPerMm), draw_mode_(Lines), draw_period_(0)
75+ pixel_per_mm_(DefaultPixelPerMm), draw_mode_(Lines), draw_period_(0),
76+ last_timestamp_(0), current_timestamp_(0),
77+ width_(MinimumWidth), height_(MinimumHeight),
78+ clear_color_(Qt::white), rotate_offset_(Position<long>(0, 0, deg(90))),
79+ now_pressed_(false)
6880 {
6981 }
7082
@@ -73,6 +85,8 @@
7385 {
7486 parent_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
7587 parent_->setMinimumSize(MinimumWidth, MinimumHeight);
88+
89+ parent_->setMouseTracking(true);
7690 }
7791
7892
@@ -93,12 +107,243 @@
93107 }
94108
95109
96- void redraw(int timestamp)
110+ void initializeGL(void)
97111 {
98- // データ描画
99- fprintf(stderr, "%d\n", draw_data_.size());
100- // !!!
112+ parent_->qglClearColor(clear_color_);
113+ glEnable(GL_CULL_FACE);
114+ glEnable(GL_TEXTURE_2D);
115+ glEnable(GL_BLEND);
116+ glDisable(GL_DEPTH_TEST);
117+
118+ glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
101119 }
120+
121+
122+ void resizeGL(int width, int height)
123+ {
124+ glViewport(0, 0, width, height);
125+
126+ glMatrixMode(GL_PROJECTION);
127+ glLoadIdentity();
128+
129+ double aspect = 1.0 * width / height;
130+ glOrtho(-1.0 * aspect, +1.0 * aspect, -1.0, +1.0,
131+ std::numeric_limits<int>::min(), std::numeric_limits<int>::max());
132+
133+ glMatrixMode(GL_MODELVIEW);
134+ width_ = width;
135+ height_ = height;
136+ }
137+
138+
139+ void paintGL(void)
140+ {
141+ parent_->qglClearColor(clear_color_);
142+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
143+ glLoadIdentity();
144+
145+ // 軸の描画
146+ drawAxis();
147+
148+ // データの描画
149+ drawData();
150+ }
151+
152+
153+ void drawAxis(void)
154+ {
155+ double zoom_ratio = zoomRatio();
156+ Grid<double> offset = drawOffset(zoom_ratio);
157+
158+ // 補助線の描画
159+ drawSubAxis(offset);
160+
161+ glColor3d(0.3, 0.3, 0.3);
162+ glBegin(GL_LINES);
163+
164+ // Y 軸
165+ glVertex2d(-offset.x, -1.0);
166+ glVertex2d(-offset.x, +1.0);
167+
168+ // X 軸
169+ double aspect = 1.0 * width_ / height_;
170+ glVertex2d(-aspect, -offset.y);
171+ glVertex2d(+aspect, -offset.y);
172+
173+ // 背後 45 [deg] の補助線
174+ glVertex2d(-offset.x, -offset.y);
175+ glVertex2d((-GridLength * zoom_ratio) - offset.x,
176+ (-GridLength * zoom_ratio) - offset.y);
177+ glVertex2d(-offset.x, -offset.y);
178+ glVertex2d((+GridLength * zoom_ratio) - offset.x,
179+ (-GridLength * zoom_ratio) - offset.y);
180+ glEnd();
181+
182+ // 円の補助線を描画
183+ for (int i = 0; i <= GridLength; i += 1000) {
184+ drawSubCircle(i, offset);
185+ }
186+ }
187+
188+
189+ void drawSubCircle(int radius, const Grid<double>& offset)
190+ {
191+ double pixel_radius = (radius / pixel_per_mm_) / (height_ / 2.0);
192+
193+ glBegin(GL_LINE_STRIP);
194+ for (int theta = -45; theta <= (180 + 45); theta += 2) {
195+
196+ double radian = M_PI * theta / 180.0;
197+ double x = pixel_radius * cos(radian);
198+ double y = pixel_radius * sin(radian);
199+ glVertex2d(x - offset.x, y - offset.y);
200+ }
201+ glEnd();
202+ }
203+
204+
205+ void drawSubAxis(const Grid<double>& offset)
206+ {
207+ Grid<int> center(width_ / 2, height_ / 2);
208+ Grid<long> first(static_cast<int>((-center.x * pixel_per_mm_)
209+ - view_center_.x),
210+ static_cast<int>((-center.y * pixel_per_mm_)
211+ - view_center_.y));
212+ Grid<long> last(static_cast<int>((+center.x * pixel_per_mm_)
213+ - view_center_.x),
214+ static_cast<int>((+center.y * pixel_per_mm_)
215+ - view_center_.y));
216+
217+ const double threshold[] = { 4.0, 16.0, 32.0 };
218+ const int interval[] = { 10, 100, 1000 };
219+ const double color[] = { 0.9, 0.6, 0.3 };
220+ size_t n = sizeof(threshold) / sizeof(threshold[0]);
221+ for (size_t i = 0; i < n; ++i) {
222+ const double draw_threshold = threshold[i];
223+ if (pixel_per_mm_ > draw_threshold) {
224+ continue;
225+ }
226+ double alpha = 1.0 - (pixel_per_mm_ / draw_threshold);
227+ glColor4d(color[i], color[i], color[i], alpha);
228+ drawSubGridLine(interval[i], offset, first, last);
229+ }
230+ }
231+
232+
233+ void drawSubGridLine(int interval, const Grid<double>& offset,
234+ const Grid<long>& first, const Grid<long>& last)
235+ {
236+ glBegin(GL_LINES);
237+ for (int x = (first.x / interval) - 1; x < ((last.x / interval) + 1); ++x) {
238+ double draw_x = (interval * x / pixel_per_mm_) / (height_ / 2.0);
239+ glVertex2d(draw_x - offset.x, -1.0);
240+ glVertex2d(draw_x - offset.x, +1.0);
241+ }
242+
243+ double aspect = 1.0 * width_ / height_;
244+ for (int y = (first.y / interval) - 1; y < ((last.y / interval) + 1); ++y) {
245+ double draw_y = (interval * y / pixel_per_mm_) / (height_ / 2.0);
246+ glVertex2d(-aspect, draw_y - offset.y);
247+ glVertex2d(+aspect, draw_y - offset.y);
248+ }
249+ glEnd();
250+ }
251+
252+
253+ void drawData(void)
254+ {
255+ // !!! 時間が経過するほど、薄い色で表示するようにする
256+
257+ double zoom_ratio = zoomRatio();
258+ Grid<double> offset = drawOffset(zoom_ratio);
259+
260+
261+ if (draw_mode_ == Lines) {
262+ // 中心から測定点への直線を描画
263+ glColor3d(0.0, 0.0, 1.0);
264+ glPointSize(1.0);
265+
266+ glBegin(GL_LINES);
267+ for (DataArray::iterator line_it = draw_data_.begin();
268+ line_it != draw_data_.end(); ++line_it) {
269+
270+ std::vector<Grid<long> >& line_data = draw_data_.front().point_data;;
271+ std::vector<Grid<long> >::iterator end_it = line_data.end();
272+ for (std::vector<Grid<long> >::iterator it = line_data.begin();
273+ it != end_it; ++it) {
274+
275+ double x = it->x * zoom_ratio;
276+ double y = it->y * zoom_ratio;
277+ glVertex2d(-offset.x, -offset.y);
278+ glVertex2d(x - offset.x, y - offset.y);
279+ }
280+ }
281+ glEnd();
282+
283+ } else if (draw_mode_ == Polygon) {
284+ // !!!
285+ // !!! これを実現するには、convert2D 時に捨ててしまった情報が必要
286+ // !!! 実装方法を見直すべき
287+ // !!!
288+ }
289+
290+ // 測定点の描画
291+ glColor3d(1.0, 0.0, 0.0);
292+ double mm_pixel = 1.0 / pixel_per_mm_;
293+ if (mm_pixel < 1.0) {
294+ mm_pixel = 1.0;
295+ }
296+ glPointSize(mm_pixel);
297+
298+ glBegin(GL_POINTS);
299+ for (DataArray::iterator line_it = draw_data_.begin();
300+ line_it != draw_data_.end(); ++line_it) {
301+
302+ std::vector<Grid<long> >::iterator end_it = line_it->point_data.end();
303+ for (std::vector<Grid<long> >::iterator it = line_it->point_data.begin();
304+ it != end_it; ++it) {
305+
306+ double x = it->x * zoom_ratio;
307+ double y = it->y * zoom_ratio;
308+ glVertex2d(x - offset.x, y - offset.y);
309+ }
310+ }
311+ glEnd();
312+ }
313+
314+
315+ // [mm] -> [pixel]
316+ double zoomRatio(void)
317+ {
318+ return (1.0 / pixel_per_mm_ / (height_ / 2.0));
319+ }
320+
321+
322+ Grid<double> drawOffset(double zoom_ratio)
323+ {
324+ return Grid<double>(-view_center_.x * zoom_ratio,
325+ -view_center_.y * zoom_ratio);
326+ }
327+
328+
329+ void setClickedPosition(QMouseEvent* event)
330+ {
331+ clicked_position_ = event->pos();
332+ }
333+
334+
335+ void updateZoomRatio(int steps)
336+ {
337+ double zoom = parent_->zoomRatio();
338+ zoom *= pow(1.1, steps);
339+
340+ if (zoom > 500.0) {
341+ zoom = 500.0;
342+ } else if (zoom < 0.05) {
343+ zoom = 0.05;
344+ }
345+ parent_->setZoomRatio(zoom);
346+ }
102347 };
103348
104349
@@ -114,10 +359,26 @@
114359 }
115360
116361
362+void UrgDrawWidget::clear(void)
363+{
364+ pimpl->draw_data_.clear();
365+ redraw();
366+}
367+
368+
369+void UrgDrawWidget::initializeView(void)
370+{
371+ pimpl->pixel_per_mm_ = DefaultPixelPerMm;
372+ pimpl->view_center_ = Grid<double>(0.0, 0.0);
373+ redraw();
374+}
375+
376+
117377 void UrgDrawWidget::redraw(int timestamp)
118378 {
119- int actual_timestamp = (timestamp == NoneSpecified) ? getTicks() : timestamp;
120- pimpl->redraw(actual_timestamp);
379+ pimpl->current_timestamp_ =
380+ (timestamp == NoneSpecified) ? pimpl->last_timestamp_ : timestamp;
381+ updateGL();
121382 }
122383
123384
@@ -131,11 +392,13 @@
131392 }
132393
133394 std::vector<Grid<long> > point_data;
134- qrk::convert2D(point_data, sensor, data);
395+ qrk::convert2D(point_data, sensor, data, pimpl->rotate_offset_);
135396
397+ // !!! 以降の処理は、繰り返されているので、まとめるべき
136398 if (! point_data.empty()) {
137399 pimpl->removeOldData(timestamp);
138400 pimpl->draw_data_.push_back(DrawData(point_data, timestamp));
401+ pimpl->last_timestamp_ = timestamp;
139402 }
140403 }
141404
@@ -144,11 +407,12 @@
144407 const RangeSensor* sensor, int timestamp)
145408 {
146409 std::vector<Grid<long> > point_data;
147- qrk::convert2D(point_data, sensor, data);
410+ qrk::convert2D(point_data, sensor, data, pimpl->rotate_offset_);
148411
149412 if (! point_data.empty()) {
150413 pimpl->removeOldData(timestamp);
151414 pimpl->draw_data_.push_back(DrawData(point_data, timestamp));
415+ pimpl->last_timestamp_ = timestamp;
152416 }
153417 }
154418
@@ -158,6 +422,7 @@
158422 if (! data.empty()) {
159423 pimpl->removeOldData(timestamp);
160424 pimpl->draw_data_.push_back(DrawData(data, timestamp));
425+ pimpl->last_timestamp_ = timestamp;
161426 }
162427 }
163428
@@ -177,6 +442,7 @@
177442 void UrgDrawWidget::setZoomRatio(double pixel_per_mm)
178443 {
179444 pimpl->pixel_per_mm_ = pixel_per_mm;
445+ redraw();
180446 }
181447
182448
@@ -188,41 +454,89 @@
188454
189455 void UrgDrawWidget::setViewCenter(const Grid<long>& point)
190456 {
191- pimpl->view_center_ = point;
457+ pimpl->view_center_.x = point.x;
458+ pimpl->view_center_.y = point.y;
192459 }
193460
194461
195462 Grid<long> UrgDrawWidget::viewCenter(void)
196463 {
197- return pimpl->view_center_;
464+ return Grid<long>(static_cast<long>(pimpl->view_center_.x),
465+ static_cast<long>(pimpl->view_center_.y));
198466 }
199467
200468
201469 void UrgDrawWidget::initializeGL(void)
202470 {
203- // !!!
471+ pimpl->initializeGL();
204472 }
205473
206474
207475 void UrgDrawWidget::resizeGL(int width, int height)
208476 {
209- // !!!
477+ pimpl->resizeGL(width, height);
210478 }
211479
212480
213481 void UrgDrawWidget::paintGL(void)
214482 {
215- // !!!
483+ pimpl->paintGL();
216484 }
217485
218486
219-void UrgDrawWidget::mousePressEvent(QMouseEvent *event)
487+void UrgDrawWidget::mousePressEvent(QMouseEvent* event)
220488 {
221- // !!!
489+ pimpl->now_pressed_ = true;
490+ pimpl->setClickedPosition(event);
222491 }
223492
224493
225-void UrgDrawWidget::mouseMoveEvent(QMouseEvent *event)
494+void UrgDrawWidget::mouseMoveEvent(QMouseEvent* event)
226495 {
227- // !!!
496+ int x = event->x();
497+ int y = event->y();
498+
499+ if (pimpl->now_pressed_) {
500+ int dx = x - pimpl->clicked_position_.x();
501+ int dy = y - pimpl->clicked_position_.y();
502+
503+ pimpl->view_center_.x += dx * pimpl->pixel_per_mm_;
504+ pimpl->view_center_.y -= dy * pimpl->pixel_per_mm_;
505+
506+ pimpl->setClickedPosition(event);
507+ }
508+
509+ // カーソル位置の座標をシグナルで送信する
510+ if ((x < 0) || (x >= static_cast<int>(pimpl->width_)) ||
511+ (y < 0) || (y >= static_cast<int>(pimpl->height_))) {
512+ emit position(false, -1, -1);
513+ return;
514+ }
515+
516+ int center_x = pimpl->width_ / 2;
517+ int center_y = pimpl->height_ / 2;
518+ int x_mm = static_cast<int>(((x - center_x) * pimpl->pixel_per_mm_)
519+ - pimpl->view_center_.x);
520+ int y_mm = static_cast<int>((-(y - center_y) * pimpl->pixel_per_mm_)
521+ - pimpl->view_center_.y);
522+ emit position(true, x_mm, y_mm);
228523 }
524+
525+
526+void UrgDrawWidget::mouseReleaseEvent(QMouseEvent* event)
527+{
528+ static_cast<void>(event);
529+
530+ pimpl->now_pressed_ = false;
531+ redraw();
532+}
533+
534+
535+void UrgDrawWidget::wheelEvent(QWheelEvent* event) {
536+
537+ int degrees = event->delta() / 8;
538+ int steps = degrees / 15;
539+
540+ event->accept();
541+ pimpl->updateZoomRatio(steps);
542+}
--- trunk/widgets/UrgDrawWidget/UrgDrawWidget.h (revision 247)
+++ trunk/widgets/UrgDrawWidget/UrgDrawWidget.h (revision 248)
@@ -21,6 +21,8 @@
2121
2222 class UrgDrawWidget : public QGLWidget
2323 {
24+ Q_OBJECT;
25+
2426 UrgDrawWidget(const UrgDrawWidget& rhs);
2527 UrgDrawWidget& operator = (const UrgDrawWidget& rhs);
2628
@@ -31,8 +33,10 @@
3133 void initializeGL(void);
3234 void resizeGL(int width, int height);
3335 void paintGL(void);
34- void mousePressEvent(QMouseEvent *event);
35- void mouseMoveEvent(QMouseEvent *event);
36+ void mousePressEvent(QMouseEvent* event);
37+ void mouseMoveEvent(QMouseEvent* event);
38+ void mouseReleaseEvent(QMouseEvent* event);
39+ void wheelEvent(QWheelEvent* event);
3640
3741 public:
3842 typedef enum {
@@ -48,6 +52,9 @@
4852 UrgDrawWidget(QWidget* parent = 0);
4953 ~UrgDrawWidget(void);
5054
55+ void clear(void);
56+ void initializeView(void);
57+
5158 void redraw(int timestamp = NoneSpecified);
5259
5360 void setUrgData(qrk::RangeSensor* sensor);
@@ -64,6 +71,9 @@
6471
6572 void setViewCenter(const qrk::Grid<long>& point);
6673 qrk::Grid<long> viewCenter(void);
74+
75+signals:
76+ void position(bool active, long x_mm, long y_mm);
6777 };
6878
6979 #endif /* !URG_DRAW_WIDGET_H */
--- trunk/widgets/UrgDrawWidget/UrgViewerWindow.cpp (revision 247)
+++ trunk/widgets/UrgDrawWidget/UrgViewerWindow.cpp (revision 248)
@@ -6,7 +6,6 @@
66
77 $Id$
88
9- \todo 再接続が繰り返し行えるように調整する
109 \todo Qt 版の system ライブラリが sdl 版と互換になるように調整する
1110 */
1211
@@ -16,10 +15,20 @@
1615 #include "mUrgCtrl.h"
1716 #include "getTicks.h"
1817 #include <QTimer>
18+#include <QShortcut>
19+#include <QMessageBox>
20+#include <QSettings>
1921
2022 using namespace qrk;
2123
2224
25+namespace
26+{
27+ const char* Organization = "Hokuyo LTD.";
28+ const char* Application = "URG Viewer";
29+};
30+
31+
2332 struct UrgViewerWindow::pImpl
2433 {
2534 UrgViewerWindow* parent_;
@@ -49,6 +58,8 @@
4958 // メニュー項目
5059 connect(parent_->action_quit_, SIGNAL(triggered()),
5160 parent_, SLOT(close()));
61+ connect(parent_->action_about_, SIGNAL(triggered()),
62+ parent_, SLOT(aboutApplication()));
5263
5364 // URG デバイス
5465 connect(&connect_widget_, SIGNAL(connectEvent()),
@@ -57,18 +68,29 @@
5768 parent_, SLOT(disconnectDevice()));
5869 connect(&capture_timer_, SIGNAL(timeout()),
5970 parent_, SLOT(captureHandler()));
71+
72+ // ステータスバー表示
73+ connect(&urg_draw_widget_, SIGNAL(position(bool, long, long)),
74+ parent_, SLOT(updateStatusBar(bool, long, long)));
75+
76+ // キー割り付け
77+ (void) new QShortcut(Qt::Key_Return, parent_, SLOT(initializeView()));
6078 }
6179
6280
6381 void saveSettings(void)
6482 {
65- // !!!
83+ QSettings settings(Organization, Application);
84+
85+ settings.setValue("geometry", parent_->saveGeometry());
6686 }
6787
6888
6989 void loadSettings(void)
7090 {
71- // !!!
91+ QSettings settings(Organization, Application);
92+
93+ parent_->restoreGeometry(settings.value("geometry").toByteArray());
7294 }
7395
7496
@@ -88,6 +110,7 @@
88110 {
89111 // データの取得停止
90112 capture_timer_.stop();
113+ urg_draw_widget_.clear();
91114 }
92115
93116
@@ -109,11 +132,13 @@
109132 {
110133 setupUi(this);
111134 pimpl->initializeForm();
135+ pimpl->loadSettings();
112136 }
113137
114138
115139 UrgViewerWindow::~UrgViewerWindow(void)
116140 {
141+ pimpl->saveSettings();
117142 }
118143
119144
@@ -133,3 +158,31 @@
133158 {
134159 pimpl->captureHandler();
135160 }
161+
162+
163+void UrgViewerWindow::initializeView(void)
164+{
165+ pimpl->urg_draw_widget_.initializeView();
166+}
167+
168+
169+void UrgViewerWindow::updateStatusBar(bool active, long x_mm, long y_mm)
170+{
171+ if (! active) {
172+ statusBar()->clearMessage();
173+ return;
174+ }
175+
176+ QString message = QString("X: %1 [mm], Y: %2 [mm]").arg(x_mm).arg(y_mm);
177+ statusBar()->showMessage(message, 60 * 1000);
178+}
179+
180+
181+void UrgViewerWindow::aboutApplication(void)
182+{
183+ QMessageBox::about(this, tr("About URG Viewer"),
184+ tr("<h2>URG Viewer ($Rev$)</h2>"
185+ "<p>Demo Application for URG sensor</p>"
186+ "<p>Report bugs to "
187+ "&lt;s-kamimura@hokuyo-aut.co.jp&gt;</p>"));
188+}
--- trunk/widgets/UrgDrawWidget/UrgViewerWindow.h (revision 247)
+++ trunk/widgets/UrgDrawWidget/UrgViewerWindow.h (revision 248)
@@ -27,6 +27,9 @@
2727 void connectDevice(void);
2828 void disconnectDevice(void);
2929 void captureHandler(void);
30+ void initializeView(void);
31+ void updateStatusBar(bool active, long x_mm, long y_mm);
32+ void aboutApplication(void);
3033
3134 public:
3235 UrgViewerWindow(int argc, char *argv[]);
旧リポジトリブラウザで表示