$search

main_window.cpp

Go to the documentation of this file.

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, Robert Bosch LLC.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Robert Bosch nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *********************************************************************/
#include "main_window.h"
#include <iostream>
#include <sstream>
#include <fstream>
#include <vlr/arial.h>
#include <vlr/vlrException.h>
#include <vlr/vlrImage.h>
#include <boost/bind.hpp>
#include <remote_power_manager/SetPortRelay.h>
#include <opencv2/core/core.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2/highgui/highgui.hpp>

using namespace sensor_msgs;
using namespace sensor_msgs::image_encodings;
using vlr::Image;
using vlr::ImageBase;
using vlr::TagBase;
using vlr::TagPoint;
using vlr::TagLine;

const int32_t MainWindow::catch_dist_ = 15;

#define DEFAULT_MODE        vlr::MODE_2D
#define DEFAULT_FRAMERATE   30
#define DEFAULT_FORMAT      QGLFormat(QGL::DoubleBuffer)

MainWindow::MainWindow() :
  width_(1024), height_(768), nh_("~"), loop_rate_(30), current_object_(NULL), current_carving_progress_(0),current_pt_(NULL), font_buffer_(NULL), action_client_("/dremel_actionserver")
{

  // Window stuff
  setWindowTitle(tr("PR2 Dremel"));
  resize(width_, height_);

  // create central widget to display
  display_widget_ = new vlr::DisplayGL(this, DEFAULT_MODE, DEFAULT_FRAMERATE, DEFAULT_FORMAT);
//  display_widget_ = new vlr::BevGLView(this);
  connect(display_widget_,SIGNAL(mousePress(int, int, uint8_t)),this, SLOT(mousePress(int, int, uint8_t)));
  connect(display_widget_,SIGNAL(mouseRelease(int, int, uint8_t)),this, SLOT(mouseRelease(int, int, uint8_t)));
  connect(display_widget_,SIGNAL(mouseMove(int, int, uint8_t)),this, SLOT(mouseMove(int, int, uint8_t)));
  connect(display_widget_,SIGNAL(keyPress(unsigned int)),this, SLOT(keyPress(unsigned int)));
  connect(display_widget_,SIGNAL(mouseWheel(int, int, uint8_t, int)),this, SLOT(mouseWheel(int, int, uint8_t, int)));
  setCentralWidget(display_widget_);

  // create right toolbox
  right_toolbox_ = new QToolBox(this);
  right_toolbox_->setMinimumWidth(200);
  QDockWidget *dock = new QDockWidget(tr("ToolBox"), this);
  dock->setAllowedAreas(Qt::LeftDockWidgetArea | Qt::RightDockWidgetArea);
  dock->setWidget(right_toolbox_);
  addDockWidget(Qt::RightDockWidgetArea, dock);

  // create menu
  menu_widget_ = new MenuWidget(this);
  right_toolbox_->addItem(menu_widget_,"Carving");

  connect(menu_widget_->send_pushButton,SIGNAL(pressed()),this, SLOT(send()));
  connect(menu_widget_->cancle_pushButton,SIGNAL(pressed()),this, SLOT(cancel()));

  connect(menu_widget_->dremel_on_pushButton,SIGNAL(pressed()),this, SLOT(enableDremel()));
  connect(menu_widget_->dremel_off_pushButton,SIGNAL(pressed()),this, SLOT(disableDremel()));
  connect(menu_widget_->vacuum_on_pushButton,SIGNAL(pressed()),this, SLOT(enableVacuum()));
  connect(menu_widget_->vacuum_off_pushButton,SIGNAL(pressed()),this, SLOT(disableVacuum()));

  // initialize visualization marker
  current_marker_id_ = 0;
  viz_marker_pub_   = nh_.advertise<visualization_msgs::Marker> ("visualization_marker", 100);

  // initialize action server
  ROS_INFO("waiting for action server");
  action_client_.waitForServer();
  ROS_INFO("found");

  nh_.param("fontfile", fontfile_name_, std::string(""));

  if(!fontfile_name_.empty()) {
          ROS_INFO("Using fontfile: %s",fontfile_name_.c_str());
          // Initialize FreeType.
          // font_advance_ = 0;
          FT_Init_FreeType(&library_);

          std::ifstream font_file(fontfile_name_.c_str(), std::ios::binary);
          std::fstream::pos_type font_file_size;
          if (font_file) {
                font_file.seekg(0, std::ios::end);
                font_file_size = font_file.tellg();
                font_file.seekg(0);
                font_buffer_ = new uint8_t[font_file_size];
                font_file.read((char*)font_buffer_, font_file_size);
          }
          else {
                throw VLRException("Cannot open font file " + fontfile_name_);
          }
          FT_New_Memory_Face(library_, font_buffer_, font_file_size, 0, &face_);
  }
  else {
          // Initialize FreeType.
          FT_Init_FreeType(&library_);
          FT_New_Memory_Face(library_, arial_resource_data, arial_resource_size, 0, &face_);
  }
//    // TODO: remove debug
//  std::string filename = "/home/soeren/frame0000.jpg";
//  cv::Mat img = cv::imread(filename, CV_LOAD_IMAGE_GRAYSCALE);
//
//  width_ = img.cols;
//  height_ = img.rows;
  img_ = new vlr::Image<uint8_t>(width_, height_);
  for (int32_t y = 0; y < height_; y++) {
    for (int32_t x = 0; x < width_; x++) {
      (*img_)(x, y) = 255;//img.data[y*img.step+x];
    }
  }

  updateImage();
}

