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;
}
