int OverlayPickerTool::processMouseEvent(rviz::ViewportMouseEvent& event)
 {
   if (event.left() && event.leftDown()) {
     if (!is_moving_) {
       onClicked(event);
     }
   }
   else if (event.left() && is_moving_) {
     onMove(event);
   }
   else if (is_moving_ && !(event.left() && event.leftDown())) {
     onRelease(event);
   }
   return 0;
 }
 int PolygonTool::processMouseEvent( rviz::ViewportMouseEvent& event )
 {
   if( !moving_flag_node_ )
   {
     return Render;
   }
   Ogre::Vector3 intersection;
   Ogre::Plane ground_plane( Ogre::Vector3::UNIT_Z, 0.0f );
   if( rviz::getPointOnPlaneFromWindowXY( event.viewport,
   ground_plane,
   event.x, event.y, intersection ))
   {
     moving_flag_node_->setVisible( true );
     moving_flag_node_->setPosition( intersection );
     current_flag_property_->setVector( intersection );
     if( event.leftDown() )
     {
       makeFlag( intersection );
       current_flag_property_ = NULL;
       return Render | Finished;
     }
   }
   else
   {
     moving_flag_node_->setVisible( false ); 
   }
   return Render;
 }
// Handling mouse events
// ^^^^^^^^^^^^^^^^^^^^^
//
// processMouseEvent() is sort of the main function of a Tool, because
// mouse interactions are the point of Tools.
//
// We use the utility function rviz::getPointOnPlaneFromWindowXY() to
// see where on the ground plane the user's mouse is pointing, then
// move the moving flag to that point and update the VectorProperty.
//
// If this mouse event was a left button press, we want to save the
// current flag location.  Therefore we make a new flag at the same
// place and drop the pointer to the VectorProperty.  Dropping the
// pointer means when the tool is deactivated the VectorProperty won't
// be deleted, which is what we want.
int PottingTool::processMouseEvent( rviz::ViewportMouseEvent& event )
{
  if( !moving_flag_node_ )
  {
    return Render;
  }
  Ogre::Vector3 intersection;
  Ogre::Plane ground_plane( Ogre::Vector3::UNIT_Z, 0.0f );
  if( rviz::getPointOnPlaneFromWindowXY( event.viewport,
                                         ground_plane,
                                         event.x, event.y, intersection ))
  {
    // set the area the obstacle can be settled
    if ((abs(intersection.x)<10)&&(abs(intersection.y)<10))
    {
      moving_flag_node_->setVisible( true );
      moving_flag_node_->setPosition( intersection );
      current_flag_property_->setVector( intersection );

      if( event.leftDown() )
      {
        makeFlag( intersection );
        ofstream myfile("obstacles.txt",ios::app);  //example.txt是你要输出的文件的名字
        if(!myfile)
        {
          cout<<"error !";
        }
        else
        {
          myfile<<2<<endl<<intersection.x+6.5<<endl<<intersection.y+6.5<<endl;
          myfile.close();
        }
        current_flag_property_ = NULL; // Drop the reference so that deactivate() won't remove it.
        return Render | Finished;
      }
      
    }
    
  }
  else
  {
    moving_flag_node_->setVisible( false ); // If the mouse is not pointing at the ground plane, don't show the flag.
  }
  return Render;
}
示例#4
0
// Handling mouse events
// ^^^^^^^^^^^^^^^^^^^^^
//
// processMouseEvent() is sort of the main function of a Tool, because
// mouse interactions are the point of Tools.
//
// We use the utility function rviz::getPointOnPlaneFromWindowXY() to
// see where on the ground plane the user's mouse is pointing, then
// move the moving flag to that point and update the VectorProperty.
//
// If this mouse event was a left button press, we want to save the
// current flag location.  Therefore we make a new flag at the same
// place and drop the pointer to the VectorProperty.  Dropping the
// pointer means when the tool is deactivated the VectorProperty won't
// be deleted, which is what we want.
int DogTool::processMouseEvent( rviz::ViewportMouseEvent& event )
{
  if( !moving_flag_node_ )
  {
    return Render;
  }
  Ogre::Vector3 intersection;
  Ogre::Plane ground_plane( Ogre::Vector3::UNIT_Z, 0.0f );
  if( rviz::getPointOnPlaneFromWindowXY( event.viewport,
                                         ground_plane,
                                         event.x, event.y, intersection ))
  {
    // set the area the obstacle can be settled
    if ((abs(intersection.x)<10)&&(abs(intersection.y)<10))
    {
      moving_flag_node_->setVisible( true );
      moving_flag_node_->setPosition( intersection );
      current_flag_property_->setVector( intersection );

      if( event.leftDown() )
      {
        makeFlag( intersection );


        char *p=getenv("USER");
        string username(p);
        string filepath="/home/";
        filepath+=username;
        filepath+="/cafe_robot_single/obstacles.txt";
        char* c;
        int len = filepath.length();
        c =new char[len+1];
        strcpy(c,filepath.c_str());
        ofstream myfile(c,ios::app);//example.txt是你要输出的文件的名字

        filepath="/home/";
        filepath+=username;
        filepath+="/cafe_robot_single/nav_staff/map/office_map_manual.yaml";
        char* d;
        len = filepath.length();
        d =new char[len+1];
        strcpy(d,filepath.c_str());
        fstream file;
        file.open(d);
        string text;
        int i=0;
        while(i<6)
        {
          file>>text;
          i++;
        } 
        stringstream ss(text);
        float f1,f2;
        char c1,c2;
        ss>>c1>>f1>>c2;
        file>>text;
        stringstream ss1(text);
        ss1>>f2>>c2;
        file.close();

        if(!myfile)
        {
          cout<<"error !";
        }
        else
        {
          myfile<<4<<endl<<intersection.x-f1<<endl<<intersection.y-f2<<endl;
          myfile.close();
        }
        current_flag_property_ = NULL; // Drop the reference so that deactivate() won't remove it.
        return Render | Finished;
      }
      
    }
示例#5
0
// Handling mouse events
// ^^^^^^^^^^^^^^^^^^^^^
//
// processMouseEvent() is sort of the main function of a Tool, because
// mouse interactions are the point of Tools.
//
// We use the utility function rviz::getPointOnPlaneFromWindowXY() to
// see where on the ground plane the user's mouse is pointing, then
// move the moving flag to that point and update the VectorProperty.
//
// If this mouse event was a left button press, we want to save the
// current flag location.  Therefore we make a new flag at the same
// place and drop the pointer to the VectorProperty.  Dropping the
// pointer means when the tool is deactivated the VectorProperty won't
// be deleted, which is what we want.
int FlagTool3D::processMouseEvent( rviz::ViewportMouseEvent& event )
{

    if( !moving_flag_node_ )
    {
        return Render;
    }

    Ogre::Vector3 intersection,intersection1;
    Ogre::Plane ground_plane( Ogre::Vector3::UNIT_Z, 0.0f );



    if( rviz::getPointOnPlaneFromWindowXY( event.viewport,
                                           ground_plane,
                                           event.x, event.y, intersection ))
    {
        //moving_flag_node_->setVisible( true );
        moving_flag_node_->setPosition( intersection );
        current_flag_property_->setVector( intersection );

        if( event.leftDown() )
        {
            std::cout << "Computing projection for " << pcd.size() << " points.\n";
            for( size_t i = 0; i < pcd.size(); i++ ) {
                Ogre::Vector3 temp3D(pcd.points[i].x,pcd.points[i].y,pcd.points[i].z);
                Ogre::Vector2 temp2D = rviz::project3DPointToViewportXY(event.viewport,temp3D);
                //std::cout << "\t3D - x: " << temp3D.x << " y: " << temp3D.y << " z: " << temp3D.z << "\t";
                //std::cout << "2D - x: " << temp2D.x << " y: " << temp2D.y << "\n";
                correspondences.push_back(std::make_pair(temp3D,temp2D));
            }

            Ogre::Vector2 mouse_pos(event.x,event.y),closest;
            std::cout << "Mouse position - x: " << event.x << " y: " << event.y << "\n";

            std::cout << "Computing distances for " << correspondences.size() << " points:\n";
            double min_dist = 1000000;
            for(std::vector<std::pair<Ogre::Vector3,Ogre::Vector2> >::iterator it=correspondences.begin(); it != correspondences.end(); ++it){
                std::pair<Ogre::Vector3,Ogre::Vector2> pair = *it;
                double dist = FlagTool3D::computeDistance(mouse_pos,pair.second);
                //std::cout << "\t2D Point - x: " << pair.second.x << " y: " << pair.second.y << "\tdistance: " << dist << "\n";
                if(dist < min_dist) {
                    min_dist = dist;
                    intersection1 = pair.first;
                    closest = pair.second;
                }
            }
            std::cout << "Closest point - x: " << closest.x << " y: " << closest.y << "\n";
            std::cout << "Distance: " << min_dist << "\n";
            std::cout << "3D point - x: " << intersection1.x << " y: " << intersection1.y << " z: " << intersection1.z << "\n";

            moving_flag_node_->setVisible( true );

            makeFlag( intersection1 );

            //std::cout << "Ogre 3D point - x: " << intersection.x << " y: " << intersection.y << " z: " << intersection.z << "\n";
            //std::cout << "--------------------------------------------------------------------------------------------------\n";

            //Adding flag pose to the list
            geometry_msgs::Pose wp_pose;
            wp_pose.position.x = intersection1.x;
            wp_pose.position.y = intersection1.y;
            wp_pose.position.z = intersection1.z;
            wplist.poses.push_back(wp_pose);

            current_flag_property_ = NULL; // Drop the reference so that deactivate() won't remove it.
            return Render | Finished;
        }

        if( event.rightDown() ){
            //Publishing the flag pose
            pub.publish(wplist);

            return Render | Finished;
        }
    }
    else
    {
        moving_flag_node_->setVisible( false ); // If the mouse is not pointing at the ground plane, don't show the flag.
    }
    correspondences.clear();
    return Render;

}