RobotNavigator.h
Go to the documentation of this file.
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 };


nav2d_navigator
Author(s): Sebastian Kasperski
autogenerated on Sun Apr 2 2017 03:53:43