00001 #include <ros/ros.h> 00002 #include <nav_msgs/GetMap.h> 00003 #include <tf/transform_listener.h> 00004 #include <actionlib/server/simple_action_server.h> 00005 #include <pluginlib/class_loader.h> 00006 #include <nav2d_navigator/SendCommand.h> 00007 #include <nav2d_navigator/MoveToPosition2DAction.h> 00008 #include <nav2d_navigator/ExploreAction.h> 00009 #include <nav2d_navigator/GetFirstMapAction.h> 00010 #include <nav2d_navigator/LocalizeAction.h> 00011 00012 #include "GridMap.h" 00013 #include "commands.h" 00014 #include "MapInflationTool.h" 00015 #include "ExplorationPlanner.h" 00016 00017 #include <queue> 00018 00019 typedef actionlib::SimpleActionServer<nav2d_navigator::MoveToPosition2DAction> MoveActionServer; 00020 typedef actionlib::SimpleActionServer<nav2d_navigator::ExploreAction> ExploreActionServer; 00021 typedef actionlib::SimpleActionServer<nav2d_navigator::GetFirstMapAction> GetMapActionServer; 00022 typedef actionlib::SimpleActionServer<nav2d_navigator::LocalizeAction> LocalizeActionServer; 00023 typedef pluginlib::ClassLoader<ExplorationPlanner> PlanLoader; 00024 00025 class RobotNavigator 00026 { 00027 public: 00028 RobotNavigator(); 00029 ~RobotNavigator(); 00030 00031 bool receiveCommand(nav2d_navigator::SendCommand::Request &req, nav2d_navigator::SendCommand::Response &res); 00032 void receiveMoveGoal(const nav2d_navigator::MoveToPosition2DGoal::ConstPtr &goal); 00033 void receiveExploreGoal(const nav2d_navigator::ExploreGoal::ConstPtr &goal); 00034 void receiveGetMapGoal(const nav2d_navigator::GetFirstMapGoal::ConstPtr &goal); 00035 void receiveLocalizeGoal(const nav2d_navigator::LocalizeGoal::ConstPtr &goal); 00036 00037 private: 00038 bool setCurrentPosition(); 00039 bool getMap(); 00040 void stop(); 00041 bool correctGoalPose(); 00042 bool generateCommand(); 00043 bool preparePlan(); 00044 bool createPlan(); 00045 void publishPlan(); 00046 00047 // Everything related to ROS 00048 tf::TransformListener mTfListener; 00049 ros::ServiceClient mGetMapClient; 00050 ros::Subscriber mGoalSubscriber; 00051 ros::Publisher mPlanPublisher; 00052 ros::Publisher mCommandPublisher; 00053 ros::Publisher mMarkerPublisher; 00054 ros::ServiceServer mCommandServer; 00055 00056 std::string mMapFrame; 00057 std::string mRobotFrame; 00058 std::string mMoveActionTopic; 00059 std::string mExploreActionTopic; 00060 std::string mGetMapActionTopic; 00061 std::string mLocalizeActionTopic; 00062 00063 MoveActionServer* mMoveActionServer; 00064 ExploreActionServer* mExploreActionServer; 00065 GetMapActionServer* mGetMapActionServer; 00066 LocalizeActionServer* mLocalizeActionServer; 00067 00068 PlanLoader* mPlanLoader; 00069 00070 // Current status and goals 00071 bool mHasNewMap; 00072 bool mIsPaused; 00073 bool mIsStopped; 00074 int mStatus; 00075 int mRobotID; 00076 unsigned int mGoalPoint; 00077 unsigned int mStartPoint; 00078 double mCurrentDirection; 00079 double mCurrentPositionX; 00080 double mCurrentPositionY; 00081 00082 // Everything related to the global map and plan 00083 MapInflationTool mInflationTool; 00084 std::string mExplorationStrategy; 00085 boost::shared_ptr<ExplorationPlanner> mExplorationPlanner; 00086 GridMap mCurrentMap; 00087 double* mCurrentPlan; 00088 00089 double mInflationRadius; 00090 double mRobotRadius; 00091 unsigned int mCellInflationRadius; 00092 unsigned int mCellRobotRadius; 00093 00094 char mCostObstacle; 00095 char mCostLethal; 00096 00097 double mNavigationGoalDistance; 00098 double mNavigationGoalAngle; 00099 double mNavigationHomingDistance; 00100 double mExplorationGoalDistance; 00101 double mMinReplanningPeriod; 00102 double mMaxReplanningPeriod; 00103 };