MainWindow::~MainWindow()
{
}

bool MainWindow::send()
{
  printf("about to send points\n");
  std::vector<pr2_dremel_server::Segment> segments; 
  // for each object in draw_objects_ -> accumulate to goal
  for(std::vector<dremel::DrawObject>::iterator it=draw_objects_.begin(); it!=draw_objects_.end();++it){
    dremel::DrawObject& object = *it;
    std::vector<pr2_dremel_server::Segment> segment_fragment=object.convertToSegments();
    segments.insert(segments.end(), segment_fragment.begin(), segment_fragment.end());
    
  }
  pr2_dremel_server::CarveSegmentsGoal goal_message;
  goal_message.segments=segments;

  action_client_.sendGoal(goal_message, actionlib::SimpleActionClient<pr2_dremel_server::CarveSegmentsAction>::SimpleDoneCallback());

  action_client_.waitForResult();
  
  if (current_carving_progress_ < 1) return false;
  
  return true;
}



void MainWindow::cancel(){
  action_client_.cancelGoal();
  
}

float MainWindow::glyphToLines(wchar_t ch, int size, float outline_width, std::vector<std::vector<TagLine> >& all_lines, float initial_font_advance) {
  float font_advance_=initial_font_advance;
  // Set the size to use.
  if (FT_Set_Char_Size(face_, size, size, 90, 90) != 0) {
    throw VLRException("Failed to set font size.");
  }
  // Load the glyph we are looking for.
  FT_UInt gindex = FT_Get_Char_Index(face_, ch);
  if (FT_Load_Glyph(face_, gindex, FT_LOAD_NO_BITMAP) != 0) {
    throw VLRException("Failed to load glyph.");
  }
  // Need an outline for this to work.
  if (face_->glyph->format != FT_GLYPH_FORMAT_OUTLINE) {
    throw VLRException("Glyph does not contain outline information.");
  }

  // Set up a stroker.
  FT_Stroker stroker;
  FT_Stroker_New(library_, &stroker);
  //  printf("stroker var %d %d ", FT_STROKER_LINECAP_ROUND, FT_STROKER_LINEJOIN_ROUND);
  FT_Stroker_Set(stroker, outline_width, FT_STROKER_LINECAP_SQUARE, FT_STROKER_LINEJOIN_MITER, 0);
 //  FT_Stroker_Set(stroker, (int) (outline_width), FT_STROKER_LINECAP_ROUND, FT_STROKER_LINEJOIN_ROUND, 0);

  FT_Glyph glyph;
  int32_t max_x=0;
  TagLine line(0, 0, 0, 0);
  if (FT_Get_Glyph(face_->glyph, &glyph) == 0) {
    FT_Glyph_StrokeBorder(&glyph, stroker, 0, 1);
    // Again, this needs to be an outline to work.
    if (glyph->format == FT_GLYPH_FORMAT_OUTLINE) {
      // Render the outline spans to the span list
      FT_Outline *o = &reinterpret_cast<FT_OutlineGlyph> (glyph)->outline;
      printf("Glyph contains %i contours with in total %i points.\n", o->n_contours, o->n_points);
      int32_t start = 0;
      for (int32_t c = 0; c < o->n_contours; c++) {
        start = (c > 0 ? o->contours[c - 1] + 1 : 0);
        printf("Contour %i with %i points: \n", c, o->contours[c] - start);
        std::vector<TagLine> lines;
        max_x = std::max(max_x, (int32_t)o->points[0].x);
        for (int32_t i = start; i < o->contours[c] - 1; i++) {
          printf("line (%i, %i)-(%i, %i)\n", (int)o->points[i].x, (int)o->points[i].y, (int)o->points[i + 1].x, (int)o->points[i + 1].y);
          line.setX0(o->points[i].x+font_advance_);
          line.setY0(300-o->points[i].y);
          line.setX1(o->points[i+1].x+font_advance_);
          line.setY1(300-o->points[i+1].y);
          lines.push_back(line);
          max_x = std::max(max_x, (int32_t)o->points[i+1].x);
        }


        line.setX0(o->points[start].x+font_advance_);
        line.setY0(300-o->points[start].y);
        line.setX1(o->points[o->contours[c] - 1].x+font_advance_);
        line.setY1(300-o->points[o->contours[c] - 1].y);
        lines.push_back(line);

        all_lines.push_back(lines);
      }
      sendLineMarker(all_lines,std::string("/odom_combined"),1.0,1.0,1.0);
    }
  }

  // This is unused in this test but you would need this to draw
  // more than one glyph.
  //  font_advance_ += face_->glyph->advance.x;

  // Clean up afterwards.
  FT_Stroker_Done(stroker);
  FT_Done_Glyph(glyph);

  return max_x; // face_->glyph->advance.x;
}

