00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include "laser_drawing_area.h"
00024 #include "visdisplay.h"
00025 #include <interfaces/Laser720Interface.h>
00026 #include <interfaces/Laser360Interface.h>
00027 #include <interfaces/ObjectPositionInterface.h>
00028 #include <interfaces/VisualDisplay2DInterface.h>
00029 #include <utils/math/angle.h>
00030 #include <gui_utils/robot/drawer.h>
00031 #include <algorithm>
00032 #include <utils/misc/string_conversions.h>
00033
00034
00035 #define CFG_PRINT_NR_TRACKELEMENTS 5
00036
00037 using namespace fawkes;
00038
00039
00040
00041
00042
00043
00044
00045 #ifdef HAVE_GLADEMM
00046
00047
00048
00049
00050
00051 LaserDrawingArea::LaserDrawingArea(BaseObjectType* cobject,
00052 const Glib::RefPtr<Gnome::Glade::Xml>& refxml)
00053 : Gtk::DrawingArea(cobject)
00054 {
00055 __draw_mode = MODE_LINES;
00056 __zoom_factor = 50;
00057 __laser360_if = NULL;
00058 __laser720_if = NULL;
00059 __l_objpos_if_persons = NULL;
00060 __l_objpos_if_legs = NULL;
00061 __l_objpos_if_misc = NULL;
00062 __laser_segmentation_if = NULL;
00063 __l_track_if = NULL;
00064 __target_if = NULL;
00065 __switch_if = NULL;
00066 __line_if = NULL;
00067 __visdisp_if = NULL;
00068 __robot_drawer = NULL;
00069 __resolution = 1;
00070 __rotation = 0;
00071 __break_drawing = false;
00072 __first_draw = true;
00073
00074 __visdisp = new VisualDisplay2D();
00075
00076 add_events(Gdk::SCROLL_MASK | Gdk::BUTTON_MOTION_MASK |
00077 Gdk::BUTTON_PRESS_MASK );
00078
00079 #ifndef GLIBMM_DEFAULT_SIGNAL_HANDLERS_ENABLED
00080 signal_expose_event().connect(sigc::mem_fun(*this, &LaserDrawingArea::on_expose_event));
00081 signal_button_press_event().connect(sigc::mem_fun(*this, &LaserDrawingArea::on_button_press_event));
00082 signal_motion_notify_event().connect(sigc::mem_fun(*this, &LaserDrawingArea::on_motion_notify_event));
00083 #endif
00084
00085
00086 }
00087 #endif
00088
00089
00090 LaserDrawingArea::LaserDrawingArea()
00091 {
00092 __draw_mode = MODE_LINES;
00093 __zoom_factor = 50;
00094 __laser360_if = NULL;
00095 __laser720_if = NULL;
00096 __l_objpos_if_persons = NULL;
00097 __l_objpos_if_legs = NULL;
00098 __l_objpos_if_misc = NULL;
00099 __laser_segmentation_if = NULL;
00100 __l_track_if = NULL;
00101 __target_if = NULL;
00102 __switch_if = NULL;
00103 __line_if = NULL;
00104 __visdisp_if = NULL;
00105 __robot_drawer = NULL;
00106 __resolution = 1;
00107 __rotation = 0;
00108 __break_drawing = false;
00109
00110 __visdisp = new VisualDisplay2D();
00111
00112 add_events(Gdk::SCROLL_MASK | Gdk::BUTTON_MOTION_MASK);
00113
00114 #ifndef GLIBMM_DEFAULT_SIGNAL_HANDLERS_ENABLED
00115 signal_expose_event().connect(sigc::mem_fun(*this, &LaserDrawingArea::on_expose_event));
00116 signal_button_press_event().connect(sigc::mem_fun(*this, &LaserDrawingArea::on_button_press_event));
00117 signal_motion_notify_event().connect(sigc::mem_fun(*this, &LaserDrawingArea::on_motion_notify_event));
00118 #endif
00119 }
00120
00121
00122
00123 LaserDrawingArea::~LaserDrawingArea()
00124 {
00125 delete __visdisp;
00126 }
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137 void
00138 LaserDrawingArea::set_objpos_if(std::list<fawkes::ObjectPositionInterface*>* l_objpos_if_persons,
00139 std::list<fawkes::ObjectPositionInterface*>* l_objpos_if_legs,
00140 std::list<fawkes::ObjectPositionInterface*>* l_objpos_if_misc,
00141 fawkes::Laser720Interface* laser_segmentation_if ,
00142 std::list<fawkes::Position2DTrackInterface*>* l_track_if,
00143 fawkes::ObjectPositionInterface* target_if,
00144 fawkes::SwitchInterface* switch_if){
00145 __l_objpos_if_persons = l_objpos_if_persons;
00146 __l_objpos_if_legs = l_objpos_if_legs;
00147 __l_objpos_if_misc = l_objpos_if_misc;
00148 __laser_segmentation_if=laser_segmentation_if;
00149 __l_track_if = l_track_if;
00150 __target_if = target_if;
00151 __switch_if = switch_if;
00152 }
00153
00154
00155
00156
00157
00158
00159 void
00160 LaserDrawingArea::set_laser360_if(Laser360Interface *laser_if)
00161 {
00162 __laser360_if = laser_if;
00163 __laser720_if = NULL;
00164 }
00165
00166
00167
00168
00169
00170 void
00171 LaserDrawingArea::set_laser720_if(Laser720Interface *laser_if)
00172 {
00173 __laser720_if = laser_if;
00174 __laser360_if = NULL;
00175 }
00176
00177
00178
00179 void
00180 LaserDrawingArea::reset_laser_ifs()
00181 {
00182 __laser360_if = NULL;
00183 __laser720_if = NULL;
00184 __l_objpos_if_persons = NULL;
00185 __l_objpos_if_legs = NULL;
00186 __l_objpos_if_misc = NULL;
00187 __laser_segmentation_if = NULL;
00188 __l_track_if = NULL;
00189 __target_if = NULL;
00190 __switch_if = NULL;
00191
00192 Gtk::Allocation allocation = get_allocation();
00193 const int width = allocation.get_width();
00194 const int height = allocation.get_height();
00195
00196 __xc = width / 2;
00197 __yc = height / 2;
00198 __zoom_factor = 50;
00199
00200 }
00201
00202
00203
00204
00205 void
00206 LaserDrawingArea::set_line_if(ObjectPositionInterface *line_if)
00207 {
00208 __line_if = line_if;
00209 }
00210
00211
00212
00213
00214
00215 void
00216 LaserDrawingArea::set_visdisp_if(VisualDisplay2DInterface *visdisp_if)
00217 {
00218 __visdisp_if = visdisp_if;
00219 __visdisp->set_interface(__visdisp_if);
00220 }
00221
00222
00223
00224
00225
00226 void
00227 LaserDrawingArea::set_robot_drawer(fawkes::CairoRobotDrawer *robot_drawer)
00228 {
00229 __robot_drawer = robot_drawer;
00230 }
00231
00232
00233
00234
00235
00236 void
00237 LaserDrawingArea::set_resolution(unsigned int resolution)
00238 {
00239 __resolution = resolution;
00240 }
00241
00242
00243
00244
00245
00246 void
00247 LaserDrawingArea::set_draw_mode(draw_mode_t mode)
00248 {
00249 __draw_mode = mode;
00250 queue_draw();
00251 }
00252
00253
00254
00255
00256 void
00257 LaserDrawingArea::zoom_in()
00258 {
00259 __zoom_factor += 20;
00260 queue_draw();
00261 }
00262
00263
00264
00265
00266 void
00267 LaserDrawingArea::zoom_out()
00268 {
00269 if ( __zoom_factor > 20 ) {
00270 __zoom_factor -= 20;
00271 } else {
00272 __zoom_factor = 1;
00273 }
00274 queue_draw();
00275 }
00276
00277
00278
00279
00280
00281 void
00282 LaserDrawingArea::set_rotation(float rot_rad)
00283 {
00284 __rotation = rot_rad;
00285 }
00286
00287
00288
00289
00290
00291
00292 bool
00293 LaserDrawingArea::on_expose_event(GdkEventExpose* event)
00294 {
00295
00296 Glib::RefPtr<Gdk::Window> window = get_window();
00297 if(window) {
00298 Gtk::Allocation allocation = get_allocation();
00299
00300 if(__first_draw)
00301 {
00302 __first_draw = false;
00303 const int width = allocation.get_width();
00304 const int height = allocation.get_height();
00305
00306
00307 __xc = width / 2;
00308 __yc = height / 2;
00309 }
00310 Cairo::RefPtr<Cairo::Context> cr = window->create_cairo_context();
00311 cr->set_line_width(1.0);
00312
00313
00314
00315 cr->rectangle(event->area.x, event->area.y,
00316 event->area.width, event->area.height);
00317 cr->set_source_rgb(1, 1, 1);
00318 cr->fill_preserve();
00319 cr->clip();
00320 cr->set_source_rgb(0, 0, 0);
00321
00322
00323
00324
00325 cr->translate(__xc, __yc);
00326
00327 cr->save();
00328 if ( (__laser360_if == NULL) && (__laser720_if == NULL) ) {
00329 Cairo::TextExtents te;
00330 std::string t = "Not connected to BlackBoard";
00331 cr->set_source_rgb(1, 0, 0);
00332 cr->set_font_size(20);
00333 cr->get_text_extents(t, te);
00334 cr->move_to(- te.width / 2, -te.height / 2);
00335 cr->show_text(t);
00336 } else if ( (__laser360_if && ! __laser360_if->has_writer()) ||
00337 (__laser720_if && ! __laser720_if->has_writer()) ) {
00338 Cairo::TextExtents te;
00339 std::string t = "No writer for 360° laser interface";
00340 if (__laser720_if) t = "No writer for 720° laser interface";
00341 cr->set_source_rgb(1, 0, 0);
00342 cr->set_font_size(20);
00343 cr->get_text_extents(t, te);
00344 cr->move_to(- te.width / 2, -te.height / 2);
00345 cr->show_text(t);
00346 } else {
00347
00348 if (! __break_drawing) {
00349 if (__laser360_if) __laser360_if->read();
00350 if (__laser720_if) __laser720_if->read();
00351 }
00352
00353 draw_beams(window, cr);
00354 if (__robot_drawer) __robot_drawer->draw_robot(window, cr);
00355 draw_segments(window, cr);
00356 draw_persons_legs(window, cr);
00357
00358 if(__switch_if != NULL && __switch_if->has_writer()){
00359 SwitchInterface::EnableSwitchMessage *esm = new SwitchInterface::EnableSwitchMessage();
00360 __switch_if->msgq_enqueue(esm);
00361 }
00362 }
00363 cr->restore();
00364
00365 cr->save();
00366 cr->rotate(0.5 * M_PI + __rotation);
00367 cr->scale(-__zoom_factor, __zoom_factor);
00368 cr->set_line_width(1. / __zoom_factor);
00369 if (__visdisp_if) {
00370 __visdisp->process_messages();
00371 __visdisp->draw(cr);
00372 }
00373
00374 const float radius = 0.01;
00375 if (__line_if) {
00376 __line_if->read();
00377 if (__line_if->has_writer() &&
00378 __line_if->is_valid() && __line_if->is_visible()) {
00379
00380 cr->set_source_rgb(1, 0, 0);
00381
00382
00383
00384
00385
00386 cr->rectangle(__line_if->world_x() - radius * 0.5, __line_if->world_y() - radius * 0.5, radius, radius);
00387 cr->rectangle(__line_if->relative_x() - radius * 0.5, __line_if->relative_y() - radius * 0.5, radius, radius);
00388 cr->fill_preserve();
00389 cr->stroke();
00390 cr->move_to(__line_if->world_x(), __line_if->world_y());
00391 cr->line_to(__line_if->relative_x(), __line_if->relative_y());
00392 cr->stroke();
00393 }
00394 }
00395 cr->restore();
00396 }
00397
00398 return true;
00399 }
00400
00401
00402
00403
00404
00405
00406
00407
00408 void
00409 LaserDrawingArea::draw_scalebox(Glib::RefPtr<Gdk::Window> &window,
00410 Cairo::RefPtr<Cairo::Context> &cr)
00411 {
00412 cr->save();
00413 cr->set_source_rgba(0, 0, 0.8, 0.2);
00414 cr->arc(0, 0, 1.0, 0, 2 * M_PI);
00415 cr->stroke();
00416 cr->restore();
00417 }
00418
00419
00420
00421
00422
00423
00424
00425
00426 void
00427 LaserDrawingArea::draw_beams(Glib::RefPtr<Gdk::Window> &window,
00428 Cairo::RefPtr<Cairo::Context> &cr)
00429 {
00430 float *distances = __laser360_if ? __laser360_if->distances() : __laser720_if->distances();
00431 size_t nd = __laser360_if ? __laser360_if->maxlenof_distances() : __laser720_if->maxlenof_distances();
00432 const float nd_factor = 360.0 / nd;
00433
00434 bool clockwise = __laser360_if ? __laser360_if->is_clockwise_angle() : __laser720_if->is_clockwise_angle();
00435
00436 float *revdists = NULL;
00437 if (! clockwise) {
00438
00439 revdists = (float *)new float[nd];
00440 for (size_t i = 0; i < nd; ++i) {
00441 revdists[nd - i] = distances[i];
00442 }
00443 distances = revdists;
00444 }
00445
00446 cr->scale(__zoom_factor, __zoom_factor);
00447 cr->rotate(__rotation);
00448 cr->set_line_width(1. / __zoom_factor);
00449
00450 draw_scalebox(window, cr);
00451
00452 if ( __draw_mode == MODE_LINES ) {
00453 for (size_t i = 0; i < nd; i += __resolution) {
00454 if ( distances[i] == 0 ) continue;
00455 const float anglerad = deg2rad(i * nd_factor);
00456 cr->move_to(0, 0);
00457 cr->line_to(distances[i] * sin(anglerad),
00458 distances[i] * -cos(anglerad));
00459 }
00460 cr->stroke();
00461 } else if ( __draw_mode == MODE_POINTS ) {
00462 const float radius = 4 / __zoom_factor;
00463 for (size_t i = 0; i < nd; i += __resolution) {
00464 if ( distances[i] == 0 ) continue;
00465 float anglerad = deg2rad(i * nd_factor);
00466 float x = distances[i] * sin(anglerad);
00467 float y = distances[i] * -cos(anglerad);
00468
00469
00470
00471 cr->rectangle(x, y, radius, radius);
00472 }
00473 cr->fill_preserve();
00474 cr->stroke();
00475 } else {
00476 cr->move_to(0, - distances[0]);
00477 for (size_t i = __resolution; i <= nd + __resolution; i += __resolution) {
00478 if ( distances[i] == 0 ) continue;
00479 const float anglerad = normalize_rad(deg2rad(i * nd_factor));
00480 cr->line_to(distances[i % nd] * sin(anglerad),
00481 distances[i % nd] * -cos(anglerad));
00482 }
00483 cr->stroke();
00484 }
00485
00486 if (revdists) delete[] revdists;
00487 }
00488
00489
00490
00491
00492
00493
00494
00495
00496 void
00497 LaserDrawingArea::draw_persons_legs(Glib::RefPtr<Gdk::Window> &window,
00498 Cairo::RefPtr<Cairo::Context> &cr)
00499 {
00500 std::list<ObjectPositionInterface*>::iterator objpos_if_itt;;
00501
00502 cr->save();
00503 if (__l_objpos_if_persons) {
00504 cr->set_source_rgb(0,0,1);
00505 for( objpos_if_itt = __l_objpos_if_persons->begin();
00506 objpos_if_itt != __l_objpos_if_persons->end() && (*objpos_if_itt)->has_writer();
00507 objpos_if_itt++ ) {
00508 if(!__break_drawing)
00509 (*objpos_if_itt)->read();
00510 if ((*objpos_if_itt)->is_valid()){
00511 std::pair<float,float> pos = transform_coords_from_fawkes((*objpos_if_itt)->relative_x(), (*objpos_if_itt)->relative_y());
00512 float x=pos.first;
00513 float y=pos.second;
00514 cr->move_to(x, y);
00515
00516 cr->arc(x, y, 0.2, 0, 2*M_PI);
00517 }
00518 }
00519 cr->stroke();
00520 }
00521
00522 if (__l_objpos_if_legs) {
00523 cr->set_source_rgb(0,1,0);
00524 for( objpos_if_itt = __l_objpos_if_legs->begin();
00525 objpos_if_itt != __l_objpos_if_legs->end() && (*objpos_if_itt)->has_writer() ;
00526 objpos_if_itt++ ) {
00527 if(!__break_drawing)
00528 (*objpos_if_itt)->read();
00529 if ((*objpos_if_itt)->is_valid()){
00530 std::pair<float,float> pos = transform_coords_from_fawkes((*objpos_if_itt)->relative_x(), (*objpos_if_itt)->relative_y());
00531 float x=pos.first;
00532 float y=pos.second;
00533 cr->move_to(x, y);
00534 cr->arc(x, y, 0.1, 0, 2*M_PI);
00535 }
00536 }
00537 cr->stroke();
00538 }
00539
00540 if (__l_objpos_if_misc) {
00541 cr->set_source_rgb(0,1,1);
00542 for( objpos_if_itt = __l_objpos_if_misc->begin();
00543 objpos_if_itt != __l_objpos_if_misc->end() && (*objpos_if_itt)->has_writer() ;
00544 objpos_if_itt++ ) {
00545 if(!__break_drawing)
00546 (*objpos_if_itt)->read();
00547 if ((*objpos_if_itt)->is_valid()){
00548
00549
00550
00551 if((*objpos_if_itt)->object_type()==ObjectPositionInterface::TYPE_BALL){
00552 std::pair<float,float> pos = transform_coords_from_fawkes((*objpos_if_itt)->relative_x(), (*objpos_if_itt)->relative_y());
00553 float x=pos.first;
00554 float y=pos.second;
00555 pos = transform_coords_from_fawkes((*objpos_if_itt)->world_x(), (*objpos_if_itt)->world_y());
00556 float begin_x=pos.first;
00557 float begin_y=pos.second;
00558 pos = transform_coords_from_fawkes((*objpos_if_itt)->world_x_velocity(), (*objpos_if_itt)->world_y_velocity());
00559 float end_x= pos.first;
00560 float end_y= pos.first;
00561 float angle1=atan2(begin_y- y, begin_x - x);
00562 float angle2=atan2(end_y- y, end_x - x);
00563 float radius=(*objpos_if_itt)->relative_x_velocity();
00564 float probability = (*objpos_if_itt)->relative_z_velocity();
00565 cr->move_to(begin_x, begin_y);
00566 cr->arc(x, y, radius, angle2, angle1);
00567
00568
00569 std::string t = StringConversions::to_string(probability);
00570 t.erase(5);
00571
00572 cr->set_font_size(0.08);
00573
00574
00575 cr->move_to(begin_x, begin_y);
00576 cr->show_text(t);
00577
00578
00579
00580
00581 }else if((*objpos_if_itt)->object_type()==ObjectPositionInterface::TYPE_LINE){
00582 std::pair<float,float> pos = transform_coords_from_fawkes((*objpos_if_itt)->world_x(), (*objpos_if_itt)->world_y());
00583 float begin_x=pos.first;
00584 float begin_y=pos.second;
00585 pos = transform_coords_from_fawkes((*objpos_if_itt)->world_x_velocity(), (*objpos_if_itt)->world_y_velocity());
00586 float end_x= pos.first;
00587 float end_y= pos.first;
00588 cr->move_to(begin_x, begin_y);
00589 cr->line_to(end_x, end_y);
00590
00591 }
00592 }
00593 }
00594
00595 cr->stroke();
00596 }
00597
00598 cr->set_source_rgb(1,0,1);
00599
00600 float r,g,b;
00601 r=g=b=0.0;
00602 int color_it=0;
00603 float delta = 0.25;
00604
00605
00606 if (__l_track_if) {
00607
00608 std::list<Position2DTrackInterface*>::iterator track_if_itt;;
00609 const float radius (0.1);
00610 float* x_positions1;
00611 float* y_positions1;
00612 int* timestamps1;
00613 float* x_positions2 = NULL;
00614 float* y_positions2 = NULL;
00615 unsigned int track_length1 = 0;
00616 unsigned int track_length2 = 0;
00617 int* timestamps2 = NULL;
00618 unsigned int id;
00619 cr->set_font_size(0.03);
00620 #ifdef LASERGUI_DEBUG_PRINT_TRACKS
00621 printf("\n\n################################\n");
00622 #endif
00623 for( track_if_itt = __l_track_if->begin();
00624 track_if_itt != __l_track_if->end() && (*track_if_itt)->has_writer();) {
00625 bool b_compound_track(false);
00626 if(!__break_drawing)
00627 (*track_if_itt)->read();
00628 if ((*track_if_itt)->is_valid()){
00629 x_positions1=(*track_if_itt)->track_x_positions();
00630 y_positions1=(*track_if_itt)->track_y_positions();
00631 timestamps1=(*track_if_itt)->track_timestamps();
00632 track_length1 = (*track_if_itt)->length();
00633 id = (*track_if_itt)->track_id();
00634 ++track_if_itt;
00635 if( track_if_itt != __l_track_if->end() && (*track_if_itt)->has_writer()){
00636 if(!__break_drawing)
00637 (*track_if_itt)->read();
00638 if( (*track_if_itt)->is_valid() && (*track_if_itt)->track_id()==id ){
00639 b_compound_track = true;
00640 x_positions2=(*track_if_itt)->track_x_positions();
00641 y_positions2=(*track_if_itt)->track_y_positions();
00642 timestamps2=(*track_if_itt)->track_timestamps();
00643 track_length2 = (*track_if_itt)->length();
00644 ++track_if_itt;
00645 }
00646 }
00647 #ifdef LASERGUI_DEBUG_PRINT_TRACKS
00648 printf("\n trackid %d\n", id);
00649 #endif
00650 unsigned int i(0);
00651 unsigned int j(0);
00652 float x = x_positions1[i];
00653 float y = y_positions1[i];
00654 if(b_compound_track){
00655 while(j+1 < track_length2 && timestamps2[j] < timestamps1[i]){
00656 ++j;
00657 }
00658 if(timestamps2[j] == timestamps1[i]){
00659 x += x_positions2[i];
00660 x /= 2;
00661 y += y_positions2[i];
00662 y /=2;
00663 }
00664 }
00665 std::pair<float,float> pos = transform_coords_from_fawkes(x,y);
00666 cr->move_to(pos.first,pos.second);
00667 for (; i < track_length1; ++i){
00668 x = x_positions1[i];
00669 y = y_positions1[i];
00670 if(b_compound_track){
00671 while(j+1 < track_length2 && timestamps2[j] < timestamps1[i]){
00672 ++j;
00673 }
00674 if(timestamps2[j] == timestamps1[i]){
00675 x += x_positions2[i];
00676 x /= 2;
00677 y += y_positions2[i];
00678 y /=2;
00679 }
00680 }
00681 std::pair<float,float> pos = transform_coords_from_fawkes(x,y);
00682
00683
00684 cr->line_to(pos.first, pos.second);
00685
00686
00687
00688 std::string t = StringConversions::to_string(timestamps1[i]);
00689
00690 cr->show_text(t);
00691 cr->move_to(pos.first, pos.second);
00692 #ifdef LASERGUI_DEBUG_PRINT_TRACKS
00693 printf("( %f,%f,[%d] )", pos.first, pos.second, timestamps1[i] );
00694 #endif
00695 }
00696
00697
00698 if (div(color_it,3).rem == 0) r+= delta;
00699 if (div(color_it,3).rem == 1) g+= delta;
00700 if (div(color_it,3).rem == 2) b+= delta;
00701 cr->set_source_rgb(r,g,b);
00702 color_it++;
00703
00704 cr->stroke();
00705
00706
00707 i = std::max(0, (int) track_length1 - CFG_PRINT_NR_TRACKELEMENTS);
00708 j = 0;
00709 for (; i < track_length1; ++i){
00710 x = x_positions1[i];
00711 y = y_positions1[i];
00712 if(b_compound_track){
00713 while(j+1 < track_length2 && timestamps2[j] < timestamps1[i]){
00714 ++j;
00715 }
00716 }
00717
00718 std::pair<float,float> pos = transform_coords_from_fawkes(x_positions1[i],y_positions1[i]);
00719 cr->move_to(pos.first - radius, pos.second);
00720 cr->arc(pos.first, pos.second, radius, 0, 2*M_PI);
00721
00722 if(b_compound_track && timestamps2[j] == timestamps1[i]){
00723 cr->move_to(pos.first, pos.second);
00724
00725 std::pair<float,float> pos = transform_coords_from_fawkes(x_positions2[j],y_positions2[j]);
00726 cr->line_to(pos.first, pos.second);
00727 cr->move_to(pos.first - radius, pos.second);
00728 cr->arc(pos.first, pos.second, radius, 0, 2*M_PI);
00729 }
00730 }
00731 cr->set_source_rgb(0,0,1);
00732 cr->stroke();
00733
00734 }
00735 else{
00736 break;
00737 }
00738 }
00739 }
00740
00741
00742 if(__target_if && __target_if->has_writer()){
00743 __target_if->read();
00744 if(__target_if->is_valid()){
00745 cr->set_source_rgb(1,0,0);
00746 std::pair<float,float> pos = transform_coords_from_fawkes(__target_if->relative_x(), __target_if->relative_y());
00747 float x=pos.first;
00748 float y=pos.second;
00749 float radius = 0.1;
00750
00751 cr->move_to(x, y);
00752 cr->arc(x, y, radius, 0, 2*M_PI);
00753 cr->move_to(x - radius, y);
00754 cr->line_to(x + radius, y);
00755 cr->move_to(x, y - radius );
00756 cr->line_to(x, y + radius);
00757 cr->stroke();
00758 }
00759 }
00760
00761
00762
00763
00764
00765
00766
00767
00768
00769
00770
00771
00772
00773
00774
00775
00776
00777
00778
00779
00780
00781 cr->restore();
00782 }
00783
00784
00785
00786
00787
00788
00789
00790 void
00791 LaserDrawingArea::draw_segments(Glib::RefPtr<Gdk::Window> &window,
00792 Cairo::RefPtr<Cairo::Context> &cr)
00793 {
00794 float *distances = __laser360_if ? __laser360_if->distances() : __laser720_if->distances();
00795 size_t nd = __laser_segmentation_if->maxlenof_distances();
00796 const float nd_factor = 360.0 / nd;
00797
00798 cr->save();
00799
00800 if( __laser_segmentation_if && __laser_segmentation_if->has_writer()){
00801 if(!__break_drawing)
00802 __laser_segmentation_if->read();
00803 float * segmentations = __laser_segmentation_if->distances();
00804 size_t nd = __laser_segmentation_if->maxlenof_distances();
00805
00806 cr->set_source_rgb(1,1,0);
00807
00808 if ( __draw_mode == MODE_POINTS ) {
00809 for (size_t i = 0; i < nd; i += __resolution) {
00810 if( segmentations[i]==0) continue;
00811 if ( distances[i] == 0 ) continue;
00812 float anglerad = deg2rad(i * nd_factor);
00813 cr->move_to(0, 0);
00814 cr->line_to(distances[i] * sin(anglerad),
00815 distances[i] * -cos(anglerad));
00816 }
00817 cr->stroke();
00818 } else {
00819 float radius = 4 / __zoom_factor;
00820 for (size_t i = 0; i < nd; i += __resolution) {
00821 if( segmentations[i]==0) continue;
00822 if ( distances[i] == 0 ) continue;
00823 float anglerad = deg2rad(i * nd_factor);
00824 float x = distances[i] * sin(anglerad);
00825 float y = distances[i] * -cos(anglerad);
00826
00827
00828
00829 cr->rectangle(x, y, radius, radius);
00830 }
00831 cr->fill_preserve();
00832 cr->stroke();
00833 }
00834
00835
00836
00837
00838
00839
00840
00841
00842
00843
00844
00845 }
00846 cr->restore();
00847 }
00848
00849
00850
00851
00852
00853
00854 bool
00855 LaserDrawingArea::on_scroll_event(GdkEventScroll *event)
00856 {
00857 if (event->direction == GDK_SCROLL_UP) {
00858 zoom_in();
00859 } else if (event->direction == GDK_SCROLL_DOWN) {
00860 zoom_out();
00861 }
00862 return true;
00863 }
00864
00865
00866 void
00867 LaserDrawingArea::toggle_break_drawing()
00868 {
00869 __break_drawing = ! __break_drawing;
00870 }
00871
00872
00873
00874
00875
00876
00877 bool
00878 LaserDrawingArea::on_button_press_event(GdkEventButton *event)
00879 {
00880 __last_mouse_x = event->x;
00881 __last_mouse_y = event->y;
00882 return true;
00883 }
00884
00885
00886
00887
00888
00889
00890 bool
00891 LaserDrawingArea::on_motion_notify_event(GdkEventMotion *event)
00892 {
00893
00894
00895 __xc -= __last_mouse_x - event->x;
00896 __yc -= __last_mouse_y - event->y;
00897
00898 __last_mouse_x = event->x;
00899 __last_mouse_y = event->y;
00900 queue_draw();
00901 return true;
00902 }
00903
00904
00905
00906
00907
00908
00909
00910
00911
00912
00913 std::pair<float,float>
00914 LaserDrawingArea::transform_coords_from_fawkes(float p_x, float p_y){
00915 std::pair<float,float> pos;
00916 pos.first = -p_y ;
00917 pos.second= -p_x ;
00918 return pos;
00919 }