Hokuyo.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, CoroWare
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Stanford U. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "Hokuyo.h"
00031 #include <QApplication>
00032 #include <QPainter>
00033 
00034 #include <QtGui>
00035 
00036 #define MAX_RANGE 0.80 // 80cm
00037 
00038 // This widgets tries to show the laser range finder data or the infrared sensor values
00039 
00040 Hokuyo::Hokuyo(QWidget *parent)
00041     : QWidget(parent)
00042 {
00043     showlines = false;
00044     flag = 1;
00045     hokuyo_points = NULL;
00046     hokuyo_points = new Hokuyo_Points [683];
00047     showIR = false;
00048     showLRF = false;
00049 
00050 }
00051 
00052 Hokuyo::~Hokuyo()
00053 {
00054 }
00055 
00056 
00057 void Hokuyo::paintEvent(QPaintEvent *event)
00058 // display the data on the winder
00059 {
00060 
00061     flag = 0;
00062         int center_h = (int) (this->parentWidget()->height() /2);
00063         int center_w = (int) (this->parentWidget()->width() /2);
00064     QPainter painter(this);
00065 
00066     //***************************************************************
00067 
00068         QPen pen2(Qt::black, 2, Qt::SolidLine);
00069         painter.setPen(pen2);
00070         painter.setBrush(Qt::red);
00071         QRectF rectangle(center_w - 2, center_h-2, 4, 4);
00072         painter.drawEllipse(rectangle);
00073     //*******************************************************
00074 
00075     if(showLRF) // if show laser range finder
00076     {
00077         QPen pen(Qt::black, 2, Qt::DashDotDotLine);
00078 
00079         painter.setPen(pen);
00080 
00081         painter.drawLine(20, 40, 120, 40);
00082     }
00083 
00084     else if(showIR) // if show small infrared sensor
00085     {
00086         QPen pen(Qt::black, 2, Qt::SolidLine);
00087 
00088         painter.setPen(pen);
00089 
00090         painter.drawLine(20, 40, 20 + center_h, 40);
00091 
00092     }
00093 //**************************************************************
00094     if(showIR)
00095     {
00096         painter.setBrush(Qt::cyan);
00097 
00098         QPen pen1(Qt::black, 2, Qt::SolidLine);
00099         
00100         QPointF points_front[3];
00101         QPointF points_back[3];
00102 
00103 
00104         points_front[0] = QPointF(center_w,center_h);
00105         points_front[1] = QPointF(center_w - 50, center_h + (float)this->IR01_update * center_h);
00106         points_front[2] = QPointF(center_w + 50, center_h + (float)this->IR01_update * center_h);
00107 
00108         painter.setPen(pen1);
00109         painter.drawPolygon(points_front,3);
00110 
00111         painter.setBrush(Qt::darkCyan);
00112 
00113         points_back[0] = QPointF(center_w, center_h);
00114         points_back[1] = QPointF(center_w - 50, center_h - (float)this->IR02_update * center_h);
00115         points_back[2] = QPointF(center_w + 50, center_h - (float)this->IR02_update * center_h);
00116 
00117         painter.drawPolygon(points_back,3);
00118     }
00119 
00120 
00121     else if(showLRF)
00122     {
00123         QPen pen3(Qt::black, 2, Qt::DashDotDotLine);
00124         painter.setPen(pen3);
00125 
00126         int count = 0;
00127 
00128         if(hokuyo_points!=NULL)
00129         {
00130             for(int it=0;it<683;it++)
00131             {
00132                 painter.drawPoint(hokuyo_points[it].x * 100 + 300, -(hokuyo_points[it].y * 100) + 300);
00133                 if(showlines)
00134                 painter.drawLine(300,300,hokuyo_points[it].x * 100 + 300, -(hokuyo_points[it].y * 100) + 300);
00135                 count ++;
00136             }
00137         }
00138 
00139         flag = 1;
00140    }
00141 
00142 
00143 }
00144 
00145 void Hokuyo::hokuyo_update(Hokuyo_Points* hokuyo_points_)
00146 // update the laser range findre values
00147 {
00148     hokuyo_points = hokuyo_points_;
00149 
00150     this->update();
00151 
00152 }
00153 
00154 void Hokuyo::IR_update(double IR01_new, double IR02_new)
00155 // update the infrared sensor values
00156 {
00157    this->IR01_update = IR01_new;
00158    this->IR02_update =  IR02_new;
00159 
00160    this->update();
00161 }


corobot_teleop
Author(s):
autogenerated on Wed Aug 26 2015 11:09:59