void MainWindow::sendLineMarker(std::vector<std::vector<TagLine> >& lines, std::string frame_id, double r, double g, double b) {
  lines_marker_.ns = "draw_gcode";
  lines_marker_.header.frame_id = frame_id;
  lines_marker_.type = visualization_msgs::Marker::LINE_STRIP;
  lines_marker_.action = visualization_msgs::Marker::ADD;
  lines_marker_.scale.x = lines_marker_.scale.y = lines_marker_.scale.z = 1;
  lines_marker_.color.a = 1.0;
  lines_marker_.color.r = r;
  lines_marker_.color.g = g;
  lines_marker_.color.b = b;

  for(std::vector<std::vector<TagLine> >::iterator it=lines.begin();it!=lines.end();++it) {
    std::vector<TagLine>& line_segment = *it;
    std::vector<geometry_msgs::Point> points;
    for(std::vector<TagLine>::iterator itT=line_segment.begin();itT!=line_segment.end();++itT) {
      TagLine& line = *itT;
      geometry_msgs::Point p0,p1;
      p0.x = line.x0();
      p0.y = line.y0();
      points.push_back(p0);
      p1.x = line.x1();
      p1.y = line.y1();
      points.push_back(p1);
    }
    lines_marker_.points = points;
    lines_marker_.id = current_marker_id_++;
    lines_marker_.header.stamp = ros::Time::now();
    viz_marker_pub_.publish(lines_marker_);
  }
  return;
}

