Exemplo n.º 1
0
int main(int argc, char **argv)
{

/*

#!/bin/sh
rosrun map_file vector_map_loader <csv files>

# EOF

*/

  ros::init(argc, argv, "sample_vector_map");
  ros::NodeHandle n;
  ros::Publisher pub = n.advertise<visualization_msgs::MarkerArray>("/vector_map", 1000, true);
  ros::Publisher stat_publisher = n.advertise<std_msgs::Bool>("/vmap_stat", 100);;
  std_msgs::Bool vmap_stat_msg;

  ros::Publisher pub_point_class = n.advertise<map_file::PointClassArray>(
	  "/vector_map_info/point_class", 1, true);
  ros::Publisher pub_vector_class = n.advertise<map_file::VectorClassArray>(
	  "/vector_map_info/vector_class", 1, true);
  ros::Publisher pub_line_class = n.advertise<map_file::LineClassArray>(
	  "/vector_map_info/line_class", 1, true);
  ros::Publisher pub_area_class = n.advertise<map_file::AreaClassArray>(
	  "/vector_map_info/area_class", 1, true);
  ros::Publisher pub_pole_class = n.advertise<map_file::PoleClassArray>(
	  "/vector_map_info/pole_class", 1, true);
  ros::Publisher pub_box_class = n.advertise<map_file::BoxClassArray>(
	  "/vector_map_info/box_class", 1, true);

  ros::Publisher pub_dtlane = n.advertise<map_file::DTLaneArray>(
	  "/vector_map_info/dtlane", 1, true);
  ros::Publisher pub_node = n.advertise<map_file::NodeArray>(
	  "/vector_map_info/node", 1, true);
  ros::Publisher pub_lane = n.advertise<map_file::LaneArray>(
	  "/vector_map_info/lane", 1, true);

  ros::Publisher pub_road_edge = n.advertise<map_file::RoadEdgeArray>(
	  "/vector_map_info/road_edge", 1, true);
  ros::Publisher pub_gutter = n.advertise<map_file::GutterArray>(
	  "/vector_map_info/gutter", 1, true);
  ros::Publisher pub_curb = n.advertise<map_file::CurbArray>(
	  "/vector_map_info/curb", 1, true);
  ros::Publisher pub_white_line = n.advertise<map_file::WhiteLineArray>(
	  "/vector_map_info/white_line", 1, true);
  ros::Publisher pub_stop_line = n.advertise<map_file::StopLineArray>(
	  "/vector_map_info/stop_line", 1, true);
  ros::Publisher pub_zebra_zone = n.advertise<map_file::ZebraZoneArray>(
	  "/vector_map_info/zebra_zone", 1, true);
  ros::Publisher pub_cross_walk = n.advertise<map_file::CrossWalkArray>(
	  "/vector_map_info/cross_walk", 1, true);
  ros::Publisher pub_road_mark = n.advertise<map_file::RoadMarkArray>(
	  "/vector_map_info/road_mark", 1, true);
  ros::Publisher pub_pole = n.advertise<map_file::PoleArray>(
	  "/vector_map_info/pole", 1, true);
  ros::Publisher pub_road_sign = n.advertise<map_file::RoadSignArray>(
	  "/vector_map_info/road_sign", 1, true);
  ros::Publisher pub_signal = n.advertise<map_file::SignalArray>(
	  "/vector_map_info/signal", 1, true);
  ros::Publisher pub_street_light = n.advertise<map_file::StreetLightArray>(
	  "/vector_map_info/street_light", 1, true);
  ros::Publisher pub_utility_pole = n.advertise<map_file::UtilityPoleArray>(
	  "/vector_map_info/utility_pole", 1, true);
  ros::Publisher pub_guard_rail = n.advertise<map_file::GuardRailArray>(
	  "/vector_map_info/guard_rail", 1, true);
  ros::Publisher pub_side_walk = n.advertise<map_file::SideWalkArray>(
	  "/vector_map_info/side_walk", 1, true);
  ros::Publisher pub_cross_road = n.advertise<map_file::CrossRoadArray>(
	  "/vector_map_info/cross_road", 1, true);

  std::vector<PointClass> pointclasses;
  std::vector<PoleClass> poleclasses;
  std::vector<VectorClass> vectorclasses;
  std::vector<AreaClass> areaclasses;
  std::vector<LineClass> lines;

  std::vector<Pole> poles;
  std::vector<Signal> signals;
  std::vector<RoadSign> roadsigns;
  std::vector<DTLane> dtlanes;
  std::vector<Node> nodes;
  std::vector<Lane> lanes;
  std::vector<WhiteLine> whitelines;
  std::vector<ZebraZone> zebrazones;
  std::vector<CrossWalk> crosswalks;
  std::vector<RoadEdge> roadedges;
  std::vector<RoadMark> roadmarks;
  std::vector<StopLine> stoplines;
  std::vector<CrossRoad> crossroads;
  std::vector<SideWalk> sidewalks;
  std::vector<Gutter> gutters;
  std::vector<Curb> curbs;
  std::vector<StreetLight> streetlights;
  std::vector<UtilityPole> utilitypoles;


  if(argc >= 3){
    std::string host_name = argv[1];
    int port = std::atoi(argv[2]);
    gf = GetFile(host_name, port);
  } else {
    gf = GetFile();
  }

  struct stat st;
  std::string dirname = "/data/map/japan/aichi/nagoya/moriyama/vector";
  std::string tmp_dir = "/tmp" + dirname;
  if(stat(tmp_dir.c_str(), &st) != 0) {
    std::istringstream ss(dirname);
    std::string column;
    std::getline(ss, column, '/');
    tmp_dir = "/tmp";
    while (std::getline(ss, column, '/')) {
      tmp_dir += "/" + column;
      errno = 0;
      int ret = mkdir(tmp_dir.c_str(), 0755);
      if(ret < 0 && errno != EEXIST) perror("mkdir");
    }
    std::cerr << "mkdir " << tmp_dir << std::endl;
  }

  for(auto x: vmap_csv_list) {
    std::cerr << "start get file = " <<dirname << "/" << x << " ... ";
    if(gf.GetHTTPFile(dirname+"/"+x) == 0) {
      vmap_file_list.push_back("/tmp"+dirname+"/"+x);
      std::cerr << " download done" << std::endl;
    } else {
      std::cerr << " download failed" << std::endl;
    }
  }

  std::cerr << "Load csv files" << std::endl;

  for(auto x: vmap_file_list) {
    std::string name(basename((char *)x.c_str()));

    if(name == "point.csv") {
      pointclasses = read_pointclass(x.c_str());
      std::cerr << "  load " << x.c_str() 
		<< ", pointclasses.size()=" <<  pointclasses.size() << std::endl;
      pub_point_class.publish(pack_point_class_array(pointclasses));
    } else if(name == "pole.csv") {
      poleclasses = read_poleclass(x.c_str());
      std::cerr << "  load " << x.c_str()
		<< ", poleclasses.size()=" <<  poleclasses.size() << std::endl;
      pub_pole_class.publish(pack_pole_class_array(poleclasses));
    } else if(name == "vector.csv") {
      vectorclasses = read_vectorclass(x.c_str());
      std::cerr << "  load " << x.c_str() 
		<< ", vectorclasses.size()" << vectorclasses.size() << std::endl;
      pub_vector_class.publish(pack_vector_class_array(vectorclasses));
    } else if(name == "area.csv") {
      areaclasses = read_areaclass(x.c_str());
      std::cerr << "  load " << x.c_str() 
		<< ", areaclasses.size()=" << areaclasses.size() << std::endl;
      pub_area_class.publish(pack_area_class_array(areaclasses));
    } else if(name == "line.csv") {
      lines = read_lineclass(x.c_str());
      std::cerr << "  load " << x.c_str() 
		<< ", lines.size()=" << lines.size() << std::endl;
      pub_line_class.publish(pack_line_class_array(lines));
    } else if(name == "poledata.csv") {
      poles = read_pole(x.c_str());
      std::cerr << "  load " << x.c_str() 
		<< ", poles.size()=" << poles.size() << std::endl;
      pub_pole.publish(pack_pole_array(poles));
    } else if(name == "signaldata.csv") {
      signals = read_signal(x.c_str());
      std::cerr << "  load " << x.c_str() 
		<< ", signals.size()=" << signals.size() << std::endl;
      pub_signal.publish(pack_signal_array(signals));
    } else if(name == "roadsign.csv") {
      roadsigns = read_roadsign(x.c_str());
      std::cerr << "  load " << x.c_str() 
		<< ", roadsigns.size()=" << roadsigns.size() << std::endl;
      pub_road_sign.publish(pack_roadsign_array(roadsigns));
    } else if(name == "dtlane.csv") {
      dtlanes = read_dtlane(x.c_str());
      std::cerr << "  load " << x.c_str() 
		<< ", dtlanes.size()=" << dtlanes.size() << std::endl;
      pub_dtlane.publish(pack_dtlane_array(dtlanes));
    } else if(name == "node.csv") {
      nodes = read_node(x.c_str());
      std::cerr << "  load " << x.c_str() 
		<< ", nodes.size()=" << nodes.size() << std::endl;
      pub_node.publish(pack_node_array(nodes));
    } else if(name == "lane.csv") {
      lanes = read_lane(x.c_str());
      std::cerr << "  load " << x.c_str() 
		<< ", lanes.size()=" << lanes.size() << std::endl;
      pub_lane.publish(pack_lane_array(lanes));
    } else if(name == "whiteline.csv") {
      whitelines = read_whiteline(x.c_str());
      std::cerr << "  load " << x.c_str() 
		<< ", whitelines.size()=" << whitelines.size() << std::endl;
      pub_white_line.publish(pack_whiteline_array(whitelines));
    } else if(name == "zebrazone.csv") {
      zebrazones = read_zebrazone(x.c_str());
      std::cerr << "  load " << x.c_str() 
		<< ", zebrazones.size()=" << zebrazones.size() << std::endl;
      pub_zebra_zone.publish(pack_zebrazone_array(zebrazones));
    } else if(name == "crosswalk.csv") {
      crosswalks = read_crosswalk(x.c_str());
      std::cerr << "  load " << x.c_str() 
		<< ", crosswalks.size()=" << crosswalks.size() << std::endl;
      pub_cross_walk.publish(pack_crosswalk_array(crosswalks));
    } else if(name == "roadedge.csv") {
      roadedges = read_roadedge(x.c_str());
      std::cerr << "  load " << x.c_str() 
		<< ", roadedges.size()=" << roadedges.size() << std::endl;
      pub_road_edge.publish(pack_roadedge_array(roadedges));
    } else if(name == "road_surface_mark.csv") {
      roadmarks = read_roadmark(x.c_str());
      std::cerr << "  load " << x.c_str() 
		<< ", roadmarks.size()=" << roadmarks.size() << std::endl;
      pub_road_mark.publish(pack_roadmark_array(roadmarks));
    } else if(name == "stopline.csv") {
      stoplines = read_stopline(x.c_str());
      std::cerr << "  load " << x.c_str() 
		<< ", stoplines.size()=" << stoplines.size() << std::endl;
      pub_stop_line.publish(pack_stopline_array(stoplines));
    } else if(name == "crossroads.csv") {
      crossroads = read_crossroad(x.c_str());
      std::cerr << "  load " << x.c_str() 
		<< ", crossroads.size()=" << crossroads.size() << std::endl;
      pub_cross_road.publish(pack_crossroad_array(crossroads));
    } else if(name == "sidewalk.csv") {
      sidewalks = read_sidewalk(x.c_str());
      std::cerr << "  load " << x.c_str() 
		<< ", sidewalks.size()=" << sidewalks.size() << std::endl;
      pub_side_walk.publish(pack_sidewalk_array(sidewalks));
    } else if(name == "gutter.csv") {
      gutters = read_gutter(x.c_str());
      std::cerr << "  load " << x.c_str() 
		<< ", gutters.size()=" << gutters.size() << std::endl;
      pub_gutter.publish(pack_gutter_array(gutters));
    } else if(name == "curb.csv") {
      curbs = read_curb(x.c_str());
      std::cerr << "  load " << x.c_str() 
		<< ", curb.size()=" << curbs.size() << std::endl;
      pub_curb.publish(pack_curb_array(curbs));
    } else if(name == "streetlight.csv") {
      streetlights = read_streetlight(x.c_str());
      std::cerr << "  load " << x.c_str() 
		<< ", streetlights.size()=" << streetlights.size() << std::endl;
      pub_street_light.publish(pack_streetlight_array(streetlights));
    } else if(name == "utilitypole.csv") {
      utilitypoles = read_utilitypole(x.c_str());
      std::cerr << "  load " << x.c_str() 
		<< ", utilitypoles.size()=" << utilitypoles.size() << std::endl;
      pub_utility_pole.publish(pack_utilitypole_array(utilitypoles));
    }
  }

  std::cerr << "finished load files" << std::endl;

  if(pointclasses.size() <= 0) {
    std::cerr << "Usage: vector_map_loader csv_file\n"
	      << "\tpoint.csv is not loaded"
              << std::endl;
    std::exit(1);
  }


  visualization_msgs::MarkerArray marker_array;
  visualization_msgs::Marker marker;
  marker.header.frame_id = "/map";
  marker.header.stamp = ros::Time();
  marker.ns = "vector_map";
  marker.action = visualization_msgs::Marker::ADD;
  marker.lifetime = ros::Duration();
  marker.frame_locked = true;
  marker.type = visualization_msgs::Marker::CYLINDER;

  size_t i;

  std::cerr << "start publish vector map" << std::endl;

  marker.id = 0;

  // road data
  if(lanes.size() > 0 && dtlanes.size() <= 0) {
    std::cerr << "error: dtlane.csv is not loaded.\n"
	      << "\tlane.csv needs dtlane.csv"
	      << std::endl;
    std::exit(1);
  }
  marker.ns = "lane";
  for (i=0; i<lanes.size(); i++) {
    if (lanes[i].lnid <= 0) {
      continue;
    }
    int did = lanes[i].did;
    int pid = dtlanes[did].pid;
    double ox, oy, oz, ow;

    marker.type = visualization_msgs::Marker::CUBE;

    ox = 0.0;
    oy = 0.0;
    oz = sin(dtlanes[did].dir / 2);
    ow = cos(dtlanes[did].dir / 2);

    if((lanes[i].span == 0 || (dtlanes[did].lw + dtlanes[did].rw) == 0)) continue;
    set_marker_data(&marker,
		    pointclasses[pid].bx - lanes[i].span/2, pointclasses[pid].ly - (dtlanes[did].lw + dtlanes[did].rw)/2, pointclasses[pid].h,
		    ox, oy, oz, ow,
		    lanes[i].span, dtlanes[did].lw + dtlanes[did].rw, 0.1,
		    0.5, 0, 0, 0.3);
    
    push_marker(&marker, &marker_array);
  }

  // pole
  if(poles.size() > 0 && (poleclasses.size() <= 0 || vectorclasses.size() <= 0)) {
    std::cerr << "error: pole.csv or vector.csv is not loaded \n"
	      << "\tpoledata.csv needs pole.csv and vector.csv "
	      << std::endl;
    std::exit(1);
  }
  marker.ns = "pole";
  for (i=0; i<poles.size(); i++) {
    if (poles[i].id <= 0) {
      continue;
    }
    int plid = poles[i].plid;
    set_poleclass_data(poleclasses[plid], 
		       1, 1, 1, 1,
		       vectorclasses,
		       pointclasses,
		       &marker, &marker_array);
  }

  // signal
  if(signals.size() > 0 && (poleclasses.size() <= 0 || vectorclasses.size() <= 0)) {
    std::cerr << "error: pole.csv or vector.csv is not loaded.\n"
	      << "\tsignaldata.csv needs pole.csv and vector.csv "
	      << std::endl;
    std::exit(1);
  }
  marker.ns = "signal";
  for (i=0; i<signals.size(); i++) {
    if (signals[i].id <= 0) {
      continue;
    }
    int vid = signals[i].vid;
    int pid = vectorclasses[vid].pid;

    marker.type = visualization_msgs::Marker::SPHERE;

    double ox, oy, oz, ow;
    calc_ang_to_xyzw(vectorclasses[vid].vang, vectorclasses[vid].hang,
		     &ox, &oy, &oz, &ow);

    double r = 0, g = 0, b = 0, a = 1;
    switch (signals[i].type) {
    case 1:
      r = 1;
      break;
    case 2:
      b = 1;
      break;
    case 3:
      r = 1;
      g = 1;
      break;
    case 4:
      marker.type = visualization_msgs::Marker::CUBE;
      r = 1;
      break;
    case 5:
      marker.type = visualization_msgs::Marker::CUBE;
      b = 1;
      break;
    default:
      break;
    }

    set_marker_data(&marker,
		    pointclasses[pid].bx, 
		    pointclasses[pid].ly, 
		    pointclasses[pid].h,
		    ox, oy, oz, ow,
		    0.5, 0.5, 0.5,
		    r, g, b, a);
    push_marker(&marker, &marker_array);


    // signal pole
    if (signals[i].type == 2) { // blue
      int plid = signals[i].plid;

      if (plid > 0) {
	set_poleclass_data(poleclasses[plid],
			   0.5, 0.5, 0.5, 1,
			   vectorclasses,
			   pointclasses,
			   &marker, &marker_array);
      }
    }
  }

  // roadsigns
  if(roadsigns.size() > 0 && (poleclasses.size() <= 0 || vectorclasses.size() <= 0)) {
    std::cerr << "error: pole.csv or vector.csv is not loaded\n"
	      << "\troadsign.csv needs pole.csv and vector.csv "
	      << std::endl;
    std::exit(1);
  }
  marker.ns = "road sign";
  for (i=0; i<roadsigns.size(); i++) {
    if (roadsigns[i].id <= 0) {
      continue;
    }
    int vid = roadsigns[i].vid;
    int pid = vectorclasses[vid].pid;

    marker.type = visualization_msgs::Marker::SPHERE;

    double ox, oy, oz, ow;
    calc_ang_to_xyzw(vectorclasses[vid].vang, vectorclasses[vid].hang,
		     &ox, &oy, &oz, &ow);

    set_marker_data(&marker,
		    pointclasses[pid].bx, 
		    pointclasses[pid].ly, 
		    pointclasses[pid].h,
		    ox, oy, oz, ow,
		    1.0, 0.1, 1.0,
		    1, 1, 1, 1);
    push_marker(&marker, &marker_array);

    // road sign pole
    int plid = roadsigns[i].plid;
    set_poleclass_data(poleclasses[plid],
		       1, 1, 1, 1,
		       vectorclasses,
		       pointclasses,
		       &marker, &marker_array);
  }

  // cross walk
  if(crosswalks.size() > 0 && (areaclasses.size() <= 0 || lines.size() <= 0)) {
    std::cerr << "error: area.csv or line.csv is not loaded.\n"
	      << "\tcrosswalk.csv needs area.csv and line.csv"
	      << std::endl;
    std::exit(1);
  }
  marker.ns = "cross walk";
  for (i=0; i<crosswalks.size(); i++) {
    if (crosswalks[i].id <= 0) {
      continue;
    }
    int aid = crosswalks[i].aid;
    int slid = areaclasses[aid].slid;
    int elid = areaclasses[aid].elid;

    marker.type = visualization_msgs::Marker::LINE_STRIP;

    set_marker_data(&marker,
		    0, 0, 0,
		    0, 0, 0, 1,
		    0.1, 0, 0,
		    1, 1, 1, 1);
    
    marker.points.clear();
    for(int lid = slid; ; lid = lines[lid].flid) {
      int pid = lines[lid].bpid;
      add_marker_points_pointclass(&marker, pointclasses[pid]);
      if (lid == elid) {
	pid = lines[lid].fpid;
	add_marker_points_pointclass(&marker, pointclasses[pid]);
	break;
      }
    }

    push_marker(&marker, &marker_array);
  }

  // zebrazone
  if(zebrazones.size() > 0 && (areaclasses.size() <= 0 || lines.size() <= 0)) {
    std::cerr << "error: area.csv or line.csv is not loaded.\n"
	      << "\tzebrazone.csv needs area.csv and line.csv."
	      << std::endl;
    std::exit(1);
  }
  marker.ns = "zebrazone";
  marker.type = visualization_msgs::Marker::LINE_STRIP;
  set_areaclass_data(zebrazones, 0.1,
		     1, 1, 1, 1,
		     areaclasses,
		     lines,
		     pointclasses,
		     &marker, &marker_array);


  // white line
  if(whitelines.size() > 0 && lines.size() <= 0) {
    std::cerr << "err: line.csv is not loaded.\n"
	      << "\twhiteline.csv needs line.csv"
	      << std::endl;
    std::exit(1);
  }
  marker.ns = "white line";
  marker.type = visualization_msgs::Marker::LINE_STRIP;
  for (i=0; i<whitelines.size(); i++) {
    if (whitelines[i].id <= 0) {
      continue;
    }
    int lid = whitelines[i].lid;
    if(lines[lid].blid == 0) {
      double r = 1.0, g = 1.0, b = 1.0;
      if(whitelines[i].color == 'Y') {
	g = 0.5;
	b = 0.0;
      }
      set_marker_data(&marker,
		      0, 0, 0,
		      0, 0, 0, 1,
		      whitelines[i].width, 0, 0,
		      r, g, b, 1);
    
      marker.points.clear();
    }

    int pid = lines[lid].bpid;
    add_marker_points_pointclass(&marker, pointclasses[pid]);
    if(lines[lid].flid == 0) {
      pid = lines[lid].fpid;
      add_marker_points_pointclass(&marker, pointclasses[pid]);

      push_marker(&marker, &marker_array);
    }
  }


  // roadedge
  if(roadedges.size() > 0 && (lines.size() <= 0)) {
    std::cerr << "error: line.csv is not loaded.\n"
	      << "\troadedge.csv needs line.csv"
	      << std::endl;
    std::exit(1);
  }
  marker.ns = "road edge";
  marker.type = visualization_msgs::Marker::LINE_STRIP;
  for (i=0; i<roadedges.size(); i++) {
    if (roadedges[i].id <= 0) continue;
    int lid = roadedges[i].lid;
    if(lines[lid].blid == 0) {
      set_marker_data(&marker,
		      0, 0, 0,
		      0, 0, 0, 1,
		      0.1, 0, 0,
		      0.5, 0.5, 0.5, 1);		// grey @@@@
    
      marker.points.clear();
    }

    int pid = lines[lid].bpid;
    add_marker_points_pointclass(&marker, pointclasses[pid]);
    if(lines[lid].flid == 0) {
      pid = lines[lid].fpid;
      add_marker_points_pointclass(&marker, pointclasses[pid]);

      push_marker(&marker, &marker_array);
    }
  }


  // road_surface_mark
  if(roadmarks.size() > 0 && (areaclasses.size() <= 0 || lines.size() <= 0)) {
    std::cerr << "error: area.csv or line.csv is not loaded.\n"
	      << "\troad_surface_mark.csv needs area.csv and line.csv"
	      << std::endl;
    std::exit(1);
  }
  marker.ns = "road surface mark";
  for (i=0; i<roadmarks.size(); i++) {
    if (roadmarks[i].id <= 0) {
      continue;
    }
    int aid = roadmarks[i].aid;
    int slid = areaclasses[aid].slid;
    int elid = areaclasses[aid].elid;
    double w = 0.2;

    marker.type = visualization_msgs::Marker::LINE_STRIP;
    set_marker_data(&marker,
		    0, 0, 0,
		    0, 0, 0, 1,
		    w, 0, 0,
		    1, 1, 1, 1);
    marker.points.clear();
    for(int lid = slid; ; lid = lines[lid].flid) {
      int pid = lines[lid].bpid;
      add_marker_points_pointclass(&marker, pointclasses[pid]);
      if (lid == elid) {
	pid = lines[lid].fpid;
	add_marker_points_pointclass(&marker, pointclasses[pid]);
	break;
      }
    }

    push_marker(&marker, &marker_array);
  }

  // stop line
  if(stoplines.size() > 0 && lines.size() <= 0) {
    std::cerr << "error: line.csv is not loaded.\n"
	      << "\tstoplines.csv needs line.csv"
	      << std::endl;
    std::exit(1);
  }
  marker.ns = "stop line";
  for (i=0; i<stoplines.size(); i++) {
    if (stoplines[i].id <= 0) {
      continue;
    }
    int lid = stoplines[i].lid;
    double r = 1.0, g = 1.0, b = 1.0;
    if(lines[lid].blid == 0) {
      set_marker_data(&marker,
		      0, 0, 0,
		      0, 0, 0, 1,
		      0.4, 0, 0,
		      r, g, b, 1);
    
      marker.points.clear();
    }
    int pid = lines[lid].bpid;
    add_marker_points_pointclass(&marker, pointclasses[pid]);
    if(lines[lid].flid == 0) {
      pid = lines[lid].fpid;
      add_marker_points_pointclass(&marker, pointclasses[pid]);

      push_marker(&marker, &marker_array);
    }
  }

  // cross road
  if(crossroads.size() > 0 && (areaclasses.size() <= 0 || lines.size() <= 0)) {
    std::cerr << "error: area.csv or line.csv is not loaded.\n"
	      << "\tcrossroad.csv needs area.csv and line.csv."
	      << std::endl;
    std::exit(1);
  }
  marker.ns = "cross road";
  marker.type = visualization_msgs::Marker::LINE_STRIP;
  set_areaclass_data(crossroads, 0.1,
		     1, 1, 1, 1,
		     areaclasses,
		     lines,
		     pointclasses,
		     &marker, &marker_array);


  // side walk (hodou) 
  if(sidewalks.size() > 0 && (areaclasses.size() <= 0 || lines.size() <= 0)) {
    std::cerr << "error: area.csv or line.csv is not loaded.\n"
	      << "\tsidewalk.csv needs area.csv and line.csv."
	      << std::endl;
    std::exit(1);
  }
  marker.ns = "side walk";
  marker.type = visualization_msgs::Marker::LINE_STRIP;
  set_areaclass_data(sidewalks, 0.1,
		     1, 1, 1, 1,
		     areaclasses,
		     lines,
		     pointclasses,
		     &marker, &marker_array);

  // gutter
  if(gutters.size() > 0 && (areaclasses.size() <= 0 || lines.size() <= 0)) {
    std::cerr << "error: area.csv or line.csv is not loaded.\n"
	      << "\tgutter.csv needs area.csv and line.csv."
	      << std::endl;
    std::exit(1);
  }
  marker.ns = "gutter";
  marker.type = visualization_msgs::Marker::LINE_STRIP;

  for (i=0; i<gutters.size(); i++) {
    if (gutters[i].id <= 0) {
      continue;
    }
    int aid = gutters[i].aid;
    int slid = areaclasses[aid].slid;
    int elid = areaclasses[aid].elid;
    double r, g, b;

    switch(gutters[i].type) {
    case 0:
      r = 0.7, g = 0.7, b = 0.7;
      break;
    case 1:
      r = 0.8, g = 0.8, b = 0.8;
      break;
    case 2:
      r = 0.5, g = 0.5, b = 0.5;
      break;
    default:
      r = 1.0, g = 1.0, b = 1.0;
      break;
    }

    set_marker_data(&marker,
		    0, 0, 0,
		    0, 0, 0, 1,
		    0.2, 0, 0,
		    r, g, b, 1);
    marker.points.clear();
    for(int lid = slid; ; lid = lines[lid].flid) {
      int pid = lines[lid].bpid;
      add_marker_points_pointclass(&marker, pointclasses[pid]);
      if (lid == elid) {
	pid = lines[lid].fpid;
	add_marker_points_pointclass(&marker, pointclasses[pid]);
	break;
      }
    }

    push_marker(&marker, &marker_array);
  }

  // curb
  if(curbs.size() > 0 && lines.size() <= 0) {
    std::cerr << "error: line.csv is not loaded.\n"
	      << "\tcurb.csv needs line.csv."
	      << std::endl;
    std::exit(1);
  }
  marker.ns = "curb";
  marker.type = visualization_msgs::Marker::LINE_STRIP;

  for (i=0; i<curbs.size(); i++) {
    if (curbs[i].id <= 0) {
      continue;
    }

    int lid = curbs[i].lid;
    if(lines[lid].blid == 0) {
      set_marker_data(&marker,
		      0, 0, 0,
		      0, 0, 0, 1,
		      curbs[i].width, 0, curbs[i].height,
		      0.7, 0.7, 0.7, 1);
    
      marker.points.clear();
    }

    int pid = lines[lid].bpid;
    add_marker_points_pointclass(&marker, pointclasses[pid]);
    if(lines[lid].flid == 0) {
      pid = lines[lid].fpid;
      add_marker_points_pointclass(&marker, pointclasses[pid]);

      push_marker(&marker, &marker_array);
    }
  }


  // streetlight
  if(streetlights.size() > 0 && (lines.size() <= 0 || poleclasses.size() <= 0 || vectorclasses.size() <= 0)) {
    std::cerr << "error: line.csv or pole.csv or vector.csv is not loaded.\n"
	      << "\tcurb.csv needs line.csv, pole.csv and vector.csv."
	      << std::endl;
    std::exit(1);
  }
  marker.ns = "streetlight";

  for (i=0; i<streetlights.size(); i++) {
    if (streetlights[i].id <= 0) {
      continue;
    }

    marker.type = visualization_msgs::Marker::LINE_STRIP;
    int lid = streetlights[i].lid;
    if(lines[lid].blid == 0) {
      set_marker_data(&marker,
		      0, 0, 0,
		      0, 0, 0, 1,
		      0.2, 0, 0,
		      1, 1, 1, 1);
      marker.points.clear();
    }

    int pid = lines[lid].bpid;
    add_marker_points_pointclass(&marker, pointclasses[pid]);
    if(lines[lid].flid == 0) {
      pid = lines[lid].fpid;
      add_marker_points_pointclass(&marker, pointclasses[pid]);

      push_marker(&marker, &marker_array);
    }

    // streetlight pole
    int plid = streetlights[i].plid;
    if(plid <= 0) continue;
    set_poleclass_data(poleclasses[plid],
		       1, 1, 1, 1,
		       vectorclasses,
		       pointclasses,
		       &marker, &marker_array);
  }

  // utilitypole
  if(utilitypoles.size() > 0 && (poleclasses.size() <= 0 || vectorclasses.size() <= 0)) {
    std::cerr << "error: pole.csv or vector.csv is not loaded \n"
	      << "\tutilitypole.csv needs pole.csv and vector.csv "
	      << std::endl;
    std::exit(1);
  }
  marker.ns = "utilitypole";
  for (i=0; i<utilitypoles.size(); i++) {
  if (utilitypoles[i].id <= 0) continue;
  int plid = utilitypoles[i].plid;
  set_poleclass_data(poleclasses[plid], 
			0.5, 0.5, 0.5, 1,
			vectorclasses,
			pointclasses,
			&marker, &marker_array);
  }

  pub.publish(marker_array);

  vmap_stat_msg.data = true;
  stat_publisher.publish(vmap_stat_msg);

#ifdef DEBUG_PRINT
  std::cerr << "publish end" << std::endl;
#endif

  ros::spin();

  return 0;
}
Exemplo n.º 2
0
void VectorMap::loadAll (const std::string &dirname)
{
    string rename = dirname + "/roadedge.csv";
    if (read_roadedge((char*)rename.c_str()) == EXIT_FAILURE) {
        std::cerr << rename << "\t load fail." << std::endl;
        exit(EXIT_FAILURE);
    }
    std::cout << rename << "\t load complete." << std::endl;

    string guttername = dirname + "/gutter.csv";
    if (read_gutter((char*)guttername.c_str()) == EXIT_FAILURE) {
        std::cerr << guttername << "\t load fail." << std::endl;
        exit(EXIT_FAILURE);
    }
    std::cout << guttername << "\t load complete." << std::endl;

    string curbname = dirname + "/curb.csv";
    if (read_curb((char*)curbname.c_str()) == EXIT_FAILURE) {
        std::cerr << curbname << "\t load fail." << std::endl;
        exit(EXIT_FAILURE);
    }
    std::cout << curbname << "\t load complete." << std::endl;

    //string clinename = dirname + "/cline.csv";
    string clinename = dirname + "/whiteline.csv";
    if (read_whiteline ((char*)clinename.c_str()) == EXIT_FAILURE) {
        std::cerr << clinename << "\t load fail." << std::endl;
        exit(EXIT_FAILURE);
    }
    std::cout << clinename << "\t load complete." << std::endl;

    string slinename = dirname + "/stopline.csv";
    if (read_stopline ((char*)slinename.c_str()) == EXIT_FAILURE) {
        std::cerr << slinename << "\t load fail." << std::endl;
        exit(EXIT_FAILURE);
    }
    std::cout << slinename << "\t load complete." << std::endl;

    string zebraname = dirname + "/zebrazone.csv";
    if (read_zebrazone ((char*)zebraname.c_str()) == EXIT_FAILURE) {
        std::cerr << zebraname << "\t load fail." << std::endl;
        exit(EXIT_FAILURE);
    }
    std::cout << zebraname << "\t load complete." << std::endl;

    string crossname = dirname + "/crosswalk.csv";
    if (read_crosswalk ((char*)crossname.c_str()) == EXIT_FAILURE) {
        std::cerr << crossname << "\t load fail." << std::endl;
        exit(EXIT_FAILURE);
    }
    std::cout << crossname << "\t load complete." << std::endl;

    string roadsurfname = dirname + "/road_surface_mark.csv";
    if (read_roadsurfacemark ((char*)roadsurfname.c_str()) == EXIT_FAILURE) {
        std::cerr << roadsurfname << "\t load fail." << std::endl;
        exit(EXIT_FAILURE);
    }
    std::cout << roadsurfname << "\t load complete." << std::endl;

    string poledataname = dirname + "/poledata.csv";
    if (read_poledata ((char*)poledataname.c_str()) == EXIT_FAILURE) {
        std::cerr << poledataname << "\t load fail." << std::endl;
        exit(EXIT_FAILURE);
    }
    std::cout << poledataname << "\t load complete." << std::endl;

    string roadsignname = dirname + "/roadsign.csv";
    if (read_roadsign ((char*)roadsignname.c_str()) == EXIT_FAILURE) {
        std::cerr << roadsignname << "\t load fail." << std::endl;
        exit(EXIT_FAILURE);
    }
    std::cout << roadsignname << "\t load complete." << std::endl;

    //string signalname = dirname + "/signal.csv";
    string signalname = dirname + "/signaldata.csv";
    if (read_signaldata ((char*)signalname.c_str()) == EXIT_FAILURE) {
        std::cerr << signalname << "\t load fail." << std::endl;
        exit(EXIT_FAILURE);
    }
    std::cout << signalname << "\t load complete." << std::endl;

    string lightname = dirname + "/streetlight.csv";
    if (read_streetlight ((char*)lightname.c_str()) == EXIT_FAILURE) {
        std::cerr << lightname << "\t load fail." << std::endl;
        exit(EXIT_FAILURE);
    }
    std::cout << lightname << "\t load complete." << std::endl;

    string utilitypolename = dirname + "/utilitypole.csv";
    if (read_utilitypole ((char*)utilitypolename.c_str()) == EXIT_FAILURE) {
        std::cerr << utilitypolename << "\t load fail." << std::endl;
        exit(EXIT_FAILURE);
    }
    std::cout << utilitypolename << "\t load complete." << std::endl;


    string ptname = dirname + "/point.csv";
    if (read_pointclass((char*)ptname.c_str()) == EXIT_FAILURE) {
        std::cerr << ptname << "\t load fail." << std::endl;
        exit(EXIT_FAILURE);
    }
    std::cout << ptname << "\t load complete." << std::endl;

    string vectorname = dirname + "/vector.csv";
    if (read_vectorclass ((char*)vectorname.c_str()) == EXIT_FAILURE) {
        std::cerr << vectorname << "\t load fail." << std::endl;
        exit(EXIT_FAILURE);
    }
    std::cout << vectorname << "\t load complete." << std::endl;

    string linename = dirname + "/line.csv";
    if (read_lineclass((char*)linename.c_str()) == EXIT_FAILURE) {
        std::cerr << linename << "\t load fail." << std::endl;
        exit(EXIT_FAILURE);
    }
    std::cout << linename << "\t load complete." << std::endl;

    string areaname = dirname + "/area.csv";
    if (read_areaclass ((char*)areaname.c_str()) == EXIT_FAILURE) {
        std::cerr << areaname << "\t load fail." << std::endl;
        exit(EXIT_FAILURE);
    }
    std::cout << areaname << "\t load complete." << std::endl;

    string polename = dirname + "/pole.csv";
    if (read_poleclass ((char*)polename.c_str()) == EXIT_FAILURE) {
        std::cerr << polename << "\t load fail." << std::endl;
        exit(EXIT_FAILURE);
    }
    std::cout << polename << "\t load complete." << std::endl;

//    string boxname = dirname + "/box.csv";
//    if (read_boxclass ((char*)boxname.c_str()) == EXIT_FAILURE) {
//        std::cerr << boxname << "\t load fail." << std::endl;
//        exit(EXIT_FAILURE);
//    }
//    std::cout << boxname << "\t load complete." << std::endl;


    string dtlanename = dirname + "/dtlane.csv";
    if (read_dtlane((char*)dtlanename.c_str()) == EXIT_FAILURE) {
        std::cerr << dtlanename << "\t load fail." << std::endl;
        exit(EXIT_FAILURE);
    }
    std::cout << dtlanename << "\t load complete." << std::endl;

    string nodename = dirname + "/node.csv";
    if (read_vectorclass ((char*)vectorname.c_str()) == EXIT_FAILURE) {
        std::cerr << nodename << "\t load fail." << std::endl;
        exit(EXIT_FAILURE);
    }
    std::cout << nodename << "\t load complete." << std::endl;


    string lanename = dirname + "/lane.csv";
    if (read_lanedata ((char*)lanename.c_str()) == EXIT_FAILURE) {
        std::cerr << lanename << "\t load fail." << std::endl;
        exit(EXIT_FAILURE);
    }
    std::cout << lanename << "\t load complete." << std::endl;


    std::cout << "all vecter maps loaded." << std::endl;

}