00001 /* 00002 Way point Manager 00003 00004 LICENSE : BSD - https://raw.github.com/yujinrobot/yujin_ocs/license/LICENSE 00005 00006 Author : Jihoon Lee 00007 Date : Dec 2013 00008 */ 00009 00010 #include "yocs_waypoint_manager/waypoint_manager.hpp" 00011 #include "yocs_waypoint_manager/yaml_parser.hpp" 00012 #include <yocs_msgs/WaypointList.h> 00013 00014 int main(int argc, char** argv) 00015 { 00016 ros::init(argc, argv, "waypoint_manager"); 00017 ros::NodeHandle priv_n("~"); 00018 ros::NodeHandle n; 00019 yocs::WaypointManager* wm; 00020 yocs_msgs::WaypointList wps; 00021 std::string filename; 00022 00023 if(!priv_n.getParam("filename", filename)) { 00024 ROS_ERROR("Waypoint Manager : filename argument is not set"); 00025 return -1; 00026 } 00027 00028 if(!yocs::loadWaypointListFromYaml(filename, wps)) { 00029 ROS_ERROR("Waypoint Manager : Failed to parse yaml[%s]",filename.c_str()); 00030 return -1; 00031 } 00032 00033 wm = new yocs::WaypointManager(n, wps); 00034 00035 ROS_INFO("Waypoint Manager : Initialized"); 00036 wm->spin(); 00037 ROS_INFO("Waypoint Manager : Bye Bye"); 00038 00039 delete wm; 00040 00041 return 0; 00042 } 00043