void MainWindow::linesToGcode(std::vector<std::vector<vlr::TagLine> >& lines) {

   std::string gcode_string= "%\n(Generated by gcodetools from inkscape.)\nM3\n(Header end.)\nG21 (All units in mm)\n\n";



   int num_separate_contours=lines.size();

   //for each set of contours

  for (int contour_ind = 0; contour_ind < num_separate_contours; contour_ind++) {
     int numlines=lines.at(contour_ind).size();
     //for segments within the coutour
    for (int line_index = 0; line_index < numlines; line_index++) {
   std::ostringstream buffer_x0;
   std::ostringstream buffer_x1;
   std::ostringstream buffer_y0;
   std::ostringstream buffer_y1;
   buffer_x0 <<  lines.at(contour_ind).at(line_index).x0()/10;
   buffer_x1 <<  lines.at(contour_ind).at(line_index).x1()/10;
   buffer_y0 <<  lines.at(contour_ind).at(line_index).y0()/10;
   buffer_y1 <<  lines.at(contour_ind).at(line_index).y1()/10;

   std::string x0_string = buffer_x0.str();
   std::string x1_string = buffer_x1.str();
   std::string y0_string = buffer_y0.str();
   std::string y1_string = buffer_y1.str();

   if(line_index==0){
     gcode_string=gcode_string + "G00 Z5.00\nG00 X" + x0_string + " Y" +  y0_string+ "\n\nG01 Z-0.1000 F100.0\n";
     gcode_string=gcode_string + "G01 X" + x1_string + " Y" +  y1_string+ " Z-0.1000 F400.0\n";
     }
   else{
     gcode_string=gcode_string + "G01 X" + x0_string + " Y" +  y0_string+ " Z-0.1000\n";
     gcode_string=gcode_string + "G01 X" + x1_string + " Y" +  y1_string+ " Z-0.1000\n";
   }
       }
     gcode_string=gcode_string+"G00 Z5.0\n\n";
   }


 gcode_string=gcode_string+"(Footer)\nM5\nG00 X0.00 Y0.00\nM2\n(Using default footer.)\n(end)\n%";
 std::string filename="test_gcode_file";
 std::FILE *fp;

 fp=fopen(filename.c_str(), "w");
 fprintf(fp, "%s", gcode_string.c_str());
 fclose(fp);
}

std::pair<dremel::DrawObject*,float> MainWindow::getClosestObject(float x, float y) {
  std::pair<dremel::DrawObject*,float> closest_object;
  closest_object.first = NULL;
  closest_object.second = FLT_MAX;
  float dist;

  for(std::vector<dremel::DrawObject>::iterator itO=draw_objects_.begin();itO!=draw_objects_.end();++itO) {
    dremel::DrawObject& object = *itO;
    std::vector<std::vector<vlr::TagLine> >& lines = object.getLines();
    for(std::vector<std::vector<vlr::TagLine> >::iterator itL=lines.begin();itL!=lines.end();++itL) {
      std::vector<vlr::TagLine>& tag_lines = *itL;
      for(std::vector<vlr::TagLine>::iterator itT=tag_lines.begin();itT!=tag_lines.end();++itT) {
        vlr::TagLine& tag_line = *itT;
        dist = sqrt( (tag_line.x0() - x)*(tag_line.x0() - x) + (tag_line.y0() - y)*(tag_line.y0() - y) );
        if(dist<closest_object.second) {
          closest_object.first = &object;
          closest_object.second = dist;
        }
        dist = sqrt( (tag_line.x1() - x)*(tag_line.x1() - x) + (tag_line.y1() - y)*(tag_line.y1() - y) );
        if(dist<closest_object.second) {
          closest_object.first = &object;
          closest_object.second = dist;
        }
      }
    }
  }
  return closest_object;
}

void MainWindow::moveDrawObject(dremel::DrawObject* object, int dx, int dy) {

  object->move(dx,dy);
  updateImage();
}

void MainWindow::scaleDrawObject(dremel::DrawObject* object, float scale) {

  object->scale(scale);
  updateImage();
}

void MainWindow::eraseObject(dremel::DrawObject* object) {
  int n=0;
  bool found = false;
  for(std::vector<dremel::DrawObject>::iterator itO=draw_objects_.begin();itO!=draw_objects_.end();++itO,n++) {
    dremel::DrawObject& object = *itO;
    if(current_object_ == &object) {
      found = true;
      break;
    }
  }
  if(found) {
    draw_objects_.erase(draw_objects_.begin()+n);
    updateImage();
  }
}


