From 63dce89f6e4222dcb8964697cbd7c22950bbdfd1 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Tue, 23 Apr 2019 12:11:22 +0200 Subject: [PATCH] Service to query trajectory segments from the pose graph. (#1222) This `/trajectory_query` service allows to look up trajectory segments from the pose graph. This can be useful if one wants to get optimized trajectory data from the current SLAM run and listening to live TF data would be too noisy. For example, to stitch buffered point cloud data from a depth sensor based on a recent localization trajectory segment of a robot. Before, the pose graph trajectory nodes were not available except for the trajectory marker topic (only positions, no orientation; inefficient) or after serialization (which is not practical in live operation). --- .../cartographer_ros/map_builder_bridge.cc | 25 ++++++++++++++ .../cartographer_ros/map_builder_bridge.h | 5 +++ cartographer_ros/cartographer_ros/node.cc | 34 ++++++++++++++----- cartographer_ros/cartographer_ros/node.h | 3 ++ .../cartographer_ros/node_constants.h | 1 + cartographer_ros_msgs/CMakeLists.txt | 1 + cartographer_ros_msgs/srv/TrajectoryQuery.srv | 18 ++++++++++ 7 files changed, 79 insertions(+), 8 deletions(-) create mode 100644 cartographer_ros_msgs/srv/TrajectoryQuery.srv diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index ea51b9a..5162a53 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -22,6 +22,7 @@ #include "cartographer/io/proto_stream.h" #include "cartographer/mapping/pose_graph.h" #include "cartographer_ros/msg_conversion.h" +#include "cartographer_ros/time_conversion.h" #include "cartographer_ros_msgs/StatusCode.h" #include "cartographer_ros_msgs/StatusResponse.h" @@ -258,6 +259,30 @@ MapBuilderBridge::GetLocalTrajectoryData() { return local_trajectory_data; } +void MapBuilderBridge::HandleTrajectoryQuery( + cartographer_ros_msgs::TrajectoryQuery::Request& request, + cartographer_ros_msgs::TrajectoryQuery::Response& response) { + // This query is safe if the trajectory doesn't exist (returns 0 poses). + // However, we can filter unwanted states at the higher level in the node. + const auto node_poses = map_builder_->pose_graph()->GetTrajectoryNodePoses(); + for (const auto& node_id_data : + node_poses.trajectory(request.trajectory_id)) { + if (!node_id_data.data.constant_pose_data.has_value()) { + continue; + } + geometry_msgs::PoseStamped pose_stamped; + pose_stamped.header.frame_id = node_options_.map_frame; + pose_stamped.header.stamp = + ToRos(node_id_data.data.constant_pose_data.value().time); + pose_stamped.pose = ToGeometryMsgPose(node_id_data.data.global_pose); + response.trajectory.push_back(pose_stamped); + } + response.status.code = cartographer_ros_msgs::StatusCode::OK; + response.status.message = absl::StrCat( + "Retrieved ", response.trajectory.size(), + " trajectory nodes from trajectory ", request.trajectory_id, "."); +} + visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() { visualization_msgs::MarkerArray trajectory_node_list; const auto node_poses = map_builder_->pose_graph()->GetTrajectoryNodePoses(); diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.h b/cartographer_ros/cartographer_ros/map_builder_bridge.h index a9ede0a..1a54e65 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.h +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.h @@ -34,6 +34,8 @@ #include "cartographer_ros_msgs/SubmapEntry.h" #include "cartographer_ros_msgs/SubmapList.h" #include "cartographer_ros_msgs/SubmapQuery.h" +#include "cartographer_ros_msgs/TrajectoryQuery.h" +#include "geometry_msgs/TransformStamped.h" #include "nav_msgs/OccupancyGrid.h" // Abseil unfortunately pulls in winnt.h, which #defines DELETE. @@ -84,6 +86,9 @@ class MapBuilderBridge { void HandleSubmapQuery( cartographer_ros_msgs::SubmapQuery::Request& request, cartographer_ros_msgs::SubmapQuery::Response& response); + void HandleTrajectoryQuery( + cartographer_ros_msgs::TrajectoryQuery::Request& request, + cartographer_ros_msgs::TrajectoryQuery::Response& response); std::map diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 437bad4..6ed5dad 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -128,6 +128,8 @@ Node::Node( kConstraintListTopic, kLatestOnlyPublisherQueueSize); service_servers_.push_back(node_handle_.advertiseService( kSubmapQueryServiceName, &Node::HandleSubmapQuery, this)); + service_servers_.push_back(node_handle_.advertiseService( + kTrajectoryQueryServiceName, &Node::HandleTrajectoryQuery, this)); service_servers_.push_back(node_handle_.advertiseService( kStartTrajectoryServiceName, &Node::HandleStartTrajectory, this)); service_servers_.push_back(node_handle_.advertiseService( @@ -174,6 +176,23 @@ bool Node::HandleSubmapQuery( return true; } +bool Node::HandleTrajectoryQuery( + ::cartographer_ros_msgs::TrajectoryQuery::Request& request, + ::cartographer_ros_msgs::TrajectoryQuery::Response& response) { + absl::MutexLock lock(&mutex_); + response.status = TrajectoryStateToStatus( + request.trajectory_id, + {TrajectoryState::ACTIVE, TrajectoryState::FINISHED, + TrajectoryState::FROZEN} /* valid states */); + if (response.status.code != cartographer_ros_msgs::StatusCode::OK) { + LOG(ERROR) << "Can't query trajectory from pose graph: " + << response.status.message; + return true; + } + map_builder_bridge_.HandleTrajectoryQuery(request, response); + return true; +} + void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) { absl::MutexLock lock(&mutex_); submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList()); @@ -485,22 +504,21 @@ cartographer_ros_msgs::StatusResponse Node::TrajectoryStateToStatus( const auto trajectory_states = map_builder_bridge_.GetTrajectoryStates(); cartographer_ros_msgs::StatusResponse status_response; - if (!(trajectory_states.count(trajectory_id))) { + const auto it = trajectory_states.find(trajectory_id); + if (it == trajectory_states.end()) { status_response.message = absl::StrCat("Trajectory ", trajectory_id, " doesn't exist."); status_response.code = cartographer_ros_msgs::StatusCode::NOT_FOUND; return status_response; } - const auto trajectory_state = trajectory_states.at(trajectory_id); status_response.message = absl::StrCat("Trajectory ", trajectory_id, " is in '", - TrajectoryStateToString(trajectory_state), "' state."); - if (valid_states.count(trajectory_state)) { - status_response.code = cartographer_ros_msgs::StatusCode::OK; - } else { - status_response.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT; - } + TrajectoryStateToString(it->second), "' state."); + status_response.code = + valid_states.count(it->second) + ? cartographer_ros_msgs::StatusCode::OK + : cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT; return status_response; } diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index d55749f..690487c 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -134,6 +134,9 @@ class Node { bool HandleSubmapQuery( cartographer_ros_msgs::SubmapQuery::Request& request, cartographer_ros_msgs::SubmapQuery::Response& response); + bool HandleTrajectoryQuery( + ::cartographer_ros_msgs::TrajectoryQuery::Request& request, + ::cartographer_ros_msgs::TrajectoryQuery::Response& response); bool HandleStartTrajectory( cartographer_ros_msgs::StartTrajectory::Request& request, cartographer_ros_msgs::StartTrajectory::Response& response); diff --git a/cartographer_ros/cartographer_ros/node_constants.h b/cartographer_ros/cartographer_ros/node_constants.h index b819121..f54a4ab 100644 --- a/cartographer_ros/cartographer_ros/node_constants.h +++ b/cartographer_ros/cartographer_ros/node_constants.h @@ -35,6 +35,7 @@ constexpr char kOccupancyGridTopic[] = "map"; constexpr char kScanMatchedPointCloudTopic[] = "scan_matched_points2"; constexpr char kSubmapListTopic[] = "submap_list"; constexpr char kSubmapQueryServiceName[] = "submap_query"; +constexpr char kTrajectoryQueryServiceName[] = "trajectory_query"; constexpr char kStartTrajectoryServiceName[] = "start_trajectory"; constexpr char kWriteStateServiceName[] = "write_state"; constexpr char kGetTrajectoryStatesServiceName[] = "get_trajectory_states"; diff --git a/cartographer_ros_msgs/CMakeLists.txt b/cartographer_ros_msgs/CMakeLists.txt index e4daaff..15c86de 100644 --- a/cartographer_ros_msgs/CMakeLists.txt +++ b/cartographer_ros_msgs/CMakeLists.txt @@ -51,6 +51,7 @@ add_service_files( ReadMetrics.srv StartTrajectory.srv SubmapQuery.srv + TrajectoryQuery.srv WriteState.srv ) diff --git a/cartographer_ros_msgs/srv/TrajectoryQuery.srv b/cartographer_ros_msgs/srv/TrajectoryQuery.srv new file mode 100644 index 0000000..ca5da3b --- /dev/null +++ b/cartographer_ros_msgs/srv/TrajectoryQuery.srv @@ -0,0 +1,18 @@ +# Copyright 2019 The Cartographer Authors +# +# 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. + +int32 trajectory_id +--- +cartographer_ros_msgs/StatusResponse status +geometry_msgs/PoseStamped[] trajectory