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


nav2d_navigator
Author(s): Sebastian Kasperski
autogenerated on Mon Oct 6 2014 02:44:06