(1) I create a toolbar

toolbar_ = new QToolBar( "Tools" );

(2) add 2dNavGoal and Interact

myToolmanagr_ = manager_->getToolManager();

myInteractionTool_ = myToolmanagr_->addTool("rviz/Interact");

 addTool(myInteractionTool_);  

 myNav_goal_tool_ = myToolmanagr_->addTool("rviz/SetGoal"); 

 addTool(myNav_goal_tool_);

(3) add Tool function

void MyViz::addTool( rviz::Tool* tool )
{

  QAction* action = new QAction( tool->getName(), toolbar_actions_ );

  action->setIcon( tool->getIcon() );

  action->setIconText( tool->getName() );

  action->setCheckable( true );

  toolbar_->addAction( action );

  action_to_tool_map_[ action ] = tool;

  tool_to_action_map_[ tool ] = action; 

}

But 2dNavGoal tool doesn't work when I press Button to send Goal, the arrow also doesn't change to green arrow.