void MainWindow::mouseMove(int x, int y, uint8_t buttons) {

  if (buttons & vlr::LEFT_BUTTON) {
    if (current_object_) {
      int dx = x-last_move_x_;
      int dy = y-last_move_y_;
      moveDrawObject(current_object_,dx,dy);
      last_move_x_ = x;
      last_move_y_ = y;
    }
  }
  
  else {
    // find close object
    std::pair<dremel::DrawObject*,float> closest_object = getClosestObject(x,y);
    std::cout << closest_object.second << std::endl;
    if(closest_object.second < catch_dist_) {
      QApplication::setOverrideCursor(QCursor(Qt::OpenHandCursor));
    }
    else {
      QApplication::setOverrideCursor(QCursor(Qt::ArrowCursor));
    }
  }
}

void MainWindow::mousePress(int x, int y, uint8_t buttons) {
  printf("%s\n", __PRETTY_FUNCTION__);
  printf("x: %i, y: %i\n", x, y);
  if (buttons & vlr::LEFT_BUTTON) {

    // deselect old point pair
    if (current_object_) {
      current_object_->changeSelected(false);
    }

    // find close object
    std::pair<dremel::DrawObject*,float> closest_object = getClosestObject(x,y);
    std::cout << closest_object.second << std::endl;
    if(closest_object.second < catch_dist_) {
      current_object_ = closest_object.first;
      current_object_->changeSelected(true);
      std::cout << "selected object" << std::endl;
    }
    else {
      current_object_ = NULL;
      std::cout << "removed selection" << std::endl;
    }

    last_move_x_ = x;
    last_move_y_ = y;

    updateImage();
  }
  else if(buttons & vlr::RIGHT_BUTTON){
    printf("Right button clicked");
    std::vector<std::vector<vlr::TagLine> > templines;
    if(!current_object_)
      { 
        //create a set of empty lines to create the object(we only know a point thus far)
        printf("start a new draw object line");
        dremel::DrawObject object(templines);
        object.setAsNotText();
        draw_objects_.push_back(object);
        current_object_=&draw_objects_.back();

        last_right_x_=x;
        last_right_y_=y;
        
      }
    else if(current_object_ && !current_object_->isText())
      {
        printf("add to existing line");
        
        //create the lines given last values and x,y and save
        
        std::vector<vlr::TagLine> newline_vector;
        vlr::TagLine newline(last_right_x_, last_right_y_, x, y);
        newline_vector.push_back(newline);
        templines.push_back(newline_vector);
        current_object_->insertNewLines(templines);
        
        last_right_x_=x;
        last_right_y_=y;
        //update Image here
        updateImage();
      }
    
  }

}

void MainWindow::mouseRelease(int x, int y, uint8_t buttons) {
  printf("x: %i, y: %i\n", x, y);
  if (buttons & vlr::LEFT_BUTTON) {
    std::pair<ImageBase::tagCIter, ImageBase::tagCIter> ptRange;
    ImageBase::tagCIter ptit;
    img_->points(ptRange);

    // deselect old point pair
    if (current_pt_) {
      current_pt_->setRGB(1.0, 0.0, 0.0);
    }

    bool newPoint = true;

    for (ptit = ptRange.first; ptit != ptRange.second; ++ptit) {
      TagPoint* pt = static_cast<TagPoint*> ((*ptit).second);
      float xf = (float) x;
      float yf = (float) y;
      if ((xf - pt->x()) * (xf - pt->x()) + (yf - pt->y()) * (yf - pt->y()) < catch_dist_ * catch_dist_) {
        newPoint = false;
        current_pt_ = static_cast<TagPoint*> ((*ptit).second);
        if (current_pt_) {
          current_pt_->setRGB(0.0, 1.0, 0.0);
        }
        break;
      }
    }

    if (newPoint) {
      std::stringstream label;
      label << "(" << x << ", " << y << ")";
      current_pt_ = img_->addPoint(x, y, label.str(), 0, 1, 0, 1);
    }

    updateImage();
  }
}

