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 );
}
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 );
}
