00001 #ifndef OPERATOR_H 00002 #define OPERATOR_H 00003 00004 #define NODE_NAME "operator" 00005 #define COMMAND_TOPIC "cmd" 00006 #define CONTROL_TOPIC "cmd_vel" 00007 #define ROUTE_TOPIC "route" 00008 #define PLAN_TOPIC "desired" 00009 #define LUT_RESOLUTION 100 00010 00011 #include <ros/ros.h> 00012 #include <costmap_2d/costmap_2d_ros.h> 00013 #include <robot_operator/cmd.h> 00014 #include <sensor_msgs/PointCloud.h> 00015 00016 #include "string" 00017 00018 using namespace ros; 00019 using namespace tf; 00020 using namespace costmap_2d; 00021 00022 class RobotOperator 00023 { 00024 public: 00025 RobotOperator(NodeHandle* n); 00026 ~RobotOperator(); 00027 00028 void receiveCommand(const robot_operator::cmd::ConstPtr& msg); 00029 void executeCommand(); 00030 00031 private: 00032 int calculateFreeSpace(sensor_msgs::PointCloud* cloud); 00033 double evaluateAction(double direction, double velocity, bool debug = false); 00034 double findBestDirection(); 00035 void initTrajTable(); 00036 00037 inline sensor_msgs::PointCloud* getPointCloud(double direction, double velocity); 00038 00039 Costmap2DROS* mLocalMap; 00040 Costmap2D* mCostmap; 00041 double mRasterSize; 00042 00043 TransformListener mTfListener; 00044 00045 Subscriber mCommandSubscriber; 00046 Publisher mControlPublisher; 00047 Publisher mTrajectoryPublisher; 00048 Publisher mPlanPublisher; 00049 Publisher mCostPublisher; 00050 00051 double mDesiredVelocity; 00052 double mDesiredDirection; 00053 double mCurrentVelocity; 00054 double mCurrentDirection; 00055 int mDriveMode; 00056 00057 sensor_msgs::PointCloud* mTrajTable[(LUT_RESOLUTION * 4) + 2]; 00058 00059 double mMaxVelocity; 00060 00061 bool mPublishRoute; 00062 double mMaxFreeSpace; 00063 double mSafetyDecay; 00064 int mDistanceWeight; 00065 int mSafetyWeight; 00066 int mConformanceWeight; 00067 int mContinueWeight; 00068 00069 std::string mOdometryFrame; 00070 std::string mRobotFrame; 00071 00072 unsigned int mRecoverySteps; 00073 }; 00074 00075 #endif