Rviz 2d Nav goal tool

edit

There are a few changes in rviz source code that allow for "single click" property to be added or deactivated in the 2d Nav Goal. You can either use rviz from my rviz repo or do the following changes yourself. Note that when you apply the changes you need to go in rviz window, panels -> tool properties -> 2d nav goal and change the property "single click":

1) in src -> rviz -> default plug in -> tools -> goal tool.cpp :

add the includes:

#include "rviz/properties/bool_property.h"
#include <OgrePlane.h>
#include <OgreRay.h>
#include <OgreSceneNode.h>
#include <OgreViewport.h>
#include "rviz/geometry.h"
#include "rviz/ogre_helpers/arrow.h"
#include "rviz/viewport_mouse_event.h"
#include "rviz/load_resource.h"
#include "rviz/render_panel.h"

add in line 50 after the topic_property_ and inside the GoalTool():

 auto_deactivate_property_ = new BoolProperty( "Single click", true, "Switch away from this tool after one click.", getPropertyContainer(), SLOT( updateAutoDeactivate() ), this );

add in line 59 inside the void GoalTool::onInitialize():

// THIS IS FOR ARROW SIZE TUNING
    // DELETED PoseTool::onInitialize();
    arrow_ = new Arrow( scene_manager_, NULL, 0.25f, 0.07f, 0.15f, 0.2f ); // ADDED
    arrow_->setColor( 0.0f, 0.0f, 1.0f, 1.0f ); // ADDED
    arrow_->getSceneNode()->setVisible( false ); // ADDED

add in line 67 after the topic_property_ = new StringProperty() :

void GoalTool::updateAutoDeactivate()  {  }

in in line 85 after the void GoalTool::onPoseSet() function:

// ADD THE WHOLE FUNCTION
int GoalTool::processMouseEvent( ViewportMouseEvent& event )
{
  int flags = 0;

  if( event.leftDown() )
  {
    ROS_ASSERT( state_ == Position );

    Ogre::Vector3 intersection;
    Ogre::Plane ground_plane( Ogre::Vector3::UNIT_Z, 0.0f );
    if( getPointOnPlaneFromWindowXY( event.viewport,
                                     ground_plane,
                                     event.x, event.y, intersection ))
    {
      pos_ = intersection;
      arrow_->setPosition( pos_ );

      state_ = Orientation;
      flags |= Render;
    }
  }
  else if( event.type == QEvent::MouseMove && event.left() )
  {
    if( state_ == Orientation )
    {
      //compute angle in x-y plane
      Ogre::Vector3 cur_pos;
      Ogre::Plane ground_plane( Ogre::Vector3::UNIT_Z, 0.0f );
      if( getPointOnPlaneFromWindowXY( event.viewport,
                                       ground_plane,
                                       event.x, event.y, cur_pos ))
      {
        double angle = atan2( cur_pos.y - pos_.y, cur_pos.x - pos_.x );

        arrow_->getSceneNode()->setVisible( true );

        //we need base_orient, since the arrow goes along the -z axis by default (for historical reasons)
        Ogre::Quaternion orient_x = Ogre::Quaternion( Ogre::Radian(-Ogre::Math::HALF_PI), Ogre::Vector3::UNIT_Y );

        arrow_->setOrientation( Ogre::Quaternion( Ogre::Radian(angle), Ogre::Vector3::UNIT_Z ) * orient_x );

        flags |= Render;
      }
    }
  }
  else if( event.leftUp() )
  {
    if( state_ == Orientation )
    {
      //compute angle in x-y plane
      Ogre::Vector3 cur_pos;
      Ogre::Plane ground_plane( Ogre::Vector3::UNIT_Z, 0.0f );
      if( getPointOnPlaneFromWindowXY( event.viewport,
                                       ground_plane,
                                       event.x, event.y, cur_pos ))
      {
        double angle = atan2( cur_pos.y - pos_.y, cur_pos.x - pos_.x );

        onPoseSet(pos_.x, pos_.y, angle);
        if ( auto_deactivate_property_->getBool() )
        {
        flags |= (Finished|Render);
        }
         else if (!auto_deactivate_property_->getBool())
        {
            state_ = Position;
            arrow_->getSceneNode()->setVisible( false );
        } // ADDED TILL HERE

      }
    }
  }
  return flags;
}

2) in src -> rviz -> default plug in -> tools -> goal tool.h :

add in line 39 after # include "rviz/default_plugin/tools/pose_tool.h"

# include <QObject>

add in line 46 after class StringProperty :

class BoolProperty;

add in line 58 after virtual void onInitialize(); :

virtual int processMouseEvent( ViewportMouseEvent& event ); // ADDED

add in line 65 after: Void updateTopic():

void updateAutoDeactivate();

add in line 73 after StringProperty* topic_property_; :

protected: 
  BoolProperty* auto_deactivate_property_;