void WaypointLoaderNode::createLaneArray(const std::vector<std::string> &paths, autoware_msgs::LaneArray *lane_array) { for (auto el : paths) { autoware_msgs::lane lane; createLaneWaypoint(el, &lane); lane_array->lanes.push_back(lane); } }
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(); }