void MainWindow::keyPress(unsigned int key) {
  printf("keycode %d\n", key);
  switch (key) {
    //    case 29:
    //      doneWithImage=true;
    //      break;
    //
    //    case 31: {
    //    }
    //    break;
    case 0:{//shift
      printf("shifted!");
    }
     break;

  case 13:{//enter key return
    //first add the current lines to a drawobject
    dremel::DrawObject object(lines_);
    draw_objects_.push_back(object);
    printf("size of draw_objects_ %d\n current object has lines_ with size %d", (int) draw_objects_.size(), (int) lines_.size());
    
    //now clear lines for the next set
    // lines_ = new std::vector<std::vector<vlr::TagLine>>;
    lines_.clear();
  }
    break;
  case ' ': {
  }
    break;
  case 8: {
    if(current_object_) {
      eraseObject(current_object_);
      current_object_ = NULL;
    }
    break;
  }
  default:

    
    lines_.clear();

    float curr_font_advance=0;
    
      if (current_object_ == NULL || !current_object_->isText()) {
        curr_font_advance = glyphToLines(key, 100, 0.0f, lines_, 0);
        dremel::DrawObject object(lines_);
        object.changeSelected(true);
        draw_objects_.push_back(object);
        current_object_=&draw_objects_.back();
        current_object_->addToFontAdvance(curr_font_advance);
        
      }
    else{
      float initial_font_advance=current_object_->getFontAdvance();
      curr_font_advance=glyphToLines(key, 100, 0.0f, lines_, initial_font_advance);
      printf("additional font advance %f", curr_font_advance);

      current_object_->insertNewLines(lines_);
      current_object_->addToFontAdvance(curr_font_advance);
    }   
    
    
    
    //    lines_.clear();
    //  curr_font_advance+=glyphToLines(key, 100, 3.0f, lines_);
//    if(!lines_.empty()) {
//      //linesToGcode(lines_);
//      addCountoursToImage(lines_);
//    }
    updateImage();
  }
}

void MainWindow::mouseWheel(int x, int y, uint8_t buttons, int delta) {
  if (current_object_) {
    float scale = 1.0 + delta*0.001;
    scaleDrawObject(current_object_,scale);
  }
}

void MainWindow::addCountoursToImage(std::vector<std::vector<vlr::TagLine> >& contours) {
  // for each set of contours
  for (size_t i=0; i<contours.size(); i++) {
    std::vector<vlr::TagLine>& lines = contours[i];
     // for segments within the coutour
     for(size_t j=0; j<lines.size(); j++) {
       vlr::TagLine& tl = lines[j];
       vlr::TagLine* tl_ptr = new vlr::TagLine(tl.x0(),tl.y0(),tl.x1(),tl.y1());
       img_->addTag(tl_ptr);
     }
  }
  display_widget_->updateImage(*img_);
}

void MainWindow::updateImage() {

  img_->clearTags();

  for(std::vector<dremel::DrawObject>::iterator itO=draw_objects_.begin();itO!=draw_objects_.end();++itO) {
    dremel::DrawObject& object = *itO;
    std::vector<std::vector<vlr::TagLine> >& lines = object.getLines();
    for(std::vector<std::vector<vlr::TagLine> >::iterator itL=lines.begin();itL!=lines.end();++itL) {
      std::vector<vlr::TagLine>& tag_lines = *itL;
      for(std::vector<vlr::TagLine>::iterator itT=tag_lines.begin();itT!=tag_lines.end();++itT) {
        vlr::TagLine& tl = *itT;
        vlr::TagLine* tl_ptr = new vlr::TagLine(tl.x0(),tl.y0(),tl.x1(),tl.y1());
        if(object.isSelected()) {
          tl_ptr->setRGB(1.0,0.0,0.0);
        }
        else {
          tl_ptr->setRGB(0.0,0.0,0.0);
        }
        img_->addTag(tl_ptr);
      }
    }
  }
  display_widget_->updateImage(*img_);
}

void MainWindow::enableDremel() {
  ros::ServiceClient client = nh_.serviceClient<remote_power_manager::SetPortRelay>("/remote_power_manager/set_port_relay");
  remote_power_manager::SetPortRelay::Request req;
  remote_power_manager::SetPortRelay::Response resp;
  req.power_port = 1;
  req.value = true;
  if (client.call(req,resp))
  {
    ROS_INFO("Enabled dremel!!");
  }
}

