From 32adccacf70fa4c5a99042f8ebde2efc866ac9a7 Mon Sep 17 00:00:00 2001 From: USUDA Hisashi Date: Mon, 7 Sep 2015 22:25:43 +0900 Subject: [PATCH] Thin color of car and pedestrian for old data. --- .../nodes/pos_downloader/pos_downloader.cpp | 169 +++++++----------- 1 file changed, 69 insertions(+), 100 deletions(-) diff --git a/ros/src/data/packages/pos_db/nodes/pos_downloader/pos_downloader.cpp b/ros/src/data/packages/pos_db/nodes/pos_downloader/pos_downloader.cpp index 0beb99f6b0a..f5e4ecfdcb7 100644 --- a/ros/src/data/packages/pos_db/nodes/pos_downloader/pos_downloader.cpp +++ b/ros/src/data/packages/pos_db/nodes/pos_downloader/pos_downloader.cpp @@ -56,9 +56,10 @@ publish data as ractangular plane #define MARKERNAME "mo_marker" #define STARTTIME (0) // sec since 1970-01-01 (0==now) #define DELAYSEC (0) // delay sec for pos_uploader -#define LIFETIME (1) // anonymous marker's lifetime #define POSUP_DZ (40) // z offset of PosUp #define PEDESTRIAN_DZ (-2) // z offset of pedestrian_pose +#define DOWNLOAD_PERIOD (500) // period (msec) +#define TOTAL_LIFETIME (10.0) // total lifetime (sec) #define TYPE_OWN (1) #define TYPE_CAR (2) @@ -75,8 +76,8 @@ static string sshpubkey; static string sshprivatekey; static int ssh_port; static string sshtunnelhost; -static int sleep_msec = 500; // period -static double life_time; +static int sleep_msec = DOWNLOAD_PERIOD; // period +static double life_time = 2*DOWNLOAD_PERIOD/1000.0; // sec static double posup_dz; static double pedestrian_dz; @@ -87,6 +88,16 @@ static SendData sd; static char mac_addr[MAC_ADDRBUFSIZ]; static int ignore_my_pose = 1; +static double color_percent(int diffmsec) +{ + return (1.0 - diffmsec/TOTAL_LIFETIME/1000.0)*((256-48)/256.0) + (48/256.0); +} + +static int create_markerid(geometry_msgs::Pose& pose, int type) +{ + return (int)(pose.position.x*11 + pose.position.y*13 + pose.position.z + type*5) % ANON_MARKER_ID_MAX; +} + static std::vector split(const string& input, char delimiter) { std::istringstream stream(input); @@ -111,16 +122,17 @@ static void dbg_out_marker(visualization_msgs::Marker marker) << marker.pose.orientation.w << std::endl; } -static int publish_car(int id, int is_current, ros::Time now, geometry_msgs::Pose& pose) +static void publish_car(int id, int is_current, ros::Time now, + geometry_msgs::Pose& pose, int diffmsec) { visualization_msgs::Marker marker; marker.header.frame_id = "/map"; marker.header.stamp = now; marker.ns = MARKERNAME; marker.action = visualization_msgs::Marker::ADD; - marker.id = id++; marker.pose = pose; if (is_current) { + marker.id = id; marker.type = visualization_msgs::Marker::MESH_RESOURCE; marker.mesh_resource = "package://pos_db/model/prius_model.dae"; marker.mesh_use_embedded_materials = true; @@ -146,9 +158,10 @@ static int publish_car(int id, int is_current, ros::Time now, geometry_msgs::Pos marker.pose.orientation.w = q3.w(); } else { + marker.id = create_markerid(pose, 1); marker.type = visualization_msgs::Marker::CUBE; marker.lifetime = ros::Duration(life_time); - marker.color.r = 1.0; + marker.color.r = 1.0 * color_percent(diffmsec); marker.color.g = 0.0; marker.color.b = 0.0; @@ -160,7 +173,7 @@ static int publish_car(int id, int is_current, ros::Time now, geometry_msgs::Pos pub.publish(marker); dbg_out_marker(marker); - marker.id = id++; + marker.id = 1 + create_markerid(pose, 1); marker.scale.x = 3.0; // #B marker.scale.y = 1.6; marker.scale.z = 0.6; // #2 @@ -171,12 +184,10 @@ static int publish_car(int id, int is_current, ros::Time now, geometry_msgs::Pos pub.publish(marker); dbg_out_marker(marker); - - return id; } -static int publish_pedestrian(int id, int is_pedestrian, ros::Time now, - geometry_msgs::Pose& pose) +static void publish_pedestrian(int id, int is_pedestrian, ros::Time now, + geometry_msgs::Pose& pose, int diffmsec) { visualization_msgs::Marker marker; marker.header.frame_id = "/map"; @@ -185,16 +196,16 @@ static int publish_pedestrian(int id, int is_pedestrian, ros::Time now, marker.action = visualization_msgs::Marker::ADD; marker.type = visualization_msgs::Marker::CYLINDER; marker.lifetime = ros::Duration(life_time); - marker.id = id++; + marker.id = create_markerid(pose, 2); if (is_pedestrian) { marker.color.r = 0.0; - marker.color.g = 1.0; - marker.color.b = 1.0; + marker.color.g = 1.0 * color_percent(diffmsec); + marker.color.b = 1.0 * color_percent(diffmsec); pose.position.z += pedestrian_dz; } else { - marker.color.r = 1.0; - marker.color.g = 1.0; - marker.color.b = 1.0; + marker.color.r = 1.0 * color_percent(diffmsec); + marker.color.g = 1.0 * color_percent(diffmsec); + marker.color.b = 1.0 * color_percent(diffmsec); pose.position.z += posup_dz; } marker.color.a = 1.0; @@ -206,7 +217,7 @@ static int publish_pedestrian(int id, int is_pedestrian, ros::Time now, pub.publish(marker); dbg_out_marker(marker); - marker.id = id++; + marker.id = 1 + create_markerid(pose, 2); marker.type = visualization_msgs::Marker::SPHERE; marker.scale.x = 0.6; // #2 marker.scale.y = 0.6; @@ -215,95 +226,88 @@ static int publish_pedestrian(int id, int is_pedestrian, ros::Time now, marker.pose.position.z += 1.2 + 0.3 + 0.1; // == #1 + #2/2 + alpha pub.publish(marker); dbg_out_marker(marker); - - return id; } static int result_to_marker(const string& idstr, ros::Time now, - geometry_msgs::Pose& pose, int type, int is_swap) + geometry_msgs::Pose& pose, int type, + int diffmsec, int is_swap) { - static int id = ANON_MARKER_ID_MIN; - int nid = 0; + int nid; switch (type) { case TYPE_OWN: /* use lower 6 bytes */ nid = ANON_MARKER_ID_MAX | (std::strtol(idstr.substr(6, 6).c_str(), NULL, 16)) << 1; - publish_car(nid, 1, now, pose); + publish_car(nid, 1, now, pose, diffmsec); break; case TYPE_CAR: - id = publish_car(id, 0, now, pose); + publish_car(0, 0, now, pose, diffmsec); break; case TYPE_PEDESTRIAN: - id = publish_pedestrian(id, 1, now, pose); + publish_pedestrian(0, 1, now, pose, diffmsec); break; /* backward compatibility */ default: if (idstr.find("current_pose", 0) != string::npos) { /* current_pose:DEF012345678 */ - if (idstr.length() >= 25) + if (idstr.length() >= 25) { /* use lower 6 bytes */ nid = ANON_MARKER_ID_MAX | (std::strtol((idstr.substr(19, 6)).c_str(), NULL, 16)) << 1; - publish_car(nid, 1, now, pose); + publish_car(nid, 1, now, pose, diffmsec); + } } else if (idstr.find("ndt_pose", 0) != string::npos) { /* ndt_pose:9ABCDEF01234 */ - if (idstr.length() >= 21) + if (idstr.length() >= 21) { /* use lower 6 bytes */ nid = ANON_MARKER_ID_MAX | (std::strtol((idstr.substr(15, 6)).c_str(), NULL, 16)) << 1; - publish_car(nid, 1, now, pose); + publish_car(nid, 1, now, pose, diffmsec); + } } else if (idstr.find("car_pose", 0) != string::npos) { - id = publish_car(id, 0, now, pose); + publish_car(0, 0, now, pose, diffmsec); } else if (idstr.find("pedestrian_pose", 0) != string::npos) { - id = publish_pedestrian(id, 1, now, pose); + publish_pedestrian(0, 1, now, pose, diffmsec); } else { - id = publish_pedestrian(id, 0, now, pose); // PosUp + publish_pedestrian(0, 0, now, pose, diffmsec); // PosUp } } - if (id >= ANON_MARKER_ID_MAX) - id = ANON_MARKER_ID_MIN; - return 0; } -// convert JST time into GMT time -static void convert_jst_to_gmt(char *tstr) +static int get_timeval(const char *tstr, time_t *sp, int *np) { - struct tm tm, *tp; - int nsec = 0; - time_t tsec; + struct tm tm; if (sscanf(tstr, "%04d-%02d-%02d %02d:%02d:%02d.%d", &tm.tm_year, &tm.tm_mon, &tm.tm_mday, &tm.tm_hour, &tm.tm_min, - &tm.tm_sec, &nsec) < 7) { + &tm.tm_sec, np) < 7) { if (sscanf(tstr, "%04d-%02d-%02d %02d:%02d:%02d", &tm.tm_year, &tm.tm_mon, &tm.tm_mday, &tm.tm_hour, &tm.tm_min, &tm.tm_sec) < 6) { std::cerr << "Cannot convert time \"" << tstr << "\"" << std::endl; - return; + return -1; } + *np = 0; } tm.tm_year -= 1900; tm.tm_mon -= 1; - tsec = mktime(&tm); - tp = gmtime(&tsec); - sprintf(tstr, "%04d-%02d-%02d %02d:%02d:%02d.%d", - tp->tm_year + 1900, tp->tm_mon + 1, tp->tm_mday, - tp->tm_hour, tp->tm_min, tp->tm_sec, nsec); + *sp = mktime(&tm); + return 0; } -static void marker_publisher(const std_msgs::String& msg, char *prev, int is_swap) +static void marker_publisher(const std_msgs::String& msg, int is_swap) { std::vector db_data = split(msg.data, '\n'); std::vector cols; ros::Time now = ros::Time::now(); geometry_msgs::Pose pose; int type; - int i = 0; + time_t now_sec, prv_sec; + int now_nsec, prv_nsec; for (const std::string& row : db_data) { if(row.empty()) @@ -316,9 +320,9 @@ static void marker_publisher(const std_msgs::String& msg, char *prev, int is_swa type = std::stoi(cols[11]); if(ignore_my_pose && (type == TYPE_OWN || - ((cols[0].find("current_pose", 0) != string::npos || - cols[0].find("ndt_pose", 0) != string::npos) && - cols[0].find(mac_addr, 0) != string::npos))) { + cols[0].find("current_pose", 0) != string::npos || + cols[0].find("ndt_pose", 0) != string::npos) && + cols[0].find(mac_addr, 0) != string::npos) { continue; // don't publish Marker of my pose } @@ -358,11 +362,11 @@ static void marker_publisher(const std_msgs::String& msg, char *prev, int is_swa pose.orientation.z = 0; pose.orientation.w = 1; } - result_to_marker(cols[0], now, pose, type, is_swap); - if (i++ == 0) { - strcpy(prev, cols[10].c_str()); - convert_jst_to_gmt(prev); - } + now_sec = now.toSec(); + now_nsec = now.toNSec()%(1000*1000*1000); + get_timeval(cols[10].c_str(), &prv_sec, &prv_nsec); + result_to_marker(cols[0], now, pose, type, + (now_sec-prv_sec)*1000+(now_nsec-prv_nsec)/1000/1000, is_swap); } } @@ -377,37 +381,8 @@ static int create_timestr(time_t sec, int nsec, char *str, size_t size) nowtm->tm_hour, nowtm->tm_min, nowtm->tm_sec, nsec/1000/1000); } -// get latest timestamp -static int get_latest_tm(char *prev) -{ - std::string data; - std::string db_response; - std::vector db_data; - int ret; - - data = make_header(1, 1); - data += "SELECT tm FROM POS ORDER BY tm DESC LIMIT 1;\r\n"; - ret = sd.Sender(data, db_response, 0); - if (ret < 0) { - std::cerr << "sd.Sender() failed (return " << ret << ")" << std::endl; - return ret; - } - - db_data = split(db_response.c_str(), '\r'); - if (db_data.size() < 1) { - std::cerr << "Invalid vector size (" << db_data.size() << ")" << std::endl; - return -12; - } - - strcpy(prev, db_data.at(0).c_str()); - // VoltDB returns JST time... - convert_jst_to_gmt(prev); - std::cerr << "Get \"" << prev << "\"" << std::endl; - return 0; -} - //wrap SendData class -static void send_sql(time_t diff_sec, char *prev) +static void send_sql(time_t diff_sec) { std::string data; string db_response; @@ -416,21 +391,19 @@ static void send_sql(time_t diff_sec, char *prev) time_t now_sec = now.toSec() - diff_sec; int now_nsec = now.toNSec()%(1000*1000*1000); char timestr[64]; // "YYYY-mm-dd HH:MM:SS.sss..." + char prevstr[64]; // "YYYY-mm-dd HH:MM:SS.sss..." - // at first, get the latest timestamp - if (prev[0] == '\0') - if (get_latest_tm(prev) < 0) - return; + create_timestr(now_sec-TOTAL_LIFETIME, now_nsec, prevstr, sizeof(prevstr)); create_timestr(now_sec, now_nsec, timestr, sizeof(timestr)); data = make_header(1, 1); // select pos data between previous latest timestamp and now data += "SELECT id,x,y,z,or_x,or_y,or_z,or_w,lon,lat,tm,type FROM POS " "WHERE tm > '"; - data += prev; + data += prevstr; data += "' AND tm < '"; data += timestr; - data += "' ORDER BY tm DESC;\r\n"; + data += "' ORDER BY tm;\r\n"; int ret = sd.Sender(data, db_response, 0); if (ret < 0) { @@ -438,7 +411,7 @@ static void send_sql(time_t diff_sec, char *prev) } else { std::cout << "return data: \"" << db_response << "\"" << std::endl; msg.data = db_response.c_str(); - marker_publisher(msg, prev, 1); + marker_publisher(msg, 1); } } @@ -446,15 +419,13 @@ static void* intervalCall(void *unused) { double *args = (double *)unused; double diff_sec = args[1]; - char prev_sec[64]; // "YYYY-mm-dd HH:MM:SS.sss..." if (args[0] != 0) diff_sec += ros::Time::now().toSec() - args[0]; cout << "diff=" << diff_sec << endl; - prev_sec[0] = '\0'; while (1) { - send_sql((time_t)diff_sec, prev_sec); + send_sql((time_t)diff_sec); usleep(sleep_msec*1000); } @@ -489,8 +460,6 @@ int main(int argc, char **argv) cout << "time=" << args[0] << endl; nh.param(MYNAME "/delay", args[1], DELAYSEC); cout << "delay=" << args[1] << endl; - nh.param(MYNAME "/life_time", life_time, LIFETIME); - cout << "life_time=" << life_time << endl; nh.param(MYNAME "/posup_dz", posup_dz, POSUP_DZ); cout << "posup_dz=" << posup_dz << endl; nh.param(MYNAME "/pedestrian_dz", pedestrian_dz, PEDESTRIAN_DZ);