void *FlagGUI_func(void *)
{
char command[PACKET_SIZE];
double x, y;
// Cycle the connection listen code to allow only one
// client at a time but allow the client to disconnect
// and reconnect
while (1) {
// Setup and wait for connection
send_socket = setup_socket_server();
if ( send_socket == -1) return NULL;
// listen for commands and run the events
while(1) {
if ( !read_signal(send_socket, command, PACKET_SIZE) ) break;
x = atof(command);
y = atof(command + strlen(command) + 1);
// Make sure that the View Rectangle has been initalized
if (!GVRec) continue;
printf("MOVE TO %f %f\n", x, y);
// Jump to the given coordinates
GVRec->GLMove(x, y);
}
close(send_socket);
unlink(SOCKET_PATH);
}
return NULL;
}
/* read_duh(): reads a DUH from an already open DUMBFILE, and returns its
* pointer, or null on error. The file is not closed.
*/
DUH *read_duh(DUMBFILE *f) {
DUH *duh;
int i;
if (dumbfile_mgetl(f) != DUH_SIGNATURE)
return NULL;
duh = malloc(sizeof(*duh));
if (!duh)
return NULL;
duh->length = dumbfile_igetl(f);
if (dumbfile_error(f) || duh->length <= 0) {
free(duh);
return NULL;
}
duh->n_signals = (int)dumbfile_igetl(f);
if (dumbfile_error(f) || duh->n_signals <= 0) {
free(duh);
return NULL;
}
duh->signal = malloc(sizeof(*duh->signal) * duh->n_signals);
if (!duh->signal) {
free(duh);
return NULL;
}
for (i = 0; i < duh->n_signals; i++)
duh->signal[i] = NULL;
for (i = 0; i < duh->n_signals; i++) {
if (!(duh->signal[i] = read_signal(duh, f))) {
unload_duh(duh);
return NULL;
}
}
return duh;
}
/* reads from a file data for a sinlge spot, data may be partial */
static
rc_t read_next_spot(const char* blk_pfx, IlluminaFileInfo* file)
{
rc_t rc = 0;
const char* tail = file->line;
if( file->ready ) {
/* data still not used */
return 0;
}
IlluminaFileInfo_init(file);
if( (rc = file_read_line(file, true)) != 0 ) {
return SRALoaderFile_LOG(file->file, klogErr, rc, "$(msg)", "msg=reading more data");
} else if( file->line == NULL ) {
return 0; /* eof */
}
switch( file->type ) {
case eIlluminaNativeFileTypeQSeq:
if( (rc = parse_qseq(file, file->line, file->line_len)) != 0 ) {
return SRALoaderFile_LOG(file->file, klogErr, rc, "$(msg)", "msg=reading qseq");
}
break;
case eIlluminaNativeFileTypeFasta:
case eIlluminaNativeFileTypeNoise:
case eIlluminaNativeFileTypeIntensity:
case eIlluminaNativeFileTypeSignal:
{{
/* read only common first 4 coords into name and prepend with DATA_BLOCK/@name */
if( (rc = read_spot_coord(file, file->line, file->line_len, &tail)) == 0 ) {
if( blk_pfx != NULL ) {
pstring tmp_name;
if( (rc = pstring_copy(&tmp_name, &file->name)) == 0 &&
(rc = pstring_assign(&file->name, blk_pfx, strlen(blk_pfx))) == 0 &&
(rc = pstring_append(&file->name, ":", 1)) == 0 ) {
rc = pstring_concat(&file->name, &tmp_name);
}
}
}
if( rc != 0 ) {
return SRALoaderFile_LOG(file->file, klogErr, rc, "$(msg)", "msg=reading spot coord");
}
break;
}}
case eIlluminaNativeFileTypeQuality4:
if( (rc = read_quality(file->line, file->line_len, &file->read)) != 0 ) {
return SRALoaderFile_LOG(file->file, klogErr, rc, "$(msg)", "msg=reading quality");
} else if( (rc = pstring_assign(&file->name, blk_pfx, strlen(blk_pfx))) != 0 ) {
return SRALoaderFile_LOG(file->file, klogErr, rc, "$(msg)", "msg=name for quality 4");
}
break;
default:
rc = RC(rcSRA, rcFormatter, rcReading, rcFileFormat, rcUnknown);
return SRALoaderFile_LOG(file->file, klogErr, rc, "$(msg)", "msg=processing data line");
break;
}
/* process tail (after coords) for some file types */
file->line_len -= tail - file->line; /* length of tail */
switch( file->type ) {
case eIlluminaNativeFileTypeQSeq:
case eIlluminaNativeFileTypeQuality4:
default:
/* completely processed before */
break;
case eIlluminaNativeFileTypeFasta:
if( (rc = pstring_assign(&file->read.seq, tail, file->line_len)) != 0 ||
!pstring_is_fasta(&file->read.seq) ) {
rc = RC(rcSRA, rcFormatter, rcReading, rcData, rcCorrupt);
return SRALoaderFile_LOG(file->file, klogErr, rc, "$(msg)", "msg=reading fasta");
}
break;
case eIlluminaNativeFileTypeNoise:
if( (rc = read_signal(tail, file->line_len, &file->read.noise)) != 0 ) {
return SRALoaderFile_LOG(file->file, klogErr, rc, "$(msg)", "msg=converting noise");
}
break;
case eIlluminaNativeFileTypeIntensity:
if( (rc = read_signal(tail, file->line_len, &file->read.intensity)) != 0 ) {
return SRALoaderFile_LOG(file->file, klogErr, rc, "$(msg)", "msg=converting intensity");
}
break;
case eIlluminaNativeFileTypeSignal:
if( (rc = read_signal(tail, file->line_len, &file->read.signal)) != 0 ) {
return SRALoaderFile_LOG(file->file, klogErr, rc, "$(msg)", "msg=converting signal");
}
break;
}
file->line = NULL;
file->ready = true;
#if _DEBUGGING
DEBUG_MSG(3, ("name:'%s' [%li:%li:%li:%li]\n", file->name.data,
file->coord[0], file->coord[1], file->coord[2], file->coord[3]));
if( file->read.seq.len ) {
DEBUG_MSG(3, ("seq:'%.*s'\n", file->read.seq.len, file->read.seq.data));
}
if( file->read.qual.len ) {
DEBUG_MSG(3, ("qual{0x%x}: %u bytes\n", file->read.qual_type, file->read.qual.len));
}
#endif
return 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;
}