void MainWindow::disableDremel() {
  ros::ServiceClient client = nh_.serviceClient<remote_power_manager::SetPortRelay>("/remote_power_manager/set_port_relay");
  remote_power_manager::SetPortRelay::Request req;
  remote_power_manager::SetPortRelay::Response resp;
  req.power_port = 1;
  req.value = false;
  if (client.call(req,resp))
  {
    ROS_INFO("Disabled dremel!!");
  }
}

void MainWindow::enableVacuum() {
  ros::ServiceClient client = nh_.serviceClient<remote_power_manager::SetPortRelay>("/remote_power_manager/set_port_relay");
  remote_power_manager::SetPortRelay::Request req;
  remote_power_manager::SetPortRelay::Response resp;
  req.power_port = 2;
  req.value = true;
  if (client.call(req,resp))
  {
    ROS_INFO("Enabled vacuum!!");
  }
}

void MainWindow::disableVacuum() {
  ros::ServiceClient client = nh_.serviceClient<remote_power_manager::SetPortRelay>("/remote_power_manager/set_port_relay");
  remote_power_manager::SetPortRelay::Request req;
  remote_power_manager::SetPortRelay::Response resp;
  req.power_port = 2;
  req.value = false;
  if (client.call(req,resp))
  {
    ROS_INFO("Disabled vacuum!!");
  }
}


//
//
//btVector2 MainWindow::normalizePixel(const btVector2& x_kk,fc,cc,kc,alpha_c)
//{
//  K = basic_compose_k_matrix(calib_results.fc, calib_results.cc, calib_results.alpha_c);
//
//}
//
//void MainWindow::unprojectPixel(const tf::Transform& camera_transform, const btVector2& pixel, const btVector3& point)
//{
//  K = basic_compose_k_matrix(calib_results.fc, calib_results.cc, calib_results.alpha_c);
//
//}
//
//
//void MainWindow::calculateWorkArea(const tf::Transform& camera_transform, std::string& target_frame, btVector3& points[4])
//{
//  btVector3 n(0, 0, 1);
//  btPlane table(n, table_height_); // TODO: table height is probably measured from floor and not from base_link origin
//  btVector3 o = camera_transform.getOrigin(); // origin of camera
//  btVector3 p = o + camera_transform.getBasis()*btVector3(0, 0, 1); // point along optical axis of camera to determine view direction
//
//}
//
//tf::Transform MainWindow::getPlaneTransform ()
//{
//  //assume plane coefficients are normalized
//  btVector3 position();
//  btVector3 z(a, b, c);
//  //make sure z points "down"
//  ROS_DEBUG("z.dot: %0.3f", z.dot(btVector3(0,0,1)));
//  ROS_DEBUG("in getPlaneTransform, z: %0.3f, %0.3f, %0.3f", z[0], z[1], z[2]);
//  if ( z.dot( btVector3(0, 0, up_direction) ) < 0)
//  {
//    z = -1.0 * z;
//    ROS_INFO("flipped z");
//  }
//  ROS_DEBUG("in getPlaneTransform, z: %0.3f, %0.3f, %0.3f", z[0], z[1], z[2]);
//
//  //try to align the x axis with the x axis of the original frame
//  //or the y axis if z and x are too close too each other
//  btVector3 x(1, 0, 0);
//  if ( fabs(z.dot(x)) > 1.0 - 1.0e-4) x = btVector3(0, 1, 0);
//  btVector3 y = z.cross(x).normalized();
//  x = y.cross(z).normalized();
//
//  btMatrix3x3 rotation;
//  rotation[0] = x;  // x
//  rotation[1] = y;  // y
//  rotation[2] = z;  // z
//  rotation = rotation.transpose();
//  btQuaternion orientation;
//  rotation.getRotation(orientation);
//  return tf::Transform(orientation, position);
//}

---

pr2_dremel_gui
Author(s): Soeren Kammel, Benjamin Pitzer (maintained by Benjamin Pitzer)
autogenerated on Sun Mar 3 12:12:51 2013