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_provider/waypoint_provider.hpp" 00011 #include "yocs_waypoint_provider/yaml_parser.hpp" 00012 #include <yocs_msgs/WaypointList.h> 00013 00014 int main(int argc, char** argv) 00015 { 00016 ros::init(argc, argv, "waypoint_provider"); 00017 ros::NodeHandle priv_n("~"); 00018 ros::NodeHandle n; 00019 yocs::WaypointProvider* wm; 00020 yocs_msgs::WaypointList wps; 00021 yocs_msgs::TrajectoryList trajs; 00022 std::string filename; 00023 00024 if(!priv_n.getParam("filename", filename)) { 00025 ROS_ERROR("Waypoint Provider : filename argument is not set"); 00026 return -1; 00027 } 00028 00029 if(!yocs::loadWaypointsAndTrajectoriesFromYaml(filename, wps, trajs)) 00030 { 00031 ROS_ERROR("Waypoint Provider : Failed to parse yaml[%s]",filename.c_str()); 00032 return -1; 00033 } 00034 00035 wm = new yocs::WaypointProvider(n, wps, trajs); 00036 00037 ROS_INFO("Waypoint Provider : Initialized"); 00038 wm->spin(); 00039 ROS_INFO("Waypoint Provider : Bye Bye"); 00040 00041 delete wm; 00042 00043 return 0; 00044 } 00045