int main(int argc, char **argv) { ros::init(argc, argv, "waypoint_loader"); ros::NodeHandle nh; ros::NodeHandle private_nh("~"); std::string driving_lane_csv; std::string passing_lane_csv; private_nh.param<std::string>("driving_lane_csv", driving_lane_csv, DRIVING_LANE_CSV); private_nh.param<std::string>("passing_lane_csv", passing_lane_csv, PASSING_LANE_CSV); private_nh.getParam("decelerate", _decelerate); ROS_INFO_STREAM("decelerate :" << _decelerate); ros::Publisher lane_pub = nh.advertise<waypoint_follower::LaneArray>("lane_waypoints_array", 10, true); waypoint_follower::LaneArray lane_array; if (!verifyFileConsistency(driving_lane_csv.c_str())) { ROS_ERROR("driving lane data is something wrong..."); exit(-1); } else { ROS_INFO("driving lane data is valid. publishing..."); lane_array.lanes.push_back(createLaneWaypoint(readWaypoint(driving_lane_csv.c_str()))); } if (!verifyFileConsistency(passing_lane_csv.c_str())) { ROS_INFO("no passing lane data..."); } else { ROS_INFO("passing lane data is valid. publishing..."); lane_array.lanes.push_back(createLaneWaypoint(readWaypoint(passing_lane_csv.c_str()))); } lane_pub.publish(lane_array); ros::spin(); }
void WaypointLoaderNode::createLaneWaypoint(const std::string &file_path, autoware_msgs::lane *lane) { if (!verifyFileConsistency(file_path.c_str())) { ROS_ERROR("lane data is something wrong..."); return; } ROS_INFO("lane data is valid. publishing..."); FileFormat format = checkFileFormat(file_path.c_str()); std::vector<autoware_msgs::waypoint> wps; if (format == FileFormat::ver1) loadWaypointsForVer1(file_path.c_str(), &wps); else if (format == FileFormat::ver2) loadWaypointsForVer2(file_path.c_str(), &wps); else loadWaypoints(file_path.c_str(), &wps); lane->header.frame_id = "/map"; lane->header.stamp = ros::Time(0); lane->waypoints = wps; }