$search
00001 /********************************************************************* 00002 * 00003 * Software License Agreement (BSD License) 00004 * 00005 * Copyright (c) 2011, Robert Bosch LLC. 00006 * All rights reserved. 00007 * 00008 * Redistribution and use in source and binary forms, with or without 00009 * modification, are permitted provided that the following conditions 00010 * are met: 00011 * 00012 * * Redistributions of source code must retain the above copyright 00013 * notice, this list of conditions and the following disclaimer. 00014 * * Redistributions in binary form must reproduce the above 00015 * copyright notice, this list of conditions and the following 00016 * disclaimer in the documentation and/or other materials provided 00017 * with the distribution. 00018 * * Neither the name of the Robert Bosch nor the names of its 00019 * contributors may be used to endorse or promote products derived 00020 * from this software without specific prior written permission. 00021 * 00022 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00023 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00024 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00025 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00026 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00027 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00028 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00029 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00030 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00031 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00032 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00033 * POSSIBILITY OF SUCH DAMAGE. 00034 * 00035 *********************************************************************/ 00036 00037 #ifndef MAINWINDOW_H 00038 #define MAINWINDOW_H 00039 00040 //== INCLUDES ================================================================== 00041 00042 // ros 00043 #include <ros/ros.h> 00044 #include <sensor_msgs/Image.h> 00045 #include <sensor_msgs/image_encodings.h> 00046 #include <sensor_msgs/CameraInfo.h> 00047 #include <visualization_msgs/Marker.h> 00048 #include <tf/tf.h> 00049 #include <tf/transform_listener.h> 00050 #include <pr2_dremel_server/CarveSegmentsAction.h> 00051 #include <actionlib/client/simple_action_client.h> 00052 00053 00054 // qt 00055 #include <QMainWindow> 00056 #include <QToolBox> 00057 00058 // display lib 00059 #include <vlr/displayGL.h> 00060 00061 // local 00062 #include "menu_widget.h" 00063 #include "draw_object.h" 00064 00065 #include <ft2build.h> 00066 #include FT_FREETYPE_H 00067 #include FT_STROKER_H 00068 00069 00070 //== CLASS DEFINITION ========================================================== 00071 class MainWindow : public QMainWindow 00072 { 00073 00074 // Call Qt Meta Object Compiler 00075 Q_OBJECT 00076 00077 public: 00078 00079 //----------------------------------------------------------------------------- 00081 MainWindow(); 00082 00083 //----------------------------------------------------------------------------- 00085 ~MainWindow(); 00086 00088 float glyphToLines(wchar_t ch, int size, float outline_width, std::vector<std::vector<vlr::TagLine> >& all_lines, float initial_font_advance); 00089 00091 void linesToGcode(std::vector<std::vector<vlr::TagLine> >& lines); 00092 00093 void sendLineMarker(std::vector<std::vector<vlr::TagLine> >& lines, std::string frame_id, double r, double g, double b); 00094 void addCountoursToImage(std::vector<std::vector<vlr::TagLine> >& contours); 00095 std::pair<dremel::DrawObject*,float> getClosestObject(float x, float y); 00096 void updateImage(); 00097 // void updateImageLine(); 00098 void moveDrawObject(dremel::DrawObject* object, int dx, int dy); 00099 void scaleDrawObject(dremel::DrawObject* object, float scale); 00100 void eraseObject(dremel::DrawObject* object); 00101 00102 public slots: 00103 void mousePress(int x, int y, uint8_t buttons); 00104 void mouseRelease(int x, int y, uint8_t buttons); 00105 void mouseMove(int x, int y, uint8_t buttons); 00106 void keyPress(unsigned int key); 00107 void mouseWheel(int x, int y, uint8_t buttons, int delta); 00109 bool send(); 00111 void cancel(); 00112 00113 void enableDremel(); 00114 void disableDremel(); 00115 void enableVacuum(); 00116 void disableVacuum(); 00117 00118 private: 00119 int32_t width_, height_; 00120 00122 ros::NodeHandle nh_; 00123 tf::TransformListener tf_listener_; 00124 ros::Rate loop_rate_; 00125 ros::Subscriber plate_img_sub_; 00126 sensor_msgs::Image plate_img_; 00127 00129 vlr::DisplayGL* display_widget_; 00131 QToolBox *right_toolbox_; 00133 MenuWidget *menu_widget_; 00134 00135 std::vector<dremel::DrawObject> draw_objects_; 00136 dremel::DrawObject* current_object_; 00137 00138 00139 float current_carving_progress_; 00140 vlr::Image<uint8_t>* img_; 00141 vlr::TagPoint* current_pt_; 00142 static const int32_t catch_dist_; 00143 std::vector<std::vector<vlr::TagLine> > lines_; 00144 int last_move_x_,last_move_y_; 00145 double table_height_; 00146 int last_right_x_,last_right_y_; 00147 00148 // font stuff 00149 FT_Library library_; 00150 FT_Face face_; 00151 std::string fontfile_name_; 00152 uint8_t* font_buffer_; 00153 00154 actionlib::SimpleActionClient<pr2_dremel_server::CarveSegmentsAction> action_client_; 00155 00156 // visualization markers 00157 visualization_msgs::Marker lines_marker_; 00158 ros::Publisher viz_marker_pub_; 00159 int current_marker_id_; 00160 00161 void feedbackCb(const pr2_dremel_server::CarveSegmentsFeedbackConstPtr& feedback){ 00162 current_carving_progress_=feedback->percent_complete; 00163 menu_widget_-> carving_progress_->setValue(current_carving_progress_*100); 00164 printf("Percent Complete %f\n", current_carving_progress_); 00165 } 00166 00167 }; 00168 00169 //============================================================================== 00170 #endif // MAINWINDOW_H defined 00171 //==============================================================================