/* * Copyright 2015-2019 Autoware Foundation. All rights reserved. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ /* ----header---- */ /* common header */ #include "ros/ros.h" #include #include #include #include "std_msgs/Time.h" /* user header */ #include "sensor_msgs/Image.h" #include "sensor_msgs/PointCloud2.h" #include "geometry_msgs/PoseStamped.h" #include "visualization_msgs/MarkerArray.h" #include "autoware_msgs/ImageObj.h" #include "autoware_msgs/PointsImage.h" #include "autoware_msgs/ImageObjRanged.h" #include "autoware_msgs/ImageObjTracked.h" #include "autoware_msgs/ObjLabel.h" #include "autoware_msgs/Centroids.h" #include "autoware_msgs/SyncTimeMonitor.h" #include "autoware_msgs/SyncTimeDiff.h" /* ----var---- */ /* common var */ /* user var */ class KeyPair { boost::circular_buffer sensor; boost::circular_buffer execution; public: KeyPair() { sensor.resize(1); execution.resize(1); } void resize(int size) { sensor.resize(size); execution.resize(size); } void push_front(ros::Time sensor_time, ros::Time execution_time) { sensor.push_front(sensor_time); execution.push_front(execution_time); } ros::Time front(void) { return execution.front(); } ros::Time find(ros::Time sensor_time) { boost::circular_buffer::iterator it = sensor.begin(); for (int i = 0; it != sensor.end(); it++, i++) { if (it->sec == sensor_time.sec && it->nsec == sensor_time.nsec) { return execution.at(i); // find } } ROS_ERROR("error:not found a pair"); ros::Time failed; failed.sec = 0; failed.nsec = 0; return failed; // not find } }; class TimeManager { /* * KeyPair */ KeyPair image_raw_; KeyPair points_raw_; KeyPair points_image_; KeyPair vscan_points_; KeyPair vscan_image_; KeyPair image_obj_; KeyPair image_obj_ranged_; KeyPair image_obj_tracked_; KeyPair current_pose_; KeyPair obj_label_; KeyPair cluster_centroids_; KeyPair obj_pose_; // sync KeyPair sync_image_obj_ranged_; KeyPair sync_image_obj_tracked_; KeyPair sync_obj_label_; KeyPair sync_obj_pose_; // time difference KeyPair time_diff_; /* * Nodehandle, Subscriber, Publisher */ // Nodehandle ros::NodeHandle nh; // Subscriber ros::Subscriber image_raw_sub; ros::Subscriber points_raw_sub; ros::Subscriber points_image_sub; ros::Subscriber vscan_points_sub; ros::Subscriber vscan_image_sub; ros::Subscriber image_obj_sub; ros::Subscriber image_obj_ranged_sub; ros::Subscriber image_obj_tracked_sub; ros::Subscriber current_pose_sub; ros::Subscriber obj_label_sub; ros::Subscriber cluster_centroids_sub; ros::Subscriber obj_pose_sub; ros::Subscriber time_diff_sub; ros::Subscriber sync_image_obj_ranged_sub; // sync ros::Subscriber sync_image_obj_tracked_sub; // sync ros::Subscriber sync_obj_label_sub; // sync ros::Subscriber sync_obj_pose_sub; // sync // Publisher ros::Publisher time_monitor_pub; bool is_points_image_; bool is_vscan_image_; ros::Time get_walltime_now() { ros::WallTime non_sim_current_time = ros::WallTime::now(); ros::Time casted_time; casted_time.sec = non_sim_current_time.sec; casted_time.nsec = non_sim_current_time.nsec; return casted_time; } double ros_time2msec(ros::Time time) { return (double)time.sec*1000L + (double)time.nsec/1000000L; } double time_diff(ros::Time sensor_time, ros::Time execution_time) { if (execution_time.sec == 0 && execution_time.nsec == 0) { // not find ROS_ERROR("error:execution time is 0"); return 0.0; } if (sensor_time.sec == 0 && sensor_time.nsec == 0) { // not find ROS_ERROR("error:execution time is 0"); return 0.0; } double time_diff = ros_time2msec(execution_time) - ros_time2msec(sensor_time); if (time_diff >= 0) return time_diff; else ROS_ERROR("error:time difference is less than 0"); return 0.0; } public: TimeManager(int); void image_raw_callback(const sensor_msgs::Image::ConstPtr& image_raw_msg); void points_raw_callback(const sensor_msgs::PointCloud2::ConstPtr& points_raw_msg); void points_image_callback(const autoware_msgs::PointsImage::ConstPtr& points_image_msg); void vscan_points_callback(const sensor_msgs::PointCloud2::ConstPtr& vscan_points_msg); void vscan_image_callback(const autoware_msgs::PointsImage::ConstPtr& vscan_image_msg); void image_obj_callback(const autoware_msgs::ImageObj::ConstPtr& image_obj_msg); void image_obj_ranged_callback(const autoware_msgs::ImageObjRanged::ConstPtr& image_obj_ranged_msg); void image_obj_tracked_callback(const autoware_msgs::ImageObjTracked::ConstPtr& image_obj_tracked_msg); void current_pose_callback(const geometry_msgs::PoseStamped::ConstPtr& current_pose_msg); void obj_label_callback(const autoware_msgs::ObjLabel::ConstPtr& obj_label_msg) ; void cluster_centroids_callback(const autoware_msgs::Centroids::ConstPtr& cluster_centroids_msg); // void obj_pose_callback(const visualization_msgs::MarkerArray::ConstPtr& obj_pose_msg); void obj_pose_callback(const std_msgs::Time::ConstPtr& obj_pose_timestamp_msg); // sync void sync_image_obj_ranged_callback(const autoware_msgs::ImageObj::ConstPtr& sync_image_obj_msg); void sync_image_obj_tracked_callback(const autoware_msgs::ImageObjRanged::ConstPtr& sync_image_obj_ranged_msg); void sync_obj_label_callback(const autoware_msgs::ImageObjTracked::ConstPtr& sync_image_obj_tracked_msg); void sync_obj_pose_callback(const autoware_msgs::ObjLabel::ConstPtr& sync_obj_label_msg); // time difference void time_diff_callback(const autoware_msgs::SyncTimeDiff::ConstPtr& time_diff_msg); void run(); }; TimeManager::TimeManager(int buffer_size) { ros::NodeHandle private_nh("~"); private_nh.param("is_points_image", is_points_image_, true); private_nh.param("is_vscan_image", is_vscan_image_, false); if (is_vscan_image_ == is_points_image_) { ROS_ERROR("choose is_points_image or is_vscan_image"); is_points_image_ = true; is_vscan_image_ = false; } time_monitor_pub = nh.advertise ("/times", 10); image_raw_sub = nh.subscribe("/sync_drivers/image_raw", 10, &TimeManager::image_raw_callback, this); points_raw_sub = nh.subscribe("/sync_drivers/points_raw", 10, &TimeManager::points_raw_callback, this); points_image_sub = nh.subscribe("/points_image", 10, &TimeManager::points_image_callback, this); vscan_points_sub = nh.subscribe("/vscan_points", 10, &TimeManager::vscan_points_callback, this); vscan_image_sub = nh.subscribe("/vscan_image", 10, &TimeManager::vscan_image_callback, this); image_obj_sub = nh.subscribe("/image_obj", 10, &TimeManager::image_obj_callback, this); image_obj_ranged_sub = nh.subscribe("/image_obj_ranged", 10, &TimeManager::image_obj_ranged_callback, this); image_obj_tracked_sub = nh.subscribe("/image_obj_tracked", 10, &TimeManager::image_obj_tracked_callback, this); current_pose_sub = nh.subscribe("/current_pose", 10, &TimeManager::current_pose_callback, this); obj_label_sub = nh.subscribe("/obj_label", 10, &TimeManager::obj_label_callback, this); cluster_centroids_sub = nh.subscribe("/cluster_centroids", 10, &TimeManager::cluster_centroids_callback, this); obj_pose_sub = nh.subscribe("/obj_pose_timestamp", 10, &TimeManager::obj_pose_callback, this); // obj_pose_sub = nh.subscribe("/obj_car/obj_pose", 10, &TimeManager::obj_pose_callback, this); // sync sync_image_obj_ranged_sub = nh.subscribe("/sync_ranging/image_obj", 10 , &TimeManager::sync_image_obj_ranged_callback, this); sync_image_obj_tracked_sub = nh.subscribe("/sync_tracking/image_obj_ranged", 10, &TimeManager::sync_image_obj_tracked_callback, this); sync_obj_label_sub = nh.subscribe("/sync_reprojection/image_obj_tracked", 10, &TimeManager::sync_obj_label_callback, this); sync_obj_pose_sub = nh.subscribe("/sync_obj_fusion/obj_label", 10, &TimeManager::sync_obj_pose_callback, this); // time difference time_diff_sub = nh.subscribe("/time_difference", 10, &TimeManager::time_diff_callback, this); image_raw_.resize(buffer_size); points_raw_.resize(buffer_size); points_image_.resize(buffer_size); vscan_points_.resize(buffer_size); vscan_image_.resize(buffer_size); image_obj_.resize(buffer_size); image_obj_ranged_.resize(buffer_size); image_obj_tracked_.resize(buffer_size); current_pose_.resize(buffer_size); obj_label_.resize(buffer_size); obj_pose_.resize(buffer_size); cluster_centroids_.resize(buffer_size); sync_image_obj_ranged_.resize(buffer_size); sync_image_obj_tracked_.resize(buffer_size); sync_obj_label_.resize(buffer_size); sync_obj_pose_.resize(buffer_size); time_diff_.resize(buffer_size); } 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()); } 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::points_image_callback(const autoware_msgs::PointsImage::ConstPtr& points_image_msg) { // ROS_INFO("vscan_image: \t\t\t%d.%d", vscan_image_msg->header.stamp.sec, vscan_image_msg->header.stamp.nsec); points_image_.push_front(points_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::vscan_image_callback(const autoware_msgs::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::image_obj_callback(const autoware_msgs::ImageObj::ConstPtr& image_obj_msg) { // ROS_INFO("image_obj: \t\t\t%d.%d", image_obj_msg->header.stamp.sec, image_obj_msg->header.stamp.nsec); image_obj_.push_front(image_obj_msg->header.stamp, get_walltime_now()); } void TimeManager::image_obj_ranged_callback(const autoware_msgs::ImageObjRanged::ConstPtr& image_obj_ranged_msg) { // ROS_INFO("image_obj_ranged: \t\t%d.%d", image_obj_ranged_msg->header.stamp.sec, image_obj_ranged_msg->header.stamp.nsec); image_obj_ranged_.push_front(image_obj_ranged_msg->header.stamp, get_walltime_now()); } void TimeManager::image_obj_tracked_callback(const autoware_msgs::ImageObjTracked::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::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 autoware_msgs::ObjLabel::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::cluster_centroids_callback(const autoware_msgs::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()); } /* sync */ void TimeManager::sync_image_obj_ranged_callback(const autoware_msgs::ImageObj::ConstPtr& sync_image_obj_msg) { // ROS_INFO("sync_image_obj_ranged: \t\t%d.%d", sync_image_obj_msg->header.stamp.sec, sync_image_obj_msg->header.stamp.nsec); sync_image_obj_ranged_.push_front(sync_image_obj_msg->header.stamp, get_walltime_now()); } void TimeManager::sync_image_obj_tracked_callback(const autoware_msgs::ImageObjRanged::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::sync_obj_label_callback(const autoware_msgs::ImageObjTracked::ConstPtr& sync_image_obj_tracked_msg) { // ROS_INFO("sync_obj_label: \t\t%d.%d", sync_image_obj_tracked_msg->header.stamp.sec, sync_image_obj_tracked_msg->header.stamp.nsec); sync_obj_label_.push_front(sync_image_obj_tracked_msg->header.stamp, get_walltime_now()); } void TimeManager::sync_obj_pose_callback(const autoware_msgs::ObjLabel::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()); } /* time difference */ void TimeManager::time_diff_callback(const autoware_msgs::SyncTimeDiff::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; autoware_msgs::SyncTimeMonitor 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::run() { ros::spin(); } int main(int argc, char **argv) { ros::init(argc, argv, "time_monitor"); TimeManager time_manager(64); time_manager.run(); return 0; }