Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Thin color of car and pedestrian for old data. #19

Merged
merged 1 commit into from
Sep 7, 2015
Merged

Thin color of car and pedestrian for old data. #19

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
169 changes: 69 additions & 100 deletions ros/src/data/packages/pos_db/nodes/pos_downloader/pos_downloader.cpp
View file
Open in desktop
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