00001 #include <ros/ros.h> 00002 #include <nav_msgs/GetMap.h> 00003 #include <std_srvs/Trigger.h> 00004 #include <tf/transform_listener.h> 00005 #include <actionlib/server/simple_action_server.h> 00006 #include <pluginlib/class_loader.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 <nav2d_navigator/GridMap.h> 00013 #include <nav2d_navigator/commands.h> 00014 #include <nav2d_navigator/MapInflationTool.h> 00015 #include <nav2d_navigator/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 receiveStop(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res); 00032 bool receivePause(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res); 00033 void receiveMoveGoal(const nav2d_navigator::MoveToPosition2DGoal::ConstPtr &goal); 00034 void receiveExploreGoal(const nav2d_navigator::ExploreGoal::ConstPtr &goal); 00035 void receiveGetMapGoal(const nav2d_navigator::GetFirstMapGoal::ConstPtr &goal); 00036 void receiveLocalizeGoal(const nav2d_navigator::LocalizeGoal::ConstPtr &goal); 00037 00038 private: 00039 bool isLocalized(); 00040 bool setCurrentPosition(); 00041 bool getMap(); 00042 void stop(); 00043 bool correctGoalPose(); 00044 bool generateCommand(); 00045 bool preparePlan(); 00046 bool createPlan(); 00047 void publishPlan(); 00048 00049 // Everything related to ROS 00050 tf::TransformListener mTfListener; 00051 ros::ServiceClient mGetMapClient; 00052 ros::Subscriber mGoalSubscriber; 00053 ros::Publisher mPlanPublisher; 00054 ros::Publisher mCommandPublisher; 00055 ros::Publisher mMarkerPublisher; 00056 ros::ServiceServer mStopServer; 00057 ros::ServiceServer mPauseServer; 00058 00059 std::string mMapFrame; 00060 std::string mRobotFrame; 00061 std::string mMoveActionTopic; 00062 std::string mExploreActionTopic; 00063 std::string mGetMapActionTopic; 00064 std::string mLocalizeActionTopic; 00065 00066 MoveActionServer* mMoveActionServer; 00067 ExploreActionServer* mExploreActionServer; 00068 GetMapActionServer* mGetMapActionServer; 00069 LocalizeActionServer* mLocalizeActionServer; 00070 00071 PlanLoader* mPlanLoader; 00072 00073 // Current status and goals 00074 bool mHasNewMap; 00075 bool mIsPaused; 00076 bool mIsStopped; 00077 int mStatus; 00078 int mRobotID; 00079 unsigned int mGoalPoint; 00080 unsigned int mStartPoint; 00081 double mCurrentDirection; 00082 double mCurrentPositionX; 00083 double mCurrentPositionY; 00084 00085 // Everything related to the global map and plan 00086 MapInflationTool mInflationTool; 00087 std::string mExplorationStrategy; 00088 boost::shared_ptr<ExplorationPlanner> mExplorationPlanner; 00089 GridMap mCurrentMap; 00090 double* mCurrentPlan; 00091 00092 double mInflationRadius; 00093 double mRobotRadius; 00094 unsigned int mCellInflationRadius; 00095 unsigned int mCellRobotRadius; 00096 00097 char mCostObstacle; 00098 char mCostLethal; 00099 00100 double mNavigationGoalDistance; 00101 double mNavigationGoalAngle; 00102 double mNavigationHomingDistance; 00103 double mExplorationGoalDistance; 00104 double mMinReplanningPeriod; 00105 double mMaxReplanningPeriod; 00106 };