/* time difference */
void TimeManager::time_diff_callback(const synchronization::time_diff::ConstPtr& time_diff_msg) {
ros::Time sensors_time_diff;
double lidar = (double)time_diff_msg->lidar.sec + (double)time_diff_msg->lidar.nsec/1000000000.0L;
double camera = (double)time_diff_msg->camera.sec + (double)time_diff_msg->camera.nsec/1000000000.0L;
if (time_diff_msg->lidar.sec > time_diff_msg->camera.sec) {
sensors_time_diff.sec = time_diff_msg->lidar.sec - time_diff_msg->camera.sec;
if (time_diff_msg->lidar.nsec >= time_diff_msg->camera.nsec) {
sensors_time_diff.nsec = time_diff_msg->lidar.nsec - time_diff_msg->camera.nsec;
} else {
sensors_time_diff.sec -= 1;
sensors_time_diff.nsec = 1000000000L + time_diff_msg->lidar.nsec - time_diff_msg->camera.nsec;
}
} else if (time_diff_msg->lidar.sec < time_diff_msg->camera.sec) {
sensors_time_diff.sec = time_diff_msg->camera.sec - time_diff_msg->lidar.sec;
if (time_diff_msg->camera.nsec >= time_diff_msg->lidar.nsec) {
sensors_time_diff.nsec = time_diff_msg->camera.nsec - time_diff_msg->lidar.nsec;
} else {
sensors_time_diff.sec -= 1;
sensors_time_diff.nsec = 1000000000L + time_diff_msg->camera.nsec - time_diff_msg->lidar.nsec;
}
} else if (time_diff_msg->lidar.sec == time_diff_msg->camera.sec) {
sensors_time_diff.sec = 0;
if (time_diff_msg->lidar.nsec >= time_diff_msg->camera.nsec) {
sensors_time_diff.nsec = time_diff_msg->lidar.nsec - time_diff_msg->camera.nsec;
} else {
sensors_time_diff.nsec = time_diff_msg->camera.nsec - time_diff_msg->lidar.nsec;
}
} else {
// not impl
ROS_ERROR("Exception error");
}
time_diff_.push_front(time_diff_msg->header.stamp, sensors_time_diff);
}
void TimeManager::obj_pose_callback(const std_msgs::Time::ConstPtr& obj_pose_timestamp_msg) {
// ROS_INFO("obj_pose: \t\t\t%d.%d", obj_pose_timestamp_msg->data.sec, obj_pose_timestamp_msg->data.nsec);
obj_pose_.push_front(obj_pose_timestamp_msg->data, get_walltime_now());
static ros::Time pre_sensor_time;
synchronization::time_monitor time_monitor_msg;
time_monitor_msg.header.frame_id = "0";
time_monitor_msg.header.stamp = ros::Time::now();
// ROS_INFO("-------------------------------------");
// ROS_INFO("image_raw");
if (!ros::Time::isSimTime()) {
time_monitor_msg.image_raw = time_diff(obj_pose_timestamp_msg->data,
image_raw_.find(obj_pose_timestamp_msg->data));
} else {
time_monitor_msg.image_raw = 0;
}
// ROS_INFO("points_raw");
if (!ros::Time::isSimTime()) {
time_monitor_msg.points_raw = time_diff(obj_pose_timestamp_msg->data,
points_raw_.find(obj_pose_timestamp_msg->data));
} else {
time_monitor_msg.points_raw = 0;
}
if (is_points_image_) {
// ROS_INFO("points_image");
time_monitor_msg.points_image = time_diff(points_raw_.find(obj_pose_timestamp_msg->data),
points_image_.find(obj_pose_timestamp_msg->data));
}
if (is_vscan_image_) {
// ROS_INFO("vscan_points");
time_monitor_msg.vscan_points = time_diff(points_raw_.find(obj_pose_timestamp_msg->data),
vscan_points_.find(obj_pose_timestamp_msg->data));
// ROS_INFO("vscan_image");
time_monitor_msg.vscan_image = time_diff(vscan_points_.find(obj_pose_timestamp_msg->data),
vscan_image_.find(obj_pose_timestamp_msg->data));
}
// ROS_INFO("image_obj");
time_monitor_msg.image_obj = time_diff(image_raw_.find(obj_pose_timestamp_msg->data),
image_obj_.find(obj_pose_timestamp_msg->data));
// ROS_INFO("image_obj_ranged");
time_monitor_msg.image_obj_ranged = time_diff(sync_image_obj_ranged_.find(obj_pose_timestamp_msg->data),
image_obj_ranged_.find(obj_pose_timestamp_msg->data));
// ROS_INFO("image_obj_tracked");
time_monitor_msg.image_obj_tracked = time_diff(sync_image_obj_tracked_.find(obj_pose_timestamp_msg->data),
image_obj_tracked_.find(obj_pose_timestamp_msg->data));
// ROS_INFO("current_pose");
time_monitor_msg.current_pose = time_diff(points_raw_.find(obj_pose_timestamp_msg->data),
current_pose_.find(obj_pose_timestamp_msg->data));
// ROS_INFO("obj_label");
time_monitor_msg.obj_label = time_diff(sync_obj_label_.find(obj_pose_timestamp_msg->data),
obj_label_.find(obj_pose_timestamp_msg->data));
// ROS_INFO("cluster_centroids");
time_monitor_msg.cluster_centroids = time_diff(points_raw_.find(obj_pose_timestamp_msg->data),
cluster_centroids_.find(obj_pose_timestamp_msg->data));
// ROS_INFO("obj_pose");
time_monitor_msg.obj_pose = time_diff(sync_obj_pose_.find(obj_pose_timestamp_msg->data),
obj_pose_.find(obj_pose_timestamp_msg->data));
// ROS_INFO("-------------------------------------");
if (!ros::Time::isSimTime()) {
time_monitor_msg.execution_time = time_diff(obj_pose_timestamp_msg->data, obj_pose_.find(obj_pose_timestamp_msg->data));
} else {
time_monitor_msg.execution_time = time_diff(points_raw_.find(obj_pose_timestamp_msg->data), obj_pose_.find(obj_pose_timestamp_msg->data));
}
time_monitor_msg.cycle_time = time_diff(pre_sensor_time, obj_pose_timestamp_msg->data); // cycle time
time_monitor_msg.time_diff = ros_time2msec(time_diff_.find(obj_pose_timestamp_msg->data)); // time difference
time_monitor_pub.publish(time_monitor_msg);
pre_sensor_time = obj_pose_timestamp_msg->data;
}
void TimeManager::sync_obj_pose_callback(const cv_tracker_msgs::obj_label::ConstPtr& sync_obj_label_msg) {
// ROS_INFO("sync_obj_pose: \t\t\t%d.%d", sync_obj_label_msg->header.stamp.sec, sync_obj_label_msg->header.stamp.nsec);
sync_obj_pose_.push_front(sync_obj_label_msg->header.stamp, get_walltime_now());
}
void TimeManager::cluster_centroids_callback(const lidar_tracker::centroids::ConstPtr& cluster_centroids_msg) {
// ROS_INFO("cluster_centroids: \t\t%d.%d", cluster_centroids_msg->header.stamp.sec, cluster_centroids_msg->header.stamp.nsec);
cluster_centroids_.push_front(cluster_centroids_msg->header.stamp, get_walltime_now());
}
void TimeManager::sync_image_obj_tracked_callback(const cv_tracker_msgs::image_obj_ranged::ConstPtr& sync_image_obj_ranged_msg) {
// ROS_INFO("sync_image_obj_tracked: \t%d.%d", sync_image_obj_ranged_msg->header.stamp.sec, sync_image_obj_ranged_msg->header.stamp.nsec);
sync_image_obj_tracked_.push_front(sync_image_obj_ranged_msg->header.stamp, get_walltime_now());
}
void TimeManager::current_pose_callback(const geometry_msgs::PoseStamped::ConstPtr& current_pose_msg) {
// ROS_INFO("current_pose: \t\t\t%d.%d", current_pose_msg->header.stamp.sec, current_pose_msg->header.stamp.nsec);
current_pose_.push_front(current_pose_msg->header.stamp, get_walltime_now());
}
void TimeManager::obj_label_callback(const cv_tracker_msgs::obj_label::ConstPtr& obj_label_msg) {
// ROS_INFO("obj_label: \t\t\t%d.%d", obj_label_msg->header.stamp.sec, obj_label_msg->header.stamp.nsec);
obj_label_.push_front(obj_label_msg->header.stamp, get_walltime_now());
}
void TimeManager::image_obj_tracked_callback(const cv_tracker_msgs::image_obj_tracked::ConstPtr& image_obj_tracked_msg) {
// ROS_INFO("image_obj_tracked: \t\t%d.%d", image_obj_tracked_msg->header.stamp.sec, image_obj_tracked_msg->header.stamp.nsec);
image_obj_tracked_.push_front(image_obj_tracked_msg->header.stamp, get_walltime_now());
}
void TimeManager::vscan_image_callback(const points2image::PointsImage::ConstPtr& vscan_image_msg) {
// ROS_INFO("vscan_image: \t\t\t%d.%d", vscan_image_msg->header.stamp.sec, vscan_image_msg->header.stamp.nsec);
vscan_image_.push_front(vscan_image_msg->header.stamp, get_walltime_now());
}
void TimeManager::vscan_points_callback(const sensor_msgs::PointCloud2::ConstPtr& vscan_points_msg) {
// ROS_INFO("vscan_points: \t\t\t%d.%d", vscan_points_msg->header.stamp.sec, vscan_points_msg->header.stamp.nsec);
vscan_points_.push_front(vscan_points_msg->header.stamp, get_walltime_now());
}
void TimeManager::points_raw_callback(const sensor_msgs::PointCloud2::ConstPtr& points_raw_msg) {
// ROS_INFO("points_raw: \t\t\t%d.%d", points_raw_msg->header.stamp.sec, points_raw_msg->header.stamp.nsec);
points_raw_.push_front(points_raw_msg->header.stamp, get_walltime_now());
}
void TimeManager::image_raw_callback(const sensor_msgs::Image::ConstPtr& image_raw_msg) {
// ROS_INFO("image_raw: \t\t\t%d.%d", image_raw_msg->header.stamp.sec, image_raw_msg->header.stamp.nsec);
image_raw_.push_front(image_raw_msg->header.stamp, get_walltime_now());
}
