00001 /* 00002 Way point Manager 00003 00004 highly inspired by yocs_waypoint_navi written by Jorge Santos 00005 00006 LICENSE : BSD - https://raw.github.com/yujinrobot/yujin_ocs/license/LICENSE 00007 00008 Author : Jihoon Lee 00009 Date : Dec 2013 00010 */ 00011 00012 00013 #ifndef _YOCS_WAYPOINT_MANAGER_HPP_ 00014 #define _YOCS_WAYPOINT_MANAGER_HPP_ 00015 00016 #include <ros/ros.h> 00017 #include <yocs_msgs/WaypointListService.h> 00018 #include <yocs_msgs/WaypointList.h> 00019 #include <visualization_msgs/MarkerArray.h> 00020 00021 /* 00022 It simply parses waypoints in yaml file and provides latch topic 00023 00024 Future improvements 00025 - Utilise database for persistent waypoint storing 00026 - add/remove waypoints via srv or pub/sub 00027 */ 00028 00029 namespace yocs { 00030 class WaypointManager { 00031 public: 00032 WaypointManager(ros::NodeHandle& n, yocs_msgs::WaypointList& wp); 00033 ~WaypointManager(); 00034 00035 void spin(); 00036 protected: 00037 void init(); 00038 00039 void generateVizmarkers(const yocs_msgs::WaypointList& wp, visualization_msgs::MarkerArray& wp_viz); 00040 bool processWaypointsService(yocs_msgs::WaypointListService::Request& request, yocs_msgs::WaypointListService::Response& response); 00041 00042 void createArrowMarker(const int i,const yocs_msgs::Waypoint& wp, visualization_msgs::Marker& marker); 00043 void createLabelMarker(const int i,const yocs_msgs::Waypoint& wp, visualization_msgs::Marker& marker); 00044 00045 private: 00046 bool initialized_; 00047 ros::NodeHandle nh_; 00048 ros::Publisher waypoints_pub_; 00049 ros::Publisher waypoints_viz_pub_; 00050 ros::ServiceServer waypoints_srv_; 00051 00052 yocs_msgs::WaypointList waypoints_; 00053 visualization_msgs::MarkerArray waypoints_viz_; 00054 00055 int marker_index_; 00056 int label_index_; 00057 }; 00058 } 00059 00060 #endif // _YOCS_WAYPOINT_MANAGER_HPP_