NavViewPanel::NavViewPanel( wxWindow* parent )
: NavViewPanelGenerated( parent )
, ogre_root_( Ogre::Root::getSingletonPtr() )
, map_origin_x_(0.0)
, map_origin_y_(0.0)
, map_object_( NULL )
, cloud_object_( NULL )
, radius_object_( NULL )
, global_plan_object_( NULL )
, local_plan_object_( NULL )
, footprint_object_( NULL )
, inflated_obstacles_object_( NULL )
, raw_obstacles_object_( NULL )
, laser_scan_object_( NULL )
, mouse_x_(0)
, mouse_y_(0)
, scale_(10.0f)
, current_tool_( NULL )
, first_map_(true)
{
  tf_client_ = new tf::TransformListener();

  scene_manager_ = ogre_root_->createSceneManager( Ogre::ST_GENERIC );
  root_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
  render_panel_ = new ogre_tools::wxOgreRenderWindow( ogre_root_, this );
  render_sizer_->Add( render_panel_, 1, wxALL|wxEXPAND, 0 );

  camera_ = scene_manager_->createCamera( "OrthoTopDown" );
  camera_->setProjectionType( Ogre::PT_ORTHOGRAPHIC );
  camera_->setPosition(0.0f, 0.0f, 30.0f);
  camera_->lookAt( 0.0f, 0.0f, 0.0f );
  camera_->setNearClipDistance(0.001f);
  camera_->setFarClipDistance(50.0f);

  render_panel_->getViewport()->setCamera( camera_ );

  nh_.param("/global_frame_id", global_frame_id_, std::string("/map"));

  particle_cloud_sub_ = nh_.subscribe("particlecloud", 1, &NavViewPanel::incomingPoseArray, this);

  //subscribe to the map as a latched topic
  map_sub_ = nh_.subscribe("map", 1, &NavViewPanel::mapCallback, this);

  global_plan_sub_.subscribe(nh_, "global_plan", 2);
  local_plan_sub_.subscribe(nh_, "local_plan", 2);
  robot_footprint_sub_.subscribe(nh_, "robot_footprint", 2);
  inflated_obs_sub_.subscribe(nh_, "inflated_obstacles", 2);
  raw_obs_sub_.subscribe(nh_, "obstacles", 2);
  global_plan_filter_.reset(new PathFilter(global_plan_sub_, *tf_client_, global_frame_id_, 2));
  local_plan_filter_.reset(new PathFilter(local_plan_sub_, *tf_client_, global_frame_id_, 2));
  robot_footprint_filter_.reset(new PolygonFilter(robot_footprint_sub_, *tf_client_, global_frame_id_, 2));
  inflated_obs_filter_.reset(new GridCellsFilter(inflated_obs_sub_, *tf_client_, global_frame_id_, 2));
  raw_obs_filter_.reset(new GridCellsFilter(raw_obs_sub_, *tf_client_, global_frame_id_, 2));

  global_plan_filter_->registerCallback(boost::bind(&NavViewPanel::incomingGuiPath, this, _1));
  local_plan_filter_->registerCallback(boost::bind(&NavViewPanel::incomingLocalPath, this, _1));
  robot_footprint_filter_->registerCallback(boost::bind(&NavViewPanel::incomingRobotFootprint, this, _1));
  inflated_obs_filter_->registerCallback(boost::bind(&NavViewPanel::incomingInflatedObstacles, this, _1));
  raw_obs_filter_->registerCallback(boost::bind(&NavViewPanel::incomingRawObstacles, this, _1));

  render_panel_->Connect( wxEVT_LEFT_DOWN, wxMouseEventHandler( NavViewPanel::onRenderWindowMouseEvents ), NULL, this );
  render_panel_->Connect( wxEVT_MIDDLE_DOWN, wxMouseEventHandler( NavViewPanel::onRenderWindowMouseEvents ), NULL, this );
  render_panel_->Connect( wxEVT_RIGHT_DOWN, wxMouseEventHandler( NavViewPanel::onRenderWindowMouseEvents ), NULL, this );
  render_panel_->Connect( wxEVT_MOTION, wxMouseEventHandler( NavViewPanel::onRenderWindowMouseEvents ), NULL, this );
  render_panel_->Connect( wxEVT_LEFT_UP, wxMouseEventHandler( NavViewPanel::onRenderWindowMouseEvents ), NULL, this );
  render_panel_->Connect( wxEVT_MIDDLE_UP, wxMouseEventHandler( NavViewPanel::onRenderWindowMouseEvents ), NULL, this );
  render_panel_->Connect( wxEVT_RIGHT_UP, wxMouseEventHandler( NavViewPanel::onRenderWindowMouseEvents ), NULL, this );
  render_panel_->Connect( wxEVT_MOUSEWHEEL, wxMouseEventHandler( NavViewPanel::onRenderWindowMouseEvents ), NULL, this );

  render_panel_->SetFocus();

  render_panel_->Connect( wxEVT_CHAR, wxKeyEventHandler( NavViewPanel::onChar ), NULL, this );

  Connect( EVT_RENDER, wxCommandEventHandler( NavViewPanel::onRender ), NULL, this );

  render_panel_->setOrthoScale( scale_ );

  update_timer_ = new wxTimer( this );
  Connect( update_timer_->GetId(), wxEVT_TIMER, wxTimerEventHandler( NavViewPanel::onUpdate ), NULL, this );
  update_timer_->Start( 100 );

  createRadiusObject();

  current_tool_ = new MoveTool( this );
}
示例#2
0
NavViewPanel::NavViewPanel( wxWindow* parent )
: NavViewPanelGenerated( parent )
, ogre_root_( Ogre::Root::getSingletonPtr() )
, map_object_( NULL )
, cloud_object_( NULL )
, radius_object_( NULL )
, path_line_object_( NULL )
, local_path_object_( NULL )
, footprint_object_( NULL )
, inflated_obstacles_object_( NULL )
, raw_obstacles_object_( NULL )
, laser_scan_object_( NULL )
, mouse_x_(0)
, mouse_y_(0)
, scale_(10.0f)
, new_cloud_( false )
, new_gui_path_( false )
, new_local_path_( false )
, new_robot_footprint_( false )
, new_inflated_obstacles_( false )
, new_raw_obstacles_( false )
, new_gui_laser_( false )
, new_occ_diff_( false )
, current_tool_( NULL )
{
  ros_node_ = ros::node::instance();

  /// @todo This should go away once creation of the ros::node is more well-defined
  if (!ros_node_)
  {
    int argc = 0;
    ros::init( argc, 0 );
    ros_node_ = new ros::node( "NavViewPanel", ros::node::DONT_HANDLE_SIGINT );
  }
  ROS_ASSERT( ros_node_ );

  tf_client_ = new rosTFClient( *ros_node_ );

  scene_manager_ = ogre_root_->createSceneManager( Ogre::ST_GENERIC );
  root_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
  render_panel_ = new ogre_tools::wxOgreRenderWindow( ogre_root_, this );
  render_sizer_->Add( render_panel_, 1, wxALL|wxEXPAND, 0 );

  camera_ = scene_manager_->createCamera( "OrthoTopDown" );
  camera_->setProjectionType( Ogre::PT_ORTHOGRAPHIC );
  camera_->setPosition(0.0f, 0.0f, 30.0f);
  camera_->lookAt( 0.0f, 0.0f, 0.0f );
  camera_->setNearClipDistance(0.001f);
  camera_->setFarClipDistance(50.0f);

  render_panel_->getViewport()->setCamera( camera_ );

  ros_node_->advertise<std_msgs::Planner2DGoal>("goal", 1);
  ros_node_->advertise<std_msgs::Pose2DFloat32>("initialpose", 1);
  ros_node_->subscribe("particlecloud", cloud_, &NavViewPanel::incomingParticleCloud, this, 1);
  ros_node_->subscribe("gui_path", path_line_, &NavViewPanel::incomingGuiPath, this, 1);
  ros_node_->subscribe("local_path", local_path_, &NavViewPanel::incomingLocalPath, this, 1);
  ros_node_->subscribe("robot_footprint", robot_footprint_, &NavViewPanel::incomingRobotFootprint, this, 1);
  ros_node_->subscribe("inflated_obstacles", inflated_obstacles_, &NavViewPanel::incomingInflatedObstacles, this, 1);
  ros_node_->subscribe("raw_obstacles", raw_obstacles_, &NavViewPanel::incomingRawObstacles, this, 1);
  ros_node_->subscribe("gui_laser", laser_scan_, &NavViewPanel::incomingGuiLaser, this, 1);

  render_panel_->Connect( wxEVT_LEFT_DOWN, wxMouseEventHandler( NavViewPanel::onRenderWindowMouseEvents ), NULL, this );
  render_panel_->Connect( wxEVT_MIDDLE_DOWN, wxMouseEventHandler( NavViewPanel::onRenderWindowMouseEvents ), NULL, this );
  render_panel_->Connect( wxEVT_RIGHT_DOWN, wxMouseEventHandler( NavViewPanel::onRenderWindowMouseEvents ), NULL, this );
  render_panel_->Connect( wxEVT_MOTION, wxMouseEventHandler( NavViewPanel::onRenderWindowMouseEvents ), NULL, this );
  render_panel_->Connect( wxEVT_LEFT_UP, wxMouseEventHandler( NavViewPanel::onRenderWindowMouseEvents ), NULL, this );
  render_panel_->Connect( wxEVT_MIDDLE_UP, wxMouseEventHandler( NavViewPanel::onRenderWindowMouseEvents ), NULL, this );
  render_panel_->Connect( wxEVT_RIGHT_UP, wxMouseEventHandler( NavViewPanel::onRenderWindowMouseEvents ), NULL, this );
  render_panel_->Connect( wxEVT_MOUSEWHEEL, wxMouseEventHandler( NavViewPanel::onRenderWindowMouseEvents ), NULL, this );

  render_panel_->SetFocus();

  render_panel_->Connect( wxEVT_CHAR, wxKeyEventHandler( NavViewPanel::onChar ), NULL, this );

  Connect( EVT_RENDER, wxCommandEventHandler( NavViewPanel::onRender ), NULL, this );

  render_panel_->setOrthoScale( scale_ );

  update_timer_ = new wxTimer( this );
  Connect( update_timer_->GetId(), wxEVT_TIMER, wxTimerEventHandler( NavViewPanel::onUpdate ), NULL, this );
  update_timer_->Start( 100 );

  createRadiusObject();

  current_tool_ = new MoveTool( this );
}