Example #1
0
/* 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);
}
Example #2
0
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;
}
Example #3
0
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());
}
Example #4
0
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());
}
Example #5
0
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());
}
Example #6
0
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());
}
Example #7
0
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());
}
Example #8
0
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());
}
Example #9
0
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());
}
Example #10
0
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());
}
Example #11
0
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());
}
Example #12
0
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());
}