From a2567ab0aba1ce285a92f7450245184f791ebf79 Mon Sep 17 00:00:00 2001 From: Holger Rapp Date: Fri, 2 Sep 2016 13:25:43 +0200 Subject: [PATCH] Use WallTimer to always publish pose and submaps. (#47) This makes the publishing independent from the inflow of sensor data and is more in line with how the ROS world works. Fixes #44. --- .../configuration_files/backpack_2d.lua | 2 + .../configuration_files/backpack_3d.lua | 2 + .../configuration_files/turtlebot.lua | 2 + .../src/cartographer_node_main.cc | 44 +++++++++---------- 4 files changed, 27 insertions(+), 23 deletions(-) diff --git a/cartographer_ros/configuration_files/backpack_2d.lua b/cartographer_ros/configuration_files/backpack_2d.lua index 9220763..dc89b3c 100644 --- a/cartographer_ros/configuration_files/backpack_2d.lua +++ b/cartographer_ros/configuration_files/backpack_2d.lua @@ -28,6 +28,8 @@ options = { laser_max_range = 30., laser_missing_echo_ray_length = 5., lookup_transform_timeout = 0.01, + submap_publish_period_sec = 0.3, + pose_publish_period_sec = 5e-3, use_multi_echo_laser_scan_2d = true } diff --git a/cartographer_ros/configuration_files/backpack_3d.lua b/cartographer_ros/configuration_files/backpack_3d.lua index d293986..8771469 100644 --- a/cartographer_ros/configuration_files/backpack_3d.lua +++ b/cartographer_ros/configuration_files/backpack_3d.lua @@ -28,6 +28,8 @@ options = { laser_max_range = 30., laser_missing_echo_ray_length = 5., lookup_transform_timeout = 0.01, + submap_publish_period_sec = 0.3, + pose_publish_period_sec = 5e-3, num_lasers_3d = 2 } diff --git a/cartographer_ros/configuration_files/turtlebot.lua b/cartographer_ros/configuration_files/turtlebot.lua index 39334f3..ee40614 100644 --- a/cartographer_ros/configuration_files/turtlebot.lua +++ b/cartographer_ros/configuration_files/turtlebot.lua @@ -28,6 +28,8 @@ options = { laser_max_range = 30., laser_missing_echo_ray_length = 5., lookup_transform_timeout = 0.01, + submap_publish_period_sec = 0.3, + pose_publish_period_sec = 5e-3, use_laser_scan_2d = true } diff --git a/cartographer_ros/src/cartographer_node_main.cc b/cartographer_ros/src/cartographer_node_main.cc index 8761afb..8af12b5 100644 --- a/cartographer_ros/src/cartographer_node_main.cc +++ b/cartographer_ros/src/cartographer_node_main.cc @@ -93,8 +93,6 @@ using carto::kalman_filter::PoseCovariance; // TODO(hrapp): Support multi trajectory mapping. constexpr int64 kTrajectoryId = 0; constexpr int kSubscriberQueueSize = 150; -constexpr int kSubmapPublishPeriodInUts = 300 * 10000ll; // 300 milliseconds -constexpr int kPosePublishPeriodInUts = 5 * 10000ll; // 5 milliseconds constexpr double kSensorDataRatesLoggingPeriodSeconds = 15.; // Unique default topic names. Expected to be remapped as needed. @@ -231,8 +229,8 @@ class Node { ::cartographer_ros_msgs::SubmapQuery::Request& request, ::cartographer_ros_msgs::SubmapQuery::Response& response); - void PublishSubmapList(int64 timestamp); - void PublishPose(int64 timestamp); + void PublishSubmapList(const ros::WallTimerEvent& timer_event); + void PublishPose(const ros::WallTimerEvent& timer_event); void SpinOccupancyGridThreadForever(); // TODO(hrapp): Pull out the common functionality between this and MapWriter @@ -264,11 +262,9 @@ class Node { std::unique_ptr sparse_pose_graph_; ::ros::Publisher submap_list_publisher_; - int64 last_submap_list_publish_timestamp_ = 0; ::ros::ServiceServer submap_query_server_; tf2_ros::TransformBroadcaster tf_broadcaster_; - int64 last_pose_publish_timestamp_ = 0; ::ros::Publisher occupancy_grid_publisher_; carto::mapping_2d::proto::SubmapsOptions submaps_options_; @@ -278,6 +274,10 @@ class Node { // Time at which we last logged the rates of incoming sensor data. std::chrono::steady_clock::time_point last_sensor_data_rates_logging_time_; std::map> rate_timers_; + + // We have to keep the timer handles of ros::WallTimers around, otherwise they + // do not fire. + std::vector wall_timers_; }; Node::Node() @@ -553,6 +553,15 @@ void Node::Initialize() { occupancy_grid_thread_ = std::thread(&Node::SpinOccupancyGridThreadForever, this); } + + wall_timers_.push_back(node_handle_.createWallTimer( + ros::WallDuration( + lua_parameter_dictionary.GetDouble("submap_publish_period_sec")), + &Node::PublishSubmapList, this)); + wall_timers_.push_back(node_handle_.createWallTimer( + ros::WallDuration( + lua_parameter_dictionary.GetDouble("pose_publish_period_sec")), + &Node::PublishPose, this)); } bool Node::HandleSubmapQuery( @@ -605,7 +614,7 @@ bool Node::HandleSubmapQuery( return true; } -void Node::PublishSubmapList(const int64 timestamp) { +void Node::PublishSubmapList(const ros::WallTimerEvent& timer_event) { carto::common::MutexLocker lock(&mutex_); const carto::mapping::Submaps* submaps = trajectory_builder_->submaps(); const std::vector submap_transforms = @@ -621,15 +630,13 @@ void Node::PublishSubmapList(const int64 timestamp) { } ::cartographer_ros_msgs::SubmapList ros_submap_list; - ros_submap_list.header.stamp = ToRos(carto::common::FromUniversal(timestamp)); + ros_submap_list.header.stamp = ros::Time::now(); ros_submap_list.header.frame_id = map_frame_; ros_submap_list.trajectory.push_back(ros_trajectory); submap_list_publisher_.publish(ros_submap_list); - last_submap_list_publish_timestamp_ = timestamp; } -void Node::PublishPose(const int64 timestamp) { - const carto::common::Time time = carto::common::FromUniversal(timestamp); +void Node::PublishPose(const ros::WallTimerEvent& timer_event) { carto::common::MutexLocker lock(&mutex_); const Rigid3d tracking_to_local = trajectory_builder_->pose_estimate().pose; const carto::mapping::Submaps* submaps = trajectory_builder_->submaps(); @@ -637,8 +644,9 @@ void Node::PublishPose(const int64 timestamp) { sparse_pose_graph_->GetLocalToGlobalTransform(*submaps); const Rigid3d tracking_to_map = local_to_map * tracking_to_local; + const ros::Time now = ros::Time::now(); geometry_msgs::TransformStamped stamped_transform; - stamped_transform.header.stamp = ToRos(time); + stamped_transform.header.stamp = now; stamped_transform.header.frame_id = map_frame_; stamped_transform.child_frame_id = odom_frame_; @@ -653,7 +661,7 @@ void Node::PublishPose(const int64 timestamp) { } else { try { const Rigid3d tracking_to_odom = - LookupToTrackingTransformOrThrow(time, odom_frame_).inverse(); + LookupToTrackingTransformOrThrow(FromRos(now), odom_frame_).inverse(); const Rigid3d odom_to_map = tracking_to_map * tracking_to_odom.inverse(); stamped_transform.transform = ToGeometryMsgTransform(odom_to_map); tf_broadcaster_.sendTransform(stamped_transform); @@ -662,7 +670,6 @@ void Node::PublishPose(const int64 timestamp) { << odom_frame_ << ": " << ex.what(); } } - last_pose_publish_timestamp_ = timestamp; } void Node::SpinOccupancyGridThreadForever() { @@ -743,15 +750,6 @@ void Node::SpinOccupancyGridThreadForever() { void Node::HandleSensorData(const int64 timestamp, std::unique_ptr sensor_data) { - if (last_submap_list_publish_timestamp_ + kSubmapPublishPeriodInUts < - timestamp) { - PublishSubmapList(timestamp); - } - - if (last_pose_publish_timestamp_ + kPosePublishPeriodInUts < timestamp) { - PublishPose(timestamp); - } - auto it = rate_timers_.find(sensor_data->frame_id); if (it == rate_timers_.end()) { it = rate_timers_