Skip to content

Commit

Permalink
Merge pull request #19 from CPFL/pos_downloader_gradation
Browse files Browse the repository at this point in the history 
Thin color of car and pedestrian for old data.
  • Loading branch information
@kuriking
kuriking committed Sep 7, 2015
2 parents 4e38a5d + 32adcca commit 83faa5f
Showing 1 changed file with 69 additions and 100 deletions.
169 changes: 69 additions & 100 deletions ros/src/data/packages/pos_db/nodes/pos_downloader/pos_downloader.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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;

Expand All @@ -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<std::string> split(const string& input, char delimiter)
{
std::istringstream stream(input);
Expand All @@ -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;
Expand All @@ -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;

Expand All @@ -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
Expand All @@ -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";
Expand All @@ -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;
Expand All @@ -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;
Expand All @@ -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<std::string> db_data = split(msg.data, '\n');
std::vector<std::string> 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())
Expand All @@ -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
}

Expand Down Expand Up @@ -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);
}
}

Expand All @@ -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<std::string> 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;
Expand All @@ -416,45 +391,41 @@ 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) {
std::cerr << "sd.Sender() failed" << std::endl;
} else {
std::cout << "return data: \"" << db_response << "\"" << std::endl;
msg.data = db_response.c_str();
marker_publisher(msg, prev, 1);
marker_publisher(msg, 1);
}
}

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

Expand Down Expand Up @@ -489,8 +460,6 @@ int main(int argc, char **argv)
cout << "time=" << args[0] << endl;
nh.param<double>(MYNAME "/delay", args[1], DELAYSEC);
cout << "delay=" << args[1] << endl;
nh.param<double>(MYNAME "/life_time", life_time, LIFETIME);
cout << "life_time=" << life_time << endl;
nh.param<double>(MYNAME "/posup_dz", posup_dz, POSUP_DZ);
cout << "posup_dz=" << posup_dz << endl;
nh.param<double>(MYNAME "/pedestrian_dz", pedestrian_dz, PEDESTRIAN_DZ);
Expand Down

0 comments on commit 83faa5f

Please sign in to comment.