00001 #ifndef COSTMAP_SERVICES_COSTMAP_SERVICES_H_ 00002 #define COSTMAP_SERVICES_COSTMAP_SERVICES_H_ 00003 #include <ros/ros.h> 00004 #include <costmap_services/GetCost.h> // service type 00005 #include <costmap_services/ScoreTraj.h> // service type 00006 #include <costmap_2d/costmap_2d_ros.h> 00007 #include <costmap_2d/costmap_2d.h> 00008 #include <base_local_planner/trajectory_planner_ros.h> 00009 #include <boost/shared_ptr.hpp> 00010 #include <boost/thread.hpp> 00011 00012 namespace costmap_services { 00013 class CostmapServices { 00014 public: 00015 CostmapServices( std::string name ); 00016 ~CostmapServices(); 00017 00018 bool getCost( costmap_services::GetCost::Request &req, 00019 costmap_services::GetCost::Response &res ); 00020 00021 00022 bool scoreTraj( costmap_services::ScoreTraj::Request &req, 00023 costmap_services::ScoreTraj::Response &res ); 00024 00025 private: 00026 tf::TransformListener tf_; 00027 costmap_2d::Costmap2DROS costmap_ros_; 00028 00029 base_local_planner::TrajectoryPlannerROS planner_; 00030 geometry_msgs::Twist cmd_vel_; 00031 double controller_frequency_; 00032 double theta_range_; 00033 int num_th_samples_, num_x_samples_; 00034 00035 ros::ServiceServer costmap_srv_; 00036 ros::ServiceServer scoreTraj_srv_; 00037 00038 boost::mutex mutex_; 00039 ros::Publisher pub_; 00040 ros::Publisher pub_score_; 00041 ros::Subscriber sub_; 00042 }; 00043 }; 00044 #endif