From 61126ec9019156ffd720db0c4b38f7ae12a61831 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Wed, 10 Apr 2019 15:30:54 +0200 Subject: [PATCH 01/94] Update clang-format to new Google style. (#1260) Apparently the format bot uses a bleeding edge clang-format that uses the new Google style and reformats a bunch of files in every PR. This is an empty commit to trigger this in a separate commit. See https://github.com/llvm-mirror/clang/commit/62e3198c4f5490a1c60ba51d81fe2e1f0dc99135 Signed-off-by: Guillaume Doisy --- cartographer_ros/cartographer_ros/msg_conversion_test.cc | 3 ++- cartographer_ros/cartographer_ros/offline_node.cc | 1 + cartographer_ros/cartographer_ros/playable_bag.h | 5 +++-- cartographer_ros/cartographer_ros/tf_bridge.cc | 4 ++-- cartographer_ros/cartographer_ros/tf_bridge.h | 3 +-- 5 files changed, 9 insertions(+), 7 deletions(-) diff --git a/cartographer_ros/cartographer_ros/msg_conversion_test.cc b/cartographer_ros/cartographer_ros/msg_conversion_test.cc index 89c8246d3..85039dd82 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion_test.cc +++ b/cartographer_ros/cartographer_ros/msg_conversion_test.cc @@ -14,11 +14,12 @@ * limitations under the License. */ +#include "cartographer_ros/msg_conversion.h" + #include #include #include "cartographer/transform/rigid_transform_test_helpers.h" -#include "cartographer_ros/msg_conversion.h" #include "cartographer_ros/time_conversion.h" #include "gmock/gmock.h" #include "gtest/gtest.h" diff --git a/cartographer_ros/cartographer_ros/offline_node.cc b/cartographer_ros/cartographer_ros/offline_node.cc index b2cf20586..71aa6e4b2 100644 --- a/cartographer_ros/cartographer_ros/offline_node.cc +++ b/cartographer_ros/cartographer_ros/offline_node.cc @@ -22,6 +22,7 @@ #include #endif #include + #include #include "absl/strings/str_split.h" diff --git a/cartographer_ros/cartographer_ros/playable_bag.h b/cartographer_ros/cartographer_ros/playable_bag.h index 06c608628..52db7df92 100644 --- a/cartographer_ros/cartographer_ros/playable_bag.h +++ b/cartographer_ros/cartographer_ros/playable_bag.h @@ -16,11 +16,12 @@ #ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_PLAYABLE_BAG_H #define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_PLAYABLE_BAG_H +#include +#include + #include #include -#include -#include #include "rosbag/bag.h" #include "rosbag/view.h" #include "tf2_ros/buffer.h" diff --git a/cartographer_ros/cartographer_ros/tf_bridge.cc b/cartographer_ros/cartographer_ros/tf_bridge.cc index 4b3c7838c..29f80b12c 100644 --- a/cartographer_ros/cartographer_ros/tf_bridge.cc +++ b/cartographer_ros/cartographer_ros/tf_bridge.cc @@ -14,10 +14,10 @@ * limitations under the License. */ -#include "absl/memory/memory.h" +#include "cartographer_ros/tf_bridge.h" +#include "absl/memory/memory.h" #include "cartographer_ros/msg_conversion.h" -#include "cartographer_ros/tf_bridge.h" namespace cartographer_ros { diff --git a/cartographer_ros/cartographer_ros/tf_bridge.h b/cartographer_ros/cartographer_ros/tf_bridge.h index d65f18983..a74f85ae7 100644 --- a/cartographer_ros/cartographer_ros/tf_bridge.h +++ b/cartographer_ros/cartographer_ros/tf_bridge.h @@ -20,9 +20,8 @@ #include #include "cartographer/transform/rigid_transform.h" -#include "tf2_ros/buffer.h" - #include "cartographer_ros/time_conversion.h" +#include "tf2_ros/buffer.h" namespace cartographer_ros { From 907080bd8fca6225fff8b7e195f517180c90e2af Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Mon, 15 Apr 2019 09:53:55 +0200 Subject: [PATCH 02/94] Unify trajectory state checks for service handlers. (#1262) Some service handlers need to check if a trajectory is in a valid state for the requested operation. If it's not, they return an error response. `Node::CheckTrajectoryState` allows to avoid code duplication in such situations. Signed-off-by: Guillaume Doisy --- cartographer_ros/cartographer_ros/node.cc | 95 +++++++++++++---------- cartographer_ros/cartographer_ros/node.h | 6 ++ 2 files changed, 58 insertions(+), 43 deletions(-) diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 48a68c3ec..437bad40c 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -49,6 +49,12 @@ namespace cartographer_ros { +namespace carto = ::cartographer; + +using carto::transform::Rigid3d; +using TrajectoryState = + ::cartographer::mapping::PoseGraphInterface::TrajectoryState; + namespace { cartographer_ros_msgs::SensorTopics DefaultSensorTopics() { @@ -80,13 +86,21 @@ ::ros::Subscriber SubscribeWithHandler( })); } -} // namespace - -namespace carto = ::cartographer; +std::string TrajectoryStateToString(const TrajectoryState trajectory_state) { + switch (trajectory_state) { + case TrajectoryState::ACTIVE: + return "ACTIVE"; + case TrajectoryState::FINISHED: + return "FINISHED"; + case TrajectoryState::FROZEN: + return "FROZEN"; + case TrajectoryState::DELETED: + return "DELETED"; + } + return ""; +} -using carto::transform::Rigid3d; -using TrajectoryState = - ::cartographer::mapping::PoseGraphInterface::TrajectoryState; +} // namespace Node::Node( const NodeOptions& node_options, @@ -466,50 +480,46 @@ bool Node::ValidateTopicNames( return true; } +cartographer_ros_msgs::StatusResponse Node::TrajectoryStateToStatus( + const int trajectory_id, const std::set& valid_states) { + const auto trajectory_states = map_builder_bridge_.GetTrajectoryStates(); + cartographer_ros_msgs::StatusResponse status_response; + + if (!(trajectory_states.count(trajectory_id))) { + 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; + } + return status_response; +} + cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock( const int trajectory_id) { - auto trajectory_states = map_builder_bridge_.GetTrajectoryStates(); - cartographer_ros_msgs::StatusResponse status_response; if (trajectories_scheduled_for_finish_.count(trajectory_id)) { - const std::string message = absl::StrCat("Trajectory ", trajectory_id, - " already pending to finish."); + status_response.message = absl::StrCat("Trajectory ", trajectory_id, + " already pending to finish."); status_response.code = cartographer_ros_msgs::StatusCode::OK; - status_response.message = message; - LOG(INFO) << message; + LOG(INFO) << status_response.message; return status_response; } // First, check if we can actually finish the trajectory. - if (!(trajectory_states.count(trajectory_id))) { - const std::string error = - absl::StrCat("Trajectory ", trajectory_id, " doesn't exist."); - LOG(ERROR) << error; - status_response.code = cartographer_ros_msgs::StatusCode::NOT_FOUND; - status_response.message = error; - return status_response; - } else if (trajectory_states.at(trajectory_id) == TrajectoryState::FROZEN) { - const std::string error = - absl::StrCat("Trajectory ", trajectory_id, " is frozen."); - LOG(ERROR) << error; - status_response.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT; - status_response.message = error; - return status_response; - } else if (trajectory_states.at(trajectory_id) == TrajectoryState::FINISHED) { - const std::string error = absl::StrCat("Trajectory ", trajectory_id, - " has already been finished."); - LOG(ERROR) << error; - status_response.code = - cartographer_ros_msgs::StatusCode::RESOURCE_EXHAUSTED; - status_response.message = error; - return status_response; - } else if (trajectory_states.at(trajectory_id) == TrajectoryState::DELETED) { - const std::string error = - absl::StrCat("Trajectory ", trajectory_id, " has been deleted."); - LOG(ERROR) << error; - status_response.code = - cartographer_ros_msgs::StatusCode::RESOURCE_EXHAUSTED; - status_response.message = error; + status_response = TrajectoryStateToStatus( + trajectory_id, {TrajectoryState::ACTIVE} /* valid states */); + if (status_response.code != cartographer_ros_msgs::StatusCode::OK) { + LOG(ERROR) << "Can't finish trajectory: " << status_response.message; return status_response; } @@ -525,10 +535,9 @@ cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock( } map_builder_bridge_.FinishTrajectory(trajectory_id); trajectories_scheduled_for_finish_.emplace(trajectory_id); - const std::string message = + status_response.message = absl::StrCat("Finished trajectory ", trajectory_id, "."); status_response.code = cartographer_ros_msgs::StatusCode::OK; - status_response.message = message; return status_response; } diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index 62eb6f927..d55749f11 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -174,6 +174,12 @@ class Node { int trajectory_id) EXCLUSIVE_LOCKS_REQUIRED(mutex_); void MaybeWarnAboutTopicMismatch(const ::ros::WallTimerEvent&); + // Helper function for service handlers that need to check trajectory states. + cartographer_ros_msgs::StatusResponse TrajectoryStateToStatus( + int trajectory_id, + const std::set< + cartographer::mapping::PoseGraphInterface::TrajectoryState>& + valid_states); const NodeOptions node_options_; tf2_ros::TransformBroadcaster tf_broadcaster_; From 1f712de2b31f72909710599c251653024ae1ae47 Mon Sep 17 00:00:00 2001 From: Ashwath Narayan Murali Date: Wed, 17 Apr 2019 14:23:38 +0200 Subject: [PATCH 03/94] Updated doc to remove abseil-cpp from ROS (#1211) I have added a few lines to the doc to alert users to remove ros abseil-cpp and also to change the rosinstall file if they wish to checkout a different version of cartographer. As discussed in #1208 Signed-off-by: Guillaume Doisy --- docs/source/compilation.rst | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/docs/source/compilation.rst b/docs/source/compilation.rst index 3d4a3be43..f02032c25 100644 --- a/docs/source/compilation.rst +++ b/docs/source/compilation.rst @@ -63,6 +63,13 @@ The command 'sudo rosdep init' will print an error if you have already executed rosdep update rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y +If you build cartographer from master. Change/remove the version in the cartographer_ros.rosinstall. + +Additionally, uninstall the ros abseil-cpp using + +.. code-block:: bash + sudo apt-get remove ros-${ROS_DISTRO}-abseil-cpp + Build and install. .. code-block:: bash From d497de98d177b3a55a07be1dfee198b1d162d6af Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Wed, 17 Apr 2019 14:46:58 +0200 Subject: [PATCH 04/94] Add explicit --save_state_filename to offline node options. (#1204) This allows to serialize the state also when no bagfile is given, e.g. when the offline node is used to run an optimization and/or trimming configuration on a pbstream. Or simply when one wants to use a custom name directly. This doesn't break compatibility with the previous CLI. Signed-off-by: Guillaume Doisy --- .../cartographer_ros/offline_node.cc | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/cartographer_ros/cartographer_ros/offline_node.cc b/cartographer_ros/cartographer_ros/offline_node.cc index 71aa6e4b2..e3a7e7abc 100644 --- a/cartographer_ros/cartographer_ros/offline_node.cc +++ b/cartographer_ros/cartographer_ros/offline_node.cc @@ -63,6 +63,11 @@ DEFINE_string(load_state_filename, "", "a saved SLAM state."); DEFINE_bool(load_frozen_state, true, "Load the saved state as frozen (non-optimized) trajectories."); +DEFINE_string(save_state_filename, "", + "Explicit name of the file to which the serialized state will be " + "written before shutdown. If left empty, the filename will be " + "inferred from the first bagfile's name as: " + ".pbstream"); DEFINE_bool(keep_running, false, "Keep running the offline node after all messages from the bag " "have been processed."); @@ -371,15 +376,19 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { LOG(INFO) << "Peak memory usage: " << usage.ru_maxrss << " KiB"; #endif - if (::ros::ok() && bag_filenames.size() > 0) { - const std::string output_filename = bag_filenames.front(); - const std::string suffix = ".pbstream"; - const std::string state_output_filename = output_filename + suffix; + // Serialize unless we have neither a bagfile nor an explicit state filename. + if (::ros::ok() && + !(bag_filenames.empty() && FLAGS_save_state_filename.empty())) { + const std::string state_output_filename = + FLAGS_save_state_filename.empty() + ? absl::StrCat(bag_filenames.front(), ".pbstream") + : FLAGS_save_state_filename; LOG(INFO) << "Writing state to '" << state_output_filename << "'..."; node.SerializeState(state_output_filename, true /* include_unfinished_submaps */); } if (FLAGS_keep_running) { + LOG(INFO) << "Finished processing and waiting for shutdown."; ::ros::waitForShutdown(); } } From 289ec49c7d92500f25664173609f1540f5af37d7 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Tue, 23 Apr 2019 12:11:22 +0200 Subject: [PATCH 05/94] 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). Signed-off-by: Guillaume Doisy --- .../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 ea51b9abf..5162a53c3 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 a9ede0ade..1a54e6502 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 437bad40c..6ed5dad0e 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 d55749f11..690487c36 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 b819121a8..f54a4abcf 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 e4daaff63..15c86de36 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 000000000..ca5da3b94 --- /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 From 2c20ec34940af738fbeb80c7376e91fbcc8444ec Mon Sep 17 00:00:00 2001 From: mgladkova Date: Tue, 7 May 2019 16:53:06 +0200 Subject: [PATCH 06/94] Simplify start_trajectory service (RFC-28) (#1245) * Simplify start_trajectory. * Ran clang-format. * Make corrections based on the review * Make corrections based on review #2, remove obsolete functions * Remove unnecessary local variables Signed-off-by: Guillaume Doisy --- .../cartographer_ros/CMakeLists.txt | 11 -- cartographer_ros/cartographer_ros/node.cc | 152 +++++++++--------- cartographer_ros/cartographer_ros/node.h | 16 +- .../cartographer_ros/node_options.h | 1 - .../cartographer_ros/start_trajectory_main.cc | 126 --------------- .../cartographer_ros/trajectory_options.cc | 81 ---------- .../cartographer_ros/trajectory_options.h | 17 -- cartographer_ros_msgs/CMakeLists.txt | 2 - cartographer_ros_msgs/msg/SensorTopics.msg | 21 --- .../msg/TrajectoryOptions.msg | 35 ---- cartographer_ros_msgs/srv/StartTrajectory.srv | 7 +- 11 files changed, 88 insertions(+), 381 deletions(-) delete mode 100644 cartographer_ros/cartographer_ros/start_trajectory_main.cc delete mode 100644 cartographer_ros_msgs/msg/SensorTopics.msg delete mode 100644 cartographer_ros_msgs/msg/TrajectoryOptions.msg diff --git a/cartographer_ros/cartographer_ros/CMakeLists.txt b/cartographer_ros/cartographer_ros/CMakeLists.txt index 667748dfb..f2616d124 100644 --- a/cartographer_ros/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/cartographer_ros/CMakeLists.txt @@ -47,17 +47,6 @@ install(TARGETS cartographer_offline_node RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -google_binary(cartographer_start_trajectory - SRCS - start_trajectory_main.cc -) - -install(TARGETS cartographer_start_trajectory - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - google_binary(cartographer_occupancy_grid_node SRCS occupancy_grid_node_main.cc diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 6ed5dad0e..1bc804a59 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -56,19 +56,6 @@ using TrajectoryState = ::cartographer::mapping::PoseGraphInterface::TrajectoryState; namespace { - -cartographer_ros_msgs::SensorTopics DefaultSensorTopics() { - cartographer_ros_msgs::SensorTopics topics; - topics.laser_scan_topic = kLaserScanTopic; - topics.multi_echo_laser_scan_topic = kMultiEchoLaserScanTopic; - topics.point_cloud2_topic = kPointCloud2Topic; - topics.imu_topic = kImuTopic; - topics.odometry_topic = kOdometryTopic; - topics.nav_sat_fix_topic = kNavSatFixTopic; - topics.landmark_topic = kLandmarkTopic; - return topics; -} - // Subscribes to the 'topic' for 'trajectory_id' using the 'node_handle' and // calls 'handler' on the 'node' to handle messages. Returns the subscriber. template @@ -342,24 +329,21 @@ void Node::PublishConstraintList( } std::set -Node::ComputeExpectedSensorIds( - const TrajectoryOptions& options, - const cartographer_ros_msgs::SensorTopics& topics) const { +Node::ComputeExpectedSensorIds(const TrajectoryOptions& options) const { using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId; using SensorType = SensorId::SensorType; std::set expected_topics; // Subscribe to all laser scan, multi echo laser scan, and point cloud topics. - for (const std::string& topic : ComputeRepeatedTopicNames( - topics.laser_scan_topic, options.num_laser_scans)) { - expected_topics.insert(SensorId{SensorType::RANGE, topic}); - } for (const std::string& topic : - ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic, - options.num_multi_echo_laser_scans)) { + ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) { expected_topics.insert(SensorId{SensorType::RANGE, topic}); } for (const std::string& topic : ComputeRepeatedTopicNames( - topics.point_cloud2_topic, options.num_point_clouds)) { + kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) { + expected_topics.insert(SensorId{SensorType::RANGE, topic}); + } + for (const std::string& topic : + ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) { expected_topics.insert(SensorId{SensorType::RANGE, topic}); } // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is @@ -368,17 +352,16 @@ Node::ComputeExpectedSensorIds( (node_options_.map_builder_options.use_trajectory_builder_2d() && options.trajectory_builder_options.trajectory_builder_2d_options() .use_imu_data())) { - expected_topics.insert(SensorId{SensorType::IMU, topics.imu_topic}); + expected_topics.insert(SensorId{SensorType::IMU, kImuTopic}); } // Odometry is optional. if (options.use_odometry) { - expected_topics.insert( - SensorId{SensorType::ODOMETRY, topics.odometry_topic}); + expected_topics.insert(SensorId{SensorType::ODOMETRY, kOdometryTopic}); } // NavSatFix is optional. if (options.use_nav_sat) { expected_topics.insert( - SensorId{SensorType::FIXED_FRAME_POSE, topics.nav_sat_fix_topic}); + SensorId{SensorType::FIXED_FRAME_POSE, kNavSatFixTopic}); } // Landmark is optional. if (options.use_landmarks) { @@ -387,15 +370,14 @@ Node::ComputeExpectedSensorIds( return expected_topics; } -int Node::AddTrajectory(const TrajectoryOptions& options, - const cartographer_ros_msgs::SensorTopics& topics) { +int Node::AddTrajectory(const TrajectoryOptions& options) { const std::set - expected_sensor_ids = ComputeExpectedSensorIds(options, topics); + expected_sensor_ids = ComputeExpectedSensorIds(options); const int trajectory_id = map_builder_bridge_.AddTrajectory(expected_sensor_ids, options); AddExtrapolator(trajectory_id, options); AddSensorSamplers(trajectory_id, options); - LaunchSubscribers(options, topics, trajectory_id); + LaunchSubscribers(options, trajectory_id); wall_timers_.push_back(node_handle_.createWallTimer( ::ros::WallDuration(kTopicMismatchCheckDelaySec), &Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true)); @@ -406,27 +388,25 @@ int Node::AddTrajectory(const TrajectoryOptions& options, } void Node::LaunchSubscribers(const TrajectoryOptions& options, - const cartographer_ros_msgs::SensorTopics& topics, const int trajectory_id) { - for (const std::string& topic : ComputeRepeatedTopicNames( - topics.laser_scan_topic, options.num_laser_scans)) { + for (const std::string& topic : + ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) { subscribers_[trajectory_id].push_back( {SubscribeWithHandler( &Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_, this), topic}); } - for (const std::string& topic : - ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic, - options.num_multi_echo_laser_scans)) { + for (const std::string& topic : ComputeRepeatedTopicNames( + kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) { subscribers_[trajectory_id].push_back( {SubscribeWithHandler( &Node::HandleMultiEchoLaserScanMessage, trajectory_id, topic, &node_handle_, this), topic}); } - for (const std::string& topic : ComputeRepeatedTopicNames( - topics.point_cloud2_topic, options.num_point_clouds)) { + for (const std::string& topic : + ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) { subscribers_[trajectory_id].push_back( {SubscribeWithHandler( &Node::HandlePointCloud2Message, trajectory_id, topic, @@ -440,37 +420,33 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options, (node_options_.map_builder_options.use_trajectory_builder_2d() && options.trajectory_builder_options.trajectory_builder_2d_options() .use_imu_data())) { - std::string topic = topics.imu_topic; subscribers_[trajectory_id].push_back( {SubscribeWithHandler(&Node::HandleImuMessage, - trajectory_id, topic, + trajectory_id, kImuTopic, &node_handle_, this), - topic}); + kImuTopic}); } if (options.use_odometry) { - std::string topic = topics.odometry_topic; subscribers_[trajectory_id].push_back( {SubscribeWithHandler(&Node::HandleOdometryMessage, - trajectory_id, topic, + trajectory_id, kOdometryTopic, &node_handle_, this), - topic}); + kOdometryTopic}); } if (options.use_nav_sat) { - std::string topic = topics.nav_sat_fix_topic; subscribers_[trajectory_id].push_back( {SubscribeWithHandler( - &Node::HandleNavSatFixMessage, trajectory_id, topic, &node_handle_, - this), - topic}); + &Node::HandleNavSatFixMessage, trajectory_id, kNavSatFixTopic, + &node_handle_, this), + kNavSatFixTopic}); } if (options.use_landmarks) { - std::string topic = topics.landmark_topic; subscribers_[trajectory_id].push_back( {SubscribeWithHandler( - &Node::HandleLandmarkMessage, trajectory_id, topic, &node_handle_, - this), - topic}); + &Node::HandleLandmarkMessage, trajectory_id, kLandmarkTopic, + &node_handle_, this), + kLandmarkTopic}); } } @@ -486,10 +462,8 @@ bool Node::ValidateTrajectoryOptions(const TrajectoryOptions& options) { return false; } -bool Node::ValidateTopicNames( - const ::cartographer_ros_msgs::SensorTopics& topics, - const TrajectoryOptions& options) { - for (const auto& sensor_id : ComputeExpectedSensorIds(options, topics)) { +bool Node::ValidateTopicNames(const TrajectoryOptions& options) { + for (const auto& sensor_id : ComputeExpectedSensorIds(options)) { const std::string& topic = sensor_id.id; if (subscribed_topics_.count(topic) > 0) { LOG(ERROR) << "Topic name [" << topic << "] is already used."; @@ -562,23 +536,56 @@ cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock( bool Node::HandleStartTrajectory( ::cartographer_ros_msgs::StartTrajectory::Request& request, ::cartographer_ros_msgs::StartTrajectory::Response& response) { - absl::MutexLock lock(&mutex_); - TrajectoryOptions options; - if (!FromRosMessage(request.options, &options) || - !ValidateTrajectoryOptions(options)) { - const std::string error = "Invalid trajectory options."; - LOG(ERROR) << error; + TrajectoryOptions trajectory_options; + std::tie(std::ignore, trajectory_options) = LoadOptions( + request.configuration_directory, request.configuration_basename); + + if (request.use_initial_pose) { + const auto pose = ToRigid3d(request.initial_pose); + if (!pose.IsValid()) { + response.status.message = + "Invalid pose argument. Orientation quaternion must be normalized."; + LOG(ERROR) << response.status.message; + response.status.code = + cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT; + return true; + } + + // Check if the requested trajectory for the relative initial pose exists. + response.status = TrajectoryStateToStatus( + request.relative_to_trajectory_id, + {TrajectoryState::ACTIVE, TrajectoryState::FROZEN, + TrajectoryState::FINISHED} /* valid states */); + if (response.status.code != cartographer_ros_msgs::StatusCode::OK) { + LOG(ERROR) << "Can't start a trajectory with initial pose: " + << response.status.message; + return true; + } + + ::cartographer::mapping::proto::InitialTrajectoryPose + initial_trajectory_pose; + initial_trajectory_pose.set_to_trajectory_id( + request.relative_to_trajectory_id); + *initial_trajectory_pose.mutable_relative_pose() = + cartographer::transform::ToProto(pose); + initial_trajectory_pose.set_timestamp(cartographer::common::ToUniversal( + ::cartographer_ros::FromRos(ros::Time(0)))); + *trajectory_options.trajectory_builder_options + .mutable_initial_trajectory_pose() = initial_trajectory_pose; + } + + if (!ValidateTrajectoryOptions(trajectory_options)) { + response.status.message = "Invalid trajectory options."; + LOG(ERROR) << response.status.message; response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT; - response.status.message = error; - } else if (!ValidateTopicNames(request.topics, options)) { - const std::string error = "Invalid topics."; - LOG(ERROR) << error; + } else if (!ValidateTopicNames(trajectory_options)) { + response.status.message = "Topics are already used by another trajectory."; + LOG(ERROR) << response.status.message; response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT; - response.status.message = error; } else { - response.trajectory_id = AddTrajectory(options, request.topics); - response.status.code = cartographer_ros_msgs::StatusCode::OK; response.status.message = "Success."; + response.trajectory_id = AddTrajectory(trajectory_options); + response.status.code = cartographer_ros_msgs::StatusCode::OK; } return true; } @@ -586,7 +593,7 @@ bool Node::HandleStartTrajectory( void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) { absl::MutexLock lock(&mutex_); CHECK(ValidateTrajectoryOptions(options)); - AddTrajectory(options, DefaultSensorTopics()); + AddTrajectory(options); } std::vector< @@ -601,8 +608,7 @@ Node::ComputeDefaultSensorIdsForMultipleBags( prefix = "bag_" + std::to_string(i + 1) + "_"; } std::set unique_sensor_ids; - for (const auto& sensor_id : - ComputeExpectedSensorIds(bags_options.at(i), DefaultSensorTopics())) { + for (const auto& sensor_id : ComputeExpectedSensorIds(bags_options.at(i))) { unique_sensor_ids.insert(SensorId{sensor_id.type, prefix + sensor_id.id}); } bags_sensor_ids.push_back(unique_sensor_ids); diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index 690487c36..ac715d02b 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -36,13 +36,11 @@ #include "cartographer_ros_msgs/FinishTrajectory.h" #include "cartographer_ros_msgs/GetTrajectoryStates.h" #include "cartographer_ros_msgs/ReadMetrics.h" -#include "cartographer_ros_msgs/SensorTopics.h" #include "cartographer_ros_msgs/StartTrajectory.h" #include "cartographer_ros_msgs/StatusResponse.h" #include "cartographer_ros_msgs/SubmapEntry.h" #include "cartographer_ros_msgs/SubmapList.h" #include "cartographer_ros_msgs/SubmapQuery.h" -#include "cartographer_ros_msgs/TrajectoryOptions.h" #include "cartographer_ros_msgs/WriteState.h" #include "nav_msgs/Odometry.h" #include "ros/ros.h" @@ -155,14 +153,9 @@ class Node { // Returns the set of SensorIds expected for a trajectory. // 'SensorId::id' is the expected ROS topic name. std::set<::cartographer::mapping::TrajectoryBuilderInterface::SensorId> - ComputeExpectedSensorIds( - const TrajectoryOptions& options, - const cartographer_ros_msgs::SensorTopics& topics) const; - int AddTrajectory(const TrajectoryOptions& options, - const cartographer_ros_msgs::SensorTopics& topics); - void LaunchSubscribers(const TrajectoryOptions& options, - const cartographer_ros_msgs::SensorTopics& topics, - int trajectory_id); + ComputeExpectedSensorIds(const TrajectoryOptions& options) const; + int AddTrajectory(const TrajectoryOptions& options); + void LaunchSubscribers(const TrajectoryOptions& options, int trajectory_id); void PublishSubmapList(const ::ros::WallTimerEvent& timer_event); void AddExtrapolator(int trajectory_id, const TrajectoryOptions& options); void AddSensorSamplers(int trajectory_id, const TrajectoryOptions& options); @@ -171,8 +164,7 @@ class Node { void PublishLandmarkPosesList(const ::ros::WallTimerEvent& timer_event); void PublishConstraintList(const ::ros::WallTimerEvent& timer_event); bool ValidateTrajectoryOptions(const TrajectoryOptions& options); - bool ValidateTopicNames(const ::cartographer_ros_msgs::SensorTopics& topics, - const TrajectoryOptions& options); + bool ValidateTopicNames(const TrajectoryOptions& options); cartographer_ros_msgs::StatusResponse FinishTrajectoryUnderLock( int trajectory_id) EXCLUSIVE_LOCKS_REQUIRED(mutex_); void MaybeWarnAboutTopicMismatch(const ::ros::WallTimerEvent&); diff --git a/cartographer_ros/cartographer_ros/node_options.h b/cartographer_ros/cartographer_ros/node_options.h index 30215812a..43dc219cf 100644 --- a/cartographer_ros/cartographer_ros/node_options.h +++ b/cartographer_ros/cartographer_ros/node_options.h @@ -44,7 +44,6 @@ NodeOptions CreateNodeOptions( std::tuple LoadOptions( const std::string& configuration_directory, const std::string& configuration_basename); - } // namespace cartographer_ros #endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_OPTIONS_H diff --git a/cartographer_ros/cartographer_ros/start_trajectory_main.cc b/cartographer_ros/cartographer_ros/start_trajectory_main.cc deleted file mode 100644 index b0181d525..000000000 --- a/cartographer_ros/cartographer_ros/start_trajectory_main.cc +++ /dev/null @@ -1,126 +0,0 @@ -/* - * Copyright 2016 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. - */ - -#include -#include - -#include "absl/memory/memory.h" -#include "cartographer/common/configuration_file_resolver.h" -#include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/common/port.h" -#include "cartographer_ros/node_constants.h" -#include "cartographer_ros/ros_log_sink.h" -#include "cartographer_ros/trajectory_options.h" -#include "cartographer_ros_msgs/StartTrajectory.h" -#include "cartographer_ros_msgs/StatusCode.h" -#include "cartographer_ros_msgs/TrajectoryOptions.h" -#include "gflags/gflags.h" -#include "ros/ros.h" - -DEFINE_string(configuration_directory, "", - "First directory in which configuration files are searched, " - "second is always the Cartographer installation to allow " - "including files from there."); -DEFINE_string(configuration_basename, "", - "Basename, i.e. not containing any directory prefix, of the " - "configuration file."); - -DEFINE_string(initial_pose, "", "Starting pose of a new trajectory"); - -namespace cartographer_ros { -namespace { - -TrajectoryOptions LoadOptions() { - auto file_resolver = - absl::make_unique( - std::vector{FLAGS_configuration_directory}); - const std::string code = - file_resolver->GetFileContentOrDie(FLAGS_configuration_basename); - auto lua_parameter_dictionary = - cartographer::common::LuaParameterDictionary::NonReferenceCounted( - code, std::move(file_resolver)); - if (!FLAGS_initial_pose.empty()) { - auto initial_trajectory_pose_file_resolver = - absl::make_unique( - std::vector{FLAGS_configuration_directory}); - auto initial_trajectory_pose = - cartographer::common::LuaParameterDictionary::NonReferenceCounted( - "return " + FLAGS_initial_pose, - std::move(initial_trajectory_pose_file_resolver)); - return CreateTrajectoryOptions(lua_parameter_dictionary.get(), - initial_trajectory_pose.get()); - } else { - return CreateTrajectoryOptions(lua_parameter_dictionary.get()); - } -} - -bool Run() { - ros::NodeHandle node_handle; - ros::ServiceClient client = - node_handle.serviceClient( - kStartTrajectoryServiceName); - cartographer_ros_msgs::StartTrajectory srv; - srv.request.options = ToRosMessage(LoadOptions()); - srv.request.topics.laser_scan_topic = node_handle.resolveName( - kLaserScanTopic, true /* apply topic remapping */); - srv.request.topics.multi_echo_laser_scan_topic = - node_handle.resolveName(kMultiEchoLaserScanTopic, true); - srv.request.topics.point_cloud2_topic = - node_handle.resolveName(kPointCloud2Topic, true); - srv.request.topics.imu_topic = node_handle.resolveName(kImuTopic, true); - srv.request.topics.odometry_topic = - node_handle.resolveName(kOdometryTopic, true); - - if (!client.call(srv)) { - LOG(ERROR) << "Failed to call " << kStartTrajectoryServiceName << "."; - return false; - } - if (srv.response.status.code != cartographer_ros_msgs::StatusCode::OK) { - LOG(ERROR) << "Error starting trajectory - message: '" - << srv.response.status.message - << "' (status code: " << std::to_string(srv.response.status.code) - << ")."; - return false; - } - LOG(INFO) << "Started trajectory " << srv.response.trajectory_id; - return true; -} - -} // namespace -} // namespace cartographer_ros - -int main(int argc, char** argv) { - google::InitGoogleLogging(argv[0]); - google::SetUsageMessage( - "\n\n" - "Convenience tool around the start_trajectory service. This takes a Lua " - "file that is accepted by the node as well and starts a new trajectory " - "using its settings.\n"); - google::ParseCommandLineFlags(&argc, &argv, true); - - CHECK(!FLAGS_configuration_directory.empty()) - << "-configuration_directory is missing."; - CHECK(!FLAGS_configuration_basename.empty()) - << "-configuration_basename is missing."; - - ::ros::init(argc, argv, "cartographer_start_trajectory"); - ::ros::start(); - - cartographer_ros::ScopedRosLogSink ros_log_sink; - int exit_code = cartographer_ros::Run() ? 0 : 1; - ::ros::shutdown(); - return exit_code; -} diff --git a/cartographer_ros/cartographer_ros/trajectory_options.cc b/cartographer_ros/cartographer_ros/trajectory_options.cc index 0c092abd0..143f5b22e 100644 --- a/cartographer_ros/cartographer_ros/trajectory_options.cc +++ b/cartographer_ros/cartographer_ros/trajectory_options.cc @@ -79,85 +79,4 @@ TrajectoryOptions CreateTrajectoryOptions( CheckTrajectoryOptions(options); return options; } - -TrajectoryOptions CreateTrajectoryOptions( - ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary, - ::cartographer::common::LuaParameterDictionary* initial_trajectory_pose) { - TrajectoryOptions options = CreateTrajectoryOptions(lua_parameter_dictionary); - *options.trajectory_builder_options.mutable_initial_trajectory_pose() = - CreateInitialTrajectoryPose(initial_trajectory_pose); - return options; -} - -::cartographer::mapping::proto::InitialTrajectoryPose -CreateInitialTrajectoryPose( - ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary) { - ::cartographer::mapping::proto::InitialTrajectoryPose pose; - pose.set_to_trajectory_id( - lua_parameter_dictionary->GetNonNegativeInt("to_trajectory_id")); - *pose.mutable_relative_pose() = - cartographer::transform::ToProto(cartographer::transform::FromDictionary( - lua_parameter_dictionary->GetDictionary("relative_pose").get())); - pose.set_timestamp( - lua_parameter_dictionary->HasKey("timestamp") - ? lua_parameter_dictionary->GetNonNegativeInt("timestamp") - : cartographer::common::ToUniversal(FromRos(ros::Time::now()))); - return pose; -} - -bool FromRosMessage(const cartographer_ros_msgs::TrajectoryOptions& msg, - TrajectoryOptions* options) { - options->tracking_frame = msg.tracking_frame; - options->published_frame = msg.published_frame; - options->odom_frame = msg.odom_frame; - options->provide_odom_frame = msg.provide_odom_frame; - options->use_odometry = msg.use_odometry; - options->use_nav_sat = msg.use_nav_sat; - options->use_landmarks = msg.use_landmarks; - options->publish_frame_projected_to_2d = msg.publish_frame_projected_to_2d; - options->num_laser_scans = msg.num_laser_scans; - options->num_multi_echo_laser_scans = msg.num_multi_echo_laser_scans; - options->num_subdivisions_per_laser_scan = - msg.num_subdivisions_per_laser_scan; - options->num_point_clouds = msg.num_point_clouds; - options->rangefinder_sampling_ratio = msg.rangefinder_sampling_ratio; - options->odometry_sampling_ratio = msg.odometry_sampling_ratio; - options->fixed_frame_pose_sampling_ratio = - msg.fixed_frame_pose_sampling_ratio; - options->imu_sampling_ratio = msg.imu_sampling_ratio; - options->landmarks_sampling_ratio = msg.landmarks_sampling_ratio; - if (!options->trajectory_builder_options.ParseFromString( - msg.trajectory_builder_options_proto)) { - LOG(ERROR) << "Failed to parse protobuf"; - return false; - } - CheckTrajectoryOptions(*options); - return true; -} - -cartographer_ros_msgs::TrajectoryOptions ToRosMessage( - const TrajectoryOptions& options) { - cartographer_ros_msgs::TrajectoryOptions msg; - msg.tracking_frame = options.tracking_frame; - msg.published_frame = options.published_frame; - msg.odom_frame = options.odom_frame; - msg.provide_odom_frame = options.provide_odom_frame; - msg.use_odometry = options.use_odometry; - msg.use_nav_sat = options.use_nav_sat; - msg.use_landmarks = options.use_landmarks; - msg.publish_frame_projected_to_2d = options.publish_frame_projected_to_2d; - msg.num_laser_scans = options.num_laser_scans; - msg.num_multi_echo_laser_scans = options.num_multi_echo_laser_scans; - msg.num_subdivisions_per_laser_scan = options.num_subdivisions_per_laser_scan; - msg.num_point_clouds = options.num_point_clouds; - msg.rangefinder_sampling_ratio = options.rangefinder_sampling_ratio; - msg.odometry_sampling_ratio = options.odometry_sampling_ratio; - msg.fixed_frame_pose_sampling_ratio = options.fixed_frame_pose_sampling_ratio; - msg.imu_sampling_ratio = options.imu_sampling_ratio; - msg.landmarks_sampling_ratio = options.landmarks_sampling_ratio; - options.trajectory_builder_options.SerializeToString( - &msg.trajectory_builder_options_proto); - return msg; -} - } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/trajectory_options.h b/cartographer_ros/cartographer_ros/trajectory_options.h index 97be4c97f..93817b7d0 100644 --- a/cartographer_ros/cartographer_ros/trajectory_options.h +++ b/cartographer_ros/cartographer_ros/trajectory_options.h @@ -22,7 +22,6 @@ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/port.h" #include "cartographer/mapping/proto/trajectory_builder_options.pb.h" -#include "cartographer_ros_msgs/TrajectoryOptions.h" namespace cartographer_ros { @@ -48,25 +47,9 @@ struct TrajectoryOptions { double landmarks_sampling_ratio; }; -::cartographer::mapping::proto::InitialTrajectoryPose -CreateInitialTrajectoryPose( - ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary); - TrajectoryOptions CreateTrajectoryOptions( ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary); -TrajectoryOptions CreateTrajectoryOptions( - ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary, - ::cartographer::common::LuaParameterDictionary* initial_trajectory_pose); - -// Try to convert 'msg' into 'options'. Returns false on failure. -bool FromRosMessage(const cartographer_ros_msgs::TrajectoryOptions& msg, - TrajectoryOptions* options); - -// Converts 'trajectory_options' into a ROS message. -cartographer_ros_msgs::TrajectoryOptions ToRosMessage( - const TrajectoryOptions& trajectory_options); - } // namespace cartographer_ros #endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TRAJECTORY_OPTIONS_H diff --git a/cartographer_ros_msgs/CMakeLists.txt b/cartographer_ros_msgs/CMakeLists.txt index 15c86de36..42d3c97e5 100644 --- a/cartographer_ros_msgs/CMakeLists.txt +++ b/cartographer_ros_msgs/CMakeLists.txt @@ -33,13 +33,11 @@ add_message_files( MetricFamily.msg MetricLabel.msg Metric.msg - SensorTopics.msg StatusCode.msg StatusResponse.msg SubmapEntry.msg SubmapList.msg SubmapTexture.msg - TrajectoryOptions.msg TrajectoryStates.msg ) diff --git a/cartographer_ros_msgs/msg/SensorTopics.msg b/cartographer_ros_msgs/msg/SensorTopics.msg deleted file mode 100644 index 1db96864d..000000000 --- a/cartographer_ros_msgs/msg/SensorTopics.msg +++ /dev/null @@ -1,21 +0,0 @@ -# Copyright 2016 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. - -string laser_scan_topic -string multi_echo_laser_scan_topic -string point_cloud2_topic -string imu_topic -string odometry_topic -string nav_sat_fix_topic -string landmark_topic diff --git a/cartographer_ros_msgs/msg/TrajectoryOptions.msg b/cartographer_ros_msgs/msg/TrajectoryOptions.msg deleted file mode 100644 index b41810ce5..000000000 --- a/cartographer_ros_msgs/msg/TrajectoryOptions.msg +++ /dev/null @@ -1,35 +0,0 @@ -# Copyright 2016 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. - -string tracking_frame -string published_frame -string odom_frame -bool provide_odom_frame -bool use_odometry -bool use_nav_sat -bool use_landmarks -bool publish_frame_projected_to_2d -int32 num_laser_scans -int32 num_multi_echo_laser_scans -int32 num_subdivisions_per_laser_scan -int32 num_point_clouds -float64 rangefinder_sampling_ratio -float64 odometry_sampling_ratio -float64 fixed_frame_pose_sampling_ratio -float64 imu_sampling_ratio -float64 landmarks_sampling_ratio - -# This is a binary-encoded -# 'cartographer.mapping.proto.TrajectoryBuilderOptions' proto. -string trajectory_builder_options_proto diff --git a/cartographer_ros_msgs/srv/StartTrajectory.srv b/cartographer_ros_msgs/srv/StartTrajectory.srv index 98e245ade..569663548 100644 --- a/cartographer_ros_msgs/srv/StartTrajectory.srv +++ b/cartographer_ros_msgs/srv/StartTrajectory.srv @@ -12,8 +12,11 @@ # See the License for the specific language governing permissions and # limitations under the License. -cartographer_ros_msgs/TrajectoryOptions options -cartographer_ros_msgs/SensorTopics topics +string configuration_directory +string configuration_basename +bool use_initial_pose +geometry_msgs/Pose initial_pose +int32 relative_to_trajectory_id --- cartographer_ros_msgs/StatusResponse status int32 trajectory_id From 920db8a99991332cb8a775eb8968a82334b1797e Mon Sep 17 00:00:00 2001 From: mgladkova Date: Tue, 9 Jul 2019 14:10:58 +0200 Subject: [PATCH 07/94] Update ROS API documentation with description of changed/new services (#1282) Signed-off-by: Guillaume Doisy --- docs/source/ros_api.rst | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/docs/source/ros_api.rst b/docs/source/ros_api.rst index 263d1090c..a4ddca984 100644 --- a/docs/source/ros_api.rst +++ b/docs/source/ros_api.rst @@ -94,9 +94,11 @@ submap_query (`cartographer_ros_msgs/SubmapQuery`_) Fetches the requested submap. start_trajectory (`cartographer_ros_msgs/StartTrajectory`_) - Starts another trajectory by specifying its sensor topics and trajectory - options as an binary-encoded proto. Returns an assigned trajectory ID. - The ``start_trajectory`` executable provides a convenient wrapper to use this service. + Starts a trajectory using default sensor topics and the provided configuration. + An initial pose can be optionally specified. Returns an assigned trajectory ID. + +trajectory_query (`cartographer_ros_msgs/TrajectoryQuery`_) + Returns the trajectory data from the pose graph. finish_trajectory (`cartographer_ros_msgs/FinishTrajectory`_) Finishes the given `trajectory_id`'s trajectory by running a final optimization. @@ -141,6 +143,7 @@ If *provide_odom_frame* is enabled in the :doc:`configuration`, a continuous .. _cartographer_ros_msgs/SubmapList: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/msg/SubmapList.msg .. _cartographer_ros_msgs/SubmapQuery: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/srv/SubmapQuery.srv .. _cartographer_ros_msgs/StartTrajectory: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/srv/StartTrajectory.srv +.. _cartographer_ros_msgs/TrajectoryQuery: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/srv/TrajectoryQuery.srv .. _cartographer_ros_msgs/WriteState: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/srv/WriteState.srv .. _cartographer_ros_msgs/GetTrajectoryStates: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/srv/GetTrajectoryStates.srv .. _cartographer_ros_msgs/ReadMetrics: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/srv/ReadMetrics.srv From 76f8989dcaea57da4f41df74a66f1e8ca4f4ceb7 Mon Sep 17 00:00:00 2001 From: Tully Foote Date: Tue, 27 Aug 2019 00:44:15 -0700 Subject: [PATCH 08/94] Remove EOL Distros from CI matrix (#1379) Indigo and Lunar are both EOL Signed-off-by: Guillaume Doisy --- .travis.yml | 2 -- 1 file changed, 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index a8e9a4291..17ee65648 100644 --- a/.travis.yml +++ b/.travis.yml @@ -22,9 +22,7 @@ cache: - /home/travis/docker/ env: - - ROS_RELEASE=indigo DOCKER_CACHE_FILE=/home/travis/docker/indigo-cache.tar.gz - ROS_RELEASE=kinetic DOCKER_CACHE_FILE=/home/travis/docker/kinetic-cache.tar.gz - - ROS_RELEASE=lunar DOCKER_CACHE_FILE=/home/travis/docker/lunar-cache.tar.gz - ROS_RELEASE=melodic DOCKER_CACHE_FILE=/home/travis/docker/melodic-cache.tar.gz before_install: From d872cbe50191ebbb7d94e67daa33072b0d597699 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Tue, 27 Aug 2019 14:03:27 +0200 Subject: [PATCH 09/94] Remove unnecessary eigen_conversions dependency. (#1278) Seems to have been used in the past, but isn't needed anymore. Signed-off-by: Guillaume Doisy --- cartographer_ros/CMakeLists.txt | 1 - cartographer_ros/package.xml | 1 - cartographer_rviz/CMakeLists.txt | 1 - cartographer_rviz/cartographer_rviz/drawable_submap.cc | 1 - cartographer_rviz/package.xml | 1 - 5 files changed, 5 deletions(-) diff --git a/cartographer_ros/CMakeLists.txt b/cartographer_ros/CMakeLists.txt index a379726b7..acd4e2c5b 100644 --- a/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/CMakeLists.txt @@ -18,7 +18,6 @@ project(cartographer_ros) set(PACKAGE_DEPENDENCIES cartographer_ros_msgs - eigen_conversions geometry_msgs message_runtime nav_msgs diff --git a/cartographer_ros/package.xml b/cartographer_ros/package.xml index 69026693b..f1f1c95e2 100644 --- a/cartographer_ros/package.xml +++ b/cartographer_ros/package.xml @@ -43,7 +43,6 @@ cartographer cartographer_ros_msgs - eigen_conversions geometry_msgs libgflags-dev libgoogle-glog-dev diff --git a/cartographer_rviz/CMakeLists.txt b/cartographer_rviz/CMakeLists.txt index c19c98675..bb981ae37 100644 --- a/cartographer_rviz/CMakeLists.txt +++ b/cartographer_rviz/CMakeLists.txt @@ -19,7 +19,6 @@ project(cartographer_rviz) set(PACKAGE_DEPENDENCIES cartographer_ros cartographer_ros_msgs - eigen_conversions message_runtime roscpp roslib diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index 2432ac243..d2c800f4b 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -27,7 +27,6 @@ #include "cartographer/common/port.h" #include "cartographer_ros/msg_conversion.h" #include "cartographer_ros_msgs/SubmapQuery.h" -#include "eigen_conversions/eigen_msg.h" #include "ros/ros.h" namespace cartographer_rviz { diff --git a/cartographer_rviz/package.xml b/cartographer_rviz/package.xml index 9b8bdcc65..28fd22403 100644 --- a/cartographer_rviz/package.xml +++ b/cartographer_rviz/package.xml @@ -41,7 +41,6 @@ cartographer cartographer_ros cartographer_ros_msgs - eigen_conversions libqt5-core libqt5-gui libqt5-widgets From f8c572b31c89e71de7ddd976b9af6c952131440b Mon Sep 17 00:00:00 2001 From: Bo Chen Date: Wed, 17 Jun 2020 13:47:28 +0200 Subject: [PATCH 10/94] Fix broken build for melodic. Use official ros docker image. (#1484) Essentially revert #1021 as image seems to be fixed Signed-off-by: Bo Chen Signed-off-by: Guillaume Doisy --- Dockerfile.base.melodic | 43 ------------------------------ Dockerfile.melodic | 9 ++++--- scripts/update_catkin_workspace.sh | 26 ------------------ 3 files changed, 6 insertions(+), 72 deletions(-) delete mode 100644 Dockerfile.base.melodic delete mode 100755 scripts/update_catkin_workspace.sh diff --git a/Dockerfile.base.melodic b/Dockerfile.base.melodic deleted file mode 100644 index 4b2274fdd..000000000 --- a/Dockerfile.base.melodic +++ /dev/null @@ -1,43 +0,0 @@ -# Copyright 2018 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. - -FROM ros:melodic - -ARG CARTOGRAPHER_VERSION=master - -# Bionic's base image doesn't ship with sudo. -RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/* - -# First, we invalidate the entire cache if googlecartographer/cartographer has -# changed. This file's content changes whenever master changes. See: -# http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone -ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/master \ - cartographer_ros/cartographer_version.json - -# wstool needs the updated rosinstall file to clone the correct repos. -COPY cartographer_ros.rosinstall cartographer_ros/ -COPY scripts/prepare_catkin_workspace.sh cartographer_ros/scripts/ -RUN CARTOGRAPHER_VERSION=$CARTOGRAPHER_VERSION \ - cartographer_ros/scripts/prepare_catkin_workspace.sh - -# rosdep needs the updated package.xml files to install the correct debs. -COPY cartographer_ros/package.xml catkin_ws/src/cartographer_ros/cartographer_ros/ -COPY cartographer_ros_msgs/package.xml catkin_ws/src/cartographer_ros/cartographer_ros_msgs/ -COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_rviz/ -COPY scripts/install_debs.sh cartographer_ros/scripts/ -RUN cartographer_ros/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/* - -# Install proto3. -RUN /catkin_ws/src/cartographer/scripts/install_proto3.sh - diff --git a/Dockerfile.melodic b/Dockerfile.melodic index 86523f7ca..3091e771f 100644 --- a/Dockerfile.melodic +++ b/Dockerfile.melodic @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -FROM eu.gcr.io/cartographer-141408/cartographer_ros_melodic_base +FROM ros:melodic ARG CARTOGRAPHER_VERSION=master @@ -30,9 +30,9 @@ ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/ # wstool needs the updated rosinstall file to clone the correct repos. COPY cartographer_ros.rosinstall cartographer_ros/ -COPY scripts/update_catkin_workspace.sh cartographer_ros/scripts/ +COPY scripts/prepare_catkin_workspace.sh cartographer_ros/scripts/ RUN CARTOGRAPHER_VERSION=$CARTOGRAPHER_VERSION \ - cartographer_ros/scripts/update_catkin_workspace.sh + cartographer_ros/scripts/prepare_catkin_workspace.sh # rosdep needs the updated package.xml files to install the correct debs. COPY cartographer_ros/package.xml catkin_ws/src/cartographer_ros/cartographer_ros/ @@ -41,6 +41,9 @@ COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_r COPY scripts/install_debs.sh cartographer_ros/scripts/ RUN cartographer_ros/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/* +# Install proto3. +RUN /catkin_ws/src/cartographer/scripts/install_proto3.sh + # Build, install, and test all packages individually to allow caching. The # ordering of these steps must match the topological package ordering as # determined by Catkin. diff --git a/scripts/update_catkin_workspace.sh b/scripts/update_catkin_workspace.sh deleted file mode 100755 index 15807cd6d..000000000 --- a/scripts/update_catkin_workspace.sh +++ /dev/null @@ -1,26 +0,0 @@ -#!/bin/sh - -# Copyright 2018 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. - -set -o errexit -set -o verbose - -. /opt/ros/${ROS_DISTRO}/setup.sh - -cd catkin_ws/src - -# Call 'status' as a workaround for https://github.com/vcstools/wstool/issues/77 -wstool status -wstool update From a4630340bebf889322de82a11e86fd8db48bf417 Mon Sep 17 00:00:00 2001 From: Bo Chen Date: Wed, 17 Jun 2020 16:16:26 +0200 Subject: [PATCH 11/94] Change Google CLA to DCO in CONTRIBUTING.md (#1482) Signed-off-by: Guillaume Doisy --- CONTRIBUTING.md | 50 ++++++++++++++++++++++++++----------------------- 1 file changed, 27 insertions(+), 23 deletions(-) diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 2827b7d3f..f123a3d31 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -1,27 +1,31 @@ -Want to contribute? Great! First, read this page (including the small print at the end). +Want to contribute? Great! First, read this page. ### Before you contribute -Before we can use your code, you must sign the -[Google Individual Contributor License Agreement] -(https://cla.developers.google.com/about/google-individual) -(CLA), which you can do online. The CLA is necessary mainly because you own the -copyright to your changes, even after your contribution becomes part of our -codebase, so we need your permission to use and distribute your code. We also -need to be sure of various other things—for instance that you'll tell us if you -know that your code infringes on other people's patents. You don't have to sign -the CLA until after you've submitted your code for review and a member has -approved it, but you must do it before we can put your code into our codebase. -Before you start working on a larger contribution, you should get in touch with -us first through the issue tracker with your idea so that we can help out and -possibly guide you. Coordinating up front makes it much easier to avoid -frustration later on. + +Any contribution that you make to this repository will +be under the Apache 2 License, as dictated by that +[license](http://www.apache.org/licenses/LICENSE-2.0): + +``` +5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. +``` + +### Developer Certificate of Origin + +Contributors must sign-off each commit by adding a `Signed-off-by: ...` +line to commit messages to certify that they have the right to submit +the code they are contributing to the project according to the +[Developer Certificate of Origin (DCO)](https://developercertificate.org/). +You can sign-off a commit via `git commit -s`. ### Code reviews -All submissions, including submissions by project members, require review. We -use Github pull requests for this purpose. - -### The small print -Contributions made by corporations are covered by a different agreement than -the one above, the -[Software Grant and Corporate Contributor License Agreement] -(https://cla.developers.google.com/about/google-corporate). + +All submissions, including submissions by project members, require review. +We use GitHub pull requests for this purpose. + From ad5e157d6e4bd383472f40781165779b4a43d5db Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 24 Jun 2020 10:55:03 +0200 Subject: [PATCH 12/94] Fix build status on front page. (#1483) * Fix build status on front page. Changes "googlecartographer" to "cartographer-project" for references to CI and GitHub. Following cartographer-project/cartographer#1693. Signed-off-by: Wolfgang Hess * Remove Indigo and Lunar Dockerfiles. Signed-off-by: Guillaume Doisy --- Dockerfile.indigo | 76 ------------------ Dockerfile.kinetic | 4 +- Dockerfile.lunar | 79 ------------------- Dockerfile.melodic | 4 +- README.rst | 12 +-- azure-pipelines.yml | 2 +- cartographer_ros.rosinstall | 4 +- .../assets_writer_backpack_2d.lua | 2 +- cartographer_ros/package.xml | 2 +- cartographer_ros_msgs/package.xml | 2 +- cartographer_rviz/package.xml | 2 +- cartographer_rviz/rviz_plugin_description.xml | 2 +- docs/source/algo_walkthrough.rst | 10 +-- docs/source/assets_writer.rst | 20 ++--- docs/source/compilation.rst | 4 +- docs/source/configuration.rst | 4 +- docs/source/faq.rst | 2 +- docs/source/getting_involved.rst | 8 +- docs/source/going_further.rst | 2 +- docs/source/index.rst | 2 +- docs/source/ros_api.rst | 26 +++--- docs/source/tuning.rst | 14 ++-- jenkins/Dockerfile.kinetic | 4 +- jenkins/Jenkinsfile | 2 +- jenkins/worker.py | 2 +- scripts/prepare_jenkins_catkin_workspace.sh | 2 +- 26 files changed, 68 insertions(+), 225 deletions(-) delete mode 100644 Dockerfile.indigo delete mode 100644 Dockerfile.lunar diff --git a/Dockerfile.indigo b/Dockerfile.indigo deleted file mode 100644 index 2ea9ed60e..000000000 --- a/Dockerfile.indigo +++ /dev/null @@ -1,76 +0,0 @@ -# Copyright 2016 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. - -FROM ros:indigo - -ARG CARTOGRAPHER_VERSION=master - -# We require a GitHub access token to be passed. -ARG github_token - -# First, we invalidate the entire cache if googlecartographer/cartographer has -# changed. This file's content changes whenever master changes. See: -# http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone -ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/master?access_token=$github_token \ - cartographer_ros/cartographer_version.json - -# wstool needs the updated rosinstall file to clone the correct repos. -COPY cartographer_ros.rosinstall cartographer_ros/ -COPY scripts/prepare_catkin_workspace.sh cartographer_ros/scripts/ -RUN CARTOGRAPHER_VERSION=$CARTOGRAPHER_VERSION \ - cartographer_ros/scripts/prepare_catkin_workspace.sh - -# rosdep needs the updated package.xml files to install the correct debs. -COPY cartographer_ros/package.xml catkin_ws/src/cartographer_ros/cartographer_ros/ -COPY cartographer_ros_msgs/package.xml catkin_ws/src/cartographer_ros/cartographer_ros_msgs/ -COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_rviz/ -COPY scripts/install_debs.sh cartographer_ros/scripts/ -RUN cartographer_ros/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/* - -# Install proto3. -RUN /catkin_ws/src/cartographer/scripts/install_proto3.sh - -# Build, install, and test all packages individually to allow caching. The -# ordering of these steps must match the topological package ordering as -# determined by Catkin. -COPY scripts/install.sh cartographer_ros/scripts/ -COPY scripts/catkin_test_results.sh cartographer_ros/scripts/ - -COPY cartographer_ros_msgs catkin_ws/src/cartographer_ros/cartographer_ros_msgs/ -RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs && \ - cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs \ - --catkin-make-args run_tests && \ - cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros_msgs - -RUN cartographer_ros/scripts/install.sh --pkg ceres-solver - -RUN cartographer_ros/scripts/install.sh --pkg cartographer && \ - cartographer_ros/scripts/install.sh --pkg cartographer --make-args test - -COPY cartographer_ros catkin_ws/src/cartographer_ros/cartographer_ros/ -RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros && \ - cartographer_ros/scripts/install.sh --pkg cartographer_ros \ - --catkin-make-args run_tests && \ - cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros - -COPY cartographer_rviz catkin_ws/src/cartographer_ros/cartographer_rviz/ -RUN cartographer_ros/scripts/install.sh --pkg cartographer_rviz && \ - cartographer_ros/scripts/install.sh --pkg cartographer_rviz \ - --catkin-make-args run_tests && \ - cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_rviz - -COPY scripts/ros_entrypoint.sh / -# A BTRFS bug may prevent us from cleaning up these directories. -# https://btrfs.wiki.kernel.org/index.php/Problem_FAQ#I_cannot_delete_an_empty_directory -RUN rm -rf cartographer_ros catkin_ws || true diff --git a/Dockerfile.kinetic b/Dockerfile.kinetic index d7a26c373..581d541cf 100644 --- a/Dockerfile.kinetic +++ b/Dockerfile.kinetic @@ -22,10 +22,10 @@ ARG github_token # Xenial's base image doesn't ship with sudo. RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/* -# First, we invalidate the entire cache if googlecartographer/cartographer has +# First, we invalidate the entire cache if cartographer-project/cartographer has # changed. This file's content changes whenever master changes. See: # http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone -ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/master?access_token=$github_token \ +ADD https://api.github.com/repos/cartographer-project/cartographer/git/refs/heads/master?access_token=$github_token \ cartographer_ros/cartographer_version.json # wstool needs the updated rosinstall file to clone the correct repos. diff --git a/Dockerfile.lunar b/Dockerfile.lunar deleted file mode 100644 index f5af22ad1..000000000 --- a/Dockerfile.lunar +++ /dev/null @@ -1,79 +0,0 @@ -# Copyright 2016 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. - -FROM ros:lunar - -ARG CARTOGRAPHER_VERSION=master - -# We require a GitHub access token to be passed. -ARG github_token - -# Xenial's base image doesn't ship with sudo. -RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/* - -# First, we invalidate the entire cache if googlecartographer/cartographer has -# changed. This file's content changes whenever master changes. See: -# http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone -ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/master?access_token=$github_token \ - cartographer_ros/cartographer_version.json - -# wstool needs the updated rosinstall file to clone the correct repos. -COPY cartographer_ros.rosinstall cartographer_ros/ -COPY scripts/prepare_catkin_workspace.sh cartographer_ros/scripts/ -RUN CARTOGRAPHER_VERSION=$CARTOGRAPHER_VERSION \ - cartographer_ros/scripts/prepare_catkin_workspace.sh - -# rosdep needs the updated package.xml files to install the correct debs. -COPY cartographer_ros/package.xml catkin_ws/src/cartographer_ros/cartographer_ros/ -COPY cartographer_ros_msgs/package.xml catkin_ws/src/cartographer_ros/cartographer_ros_msgs/ -COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_rviz/ -COPY scripts/install_debs.sh cartographer_ros/scripts/ -RUN cartographer_ros/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/* - -# Install proto3. -RUN /catkin_ws/src/cartographer/scripts/install_proto3.sh - -# Build, install, and test all packages individually to allow caching. The -# ordering of these steps must match the topological package ordering as -# determined by Catkin. -COPY scripts/install.sh cartographer_ros/scripts/ -COPY scripts/catkin_test_results.sh cartographer_ros/scripts/ - -COPY cartographer_ros_msgs catkin_ws/src/cartographer_ros/cartographer_ros_msgs/ -RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs && \ - cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs \ - --catkin-make-args run_tests && \ - cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros_msgs - -RUN cartographer_ros/scripts/install.sh --pkg ceres-solver - -RUN cartographer_ros/scripts/install.sh --pkg cartographer && \ - cartographer_ros/scripts/install.sh --pkg cartographer --make-args test - -COPY cartographer_ros catkin_ws/src/cartographer_ros/cartographer_ros/ -RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros && \ - cartographer_ros/scripts/install.sh --pkg cartographer_ros \ - --catkin-make-args run_tests && \ - cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros - -COPY cartographer_rviz catkin_ws/src/cartographer_ros/cartographer_rviz/ -RUN cartographer_ros/scripts/install.sh --pkg cartographer_rviz && \ - cartographer_ros/scripts/install.sh --pkg cartographer_rviz \ - --catkin-make-args run_tests && \ - cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_rviz - -COPY scripts/ros_entrypoint.sh / -# A BTRFS bug may prevent us from cleaning up these directories. -# https://btrfs.wiki.kernel.org/index.php/Problem_FAQ#I_cannot_delete_an_empty_directory -RUN rm -rf cartographer_ros catkin_ws || true diff --git a/Dockerfile.melodic b/Dockerfile.melodic index 3091e771f..c1c48ce33 100644 --- a/Dockerfile.melodic +++ b/Dockerfile.melodic @@ -22,10 +22,10 @@ ARG github_token # Bionic's base image doesn't ship with sudo. RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/* -# First, we invalidate the entire cache if googlecartographer/cartographer has +# First, we invalidate the entire cache if cartographer-project/cartographer has # changed. This file's content changes whenever master changes. See: # http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone -ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/master?access_token=$github_token \ +ADD https://api.github.com/repos/cartographer-project/cartographer/git/refs/heads/master?access_token=$github_token \ cartographer_ros/cartographer_version.json # wstool needs the updated rosinstall file to clone the correct repos. diff --git a/README.rst b/README.rst index 9032f42ff..fb72eb6e2 100644 --- a/README.rst +++ b/README.rst @@ -25,7 +25,7 @@ Purpose and mapping (`SLAM`_) in 2D and 3D across multiple platforms and sensor configurations. This project provides Cartographer's ROS integration. -.. _Cartographer: https://github.com/googlecartographer/cartographer +.. _Cartographer: https://github.com/cartographer-project/cartographer .. _SLAM: https://en.wikipedia.org/wiki/Simultaneous_localization_and_mapping Getting started @@ -35,7 +35,7 @@ Getting started * You can ask a question by `creating an issue`_. .. _our Read the Docs site: https://google-cartographer-ros.readthedocs.io -.. _creating an issue: https://github.com/googlecartographer/cartographer_ros/issues/new?labels=question +.. _creating an issue: https://github.com/cartographer-project/cartographer_ros/issues/new?labels=question Contributing ============ @@ -43,12 +43,12 @@ Contributing You can find information about contributing to Cartographer's ROS integration at `our Contribution page`_. -.. _our Contribution page: https://github.com/googlecartographer/cartographer_ros/blob/master/CONTRIBUTING.md +.. _our Contribution page: https://github.com/cartographer-project/cartographer_ros/blob/master/CONTRIBUTING.md -.. |build| image:: https://travis-ci.org/googlecartographer/cartographer_ros.svg?branch=master +.. |build| image:: https://travis-ci.org/cartographer-project/cartographer_ros.svg?branch=master :alt: Build Status :scale: 100% - :target: https://travis-ci.org/googlecartographer/cartographer_ros + :target: https://travis-ci.org/cartographer-project/cartographer_ros .. |docs| image:: https://readthedocs.org/projects/google-cartographer-ros/badge/?version=latest :alt: Documentation Status :scale: 100% @@ -56,5 +56,5 @@ at `our Contribution page`_. .. |license| image:: https://img.shields.io/badge/License-Apache%202.0-blue.svg :alt: Apache 2 license. :scale: 100% - :target: https://github.com/googlecartographer/cartographer_ros/blob/master/LICENSE + :target: https://github.com/cartographer-project/cartographer_ros/blob/master/LICENSE diff --git a/azure-pipelines.yml b/azure-pipelines.yml index e359dd092..a778158c9 100644 --- a/azure-pipelines.yml +++ b/azure-pipelines.yml @@ -25,7 +25,7 @@ jobs: choco upgrade %ROS_METAPACKAGE% -y choco upgrade ros-melodic-perception -y robocopy "." ".\src\cartographer_ros" /E /MOVE /XD "src" > NUL - git clone https://github.com/googlecartographer/cartographer src\cartographer + git clone https://github.com/cartographer-project/cartographer src\cartographer call "C:\opt\ros\melodic\x64\env.bat" rosdep install --from-paths src --ignore-src -r -y env: ROS_METAPACKAGE: 'ros-melodic-desktop' diff --git a/cartographer_ros.rosinstall b/cartographer_ros.rosinstall index 89dae9bd4..6927cb835 100644 --- a/cartographer_ros.rosinstall +++ b/cartographer_ros.rosinstall @@ -1,3 +1,3 @@ -- git: {local-name: cartographer, uri: 'https://github.com/googlecartographer/cartographer.git', version: '1.0.0'} -- git: {local-name: cartographer_ros, uri: 'https://github.com/googlecartographer/cartographer_ros.git', version: '1.0.0'} +- git: {local-name: cartographer, uri: 'https://github.com/cartographer-project/cartographer.git', version: '1.0.0'} +- git: {local-name: cartographer_ros, uri: 'https://github.com/cartographer-project/cartographer_ros.git', version: '1.0.0'} - git: {local-name: ceres-solver, uri: 'https://ceres-solver.googlesource.com/ceres-solver.git', version: '1.13.0'} diff --git a/cartographer_ros/configuration_files/assets_writer_backpack_2d.lua b/cartographer_ros/configuration_files/assets_writer_backpack_2d.lua index de48eb086..9a2d3bf8e 100644 --- a/cartographer_ros/configuration_files/assets_writer_backpack_2d.lua +++ b/cartographer_ros/configuration_files/assets_writer_backpack_2d.lua @@ -87,7 +87,7 @@ options = { -- We also write a PLY file at this stage, because gray points look good. -- The points in the PLY can be visualized using - -- https://github.com/googlecartographer/point_cloud_viewer. + -- https://github.com/cartographer-project/point_cloud_viewer. { action = "write_ply", filename = "points.ply", diff --git a/cartographer_ros/package.xml b/cartographer_ros/package.xml index f1f1c95e2..fe5cc9c6c 100644 --- a/cartographer_ros/package.xml +++ b/cartographer_ros/package.xml @@ -28,7 +28,7 @@ Apache 2.0 - https://github.com/googlecartographer/cartographer_ros + https://github.com/cartographer-project/cartographer_ros The Cartographer Authors diff --git a/cartographer_ros_msgs/package.xml b/cartographer_ros_msgs/package.xml index 2e6825d04..e26713349 100644 --- a/cartographer_ros_msgs/package.xml +++ b/cartographer_ros_msgs/package.xml @@ -26,7 +26,7 @@ Apache 2.0 - https://github.com/googlecartographer/cartographer_ros + https://github.com/cartographer-project/cartographer_ros The Cartographer Authors diff --git a/cartographer_rviz/package.xml b/cartographer_rviz/package.xml index 28fd22403..b43038907 100644 --- a/cartographer_rviz/package.xml +++ b/cartographer_rviz/package.xml @@ -28,7 +28,7 @@ Apache 2.0 - https://github.com/googlecartographer/cartographer_ros + https://github.com/cartographer-project/cartographer_ros The Cartographer Authors diff --git a/cartographer_rviz/rviz_plugin_description.xml b/cartographer_rviz/rviz_plugin_description.xml index 833b60194..58db7086d 100644 --- a/cartographer_rviz/rviz_plugin_description.xml +++ b/cartographer_rviz/rviz_plugin_description.xml @@ -20,7 +20,7 @@ base_class_type="rviz::Display"> Displays submaps as a unified map in RViz. - https://github.com/googlecartographer/cartographer_ros + https://github.com/cartographer-project/cartographer_ros diff --git a/docs/source/algo_walkthrough.rst b/docs/source/algo_walkthrough.rst index a5da0fba0..483dcf59f 100644 --- a/docs/source/algo_walkthrough.rst +++ b/docs/source/algo_walkthrough.rst @@ -31,8 +31,8 @@ IEEE, 2016. pp. 1271–1278. Overview -------- -.. image:: https://raw.githubusercontent.com/googlecartographer/cartographer/master/docs/source/high_level_system_overview.png - :target: https://github.com/googlecartographer/cartographer/blob/master/docs/source/high_level_system_overview.png +.. image:: https://raw.githubusercontent.com/cartographer-project/cartographer/master/docs/source/high_level_system_overview.png + :target: https://github.com/cartographer-project/cartographer/blob/master/docs/source/high_level_system_overview.png Cartographer can be seen as two separate, but related subsystems. The first one is **local SLAM** (sometimes also called **frontend** or local trajectory builder). @@ -40,8 +40,8 @@ Its job is to build a succession of **submaps**. Each submap is meant to be locally consistent but we accept that local SLAM drifts over time. Most of the local SLAM options can be found in `install_isolated/share/cartographer/configuration_files/trajectory_builder_2d.lua`_ for 2D and `install_isolated/share/cartographer/configuration_files/trajectory_builder_3d.lua`_ for 3D. (for the rest of this page we will refer to `TRAJECTORY_BUILDER_nD` for the common options) -.. _install_isolated/share/cartographer/configuration_files/trajectory_builder_2d.lua: https://github.com/googlecartographer/cartographer/blob/df337194e21f98f8c7b0b88dab33f878066d4b56/configuration_files/trajectory_builder_2d.lua -.. _install_isolated/share/cartographer/configuration_files/trajectory_builder_3d.lua: https://github.com/googlecartographer/cartographer/blob/df337194e21f98f8c7b0b88dab33f878066d4b56/configuration_files/trajectory_builder_3d.lua +.. _install_isolated/share/cartographer/configuration_files/trajectory_builder_2d.lua: https://github.com/cartographer-project/cartographer/blob/df337194e21f98f8c7b0b88dab33f878066d4b56/configuration_files/trajectory_builder_2d.lua +.. _install_isolated/share/cartographer/configuration_files/trajectory_builder_3d.lua: https://github.com/cartographer-project/cartographer/blob/df337194e21f98f8c7b0b88dab33f878066d4b56/configuration_files/trajectory_builder_3d.lua The other subsystem is **global SLAM** (sometimes called the **backend**). It runs in background threads and its main job is to find **loop closure constraints**. @@ -50,7 +50,7 @@ It also incorporates other sensor data to get a higher level view and identify t In 3D, it also tries to find the direction of gravity. Most of its options can be found in `install_isolated/share/cartographer/configuration_files/pose_graph.lua`_ -.. _install_isolated/share/cartographer/configuration_files/pose_graph.lua: https://github.com/googlecartographer/cartographer/blob/df337194e21f98f8c7b0b88dab33f878066d4b56/configuration_files/pose_graph.lua +.. _install_isolated/share/cartographer/configuration_files/pose_graph.lua: https://github.com/cartographer-project/cartographer/blob/df337194e21f98f8c7b0b88dab33f878066d4b56/configuration_files/pose_graph.lua On a higher abstraction, the job of local SLAM is to generate good submaps and the job of global SLAM is to tie them most consistently together. diff --git a/docs/source/assets_writer.rst b/docs/source/assets_writer.rst index cf5a4da76..d3d6b4eb7 100644 --- a/docs/source/assets_writer.rst +++ b/docs/source/assets_writer.rst @@ -66,7 +66,7 @@ When running as an online node, Cartographer doesn't know when your bag (or sens Once you've retrieved your ``.pbstream`` file, you can run the assets writer with the `sample pipeline`_ for the 3D backpack: -.. _sample pipeline: https://github.com/googlecartographer/cartographer_ros/blob/44459e18102305745c56f92549b87d8e91f434fe/cartographer_ros/configuration_files/assets_writer_backpack_3d.lua +.. _sample pipeline: https://github.com/cartographer-project/cartographer_ros/blob/44459e18102305745c56f92549b87d8e91f434fe/cartographer_ros/configuration_files/assets_writer_backpack_3d.lua .. code-block:: bash @@ -83,17 +83,17 @@ Configuration The assets writer is modeled as a pipeline of `PointsProcessor`_s. `PointsBatch`_\ s flow through each processor and they all have the chance to modify the ``PointsBatch`` before passing it on. -.. _PointsProcessor: https://github.com/googlecartographer/cartographer/blob/30f7de1a325d6604c780f2f74d9a345ec369d12d/cartographer/io/points_processor.h -.. _PointsBatch: https://github.com/googlecartographer/cartographer/blob/30f7de1a325d6604c780f2f74d9a345ec369d12d/cartographer/io/points_batch.h +.. _PointsProcessor: https://github.com/cartographer-project/cartographer/blob/30f7de1a325d6604c780f2f74d9a345ec369d12d/cartographer/io/points_processor.h +.. _PointsBatch: https://github.com/cartographer-project/cartographer/blob/30f7de1a325d6604c780f2f74d9a345ec369d12d/cartographer/io/points_batch.h For example the `assets_writer_backpack_3d.lua`_ pipeline uses ``min_max_range_filter`` to remove points that are either too close or too far from the sensor. After this, it saves "*X-Rays*" (translucent side views of the map), then recolors the ``PointsBatch``\ s depending on the sensor frame ids and writes another set of X-Rays using these new colors. -.. _assets_writer_backpack_3d.lua: https://github.com/googlecartographer/cartographer_ros/blob/44459e18102305745c56f92549b87d8e91f434fe/cartographer_ros/configuration_files/assets_writer_backpack_3d.lua +.. _assets_writer_backpack_3d.lua: https://github.com/cartographer-project/cartographer_ros/blob/44459e18102305745c56f92549b87d8e91f434fe/cartographer_ros/configuration_files/assets_writer_backpack_3d.lua The available ``PointsProcessor``\ s are all defined in the `cartographer/io`_ sub-directory and documented in their individual header files. -.. _cartographer/io: https://github.com/googlecartographer/cartographer/tree/f1ac8967297965b8eb6f2f4b08a538e052b5a75b/cartographer/io +.. _cartographer/io: https://github.com/cartographer-project/cartographer/tree/f1ac8967297965b8eb6f2f4b08a538e052b5a75b/cartographer/io * **color_points**: Colors points with a fixed color by frame_id. * **dump_num_points**: Passes through points, but keeps track of how many points it saw and output that on Flush. @@ -115,18 +115,18 @@ First-person visualization of point clouds Two ``PointsProcessor``\ s are of particular interest: ``pcd_writing`` and ``ply_writing`` can save a point cloud in a ``.pcd`` or ``.ply`` file format. These file formats can then be used by specialized software such as `point_cloud_viewer`_ or `meshlab`_ to navigate through the high resolution map. -.. _point_cloud_viewer: https://github.com/googlecartographer/point_cloud_viewer +.. _point_cloud_viewer: https://github.com/cartographer-project/point_cloud_viewer .. _meshlab: http://www.meshlab.net/ The typical assets writer pipeline for this outcome is composed of an IntensityToColorPointsProcessor_ giving points a non-white color, then a PlyWritingPointsProcessor_ exporting the results to a ``.ply`` point cloud. An example of such a pipeline is in `assets_writer_backpack_2d.lua`_. -.. _IntensityToColorPointsProcessor: https://github.com/googlecartographer/cartographer/blob/30f7de1a325d6604c780f2f74d9a345ec369d12d/cartographer/io/intensity_to_color_points_processor.cc -.. _PlyWritingPointsProcessor: https://github.com/googlecartographer/cartographer/blob/30f7de1a325d6604c780f2f74d9a345ec369d12d/cartographer/io/ply_writing_points_processor.h -.. _assets_writer_backpack_2d.lua: https://github.com/googlecartographer/cartographer_ros/blob/44459e18102305745c56f92549b87d8e91f434fe/cartographer_ros/configuration_files/assets_writer_backpack_2d.lua +.. _IntensityToColorPointsProcessor: https://github.com/cartographer-project/cartographer/blob/30f7de1a325d6604c780f2f74d9a345ec369d12d/cartographer/io/intensity_to_color_points_processor.cc +.. _PlyWritingPointsProcessor: https://github.com/cartographer-project/cartographer/blob/30f7de1a325d6604c780f2f74d9a345ec369d12d/cartographer/io/ply_writing_points_processor.h +.. _assets_writer_backpack_2d.lua: https://github.com/cartographer-project/cartographer_ros/blob/44459e18102305745c56f92549b87d8e91f434fe/cartographer_ros/configuration_files/assets_writer_backpack_2d.lua Once you have the ``.ply``, follow the README of `point_cloud_viewer`_ to generate an on-disk octree data structure which can be viewed by one of the viewers (SDL or web based) in the same repo. -.. _point_cloud_viewer: https://github.com/googlecartographer/point_cloud_viewer +.. _point_cloud_viewer: https://github.com/cartographer-project/point_cloud_viewer .. image:: point_cloud_viewer_demo_3d.jpg diff --git a/docs/source/compilation.rst b/docs/source/compilation.rst index f02032c25..8ed7218f8 100644 --- a/docs/source/compilation.rst +++ b/docs/source/compilation.rst @@ -23,9 +23,7 @@ The Cartographer ROS requirements are the same as `the ones from Cartographer`_. The following `ROS distributions`_ are currently supported: -* Indigo * Kinetic -* Lunar * Melodic .. _the ones from Cartographer: https://google-cartographer.readthedocs.io/en/latest/#system-requirements @@ -50,7 +48,7 @@ Create a new cartographer_ros workspace in 'catkin_ws'. mkdir catkin_ws cd catkin_ws wstool init src - wstool merge -t src https://raw.githubusercontent.com/googlecartographer/cartographer_ros/master/cartographer_ros.rosinstall + wstool merge -t src https://raw.githubusercontent.com/cartographer-project/cartographer_ros/master/cartographer_ros.rosinstall wstool update -t src Install cartographer_ros' dependencies (proto3 and deb packages). diff --git a/docs/source/configuration.rst b/docs/source/configuration.rst index 1975e8b73..0d34b256b 100644 --- a/docs/source/configuration.rst +++ b/docs/source/configuration.rst @@ -130,6 +130,6 @@ landmarks_sampling_ratio .. _sensor_msgs/MultiEchoLaserScan: http://docs.ros.org/api/sensor_msgs/html/msg/MultiEchoLaserScan.html .. _sensor_msgs/PointCloud2: http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html .. _sensor_msgs/NavSatFix: http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html -.. _cartographer_ros_msgs/LandmarkList: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/msg/LandmarkList.msg -.. _cartographer_ros_msgs/LandmarkEntry: https://github.com/googlecartographer/cartographer_ros/blob/4b39ee68c7a4d518bf8d01a509331e2bc1f514a0/cartographer_ros_msgs/msg/LandmarkEntry.msg +.. _cartographer_ros_msgs/LandmarkList: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/msg/LandmarkList.msg +.. _cartographer_ros_msgs/LandmarkEntry: https://github.com/cartographer-project/cartographer_ros/blob/4b39ee68c7a4d518bf8d01a509331e2bc1f514a0/cartographer_ros_msgs/msg/LandmarkEntry.msg .. _tf2: http://wiki.ros.org/tf2 diff --git a/docs/source/faq.rst b/docs/source/faq.rst index ce72a7ae1..c0e607fb8 100644 --- a/docs/source/faq.rst +++ b/docs/source/faq.rst @@ -33,7 +33,7 @@ incorporates motion estimation by combining constant velocity and IMU measurements, for matching. Since there are two VLP-16s, 160 UDP packets is enough for roughly 2 revolutions, one per VLP-16. -__ https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros/configuration_files/backpack_3d.lua +__ https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros/configuration_files/backpack_3d.lua Why is IMU data required for 3D SLAM but not for 2D? ---------------------------------------------------- diff --git a/docs/source/getting_involved.rst b/docs/source/getting_involved.rst index 3e4509777..c3750872b 100644 --- a/docs/source/getting_involved.rst +++ b/docs/source/getting_involved.rst @@ -32,15 +32,15 @@ If you want to stay tuned with announcements (such as new major releases or new If you think you've found an issue in Cartographer, you are welcome to open a `GitHub issue`_ but don't forget to provide a way to reproduce your bug! Typically, join a ``.bag`` and a link to a fork of the ``cartographer_ros`` repository containing your configuration and launch files. -.. _GitHub issue: https://github.com/googlecartographer/cartographer/issues +.. _GitHub issue: https://github.com/cartographer-project/cartographer/issues If you have an idea of a significant change that should be documented and discussed before finding its way into Cartographer, you should submit it as a pull request to `the RFCs repository`_ first. Simpler changes can also be discussed in GitHub issues so that developers can help you get things right from the first try. -.. _the RFCs repository: https://github.com/googlecartographer/rfcs +.. _the RFCs repository: https://github.com/cartographer-project/rfcs If you want to contribute code or documentation, this is done through `GitHub pull requests`_. However, make sure you have signed (online) the `Contributor License Agreement`_ first! -.. _GitHub pull requests: https://github.com/googlecartographer/cartographer/pulls -.. _Contributor License Agreement: https://github.com/googlecartographer/cartographer/blob/master/CONTRIBUTING.md +.. _GitHub pull requests: https://github.com/cartographer-project/cartographer/pulls +.. _Contributor License Agreement: https://github.com/cartographer-project/cartographer/blob/master/CONTRIBUTING.md diff --git a/docs/source/going_further.rst b/docs/source/going_further.rst index 47c803ffd..3e94757ac 100644 --- a/docs/source/going_further.rst +++ b/docs/source/going_further.rst @@ -42,7 +42,7 @@ For landmarks publishing on a ``cartographer_ros_msgs/LandmarkList`` (`message d use_landmarks = true -.. _message defined in cartographer_ros: https://github.com/googlecartographer/cartographer_ros/blob/4b39ee68c7a4d518bf8d01a509331e2bc1f514a0/cartographer_ros_msgs/msg/LandmarkList.msg +.. _message defined in cartographer_ros: https://github.com/cartographer-project/cartographer_ros/blob/4b39ee68c7a4d518bf8d01a509331e2bc1f514a0/cartographer_ros_msgs/msg/LandmarkList.msg Localization only ================= diff --git a/docs/source/index.rst b/docs/source/index.rst index 0cbb5e406..03b85c02f 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -38,5 +38,5 @@ configurations. This project provides Cartographer's ROS integration. data faq -.. _Cartographer: https://github.com/googlecartographer/cartographer +.. _Cartographer: https://github.com/cartographer-project/cartographer .. _SLAM: https://en.wikipedia.org/wiki/Simultaneous_localization_and_mapping diff --git a/docs/source/ros_api.rst b/docs/source/ros_api.rst index a4ddca984..6e89f8ece 100644 --- a/docs/source/ros_api.rst +++ b/docs/source/ros_api.rst @@ -23,7 +23,7 @@ Cartographer Node The `cartographer_node`_ is the SLAM node used for online, real-time SLAM. -.. _cartographer_node: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros/cartographer_ros/node_main.cc +.. _cartographer_node: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros/cartographer_ros/node_main.cc Command-line Flags ------------------ @@ -139,14 +139,14 @@ If *provide_odom_frame* is enabled in the :doc:`configuration`, a continuous .. _robot_state_publisher: http://wiki.ros.org/robot_state_publisher .. _static_transform_publisher: http://wiki.ros.org/tf#static_transform_publisher -.. _cartographer_ros_msgs/FinishTrajectory: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/srv/FinishTrajectory.srv -.. _cartographer_ros_msgs/SubmapList: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/msg/SubmapList.msg -.. _cartographer_ros_msgs/SubmapQuery: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/srv/SubmapQuery.srv -.. _cartographer_ros_msgs/StartTrajectory: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/srv/StartTrajectory.srv -.. _cartographer_ros_msgs/TrajectoryQuery: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/srv/TrajectoryQuery.srv -.. _cartographer_ros_msgs/WriteState: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/srv/WriteState.srv -.. _cartographer_ros_msgs/GetTrajectoryStates: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/srv/GetTrajectoryStates.srv -.. _cartographer_ros_msgs/ReadMetrics: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/srv/ReadMetrics.srv +.. _cartographer_ros_msgs/FinishTrajectory: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/srv/FinishTrajectory.srv +.. _cartographer_ros_msgs/SubmapList: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/msg/SubmapList.msg +.. _cartographer_ros_msgs/SubmapQuery: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/srv/SubmapQuery.srv +.. _cartographer_ros_msgs/StartTrajectory: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/srv/StartTrajectory.srv +.. _cartographer_ros_msgs/TrajectoryQuery: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/srv/TrajectoryQuery.srv +.. _cartographer_ros_msgs/WriteState: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/srv/WriteState.srv +.. _cartographer_ros_msgs/GetTrajectoryStates: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/srv/GetTrajectoryStates.srv +.. _cartographer_ros_msgs/ReadMetrics: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/srv/ReadMetrics.srv .. _nav_msgs/OccupancyGrid: http://docs.ros.org/api/nav_msgs/html/msg/OccupancyGrid.html .. _nav_msgs/Odometry: http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html .. _sensor_msgs/Imu: http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html @@ -164,7 +164,7 @@ In all other regards, it behaves like the ``cartographer_node``. Each bag will become a separate trajectory in the final state. Once it is done processing all data, it writes out the final Cartographer state and exits. -.. _offline_node: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros/cartographer_ros/offline_node_main.cc +.. _offline_node: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros/cartographer_ros/offline_node_main.cc Published Topics @@ -176,7 +176,7 @@ In addition to the topics that are published by the online node, this node also Bag files processing progress including detailed information about the bag currently being processed which will be published with a predefined interval that can be specified using ``~bagfile_progress_pub_interval`` ROS parameter. -.. _cartographer_ros_msgs/BagfileProgress: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/msg/BagfileProgress.msg +.. _cartographer_ros_msgs/BagfileProgress: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/msg/BagfileProgress.msg Parameters ---------- @@ -194,7 +194,7 @@ Generating the map is expensive and slow, so map updates are in the order of sec You can can selectively include/exclude submaps from frozen (static) or active trajectories with a command line option. Call the node with the ``--help`` flag to see these options. -.. _occupancy_grid_node: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc +.. _occupancy_grid_node: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc Subscribed Topics ----------------- @@ -216,7 +216,7 @@ Pbstream Map Publisher Node The `pbstream_map_publisher`_ is a simple node that creates a static occupancy grid out of a serialized Cartographer state (pbstream format). It is an efficient alternative to the occupancy grid node if live updates are not important. -.. _pbstream_map_publisher: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc +.. _pbstream_map_publisher: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc Subscribed Topics ----------------- diff --git a/docs/source/tuning.rst b/docs/source/tuning.rst index fd4f7702e..53ad0d315 100644 --- a/docs/source/tuning.rst +++ b/docs/source/tuning.rst @@ -46,8 +46,8 @@ It is visible in the laser scan data that contradicting information is passed to But the slipping also indicates that we trust the point cloud matching too much and disregard the other sensors quite strongly. Our aim is to improve the situation through tuning. -.. _aba4575: https://github.com/googlecartographer/cartographer/commit/aba4575d937df4c9697f61529200c084f2562584 -.. _99c23b6: https://github.com/googlecartographer/cartographer_ros/commit/99c23b6ac7874f7974e9ed808ace841da6f2c8b0 +.. _aba4575: https://github.com/cartographer-project/cartographer/commit/aba4575d937df4c9697f61529200c084f2562584 +.. _99c23b6: https://github.com/cartographer-project/cartographer_ros/commit/99c23b6ac7874f7974e9ed808ace841da6f2c8b0 If we only look at this particular submap, that the error is fully contained in one submap. We also see that over time, global SLAM figures out that something weird happened and partially corrects for it. @@ -103,7 +103,7 @@ Before checking them in, we normalize all weights, since they only have relative The result of this tuning was `PR 428`_. In general, always try to tune for a platform, not a particular bag. -.. _PR 428: https://github.com/googlecartographer/cartographer/pull/428 +.. _PR 428: https://github.com/cartographer-project/cartographer/pull/428 Special Cases ------------- @@ -203,7 +203,7 @@ Developers are keen to help, but they can only be helpful if you follow `an issu There are already lots of GitHub issues with all sorts of problems solved by the developers. Going through `the closed issues of cartographer_ros`_ and `of cartographer`_ is a great way to learn more about Cartographer and maybe find a solution to your problem ! -.. _GitHub issue: https://github.com/googlecartographer/cartographer_ros/issues -.. _an issue template: https://github.com/googlecartographer/cartographer_ros/issues/new?labels=question -.. _the closed issues of cartographer_ros: https://github.com/googlecartographer/cartographer_ros/issues?q=is%3Aissue+is%3Aclosed -.. _of cartographer: https://github.com/googlecartographer/cartographer_ros/issues?q=is%3Aissue+is%3Aclosed +.. _GitHub issue: https://github.com/cartographer-project/cartographer_ros/issues +.. _an issue template: https://github.com/cartographer-project/cartographer_ros/issues/new?labels=question +.. _the closed issues of cartographer_ros: https://github.com/cartographer-project/cartographer_ros/issues?q=is%3Aissue+is%3Aclosed +.. _of cartographer: https://github.com/cartographer-project/cartographer_ros/issues?q=is%3Aissue+is%3Aclosed diff --git a/jenkins/Dockerfile.kinetic b/jenkins/Dockerfile.kinetic index 227aa79f3..ada39e5a1 100644 --- a/jenkins/Dockerfile.kinetic +++ b/jenkins/Dockerfile.kinetic @@ -18,10 +18,10 @@ ARG CARTOGRAPHER_VERSION=master # Xenial's base image doesn't ship with sudo. RUN apt-get update && apt-get install -y sudo time && rm -rf /var/lib/apt/lists/* -# First, we invalidate the entire cache if googlecartographer/cartographer has +# First, we invalidate the entire cache if cartographer-project/cartographer has # changed. This file's content changes whenever master changes. See: # http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone -ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/master \ +ADD https://api.github.com/repos/cartographer-project/cartographer/git/refs/heads/master \ cartographer_ros/cartographer_version.json # wstool needs the updated rosinstall file to clone the correct repos. diff --git a/jenkins/Jenkinsfile b/jenkins/Jenkinsfile index 06bb2fcf4..3a13d5446 100644 --- a/jenkins/Jenkinsfile +++ b/jenkins/Jenkinsfile @@ -20,7 +20,7 @@ podTemplate(label: 'node-0', containers: [ stage('Compile') { sh 'gcloud auth activate-service-account --key-file=/opt/config/gcloud-svc-account.json' sh 'cd /data && rm -Rf *' - sh 'cd /data && git clone https://github.com/googlecartographer/cartographer_ros' + sh 'cd /data && git clone https://github.com/cartographer-project/cartographer_ros' sh 'cd /data/cartographer_ros && docker build -f jenkins/Dockerfile.kinetic -t kinetic-jenkins-slave --build-arg CACHEBUST=$(date +%s) .' } stage('Push') { diff --git a/jenkins/worker.py b/jenkins/worker.py index e23213e34..de6b47d3b 100644 --- a/jenkins/worker.py +++ b/jenkins/worker.py @@ -104,7 +104,7 @@ def get_head_git_sha1(): """Returns the SHA-1 hash of the commit tagged HEAD.""" output = subprocess.check_output([ 'git', 'ls-remote', - 'https://github.com/googlecartographer/cartographer.git' + 'https://github.com/cartographer-project/cartographer.git' ]) parsed = GIT_SHA1_PATTERN.extract(output) return parsed['sha1'] diff --git a/scripts/prepare_jenkins_catkin_workspace.sh b/scripts/prepare_jenkins_catkin_workspace.sh index 1db3b0644..9c9654210 100755 --- a/scripts/prepare_jenkins_catkin_workspace.sh +++ b/scripts/prepare_jenkins_catkin_workspace.sh @@ -26,7 +26,7 @@ wstool init # Merge the cartographer_ros.rosinstall file and fetch code for dependencies. wstool merge ../../cartographer_ros/cartographer_ros.rosinstall -wstool merge -y https://raw.githubusercontent.com/googlecartographer/cartographer_fetch/master/cartographer_fetch.rosinstall +wstool merge -y https://raw.githubusercontent.com/cartographer-project/cartographer_fetch/master/cartographer_fetch.rosinstall wstool merge -y https://raw.githubusercontent.com/magazino/cartographer_magazino/master/cartographer_magazino.rosinstall wstool set cartographer -v ${CARTOGRAPHER_VERSION} -y wstool remove cartographer_ros From 2d9a4ab222f8b54c59be70cdf6cddde39601b276 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Thu, 25 Jun 2020 13:04:21 +0200 Subject: [PATCH 13/94] Build Abseil dependency in CI. (#1485) cartographer-project/cartographer#1711 removed building Abseil from the Cartographer library. We follow here the same approach as in cartographer CI. An alternative approach which is now possible is adding an Abseil package for catkin and depending on that. Signed-off-by: Wolfgang Hess Signed-off-by: Guillaume Doisy --- Dockerfile.kinetic | 3 ++- Dockerfile.melodic | 3 ++- cartographer_ros/CMakeLists.txt | 2 +- cartographer_rviz/CMakeLists.txt | 2 +- scripts/install_debs.sh | 11 ++++------- 5 files changed, 10 insertions(+), 11 deletions(-) diff --git a/Dockerfile.kinetic b/Dockerfile.kinetic index 581d541cf..72f0e41d2 100644 --- a/Dockerfile.kinetic +++ b/Dockerfile.kinetic @@ -41,7 +41,8 @@ COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_r COPY scripts/install_debs.sh cartographer_ros/scripts/ RUN cartographer_ros/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/* -# Install proto3. +# Install Abseil and proto3. +RUN /catkin_ws/src/cartographer/scripts/install_abseil.sh RUN /catkin_ws/src/cartographer/scripts/install_proto3.sh # Build, install, and test all packages individually to allow caching. The diff --git a/Dockerfile.melodic b/Dockerfile.melodic index c1c48ce33..38a9a89e0 100644 --- a/Dockerfile.melodic +++ b/Dockerfile.melodic @@ -41,7 +41,8 @@ COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_r COPY scripts/install_debs.sh cartographer_ros/scripts/ RUN cartographer_ros/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/* -# Install proto3. +# Install Abseil and proto3. +RUN /catkin_ws/src/cartographer/scripts/install_abseil.sh RUN /catkin_ws/src/cartographer/scripts/install_proto3.sh # Build, install, and test all packages individually to allow caching. The diff --git a/cartographer_ros/CMakeLists.txt b/cartographer_ros/CMakeLists.txt index acd4e2c5b..92eca9e03 100644 --- a/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/CMakeLists.txt @@ -54,7 +54,7 @@ find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES}) include(FindPkgConfig) -find_package(Abseil REQUIRED) +find_package(absl REQUIRED) find_package(LuaGoogle REQUIRED) find_package(Eigen3 REQUIRED) diff --git a/cartographer_rviz/CMakeLists.txt b/cartographer_rviz/CMakeLists.txt index bb981ae37..d9be9729e 100644 --- a/cartographer_rviz/CMakeLists.txt +++ b/cartographer_rviz/CMakeLists.txt @@ -33,7 +33,7 @@ find_package(cartographer REQUIRED) include("${CARTOGRAPHER_CMAKE_DIR}/functions.cmake") google_initialize_cartographer_project() -find_package(Abseil REQUIRED) +find_package(absl REQUIRED) find_package(Eigen3 REQUIRED) find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES}) diff --git a/scripts/install_debs.sh b/scripts/install_debs.sh index 9cc45d766..ca42f8d57 100755 --- a/scripts/install_debs.sh +++ b/scripts/install_debs.sh @@ -17,13 +17,10 @@ set -o errexit set -o verbose -# Install CMake 3.2 for Ubuntu Trusty and Debian Jessie. +# Install CMake 3.2 for Debian Jessie. sudo apt-get update sudo apt-get install lsb-release -y -if [[ "$(lsb_release -sc)" = "trusty" ]] -then - sudo apt-get install cmake3 -y -elif [[ "$(lsb_release -sc)" = "jessie" ]] +if [[ "$(lsb_release -sc)" = "jessie" ]] then sudo sh -c "echo 'deb http://ftp.debian.org/debian jessie-backports main' >> /etc/apt/sources.list" sudo apt-get update @@ -36,9 +33,9 @@ fi cd catkin_ws -# Install Ninja. +# Install Ninja and stow. apt-get update -apt-get install -y ninja-build +apt-get install -y ninja-build stow # Install rosdep dependencies. rosdep update From 82dce2e92d0cf7e5525e1c6400ebcea225d0dd90 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Thu, 23 Jul 2020 10:57:35 +0200 Subject: [PATCH 14/94] Remove support for Debian Jessie. (#1496) It has reached end-of-life with the end of LTS on June 30, 2020. Signed-off-by: Wolfgang Hess Signed-off-by: Guillaume Doisy --- scripts/install_debs.sh | 16 ++-------------- 1 file changed, 2 insertions(+), 14 deletions(-) diff --git a/scripts/install_debs.sh b/scripts/install_debs.sh index ca42f8d57..d34352661 100755 --- a/scripts/install_debs.sh +++ b/scripts/install_debs.sh @@ -17,26 +17,14 @@ set -o errexit set -o verbose -# Install CMake 3.2 for Debian Jessie. +# Install CMake, Ninja, stow. sudo apt-get update -sudo apt-get install lsb-release -y -if [[ "$(lsb_release -sc)" = "jessie" ]] -then - sudo sh -c "echo 'deb http://ftp.debian.org/debian jessie-backports main' >> /etc/apt/sources.list" - sudo apt-get update - sudo apt-get -t jessie-backports install cmake -y -else - sudo apt-get install cmake -y -fi +sudo apt-get install -y lsb-release cmake ninja-build stow . /opt/ros/${ROS_DISTRO}/setup.sh cd catkin_ws -# Install Ninja and stow. -apt-get update -apt-get install -y ninja-build stow - # Install rosdep dependencies. rosdep update rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y From d903fab1be2ec79fb2ca5d3dc1a4f8aec775137a Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Mon, 27 Jul 2020 19:45:40 +0200 Subject: [PATCH 15/94] Prepare GMock support for Noetic. (#1499) This follows Cartographer installing libgmock-dev. Signed-off-by: Wolfgang Hess Signed-off-by: Guillaume Doisy --- cartographer_ros/CMakeLists.txt | 4 +++- scripts/install_debs.sh | 6 ++++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/cartographer_ros/CMakeLists.txt b/cartographer_ros/CMakeLists.txt index 92eca9e03..a7e483c6c 100644 --- a/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/CMakeLists.txt @@ -49,6 +49,7 @@ include("${CARTOGRAPHER_CMAKE_DIR}/functions.cmake") option(BUILD_GRPC "build features that require Cartographer gRPC support" false) google_initialize_cartographer_project() google_enable_testing() +set(CARTOGRAPHER_GMOCK_LIBRARIES ${GMOCK_LIBRARIES}) find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES}) @@ -72,7 +73,7 @@ include_directories( # Override Catkin's GTest configuration to use GMock. set(GTEST_FOUND TRUE) set(GTEST_INCLUDE_DIRS ${GMOCK_INCLUDE_DIRS}) -set(GTEST_LIBRARIES ${GMOCK_LIBRARIES}) +set(GTEST_LIBRARIES ${CARTOGRAPHER_GMOCK_LIBRARIES}) catkin_package( CATKIN_DEPENDS @@ -152,6 +153,7 @@ if (CATKIN_ENABLE_TESTING) catkin_add_gtest(${TEST_NAME} ${TEST_SOURCE_FILENAME}) # catkin_add_gtest uses a plain (i.e. no PUBLIC/PRIVATE/INTERFACE) call to # target_link_libraries. That forces us to do the same. + target_link_libraries(${TEST_NAME} ${GMOCK_LIBRARIES} ${GTEST_MAIN_LIBRARIES}) target_include_directories(${TEST_NAME} SYSTEM PUBLIC ${LUA_INCLUDE_DIR}) target_link_libraries(${TEST_NAME} ${LUA_LIBRARIES}) target_include_directories(${TEST_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS}) diff --git a/scripts/install_debs.sh b/scripts/install_debs.sh index d34352661..0af46489a 100755 --- a/scripts/install_debs.sh +++ b/scripts/install_debs.sh @@ -21,6 +21,12 @@ set -o verbose sudo apt-get update sudo apt-get install -y lsb-release cmake ninja-build stow +# Install GMock library and header files for newer distributions. +if [[ "$(lsb_release -sc)" = "focal" || "$(lsb_release -sc)" = "buster" ]] +then + sudo apt-get install -y libgmock-dev +fi + . /opt/ros/${ROS_DISTRO}/setup.sh cd catkin_ws From 2654412808f7a3decbb0bec607ae0c66de5f3f33 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Tue, 28 Jul 2020 14:39:55 +0200 Subject: [PATCH 16/94] Changes for ROS Noetic support (#1494) Signed-off-by: Michael Grupp Signed-off-by: Guillaume Doisy --- .travis.yml | 1 + Dockerfile.noetic | 84 ++++++++++++++++++++++++++++++++++++ cartographer_ros/package.xml | 2 +- 3 files changed, 86 insertions(+), 1 deletion(-) create mode 100644 Dockerfile.noetic diff --git a/.travis.yml b/.travis.yml index 17ee65648..41286c677 100644 --- a/.travis.yml +++ b/.travis.yml @@ -24,6 +24,7 @@ cache: env: - ROS_RELEASE=kinetic DOCKER_CACHE_FILE=/home/travis/docker/kinetic-cache.tar.gz - ROS_RELEASE=melodic DOCKER_CACHE_FILE=/home/travis/docker/melodic-cache.tar.gz + - ROS_RELEASE=noetic DOCKER_CACHE_FILE=/home/travis/docker/noetic-cache.tar.gz before_install: # $GITHUB_TOKEN must be a valid GitHub access token without access rights (https://github.com/settings/tokens). diff --git a/Dockerfile.noetic b/Dockerfile.noetic new file mode 100644 index 000000000..97cf6e174 --- /dev/null +++ b/Dockerfile.noetic @@ -0,0 +1,84 @@ +# Copyright 2020 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. + +FROM ros:noetic + +ARG CARTOGRAPHER_VERSION=master + +# We require a GitHub access token to be passed. +ARG github_token + +# Prevent any interaction required by apt-get. +# https://stackoverflow.com/questions/22466255 +ARG DEBIAN_FRONTEND=noninteractive + +# ROS Noetic's base image doesn't ship with sudo and git. +RUN apt-get update && apt-get install -y sudo git && rm -rf /var/lib/apt/lists/* + +# First, we invalidate the entire cache if cartographer-project/cartographer has +# changed. This file's content changes whenever master changes. See: +# http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone +ADD https://api.github.com/repos/cartographer-project/cartographer/git/refs/heads/master?access_token=$github_token \ + cartographer_ros/cartographer_version.json + +# wstool needs the updated rosinstall file to clone the correct repos. +COPY cartographer_ros.rosinstall cartographer_ros/ +COPY scripts/prepare_catkin_workspace.sh cartographer_ros/scripts/ +RUN CARTOGRAPHER_VERSION=$CARTOGRAPHER_VERSION \ + cartographer_ros/scripts/prepare_catkin_workspace.sh + +# rosdep needs the updated package.xml files to install the correct debs. +COPY cartographer_ros/package.xml catkin_ws/src/cartographer_ros/cartographer_ros/ +COPY cartographer_ros_msgs/package.xml catkin_ws/src/cartographer_ros/cartographer_ros_msgs/ +COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_rviz/ +COPY scripts/install_debs.sh cartographer_ros/scripts/ +RUN cartographer_ros/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/* + +# Install Abseil and proto3. +RUN /catkin_ws/src/cartographer/scripts/install_abseil.sh +RUN /catkin_ws/src/cartographer/scripts/install_proto3.sh + +# Build, install, and test all packages individually to allow caching. The +# ordering of these steps must match the topological package ordering as +# determined by Catkin. +COPY scripts/install.sh cartographer_ros/scripts/ +COPY scripts/catkin_test_results.sh cartographer_ros/scripts/ + +COPY cartographer_ros_msgs catkin_ws/src/cartographer_ros/cartographer_ros_msgs/ +RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs && \ + cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs \ + --catkin-make-args run_tests && \ + cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros_msgs + +RUN cartographer_ros/scripts/install.sh --pkg ceres-solver + +RUN cartographer_ros/scripts/install.sh --pkg cartographer && \ + cartographer_ros/scripts/install.sh --pkg cartographer --make-args test + +COPY cartographer_ros catkin_ws/src/cartographer_ros/cartographer_ros/ +RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros && \ + cartographer_ros/scripts/install.sh --pkg cartographer_ros \ + --catkin-make-args run_tests && \ + cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros + +COPY cartographer_rviz catkin_ws/src/cartographer_ros/cartographer_rviz/ +RUN cartographer_ros/scripts/install.sh --pkg cartographer_rviz && \ + cartographer_ros/scripts/install.sh --pkg cartographer_rviz \ + --catkin-make-args run_tests && \ + cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_rviz + +COPY scripts/ros_entrypoint.sh / +# A BTRFS bug may prevent us from cleaning up these directories. +# https://btrfs.wiki.kernel.org/index.php/Problem_FAQ#I_cannot_delete_an_empty_directory +RUN rm -rf cartographer_ros catkin_ws || true diff --git a/cartographer_ros/package.xml b/cartographer_ros/package.xml index fe5cc9c6c..cd646489a 100644 --- a/cartographer_ros/package.xml +++ b/cartographer_ros/package.xml @@ -39,7 +39,7 @@ git google-mock protobuf-dev - python-sphinx + python3-sphinx cartographer cartographer_ros_msgs From 622abfb43021841e446beca216fbd7aeee617f99 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Fri, 28 Aug 2020 15:08:15 +0200 Subject: [PATCH 17/94] Remove legacy Jenkins quality pipeline. (#1510) - it has been used only at Google for a quality pipeline - the worker.py is Python 2 and closely tied to Google Cloud, so I don't believe there is much to be reused by someone else nowadays and not worth the effort to port to Python 3. Signed-off-by: Michael Grupp Signed-off-by: Guillaume Doisy --- jenkins/Dockerfile.kinetic | 122 ------------- jenkins/Jenkinsfile | 110 ------------ jenkins/jenkins-slave | 84 --------- jenkins/worker.py | 350 ------------------------------------- 4 files changed, 666 deletions(-) delete mode 100644 jenkins/Dockerfile.kinetic delete mode 100644 jenkins/Jenkinsfile delete mode 100755 jenkins/jenkins-slave delete mode 100644 jenkins/worker.py diff --git a/jenkins/Dockerfile.kinetic b/jenkins/Dockerfile.kinetic deleted file mode 100644 index ada39e5a1..000000000 --- a/jenkins/Dockerfile.kinetic +++ /dev/null @@ -1,122 +0,0 @@ -# Copyright 2016 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. - -FROM ros:kinetic - -ARG CARTOGRAPHER_VERSION=master - -# Xenial's base image doesn't ship with sudo. -RUN apt-get update && apt-get install -y sudo time && rm -rf /var/lib/apt/lists/* -# First, we invalidate the entire cache if cartographer-project/cartographer has -# changed. This file's content changes whenever master changes. See: -# http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone -ADD https://api.github.com/repos/cartographer-project/cartographer/git/refs/heads/master \ - cartographer_ros/cartographer_version.json - -# wstool needs the updated rosinstall file to clone the correct repos. -COPY cartographer_ros.rosinstall cartographer_ros/ -COPY scripts/prepare_jenkins_catkin_workspace.sh cartographer_ros/scripts/ - -# Invalidates the Docker cache to ensure this command is always executed. -ARG CACHEBUST=1 -RUN CARTOGRAPHER_VERSION=$CARTOGRAPHER_VERSION \ - cartographer_ros/scripts/prepare_jenkins_catkin_workspace.sh - -# rosdep needs the updated package.xml files to install the correct debs. -COPY cartographer_ros/package.xml catkin_ws/src/cartographer_ros/cartographer_ros/ -COPY cartographer_ros_msgs/package.xml catkin_ws/src/cartographer_ros/cartographer_ros_msgs/ -COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_rviz/ -COPY scripts/install_debs.sh cartographer_ros/scripts/ -RUN cartographer_ros/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/* - -# Install proto3. -RUN /catkin_ws/src/cartographer/scripts/install_proto3.sh - -# Build, install, and test all packages individually to allow caching. The -# ordering of these steps must match the topological package ordering as -# determined by Catkin. -COPY scripts/install.sh cartographer_ros/scripts/ -COPY scripts/catkin_test_results.sh cartographer_ros/scripts/ - -COPY cartographer_ros_msgs catkin_ws/src/cartographer_ros/cartographer_ros_msgs/ -RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs && \ - cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs \ - --catkin-make-args run_tests && \ - cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros_msgs - -RUN cartographer_ros/scripts/install.sh --pkg ceres-solver - -RUN cartographer_ros/scripts/install.sh --pkg cartographer && \ - cartographer_ros/scripts/install.sh --pkg cartographer --make-args test - -COPY cartographer_ros catkin_ws/src/cartographer_ros/cartographer_ros/ -RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros && \ - cartographer_ros/scripts/install.sh --pkg cartographer_ros \ - --catkin-make-args run_tests && \ - cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros - -COPY cartographer_rviz catkin_ws/src/cartographer_ros/cartographer_rviz/ -RUN cartographer_ros/scripts/install.sh --pkg cartographer_rviz && \ - cartographer_ros/scripts/install.sh --pkg cartographer_rviz \ - --catkin-make-args run_tests && \ - cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_rviz - -RUN cartographer_ros/scripts/install.sh --pkg cartographer_toru -RUN cartographer_ros/scripts/install.sh --pkg cartographer_fetch - -COPY scripts/ros_entrypoint.sh / -# A BTRFS bug may prevent us from cleaning up these directories. -# https://btrfs.wiki.kernel.org/index.php/Problem_FAQ#I_cannot_delete_an_empty_directory -RUN rm -rf cartographer_ros catkin_ws || true - -RUN sudo apt-get update -RUN sudo apt-get -y install openjdk-8-jdk python-pip - -ENV HOME /home/jenkins -RUN addgroup --system --gid 10000 jenkins -RUN adduser --system --ingroup jenkins --home $HOME --uid 10000 jenkins - -LABEL Description="This is a base image, which provides the Jenkins agent executable (slave.jar)" Vendor="Jenkins project" Version="3.17" - -ARG VERSION=3.17 -ARG AGENT_WORKDIR=/home/jenkins/agent - -RUN curl --create-dirs -sSLo /usr/share/jenkins/slave.jar https://repo.jenkins-ci.org/public/org/jenkins-ci/main/remoting/${VERSION}/remoting-${VERSION}.jar \ - && chmod 755 /usr/share/jenkins \ - && chmod 644 /usr/share/jenkins/slave.jar -# USER jenkins -ENV AGENT_WORKDIR=${AGENT_WORKDIR} -RUN mkdir /home/jenkins/.jenkins && mkdir -p ${AGENT_WORKDIR} - -VOLUME /home/jenkins/.jenkins -VOLUME ${AGENT_WORKDIR} -WORKDIR /home/jenkins - -COPY jenkins/jenkins-slave /usr/local/bin/jenkins-slave - -ENV CLOUDSDK_CORE_DISABLE_PROMPTS 1 -ENV PATH /opt/google-cloud-sdk/bin:$PATH - -USER root - -# Install Google Cloud Components -RUN curl https://sdk.cloud.google.com | bash && mv google-cloud-sdk /opt -RUN gcloud components install kubectl - -RUN pip install --upgrade google-cloud-datastore -RUN pip install --upgrade google-cloud-bigquery -COPY jenkins/worker.py /worker.py - -# USER root -ENTRYPOINT ["jenkins-slave"] diff --git a/jenkins/Jenkinsfile b/jenkins/Jenkinsfile deleted file mode 100644 index 3a13d5446..000000000 --- a/jenkins/Jenkinsfile +++ /dev/null @@ -1,110 +0,0 @@ -podTemplate(label: 'node-0', containers: [ - containerTemplate( - name: 'jnlp', - image: 'eggsy84/gcp-jenkins-slave-k8s-seed:latest', - ttyEnabled: false, - command: '', - privileged: true, - alwaysPullImage: false, - workingDir: '/home/jenkins', - args: '${computer.jnlpmac} ${computer.name}' - ) - ], - volumes: [ - secretVolume(mountPath: '/opt/config', secretName: 'gcloud-svc-account'), - hostPathVolume(mountPath: '/var/run/docker.sock', hostPath: '/var/run/docker.sock'), - persistentVolumeClaim(claimName: 'data-claim-compile', mountPath: '/data'), - ] -) { - node('node-0') { - stage('Compile') { - sh 'gcloud auth activate-service-account --key-file=/opt/config/gcloud-svc-account.json' - sh 'cd /data && rm -Rf *' - sh 'cd /data && git clone https://github.com/cartographer-project/cartographer_ros' - sh 'cd /data/cartographer_ros && docker build -f jenkins/Dockerfile.kinetic -t kinetic-jenkins-slave --build-arg CACHEBUST=$(date +%s) .' - } - stage('Push') { - sh 'docker tag kinetic-jenkins-slave eu.gcr.io/cartographer-141408/kinetic-jenkins-slave' - sh 'gcloud docker -- push eu.gcr.io/cartographer-141408/kinetic-jenkins-slave' - sh 'cd /data && rm -Rf *' - } - } -} - -podTemplate(label: 'node-1', containers: [ - containerTemplate( - name: 'jnlp', - image: 'eu.gcr.io/cartographer-141408/kinetic-jenkins-slave:latest', - ttyEnabled: false, - command: '', - privileged: true, - alwaysPullImage: true, - workingDir: '/home/jenkins', - args: '${computer.jnlpmac} ${computer.name}' - ) - ], - volumes: [ - secretVolume(mountPath: '/opt/config', secretName: 'gcloud-svc-account'), - hostPathVolume(mountPath: '/var/run/docker.sock', hostPath: '/var/run/docker.sock'), - persistentVolumeClaim(claimName: 'data-claim-compile', mountPath: '/data'), - ] -) { - node('node-1') { - stage('Run Fetch Pipeline') { - sh 'gcloud auth activate-service-account --key-file=/opt/config/gcloud-svc-account.json' - sh 'GOOGLE_APPLICATION_CREDENTIALS="/opt/config/gcloud-svc-account.json" GOOGLE_CLOUD_DISABLE_GRPC=True python /worker.py --worker_id 0 --num_workers 1 --pipeline_id fetch' - } - } -} - -podTemplate(label: 'node-2', containers: [ - containerTemplate( - name: 'jnlp', - image: 'eu.gcr.io/cartographer-141408/kinetic-jenkins-slave:latest', - ttyEnabled: false, - command: '', - privileged: true, - alwaysPullImage: true, - workingDir: '/home/jenkins', - args: '${computer.jnlpmac} ${computer.name}' - ) - ], - volumes: [ - secretVolume(mountPath: '/opt/config', secretName: 'gcloud-svc-account'), - hostPathVolume(mountPath: '/var/run/docker.sock', hostPath: '/var/run/docker.sock'), - persistentVolumeClaim(claimName: 'data-claim-compile', mountPath: '/data'), - ] -) { - node('node-2') { - stage('Run Backpack Pipeline') { - sh 'gcloud auth activate-service-account --key-file=/opt/config/gcloud-svc-account.json' - sh 'GOOGLE_APPLICATION_CREDENTIALS="/opt/config/gcloud-svc-account.json" GOOGLE_CLOUD_DISABLE_GRPC=True python /worker.py --worker_id 0 --num_workers 1 --pipeline_id backpack' - } - } -} - -podTemplate(label: 'node-3', containers: [ - containerTemplate( - name: 'jnlp', - image: 'eu.gcr.io/cartographer-141408/kinetic-jenkins-slave:latest', - ttyEnabled: false, - command: '', - privileged: true, - alwaysPullImage: true, - workingDir: '/home/jenkins', - args: '${computer.jnlpmac} ${computer.name}' - ) - ], - volumes: [ - secretVolume(mountPath: '/opt/config', secretName: 'gcloud-svc-account'), - hostPathVolume(mountPath: '/var/run/docker.sock', hostPath: '/var/run/docker.sock'), - persistentVolumeClaim(claimName: 'data-claim-compile', mountPath: '/data'), - ] -) { - node('node-3') { - stage('Run Toru Pipeline') { - sh 'gcloud auth activate-service-account --key-file=/opt/config/gcloud-svc-account.json' - sh 'GOOGLE_APPLICATION_CREDENTIALS="/opt/config/gcloud-svc-account.json" GOOGLE_CLOUD_DISABLE_GRPC=True python /worker.py --worker_id 0 --num_workers 1 --pipeline_id toru' - } - } -} diff --git a/jenkins/jenkins-slave b/jenkins/jenkins-slave deleted file mode 100755 index bf73566ae..000000000 --- a/jenkins/jenkins-slave +++ /dev/null @@ -1,84 +0,0 @@ -#!/usr/bin/env sh - -# The MIT License -# -# Copyright (c) 2015, CloudBees, Inc. -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in -# all copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -# THE SOFTWARE. - -# Usage jenkins-slave.sh [options] -url http://jenkins [SECRET] [AGENT_NAME] -# Optional environment variables : -# * JENKINS_TUNNEL : HOST:PORT for a tunnel to route TCP traffic to jenkins host, when jenkins can't be directly accessed over network -# * JENKINS_URL : alternate jenkins URL -# * JENKINS_SECRET : agent secret, if not set as an argument -# * JENKINS_AGENT_NAME : agent name, if not set as an argument - -if [ $# -eq 1 ]; then - - # if `docker run` only has one arguments, we assume user is running alternate command like `bash` to inspect the image - exec "$@" - -else - - # if -tunnel is not provided try env vars - case "$@" in - *"-tunnel "*) ;; - *) - if [ ! -z "$JENKINS_TUNNEL" ]; then - TUNNEL="-tunnel $JENKINS_TUNNEL" - fi ;; - esac - - if [ -n "$JENKINS_URL" ]; then - URL="-url $JENKINS_URL" - fi - - if [ -n "$JENKINS_NAME" ]; then - JENKINS_AGENT_NAME="$JENKINS_NAME" - fi - - if [ -z "$JNLP_PROTOCOL_OPTS" ]; then - echo "Warning: JnlpProtocol3 is disabled by default, use JNLP_PROTOCOL_OPTS to alter the behavior" - JNLP_PROTOCOL_OPTS="-Dorg.jenkinsci.remoting.engine.JnlpProtocol3.disabled=true" - fi - - # If both required options are defined, do not pass the parameters - OPT_JENKINS_SECRET="" - if [ -n "$JENKINS_SECRET" ]; then - case "$@" in - *"${JENKINS_SECRET}"*) echo "Warning: SECRET is defined twice in command-line arguments and the environment variable" ;; - *) - OPT_JENKINS_SECRET="${JENKINS_SECRET}" ;; - esac - fi - - OPT_JENKINS_AGENT_NAME="" - if [ -n "$JENKINS_AGENT_NAME" ]; then - case "$@" in - *"${JENKINS_AGENT_NAME}"*) echo "Warning: AGENT_NAME is defined twice in command-line arguments and the environment variable" ;; - *) - OPT_JENKINS_AGENT_NAME="${JENKINS_AGENT_NAME}" ;; - esac - fi - - #TODO: Handle the case when the command-line and Environment variable contain different values. - #It is fine it blows up for now since it should lead to an error anyway. - - exec java $JAVA_OPTS $JNLP_PROTOCOL_OPTS -cp /usr/share/jenkins/slave.jar hudson.remoting.jnlp.Main -headless $TUNNEL $URL $OPT_JENKINS_SECRET $OPT_JENKINS_AGENT_NAME "$@" -fi diff --git a/jenkins/worker.py b/jenkins/worker.py deleted file mode 100644 index de6b47d3b..000000000 --- a/jenkins/worker.py +++ /dev/null @@ -1,350 +0,0 @@ -"""This is the script executed by workers of the quality control pipline.""" - -import argparse -import datetime -from os.path import basename -from pprint import pprint -import re -import subprocess - -from google.cloud import bigquery -from google.cloud import datastore - - -class Pattern(object): - """Defines a pattern for regular expression matching.""" - - def __init__(self, pattern): - self.regex = re.compile(pattern, re.MULTILINE) - - def extract(self, text): - """Returns a dictionary of named capture groups to extracted output. - - Args: - text: input to parse - - Returns an empty dict if no match was found. - """ - match = self.regex.search(text) - if match is None: - return {} - return match.groupdict() - - def extract_last_occurence(self, text): - """Returns tuple of extracted outputs. - - Args: - text: input to parse - - Returns the information extracted from the last match. Returns - None if no match was found. - """ - matches = self.regex.findall(text) - if not matches: - return None - return matches[-1] - - -# BigQuery table schema -SCHEMA = [ - bigquery.SchemaField('date', 'DATE'), - bigquery.SchemaField('commit_sha1', 'STRING'), - bigquery.SchemaField('job_id', 'INTEGER'), - bigquery.SchemaField('rosbag', 'STRING'), - bigquery.SchemaField('user_time_secs', 'FLOAT'), - bigquery.SchemaField('system_time_secs', 'FLOAT'), - bigquery.SchemaField('wall_time_secs', 'FLOAT'), - bigquery.SchemaField('max_set_size_kbytes', 'INTEGER'), - bigquery.SchemaField('constraints_count', 'INTEGER'), - bigquery.SchemaField('constraints_score_minimum', 'FLOAT'), - bigquery.SchemaField('constraints_score_maximum', 'FLOAT'), - bigquery.SchemaField('constraints_score_mean', 'FLOAT'), - bigquery.SchemaField('ground_truth_abs_trans_err', 'FLOAT'), - bigquery.SchemaField('ground_truth_abs_trans_err_dev', 'FLOAT'), - bigquery.SchemaField('ground_truth_sqr_trans_err', 'FLOAT'), - bigquery.SchemaField('ground_truth_sqr_trans_err_dev', 'FLOAT'), - bigquery.SchemaField('ground_truth_abs_rot_err', 'FLOAT'), - bigquery.SchemaField('ground_truth_abs_rot_err_dev', 'FLOAT'), - bigquery.SchemaField('ground_truth_sqr_rot_err', 'FLOAT'), - bigquery.SchemaField('ground_truth_sqr_rot_err_dev', 'FLOAT') -] - -# Pattern matchers for the various fields of the '/usr/bin/time -v' output -USER_TIME_PATTERN = Pattern( - r'^\s*User time \(seconds\): (?P\d+.\d+|\d+)') -SYSTEM_TIME_PATTERN = Pattern( - r'^\s*System time \(seconds\): (?P\d+.\d+|\d+)') -WALL_TIME_PATTERN = Pattern( - r'^\s*Elapsed \(wall clock\) time \(h:mm:ss or m:ss\): ' - r'((?P\d{1,2}):|)(?P\d{1,2}):(?P\d{2}\.\d{2})') -MAX_RES_SET_SIZE_PATTERN = Pattern( - r'^\s*Maximum resident set size \(kbytes\): (?P\d+)') -CONSTRAINT_STATS_PATTERN = Pattern( - r'Score histogram:[\n\r]+' - r'Count:\s+(?P\d+)\s+' - r'Min:\s+(?P\d+\.\d+)\s+' - r'Max:\s+(?P\d+\.\d+)\s+' - r'Mean:\s+(?P\d+\.\d+)') -GROUND_TRUTH_STATS_PATTERN = Pattern( - r'Result:[\n\r]+' - r'Abs translational error (?P\d+\.\d+) ' - r'\+/- (?P\d+\.\d+) m[\n\r]+' - r'Sqr translational error (?P\d+\.\d+) ' - r'\+/- (?P\d+\.\d+) m\^2[\n\r]+' - r'Abs rotational error (?P\d+\.\d+) ' - r'\+/- (?P\d+\.\d+) deg[\n\r]+' - r'Sqr rotational error (?P\d+\.\d+) ' - r'\+/- (?P\d+\.\d+) deg\^2') - -# Pattern matcher for extracting the HEAD commit SHA-1 hash. -GIT_SHA1_PATTERN = Pattern(r'^(?P[0-9a-f]{40})\s+HEAD') - - -def get_head_git_sha1(): - """Returns the SHA-1 hash of the commit tagged HEAD.""" - output = subprocess.check_output([ - 'git', 'ls-remote', - 'https://github.com/cartographer-project/cartographer.git' - ]) - parsed = GIT_SHA1_PATTERN.extract(output) - return parsed['sha1'] - - -def extract_stats(inp): - """Returns a dictionary of stats.""" - result = {} - - parsed = USER_TIME_PATTERN.extract(inp) - result['user_time_secs'] = float(parsed['user_time']) - - parsed = SYSTEM_TIME_PATTERN.extract(inp) - result['system_time_secs'] = float(parsed['system_time']) - - parsed = WALL_TIME_PATTERN.extract(inp) - result['wall_time_secs'] = float(parsed['hours'] or 0.) * 3600 + float( - parsed['minutes']) * 60 + float(parsed['seconds']) - - parsed = MAX_RES_SET_SIZE_PATTERN.extract(inp) - result['max_set_size_kbytes'] = int(parsed['max_set_size']) - - parsed = CONSTRAINT_STATS_PATTERN.extract_last_occurence(inp) - print parsed - result['constraints_count'] = int(parsed[0]) - result['constraints_score_min'] = float(parsed[1]) - result['constraints_score_max'] = float(parsed[2]) - result['constraints_score_mean'] = float(parsed[3]) - - return result - - -def extract_ground_truth_stats(input_text): - """Returns a dictionary of ground truth stats.""" - result = {} - parsed = GROUND_TRUTH_STATS_PATTERN.extract(input_text) - for name in ('abs_trans_err', 'sqr_trans_err', 'abs_rot_err', 'sqr_rot_err'): - result['ground_truth_{}'.format(name)] = float(parsed[name]) - result['ground_truth_{}_dev'.format(name)] = float( - parsed['{}_dev'.format(name)]) - return result - - -def retrieve_entity(datastore_client, kind, identifier): - """Convenience function for Datastore entity retrieval.""" - key = datastore_client.key(kind, identifier) - return datastore_client.get(key) - - -def create_job_selector(worker_id, num_workers): - """Constructs a round-robin job selector.""" - return lambda job_id: job_id % num_workers == worker_id - - -def run_cmd(cmd): - """Runs command both printing its stdout output and returning it as string.""" - print cmd - p = subprocess.Popen( - cmd, shell=True, stdout=subprocess.PIPE, stderr=subprocess.STDOUT) - run_cmd.output = [] - - def process(line): - run_cmd.output.append(line) - print line.rstrip() - - while p.poll() is None: - process(p.stdout.readline()) - process(p.stdout.read()) - return '\n'.join(run_cmd.output) - - -def run_ros_cmd(ros_distro, ros_cmd): - """Runs command similar to run_cmd but sets ROS environment variables.""" - cmd = ('/bin/bash -c \"source /opt/ros/{}/setup.bash && source ' - '/opt/cartographer_ros/setup.bash && {}\"').format( - ros_distro, ros_cmd) - return run_cmd(cmd) - - -class Job(object): - """Represents a single job to be executed. - - A job consists of a combination of rosbag and configuration and launch files. - """ - - def __init__(self, datastore_client, job_id): - self.id = job_id - entity = retrieve_entity(datastore_client, 'Job', job_id) - self.launch_file = entity['launch_file'] - self.assets_writer_launch_file = entity['assets_writer_launch_file'] - self.assets_writer_config_file = entity['assets_writer_config_file'] - self.rosbag = entity['rosbag'] - self.ros_package = entity['ros_package'] - - def __repr__(self): - return 'Job: id : {} launch_file: {} rosbag: {}'.format( - self.id, self.launch_file, self.rosbag) - - def run(self, ros_distro, run_id): - """Runs the job with ROS distro 'ros_distro'.""" - print 'running job {}'.format(self.id) - - # Garbage collect any left-overs from previous runs. - run_cmd('rm -rf /data/*') - - # Copy the rosbag to scratch space - scratch_dir = '/data/{}'.format(self.id) - rosbag_filename = basename(self.rosbag) - run_cmd('mkdir {}'.format(scratch_dir)) - run_cmd('gsutil cp gs://{} {}/{}'.format(self.rosbag, scratch_dir, - rosbag_filename)) - - # Creates pbstream - output = run_ros_cmd(ros_distro, - '/usr/bin/time -v roslaunch {} {} ' - 'bag_filenames:={}/{} no_rviz:=true'.format( - self.ros_package, self.launch_file, scratch_dir, - rosbag_filename)) - info = extract_stats(output) - - # Creates assets. - run_ros_cmd( - ros_distro, '/usr/bin/time -v roslaunch {} {} ' - 'bag_filenames:={}/{} pose_graph_filename:=' - '{}/{}.pbstream config_file:={}'.format( - self.ros_package, self.assets_writer_launch_file, scratch_dir, - rosbag_filename, scratch_dir, rosbag_filename, - self.assets_writer_config_file)) - - # Copies assets to bucket. - run_cmd('gsutil cp {}/{}.pbstream ' - 'gs://cartographer-ci-artifacts/{}/{}/{}.pbstream'.format( - scratch_dir, rosbag_filename, run_id, self.id, rosbag_filename)) - run_cmd('gsutil cp {}/{}_* gs://cartographer-ci-artifacts/{}/{}/'.format( - scratch_dir, rosbag_filename, run_id, self.id)) - - # Download ground truth relations file. - run_cmd('gsutil cp gs://cartographer-ci-ground-truth/{}/relations.pb ' - '{}/relations.pb'.format(self.id, scratch_dir)) - - # Calculate metrics. - output = run_ros_cmd(ros_distro, 'cartographer_compute_relations_metrics ' - '-relations_filename {}/relations.pb ' - '-pose_graph_filename {}/{}.pbstream'.format( - scratch_dir, scratch_dir, rosbag_filename)) - - # Add ground truth stats. - info.update(extract_ground_truth_stats(output)) - - info['rosbag'] = rosbag_filename - return info - - -class Worker(object): - """Represents a single worker that executes a sequence of Jobs.""" - - def __init__(self, datastore_client, pipeline_id, run_id): - entity = retrieve_entity(datastore_client, 'PipelineConfig', pipeline_id) - self.pipeline_id = pipeline_id - self.jobs = [Job(datastore_client, job_id) for job_id in entity['jobs']] - self.scratch_dir = entity['scratch_dir'] - self.ros_distro = entity['ros_distro'] - self.run_id = run_id - - def __repr__(self): - result = 'Worker: pipeline_id: {}\n'.format(self.pipeline_id) - for job in self.jobs: - result += '{}\n'.format(str(job)) - return result - - def run_jobs(self, selector): - outputs = {} - for idx, job in enumerate(self.jobs): - if selector(idx): - output = job.run(self.ros_distro, self.run_id) - outputs[job.id] = output - else: - print 'job {}: skip'.format(job.id) - return outputs - - -def publish_stats_to_big_query(stats_dict, now, head_sha1): - """Publishes metrics to BigQuery.""" - bigquery_client = bigquery.Client() - dataset = bigquery_client.dataset('Cartographer') - table = dataset.table('metrics') - rows_to_insert = [] - for job_identifier, job_info in stats_dict.iteritems(): - print job_info - data = ('{}-{}-{}'.format( - now.year, now.month, - now.day), head_sha1, job_identifier, job_info['rosbag'], - job_info['user_time_secs'], job_info['system_time_secs'], - job_info['wall_time_secs'], job_info['max_set_size_kbytes'], - job_info['constraints_count'], job_info['constraints_score_min'], - job_info['constraints_score_max'], - job_info['constraints_score_mean'], - job_info['ground_truth_abs_trans_err'], - job_info['ground_truth_abs_trans_err_dev'], - job_info['ground_truth_sqr_trans_err'], - job_info['ground_truth_sqr_trans_err_dev'], - job_info['ground_truth_abs_rot_err'], - job_info['ground_truth_abs_rot_err_dev'], - job_info['ground_truth_sqr_rot_err'], - job_info['ground_truth_sqr_rot_err_dev']) - - rows_to_insert.append(data) - - errors = bigquery_client.create_rows( - table, rows_to_insert, selected_fields=SCHEMA) - if not errors: - print 'Pushed {} row(s) into Cartographer:metrics'.format( - len(rows_to_insert)) - else: - print 'Errors:' - pprint(errors) - - -def parse_arguments(): - """Parses the command line arguments.""" - parser = argparse.ArgumentParser( - description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) - parser.add_argument('--worker_id', type=int) - parser.add_argument('--num_workers', type=int) - parser.add_argument('--pipeline_id', type=str) - return parser.parse_args() - - -def main(): - args = parse_arguments() - ds_client = datastore.Client() - job_selector = create_job_selector(int(args.worker_id), int(args.num_workers)) - head_sha1 = get_head_git_sha1() - now = datetime.datetime.now() - pipeline_run_id = '{}-{}-{}_{}'.format(now.year, now.month, now.day, - head_sha1) - worker = Worker(ds_client, args.pipeline_id, pipeline_run_id) - stats_dict = worker.run_jobs(job_selector) - publish_stats_to_big_query(stats_dict, now, head_sha1) - - -if __name__ == '__main__': - main() From 30d643012cd400cbf9e247f038d7a8717e3132a6 Mon Sep 17 00:00:00 2001 From: Geonhee Date: Sat, 29 Aug 2020 18:49:05 +0900 Subject: [PATCH 18/94] Update assets_writer.rst (#1442) Signed-off-by: Geonhee-LEE Signed-off-by: Guillaume Doisy --- docs/source/assets_writer.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/source/assets_writer.rst b/docs/source/assets_writer.rst index d3d6b4eb7..1cf223c7d 100644 --- a/docs/source/assets_writer.rst +++ b/docs/source/assets_writer.rst @@ -36,7 +36,7 @@ The assets writer takes as input 4. and a pipeline configuration, which is defined in a ``.lua`` file. The assets writer runs through the ``.bag`` data in batches with the trajectory found in the ``.pbstream``. -The pipeline can be can be used to color, filter and export SLAM point cloud data into a variety of formats. +The pipeline can be used to color, filter and export SLAM point cloud data into a variety of formats. There are multiple of such points processing steps that can be interleaved in a pipeline - several ones are already available from `cartographer/io`_. Sample Usage From 108dbf99d2ea6911e573dd3a556c5af65364ef35 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Mon, 31 Aug 2020 15:59:36 +0200 Subject: [PATCH 19/94] Remove unnecessary bottlenecks from Dockerfiles to speed up CI. (#1511) This should solve #1506. - Use osrf/ros--desktop as base image for CI The basic ROS base image requires rosdep to install packages with many dependencies, e.g. rviz. Using the desktop base image should speed up CI because it has a good overlap with what we need and only little extra stuff. - Remove apt list only after installing everything. - Don't build protobuf 3 from source on Ubuntu 18 & 20. It should come with rosdep. Only on Ubuntu 16 we need to build manually because it ships with Protobuf 2. - No need to build Ceres from source on Ubuntu 18 & 20. The versions are greater or equal to 1.13, which is the one specified in the source build script. The apt packages should work just fine. The cartographer library is built from source in all cases, which is good to ensure that we always test compatibility with the latest version of it. Signed-off-by: Guillaume Doisy --- Dockerfile.kinetic | 8 +++++--- Dockerfile.melodic | 13 +++++++------ Dockerfile.noetic | 13 +++++++------ 3 files changed, 19 insertions(+), 15 deletions(-) diff --git a/Dockerfile.kinetic b/Dockerfile.kinetic index 72f0e41d2..77fe91904 100644 --- a/Dockerfile.kinetic +++ b/Dockerfile.kinetic @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -FROM ros:kinetic +FROM osrf/ros:kinetic-desktop ARG CARTOGRAPHER_VERSION=master @@ -20,7 +20,7 @@ ARG CARTOGRAPHER_VERSION=master ARG github_token # Xenial's base image doesn't ship with sudo. -RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/* +RUN apt-get update && apt-get install -y sudo # First, we invalidate the entire cache if cartographer-project/cartographer has # changed. This file's content changes whenever master changes. See: @@ -39,7 +39,7 @@ COPY cartographer_ros/package.xml catkin_ws/src/cartographer_ros/cartographer_ro COPY cartographer_ros_msgs/package.xml catkin_ws/src/cartographer_ros/cartographer_ros_msgs/ COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_rviz/ COPY scripts/install_debs.sh cartographer_ros/scripts/ -RUN cartographer_ros/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/* +RUN cartographer_ros/scripts/install_debs.sh # Install Abseil and proto3. RUN /catkin_ws/src/cartographer/scripts/install_abseil.sh @@ -75,6 +75,8 @@ RUN cartographer_ros/scripts/install.sh --pkg cartographer_rviz && \ cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_rviz COPY scripts/ros_entrypoint.sh / + +RUN rm -rf /var/lib/apt/lists/* # A BTRFS bug may prevent us from cleaning up these directories. # https://btrfs.wiki.kernel.org/index.php/Problem_FAQ#I_cannot_delete_an_empty_directory RUN rm -rf cartographer_ros catkin_ws || true diff --git a/Dockerfile.melodic b/Dockerfile.melodic index 38a9a89e0..55baf91ac 100644 --- a/Dockerfile.melodic +++ b/Dockerfile.melodic @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -FROM ros:melodic +FROM osrf/ros:melodic-desktop ARG CARTOGRAPHER_VERSION=master @@ -20,7 +20,7 @@ ARG CARTOGRAPHER_VERSION=master ARG github_token # Bionic's base image doesn't ship with sudo. -RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/* +RUN apt-get update && apt-get install -y sudo # First, we invalidate the entire cache if cartographer-project/cartographer has # changed. This file's content changes whenever master changes. See: @@ -39,11 +39,10 @@ COPY cartographer_ros/package.xml catkin_ws/src/cartographer_ros/cartographer_ro COPY cartographer_ros_msgs/package.xml catkin_ws/src/cartographer_ros/cartographer_ros_msgs/ COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_rviz/ COPY scripts/install_debs.sh cartographer_ros/scripts/ -RUN cartographer_ros/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/* +RUN cartographer_ros/scripts/install_debs.sh -# Install Abseil and proto3. +# Install Abseil. RUN /catkin_ws/src/cartographer/scripts/install_abseil.sh -RUN /catkin_ws/src/cartographer/scripts/install_proto3.sh # Build, install, and test all packages individually to allow caching. The # ordering of these steps must match the topological package ordering as @@ -57,7 +56,7 @@ RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs && \ --catkin-make-args run_tests && \ cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros_msgs -RUN cartographer_ros/scripts/install.sh --pkg ceres-solver +RUN apt-get install -y libceres-dev RUN cartographer_ros/scripts/install.sh --pkg cartographer && \ cartographer_ros/scripts/install.sh --pkg cartographer --make-args test @@ -75,6 +74,8 @@ RUN cartographer_ros/scripts/install.sh --pkg cartographer_rviz && \ cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_rviz COPY scripts/ros_entrypoint.sh / + +RUN rm -rf /var/lib/apt/lists/* # A BTRFS bug may prevent us from cleaning up these directories. # https://btrfs.wiki.kernel.org/index.php/Problem_FAQ#I_cannot_delete_an_empty_directory RUN rm -rf cartographer_ros catkin_ws || true diff --git a/Dockerfile.noetic b/Dockerfile.noetic index 97cf6e174..87cd65313 100644 --- a/Dockerfile.noetic +++ b/Dockerfile.noetic @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -FROM ros:noetic +FROM osrf/ros:noetic-desktop ARG CARTOGRAPHER_VERSION=master @@ -24,7 +24,7 @@ ARG github_token ARG DEBIAN_FRONTEND=noninteractive # ROS Noetic's base image doesn't ship with sudo and git. -RUN apt-get update && apt-get install -y sudo git && rm -rf /var/lib/apt/lists/* +RUN apt-get update && apt-get install -y sudo git # First, we invalidate the entire cache if cartographer-project/cartographer has # changed. This file's content changes whenever master changes. See: @@ -43,11 +43,10 @@ COPY cartographer_ros/package.xml catkin_ws/src/cartographer_ros/cartographer_ro COPY cartographer_ros_msgs/package.xml catkin_ws/src/cartographer_ros/cartographer_ros_msgs/ COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_rviz/ COPY scripts/install_debs.sh cartographer_ros/scripts/ -RUN cartographer_ros/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/* +RUN cartographer_ros/scripts/install_debs.sh -# Install Abseil and proto3. +# Install Abseil. RUN /catkin_ws/src/cartographer/scripts/install_abseil.sh -RUN /catkin_ws/src/cartographer/scripts/install_proto3.sh # Build, install, and test all packages individually to allow caching. The # ordering of these steps must match the topological package ordering as @@ -61,7 +60,7 @@ RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs && \ --catkin-make-args run_tests && \ cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros_msgs -RUN cartographer_ros/scripts/install.sh --pkg ceres-solver +RUN apt-get install -y libceres-dev RUN cartographer_ros/scripts/install.sh --pkg cartographer && \ cartographer_ros/scripts/install.sh --pkg cartographer --make-args test @@ -79,6 +78,8 @@ RUN cartographer_ros/scripts/install.sh --pkg cartographer_rviz && \ cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_rviz COPY scripts/ros_entrypoint.sh / + +RUN rm -rf /var/lib/apt/lists/* # A BTRFS bug may prevent us from cleaning up these directories. # https://btrfs.wiki.kernel.org/index.php/Problem_FAQ#I_cannot_delete_an_empty_directory RUN rm -rf cartographer_ros catkin_ws || true From 9023f6e23af7097c4e94d68717ddcb09a5ed9210 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Mon, 14 Sep 2020 13:26:19 +0200 Subject: [PATCH 20/94] Follow cartographer-project/cartographer#1749. (#1516) Signed-off-by: Wolfgang Hess Signed-off-by: Guillaume Doisy --- .../cartographer_ros/ros_map_writing_points_processor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h b/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h index 71c165b42..f2f4f5bcd 100644 --- a/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h +++ b/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h @@ -22,7 +22,7 @@ #include "cartographer/io/points_processor.h" #include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" -#include "cartographer/mapping/proto/2d/probability_grid_range_data_inserter_options_2d.pb.h" +#include "cartographer/mapping/proto/probability_grid_range_data_inserter_options_2d.pb.h" #include "cartographer/mapping/value_conversion_tables.h" namespace cartographer_ros { From 4c8d47a1529b74f77ea5071ef2d50e5ab25cf246 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Thu, 24 Sep 2020 12:26:31 +0200 Subject: [PATCH 21/94] Follow cartographer-project/cartographer#1750. (#1518) Signed-off-by: Wolfgang Hess Signed-off-by: Guillaume Doisy --- Dockerfile.kinetic | 8 +++----- Dockerfile.melodic | 8 +++----- Dockerfile.noetic | 8 +++----- 3 files changed, 9 insertions(+), 15 deletions(-) diff --git a/Dockerfile.kinetic b/Dockerfile.kinetic index 77fe91904..7e2bb8d5f 100644 --- a/Dockerfile.kinetic +++ b/Dockerfile.kinetic @@ -51,17 +51,15 @@ RUN /catkin_ws/src/cartographer/scripts/install_proto3.sh COPY scripts/install.sh cartographer_ros/scripts/ COPY scripts/catkin_test_results.sh cartographer_ros/scripts/ +RUN cartographer_ros/scripts/install.sh --pkg cartographer && \ + cartographer_ros/scripts/install.sh --pkg cartographer --make-args test + COPY cartographer_ros_msgs catkin_ws/src/cartographer_ros/cartographer_ros_msgs/ RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs && \ cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs \ --catkin-make-args run_tests && \ cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros_msgs -RUN cartographer_ros/scripts/install.sh --pkg ceres-solver - -RUN cartographer_ros/scripts/install.sh --pkg cartographer && \ - cartographer_ros/scripts/install.sh --pkg cartographer --make-args test - COPY cartographer_ros catkin_ws/src/cartographer_ros/cartographer_ros/ RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros && \ cartographer_ros/scripts/install.sh --pkg cartographer_ros \ diff --git a/Dockerfile.melodic b/Dockerfile.melodic index 55baf91ac..35e8399be 100644 --- a/Dockerfile.melodic +++ b/Dockerfile.melodic @@ -50,17 +50,15 @@ RUN /catkin_ws/src/cartographer/scripts/install_abseil.sh COPY scripts/install.sh cartographer_ros/scripts/ COPY scripts/catkin_test_results.sh cartographer_ros/scripts/ +RUN cartographer_ros/scripts/install.sh --pkg cartographer && \ + cartographer_ros/scripts/install.sh --pkg cartographer --make-args test + COPY cartographer_ros_msgs catkin_ws/src/cartographer_ros/cartographer_ros_msgs/ RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs && \ cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs \ --catkin-make-args run_tests && \ cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros_msgs -RUN apt-get install -y libceres-dev - -RUN cartographer_ros/scripts/install.sh --pkg cartographer && \ - cartographer_ros/scripts/install.sh --pkg cartographer --make-args test - COPY cartographer_ros catkin_ws/src/cartographer_ros/cartographer_ros/ RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros && \ cartographer_ros/scripts/install.sh --pkg cartographer_ros \ diff --git a/Dockerfile.noetic b/Dockerfile.noetic index 87cd65313..7168d10e6 100644 --- a/Dockerfile.noetic +++ b/Dockerfile.noetic @@ -54,17 +54,15 @@ RUN /catkin_ws/src/cartographer/scripts/install_abseil.sh COPY scripts/install.sh cartographer_ros/scripts/ COPY scripts/catkin_test_results.sh cartographer_ros/scripts/ +RUN cartographer_ros/scripts/install.sh --pkg cartographer && \ + cartographer_ros/scripts/install.sh --pkg cartographer --make-args test + COPY cartographer_ros_msgs catkin_ws/src/cartographer_ros/cartographer_ros_msgs/ RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs && \ cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs \ --catkin-make-args run_tests && \ cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros_msgs -RUN apt-get install -y libceres-dev - -RUN cartographer_ros/scripts/install.sh --pkg cartographer && \ - cartographer_ros/scripts/install.sh --pkg cartographer --make-args test - COPY cartographer_ros catkin_ws/src/cartographer_ros/cartographer_ros/ RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros && \ cartographer_ros/scripts/install.sh --pkg cartographer_ros \ From 4f730df895653272acd19a38ac9683c563be9c7a Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Mon, 5 Oct 2020 14:41:05 +0200 Subject: [PATCH 22/94] Minor documentation tweaks for Noetic. (#1520) Signed-off-by: Wolfgang Hess Signed-off-by: Guillaume Doisy --- docs/source/compilation.rst | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/docs/source/compilation.rst b/docs/source/compilation.rst index 8ed7218f8..4607db2aa 100644 --- a/docs/source/compilation.rst +++ b/docs/source/compilation.rst @@ -25,6 +25,7 @@ The following `ROS distributions`_ are currently supported: * Kinetic * Melodic +* Noetic .. _the ones from Cartographer: https://google-cartographer.readthedocs.io/en/latest/#system-requirements .. _ROS distributions: http://wiki.ros.org/Distributions @@ -36,12 +37,21 @@ In order to build Cartographer ROS, we recommend using `wstool `_. For faster builds, we also recommend using `Ninja `_. +On Ubuntu Focal with ROS Noetic use these commands to install the above tools: + +.. code-block:: bash + + sudo apt-get update + sudo apt-get install -y python3-wstool python3-rosdep ninja-build + +On older distributions: + .. code-block:: bash sudo apt-get update sudo apt-get install -y python-wstool python-rosdep ninja-build -Create a new cartographer_ros workspace in 'catkin_ws'. +After the tools are installed, create a new cartographer_ros workspace in 'catkin_ws'. .. code-block:: bash From 8712d1db2c17ba19ba55c810e66e61d8a06d6886 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Wed, 7 Oct 2020 12:56:54 +0200 Subject: [PATCH 23/94] Use distinct issue templates for bugs, tuning and general things. (#1521) This uses the more modern issue template mechanism of GitHub. Users will be able to choose one of these after clicking "add issue". --- The template for tuning is reworded to reflect that tuning issues are not guaranteed to be handled by maintainers. Signed-off-by: Michael Grupp Signed-off-by: Guillaume Doisy --- .github/ISSUE_TEMPLATE | 25 --------------------- .github/ISSUE_TEMPLATE/bug-report.md | 19 ++++++++++++++++ .github/ISSUE_TEMPLATE/general-issue.md | 5 +++++ .github/ISSUE_TEMPLATE/tuning-advice.md | 30 +++++++++++++++++++++++++ 4 files changed, 54 insertions(+), 25 deletions(-) delete mode 100644 .github/ISSUE_TEMPLATE create mode 100644 .github/ISSUE_TEMPLATE/bug-report.md create mode 100644 .github/ISSUE_TEMPLATE/general-issue.md create mode 100644 .github/ISSUE_TEMPLATE/tuning-advice.md diff --git a/.github/ISSUE_TEMPLATE b/.github/ISSUE_TEMPLATE deleted file mode 100644 index 196642772..000000000 --- a/.github/ISSUE_TEMPLATE +++ /dev/null @@ -1,25 +0,0 @@ -Thank you for filing an issue! - -It would help us tremendously if you run through the following checklist. This -ensures that we have the most information to quickly understand, analyze, -reproduce and eventually resolve your issue. - -Please - -1. run `rosbag_validate` which does some checks on your sensor data. This - tool often finds issues that can explain poor performance and must be fixed - at recording time. Please post the full output of the tool into a - GitHub Gist at https://gist.github.com/, then link it in the issue even if - it does not report anything. You can run the tool like this: - - rosrun cartographer_ros cartographer_rosbag_validate -bag_filename - -2. post a link to a Git repository containing a branch of - `cartographer_ros` containing all the configuration, launch, and URDF files - required to reproduce your issue. -3. post a link to a bag file we can use to reproduce your issue. Put it on - Google Drive, Dropbox, any webserver or wherever it is publicly - downloadable. -4. remove this boilerplate text before submitting your issue. - -We will likely be unable to help you without all of these points addressed. diff --git a/.github/ISSUE_TEMPLATE/bug-report.md b/.github/ISSUE_TEMPLATE/bug-report.md new file mode 100644 index 000000000..b1d8c54bb --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug-report.md @@ -0,0 +1,19 @@ +--- +name: Bug report +about: Report a bug that you've found in this repository or in a release. + +--- + +Please provide information for your bug report: + +- if you're using cartographer_ros from source, provide the Git commit hash + (via `git log -1 --format="%H"`): + +- if you're using a release package, provide the version + (e.g. via `apt show `): + +- provide all information that is needed to analyze and reproduce the bug + (logs, commands that have been used, ...) + +PLEASE NOTE: we don't support custom forks or extensions. +If you need tuning advice, you can open a tuning issue instead. diff --git a/.github/ISSUE_TEMPLATE/general-issue.md b/.github/ISSUE_TEMPLATE/general-issue.md new file mode 100644 index 000000000..24180eb67 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/general-issue.md @@ -0,0 +1,5 @@ +--- +name: General issue +about: Use this issue for general questions. + +--- diff --git a/.github/ISSUE_TEMPLATE/tuning-advice.md b/.github/ISSUE_TEMPLATE/tuning-advice.md new file mode 100644 index 000000000..48442f65e --- /dev/null +++ b/.github/ISSUE_TEMPLATE/tuning-advice.md @@ -0,0 +1,30 @@ +--- +name: Help request for tuning +about: Describe tuning problems with your custom Cartographer setup to request help from the community. + +--- + +To improve the chances that other Cartographer users can help you, +here are some guidelines for describing your tuning problem: + +1. check the tuning guide on the documentation page and previous issues - + some problems might have been solved already by other people. + +2. run `rosbag_validate` which does some checks on your sensor data. This + tool often finds issues that can explain poor performance and must be fixed + at recording time. Please post the full output of the tool into a + GitHub Gist at https://gist.github.com/, then link it in the issue even if + it does not report anything. You can run the tool like this: + + rosrun cartographer_ros cartographer_rosbag_validate -bag_filename + +3. post a link to a Git repository containing a branch of + `cartographer_ros` containing all the configuration, launch, and URDF files + required to reproduce your issue. +4. post a link to a bag file we can use to reproduce your issue. Put it on + Google Drive, Dropbox, any webserver or wherever it is publicly + downloadable. +5. remove this boilerplate text before submitting your issue. + +PLEASE NOTE: tuning issues are not necessarily handled by the maintainers and +can be closed after a period of inactivity. From 108f2c6c5f01649cfc0a9ea376f4aa434c075d6e Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Tue, 13 Oct 2020 14:37:06 +0200 Subject: [PATCH 24/94] Fix includes to follow style guide in playable_bag.h (#1524) Signed-off-by: Michael Grupp Signed-off-by: Guillaume Doisy --- cartographer_ros/cartographer_ros/playable_bag.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cartographer_ros/cartographer_ros/playable_bag.h b/cartographer_ros/cartographer_ros/playable_bag.h index 52db7df92..1ceed0619 100644 --- a/cartographer_ros/cartographer_ros/playable_bag.h +++ b/cartographer_ros/cartographer_ros/playable_bag.h @@ -16,12 +16,12 @@ #ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_PLAYABLE_BAG_H #define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_PLAYABLE_BAG_H -#include -#include #include #include +#include "cartographer_ros_msgs/BagfileProgress.h" +#include "ros/node_handle.h" #include "rosbag/bag.h" #include "rosbag/view.h" #include "tf2_ros/buffer.h" From ee785adc94bd71b1969d6ea1c294f0ed7a67317a Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Tue, 13 Oct 2020 15:00:12 +0200 Subject: [PATCH 25/94] Fix Sphinx documentation errors & warnings. (#1523) Fixes broken links, code blocks and other things that `sphinx-build docs/source docs/out -E` complained about. Signed-off-by: Michael Grupp Signed-off-by: Guillaume Doisy --- docs/source/assets_writer.rst | 4 ++-- docs/source/compilation.rst | 1 + docs/source/demos.rst | 2 +- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/docs/source/assets_writer.rst b/docs/source/assets_writer.rst index 1cf223c7d..de936d6a5 100644 --- a/docs/source/assets_writer.rst +++ b/docs/source/assets_writer.rst @@ -80,8 +80,8 @@ For the last example, if you specify ``points.ply`` in the pipeline configuratio Configuration ------------- -The assets writer is modeled as a pipeline of `PointsProcessor`_s. -`PointsBatch`_\ s flow through each processor and they all have the chance to modify the ``PointsBatch`` before passing it on. +The assets writer is modeled as a pipeline of `PointsProcessor`_ steps. +`PointsBatch`_ data flows through each processor and they all have the chance to modify the ``PointsBatch`` before passing it on. .. _PointsProcessor: https://github.com/cartographer-project/cartographer/blob/30f7de1a325d6604c780f2f74d9a345ec369d12d/cartographer/io/points_processor.h .. _PointsBatch: https://github.com/cartographer-project/cartographer/blob/30f7de1a325d6604c780f2f74d9a345ec369d12d/cartographer/io/points_batch.h diff --git a/docs/source/compilation.rst b/docs/source/compilation.rst index 4607db2aa..0d922a938 100644 --- a/docs/source/compilation.rst +++ b/docs/source/compilation.rst @@ -76,6 +76,7 @@ If you build cartographer from master. Change/remove the version in the cartogra Additionally, uninstall the ros abseil-cpp using .. code-block:: bash + sudo apt-get remove ros-${ROS_DISTRO}-abseil-cpp Build and install. diff --git a/docs/source/demos.rst b/docs/source/demos.rst index 4e2e19e16..dac214f36 100644 --- a/docs/source/demos.rst +++ b/docs/source/demos.rst @@ -81,7 +81,7 @@ Generate the map (wait until cartographer_offline_node finishes) and then run pu bag_filename:=${HOME}/Downloads/b3-2016-04-05-15-52-20.bag Static landmarks -======== +================ .. raw:: html From f3722636c759c8435f3d9e803dde81564c58ff6e Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Tue, 13 Oct 2020 15:50:12 +0200 Subject: [PATCH 26/94] Follow cartographer-project/cartographer#1759. (#1525) Signed-off-by: Wolfgang Hess Signed-off-by: Guillaume Doisy --- .../cartographer_ros/ros_map_writing_points_processor.cc | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc b/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc index 5a4016e70..25c0e8312 100644 --- a/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc +++ b/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc @@ -51,8 +51,9 @@ RosMapWritingPointsProcessor::FromDictionary( void RosMapWritingPointsProcessor::Process( std::unique_ptr<::cartographer::io::PointsBatch> batch) { - range_data_inserter_.Insert({batch->origin, batch->points, {}}, - &probability_grid_); + range_data_inserter_.Insert( + {batch->origin, ::cartographer::sensor::PointCloud(batch->points), {}}, + &probability_grid_); next_->Process(std::move(batch)); } From 6daa4ac1e6d30e2788eabf11406fadcbb7b0f4c0 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Thu, 15 Oct 2020 10:49:24 +0200 Subject: [PATCH 27/94] Update "Getting involved" documentation page. (#1522) Some things were outdated after recent changes. Signed-off-by: Michael Grupp Signed-off-by: Guillaume Doisy --- docs/source/getting_involved.rst | 15 ++++----------- 1 file changed, 4 insertions(+), 11 deletions(-) diff --git a/docs/source/getting_involved.rst b/docs/source/getting_involved.rst index c3750872b..e3334eab9 100644 --- a/docs/source/getting_involved.rst +++ b/docs/source/getting_involved.rst @@ -19,18 +19,11 @@ Getting involved Cartographer is developed in the open and allows anyone to contribute to the project. There are multiple ways to get involved! -Twice a month, the project hosts "Open House Hangouts" sessions that are essentially meetings open to everyone to join in. -The call typically recaps the recent and ongoing development around Cartographer and Cartographer ROS. -The developers are then open to questions from the community, this is a great time to ask about contribution ideas. -If you don't feel like talking or being seen, you are free to join anyway and skulk! -The slides are also made available after each session but there is no video recording. - -If you want to stay tuned with announcements (such as new major releases or new open house sessions), you can join `the Cartographer mailing list`_ although you can not interact with this mailing list anymore. +If you want to stay tuned with announcements (such as new major releases), you can join `the Cartographer mailing list`_ although you can not interact with this mailing list anymore. .. _the Cartographer mailing list: https://groups.google.com/forum/#!forum/google-cartographer -If you think you've found an issue in Cartographer, you are welcome to open a `GitHub issue`_ but don't forget to provide a way to reproduce your bug! -Typically, join a ``.bag`` and a link to a fork of the ``cartographer_ros`` repository containing your configuration and launch files. +If you think you've found an issue in Cartographer, you are welcome to open a `GitHub issue`_. .. _GitHub issue: https://github.com/cartographer-project/cartographer/issues @@ -40,7 +33,7 @@ Simpler changes can also be discussed in GitHub issues so that developers can he .. _the RFCs repository: https://github.com/cartographer-project/rfcs If you want to contribute code or documentation, this is done through `GitHub pull requests`_. -However, make sure you have signed (online) the `Contributor License Agreement`_ first! +Pull requests need to follow the `contribution guidelines`_. .. _GitHub pull requests: https://github.com/cartographer-project/cartographer/pulls -.. _Contributor License Agreement: https://github.com/cartographer-project/cartographer/blob/master/CONTRIBUTING.md +.. _contribution guidelines: https://github.com/cartographer-project/cartographer/blob/master/CONTRIBUTING.md From 804595b9cea1f8238a08837f96c98905c00e49ec Mon Sep 17 00:00:00 2001 From: Jonathan Huber Date: Wed, 21 Oct 2020 14:03:15 +0200 Subject: [PATCH 28/94] Make publishing tf optional, enable publishing PoseStamped (#1099) If the output of cartographer should be used as an input to an additional sensor fusion method, using the published TF transform is inconvenient or in our specific use case even harmful because we don't want to add the raw cartographer output to our TF tree. With this change it becomes optional to broadcast to /tf. Morever there is an option to publish the tracked frame pose as a PoseStamped message. We add two new optional parameters: - `publish_to_tf` if false no tf transform is broadcasted - `publish_tracked_pose` If set `true` a PoseStamped representing the position of the tracked pose w.r.t. map frame is published. If default launchers and settings are used this PR causes no change in the behavior. Signed-off-by: Guillaume Doisy --- cartographer_ros/cartographer_ros/node.cc | 67 ++++++++++++------- cartographer_ros/cartographer_ros/node.h | 1 + .../cartographer_ros/node_constants.h | 1 + .../cartographer_ros/node_options.cc | 8 +++ .../cartographer_ros/node_options.h | 2 + docs/source/configuration.rst | 7 ++ docs/source/ros_api.rst | 11 ++- 7 files changed, 68 insertions(+), 29 deletions(-) diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 1bc804a59..3354729aa 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -40,6 +40,7 @@ #include "cartographer_ros/time_conversion.h" #include "cartographer_ros_msgs/StatusCode.h" #include "cartographer_ros_msgs/StatusResponse.h" +#include "geometry_msgs/PoseStamped.h" #include "glog/logging.h" #include "nav_msgs/Odometry.h" #include "ros/serialization.h" @@ -113,6 +114,11 @@ Node::Node( constraint_list_publisher_ = node_handle_.advertise<::visualization_msgs::MarkerArray>( kConstraintListTopic, kLatestOnlyPublisherQueueSize); + if (node_options_.publish_tracked_pose) { + tracked_pose_publisher_ = + node_handle_.advertise<::geometry_msgs::PoseStamped>( + kTrackedPoseTopic, kLatestOnlyPublisherQueueSize); + } service_servers_.push_back(node_handle_.advertiseService( kSubmapQueryServiceName, &Node::HandleSubmapQuery, this)); service_servers_.push_back(node_handle_.advertiseService( @@ -271,32 +277,41 @@ void Node::PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event) { trajectory_data.local_to_map * tracking_to_local; if (trajectory_data.published_to_tracking != nullptr) { - if (trajectory_data.trajectory_options.provide_odom_frame) { - std::vector stamped_transforms; - - stamped_transform.header.frame_id = node_options_.map_frame; - stamped_transform.child_frame_id = - trajectory_data.trajectory_options.odom_frame; - stamped_transform.transform = - ToGeometryMsgTransform(trajectory_data.local_to_map); - stamped_transforms.push_back(stamped_transform); - - stamped_transform.header.frame_id = - trajectory_data.trajectory_options.odom_frame; - stamped_transform.child_frame_id = - trajectory_data.trajectory_options.published_frame; - stamped_transform.transform = ToGeometryMsgTransform( - tracking_to_local * (*trajectory_data.published_to_tracking)); - stamped_transforms.push_back(stamped_transform); - - tf_broadcaster_.sendTransform(stamped_transforms); - } else { - stamped_transform.header.frame_id = node_options_.map_frame; - stamped_transform.child_frame_id = - trajectory_data.trajectory_options.published_frame; - stamped_transform.transform = ToGeometryMsgTransform( - tracking_to_map * (*trajectory_data.published_to_tracking)); - tf_broadcaster_.sendTransform(stamped_transform); + if (node_options_.publish_to_tf) { + if (trajectory_data.trajectory_options.provide_odom_frame) { + std::vector stamped_transforms; + + stamped_transform.header.frame_id = node_options_.map_frame; + stamped_transform.child_frame_id = + trajectory_data.trajectory_options.odom_frame; + stamped_transform.transform = + ToGeometryMsgTransform(trajectory_data.local_to_map); + stamped_transforms.push_back(stamped_transform); + + stamped_transform.header.frame_id = + trajectory_data.trajectory_options.odom_frame; + stamped_transform.child_frame_id = + trajectory_data.trajectory_options.published_frame; + stamped_transform.transform = ToGeometryMsgTransform( + tracking_to_local * (*trajectory_data.published_to_tracking)); + stamped_transforms.push_back(stamped_transform); + + tf_broadcaster_.sendTransform(stamped_transforms); + } else { + stamped_transform.header.frame_id = node_options_.map_frame; + stamped_transform.child_frame_id = + trajectory_data.trajectory_options.published_frame; + stamped_transform.transform = ToGeometryMsgTransform( + tracking_to_map * (*trajectory_data.published_to_tracking)); + tf_broadcaster_.sendTransform(stamped_transform); + } + } + if (node_options_.publish_tracked_pose) { + ::geometry_msgs::PoseStamped pose_msg; + pose_msg.header.frame_id = node_options_.map_frame; + pose_msg.header.stamp = stamped_transform.header.stamp; + pose_msg.pose = ToGeometryMsgPose(tracking_to_map); + tracked_pose_publisher_.publish(pose_msg); } } } diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index ac715d02b..b02ca9635 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -188,6 +188,7 @@ class Node { ::ros::Publisher trajectory_node_list_publisher_; ::ros::Publisher landmark_poses_list_publisher_; ::ros::Publisher constraint_list_publisher_; + ::ros::Publisher tracked_pose_publisher_; // These ros::ServiceServers need to live for the lifetime of the node. std::vector<::ros::ServiceServer> service_servers_; ::ros::Publisher scan_matched_point_cloud_publisher_; diff --git a/cartographer_ros/cartographer_ros/node_constants.h b/cartographer_ros/cartographer_ros/node_constants.h index f54a4abcf..07a85f379 100644 --- a/cartographer_ros/cartographer_ros/node_constants.h +++ b/cartographer_ros/cartographer_ros/node_constants.h @@ -34,6 +34,7 @@ constexpr char kFinishTrajectoryServiceName[] = "finish_trajectory"; constexpr char kOccupancyGridTopic[] = "map"; constexpr char kScanMatchedPointCloudTopic[] = "scan_matched_points2"; constexpr char kSubmapListTopic[] = "submap_list"; +constexpr char kTrackedPoseTopic[] = "tracked_pose"; constexpr char kSubmapQueryServiceName[] = "submap_query"; constexpr char kTrajectoryQueryServiceName[] = "trajectory_query"; constexpr char kStartTrajectoryServiceName[] = "start_trajectory"; diff --git a/cartographer_ros/cartographer_ros/node_options.cc b/cartographer_ros/cartographer_ros/node_options.cc index 4812d08f0..17fae24ab 100644 --- a/cartographer_ros/cartographer_ros/node_options.cc +++ b/cartographer_ros/cartographer_ros/node_options.cc @@ -40,6 +40,14 @@ NodeOptions CreateNodeOptions( lua_parameter_dictionary->GetDouble("pose_publish_period_sec"); options.trajectory_publish_period_sec = lua_parameter_dictionary->GetDouble("trajectory_publish_period_sec"); + if (lua_parameter_dictionary->HasKey("publish_to_tf")) { + options.publish_to_tf = + lua_parameter_dictionary->GetBool("publish_to_tf"); + } + if (lua_parameter_dictionary->HasKey("publish_tracked_pose")) { + options.publish_tracked_pose = + lua_parameter_dictionary->GetBool("publish_tracked_pose"); + } if (lua_parameter_dictionary->HasKey("use_pose_extrapolator")) { options.use_pose_extrapolator = lua_parameter_dictionary->GetBool("use_pose_extrapolator"); diff --git a/cartographer_ros/cartographer_ros/node_options.h b/cartographer_ros/cartographer_ros/node_options.h index 43dc219cf..95bb15990 100644 --- a/cartographer_ros/cartographer_ros/node_options.h +++ b/cartographer_ros/cartographer_ros/node_options.h @@ -35,6 +35,8 @@ struct NodeOptions { double submap_publish_period_sec; double pose_publish_period_sec; double trajectory_publish_period_sec; + bool publish_to_tf = true; + bool publish_tracked_pose = false; bool use_pose_extrapolator = true; }; diff --git a/docs/source/configuration.rst b/docs/source/configuration.rst index 0d34b256b..f3b27e6ab 100644 --- a/docs/source/configuration.rst +++ b/docs/source/configuration.rst @@ -103,6 +103,12 @@ pose_publish_period_sec Interval in seconds at which to publish poses, e.g. 5e-3 for a frequency of 200 Hz. +publish_to_tf + Enable or disable providing of TF transforms. + +publish_tracked_pose_msg + Enable publishing of tracked pose as a `geometry_msgs/PoseStamped`_ to topic "tracked_pose". + trajectory_publish_period_sec Interval in seconds at which to publish the trajectory markers, e.g. 30e-3 for 30 milliseconds. @@ -124,6 +130,7 @@ landmarks_sampling_ratio .. _REP 105: http://www.ros.org/reps/rep-0105.html .. _ROS Names: http://wiki.ros.org/Names +.. _geometry_msgs/PoseStamped: http://docs.ros.org/api/geometry_msgs/html/msg/PoseStamped.html .. _nav_msgs/OccupancyGrid: http://docs.ros.org/api/nav_msgs/html/msg/OccupancyGrid.html .. _nav_msgs/Odometry: http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html .. _sensor_msgs/LaserScan: http://docs.ros.org/api/sensor_msgs/html/msg/LaserScan.html diff --git a/docs/source/ros_api.rst b/docs/source/ros_api.rst index 6e89f8ece..7227d016a 100644 --- a/docs/source/ros_api.rst +++ b/docs/source/ros_api.rst @@ -82,6 +82,10 @@ submap_list (`cartographer_ros_msgs/SubmapList`_) List of all submaps, including the pose and latest version number of each submap, across all trajectories. +tracked_pose (`geometry_msgs/PoseStamped`_) + Only published if the parameter ``publish_tracked_pose`` is set to ``true``. + The pose of the tracked frame with respect to the map frame. + Services -------- @@ -94,7 +98,7 @@ submap_query (`cartographer_ros_msgs/SubmapQuery`_) Fetches the requested submap. start_trajectory (`cartographer_ros_msgs/StartTrajectory`_) - Starts a trajectory using default sensor topics and the provided configuration. + Starts a trajectory using default sensor topics and the provided configuration. An initial pose can be optionally specified. Returns an assigned trajectory ID. trajectory_query (`cartographer_ros_msgs/TrajectoryQuery`_) @@ -131,9 +135,9 @@ Provided tf Transforms ---------------------- The transformation between the :doc:`configured ` *map_frame* -and *published_frame* is always provided. +and *published_frame* is provided unless the parameter ``publish_to_tf`` is set to ``false``. -If *provide_odom_frame* is enabled in the :doc:`configuration`, a continuous +If *provide_odom_frame* is enabled in the :doc:`configuration`, additionally a continuous (i.e. unaffected by loop closure) transform between the :doc:`configured ` *odom_frame* and *published_frame* will be provided. @@ -147,6 +151,7 @@ If *provide_odom_frame* is enabled in the :doc:`configuration`, a continuous .. _cartographer_ros_msgs/WriteState: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/srv/WriteState.srv .. _cartographer_ros_msgs/GetTrajectoryStates: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/srv/GetTrajectoryStates.srv .. _cartographer_ros_msgs/ReadMetrics: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/srv/ReadMetrics.srv +.. _geometry_msgs/PoseStamped: http://docs.ros.org/api/geometry_msgs/html/msg/PoseStamped.html .. _nav_msgs/OccupancyGrid: http://docs.ros.org/api/nav_msgs/html/msg/OccupancyGrid.html .. _nav_msgs/Odometry: http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html .. _sensor_msgs/Imu: http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html From 1cc08e957a99a4eac904f41851cd965dd5bcfd34 Mon Sep 17 00:00:00 2001 From: Erik Reed Date: Wed, 21 Oct 2020 10:49:50 -0700 Subject: [PATCH 29/94] Add color requirement note for point_cloud_viewer (#1424) I was running through these docs and having [trouble](https://github.com/googlecartographer/point_cloud_viewer/issues/389) with map visualization via `point_cloud_viewer`. Notably, color is required for plys ingested by `point_cloud_viewer` (in contrast to Meshlab). This PR adds a note to make this clear. Signed-off-by: Erik Reed Signed-off-by: Guillaume Doisy --- docs/source/assets_writer.rst | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/docs/source/assets_writer.rst b/docs/source/assets_writer.rst index de936d6a5..c5e2cf14e 100644 --- a/docs/source/assets_writer.rst +++ b/docs/source/assets_writer.rst @@ -105,7 +105,7 @@ The available ``PointsProcessor``\ s are all defined in the `cartographer/io`_ s * **voxel_filter_and_remove_moving_objects**: Voxel filters the data and only passes on points that we believe are on non-moving objects. * **write_pcd**: Streams a PCD file to disk. The header is written in 'Flush'. * **write_ply**: Streams a PLY file to disk. The header is written in 'Flush'. -* **write_probability_grid**: Creates a probability grid with the specified 'resolution'. As all points are projected into the x-y plane the z component of the data is ignored. 'range_data_inserter' options are used to cofnigure the range data ray tracing through the probability grid. +* **write_probability_grid**: Creates a probability grid with the specified 'resolution'. As all points are projected into the x-y plane the z component of the data is ignored. 'range_data_inserter' options are used to configure the range data ray tracing through the probability grid. * **write_xray_image**: Creates X-ray cuts through the points with pixels being 'voxel_size' big. * **write_xyz**: Writes ASCII xyz points. @@ -126,6 +126,7 @@ An example of such a pipeline is in `assets_writer_backpack_2d.lua`_. .. _assets_writer_backpack_2d.lua: https://github.com/cartographer-project/cartographer_ros/blob/44459e18102305745c56f92549b87d8e91f434fe/cartographer_ros/configuration_files/assets_writer_backpack_2d.lua Once you have the ``.ply``, follow the README of `point_cloud_viewer`_ to generate an on-disk octree data structure which can be viewed by one of the viewers (SDL or web based) in the same repo. +Note that color is required for ``point_cloud_viewer`` to function. .. _point_cloud_viewer: https://github.com/cartographer-project/point_cloud_viewer From 820881c882a404a39c47535f1fc437548b46d231 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Thu, 22 Oct 2020 17:33:45 +0200 Subject: [PATCH 30/94] Default to master in rosinstall (fixes #1122). (#1530) This makes much more sense than 1.0.0, which is counter-intuitive. If someone is building from source they usually do it to build the latest bleeding-edge version. Signed-off-by: Michael Grupp Signed-off-by: Guillaume Doisy --- cartographer_ros.rosinstall | 4 ++-- docs/source/compilation.rst | 3 ++- scripts/prepare_catkin_workspace.sh | 6 +++++- 3 files changed, 9 insertions(+), 4 deletions(-) diff --git a/cartographer_ros.rosinstall b/cartographer_ros.rosinstall index 6927cb835..c0b4b4fe5 100644 --- a/cartographer_ros.rosinstall +++ b/cartographer_ros.rosinstall @@ -1,3 +1,3 @@ -- git: {local-name: cartographer, uri: 'https://github.com/cartographer-project/cartographer.git', version: '1.0.0'} -- git: {local-name: cartographer_ros, uri: 'https://github.com/cartographer-project/cartographer_ros.git', version: '1.0.0'} +- git: {local-name: cartographer, uri: 'https://github.com/cartographer-project/cartographer.git', version: 'master'} +- git: {local-name: cartographer_ros, uri: 'https://github.com/cartographer-project/cartographer_ros.git', version: 'master'} - git: {local-name: ceres-solver, uri: 'https://ceres-solver.googlesource.com/ceres-solver.git', version: '1.13.0'} diff --git a/docs/source/compilation.rst b/docs/source/compilation.rst index 0d922a938..279ece8b4 100644 --- a/docs/source/compilation.rst +++ b/docs/source/compilation.rst @@ -71,7 +71,8 @@ The command 'sudo rosdep init' will print an error if you have already executed rosdep update rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y -If you build cartographer from master. Change/remove the version in the cartographer_ros.rosinstall. +This builds Cartographer from the latest HEAD of the master branch. +If you want a specific version, you need to change the version in the cartographer_ros.rosinstall. Additionally, uninstall the ros abseil-cpp using diff --git a/scripts/prepare_catkin_workspace.sh b/scripts/prepare_catkin_workspace.sh index 7e38d2475..77b870ee5 100755 --- a/scripts/prepare_catkin_workspace.sh +++ b/scripts/prepare_catkin_workspace.sh @@ -26,6 +26,10 @@ wstool init # Merge the cartographer_ros.rosinstall file and fetch code for dependencies. wstool merge ../../cartographer_ros/cartographer_ros.rosinstall -wstool set cartographer -v ${CARTOGRAPHER_VERSION} -y +# We default to master and wstool seems to have a bug when it's being set twice. +# TODO(MichaelGrupp): wstool is unmaintained, use vcstool. +if [ ${CARTOGRAPHER_VERSION} != "master" ]; then + wstool set cartographer -v ${CARTOGRAPHER_VERSION} -y +fi wstool remove cartographer_ros wstool update From 5ef6a4f56e69898971e2806c17d39782c9fda268 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Fri, 23 Oct 2020 17:24:20 +0200 Subject: [PATCH 31/94] Remove ceres-solver from cartographer_ros.rosinstall. (#1532) Recent versions no longer build ceres-solver and instead depend on libceres-dev. See also #1518. Signed-off-by: Wolfgang Hess Signed-off-by: Guillaume Doisy --- cartographer_ros.rosinstall | 1 - 1 file changed, 1 deletion(-) diff --git a/cartographer_ros.rosinstall b/cartographer_ros.rosinstall index c0b4b4fe5..430ce8497 100644 --- a/cartographer_ros.rosinstall +++ b/cartographer_ros.rosinstall @@ -1,3 +1,2 @@ - git: {local-name: cartographer, uri: 'https://github.com/cartographer-project/cartographer.git', version: 'master'} - git: {local-name: cartographer_ros, uri: 'https://github.com/cartographer-project/cartographer_ros.git', version: 'master'} -- git: {local-name: ceres-solver, uri: 'https://ceres-solver.googlesource.com/ceres-solver.git', version: '1.13.0'} From 00825c9e70429d8a633b8bd268820e5205bbd5af Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Tue, 27 Oct 2020 09:28:18 +0100 Subject: [PATCH 32/94] Fix outdated build instructions (closes #1531). (#1537) Signed-off-by: Michael Grupp Signed-off-by: Guillaume Doisy --- docs/source/compilation.rst | 24 +++++++++++++++++------- 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/docs/source/compilation.rst b/docs/source/compilation.rst index 279ece8b4..ace433fd5 100644 --- a/docs/source/compilation.rst +++ b/docs/source/compilation.rst @@ -42,14 +42,14 @@ On Ubuntu Focal with ROS Noetic use these commands to install the above tools: .. code-block:: bash sudo apt-get update - sudo apt-get install -y python3-wstool python3-rosdep ninja-build + sudo apt-get install -y python3-wstool python3-rosdep ninja-build stow On older distributions: .. code-block:: bash sudo apt-get update - sudo apt-get install -y python-wstool python-rosdep ninja-build + sudo apt-get install -y python-wstool python-rosdep ninja-build stow After the tools are installed, create a new cartographer_ros workspace in 'catkin_ws'. @@ -61,20 +61,25 @@ After the tools are installed, create a new cartographer_ros workspace in 'catki wstool merge -t src https://raw.githubusercontent.com/cartographer-project/cartographer_ros/master/cartographer_ros.rosinstall wstool update -t src -Install cartographer_ros' dependencies (proto3 and deb packages). +Now you need to install cartographer_ros' dependencies. +First, we use ``rosdep`` to install the required packages. The command 'sudo rosdep init' will print an error if you have already executed it since installing ROS. This error can be ignored. .. code-block:: bash - src/cartographer/scripts/install_proto3.sh sudo rosdep init rosdep update rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y -This builds Cartographer from the latest HEAD of the master branch. -If you want a specific version, you need to change the version in the cartographer_ros.rosinstall. + # Only on Ubuntu 16 / ROS Kinetic: src/cartographer/scripts/install_proto3.sh + +Cartographer uses the `abseil-cpp`_ library that needs to be manually installed using this script: + +.. code-block:: bash -Additionally, uninstall the ros abseil-cpp using + src/cartographer/scripts/install_abseil.sh + +Due to conflicting versions you might need to uninstall the ROS abseil-cpp using .. code-block:: bash @@ -85,3 +90,8 @@ Build and install. .. code-block:: bash catkin_make_isolated --install --use-ninja + +This builds Cartographer from the latest HEAD of the master branch. +If you want a specific version, you need to change the version in the cartographer_ros.rosinstall. + +.. _abseil-cpp: https://abseil.io/ From fb81a3fc9632fd7a93e59110456ec17479f4dc06 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 4 Nov 2020 15:57:50 +0100 Subject: [PATCH 33/94] Use PoseGraphInterface instead of PoseGraph. (#1547) Tiny improvement to not depend on more than is needed. Signed-off-by: Wolfgang Hess Signed-off-by: Guillaume Doisy --- cartographer_ros/cartographer_ros/map_builder_bridge.cc | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index 5162a53c3..c89a4f0f8 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -20,7 +20,6 @@ #include "absl/strings/str_cat.h" #include "cartographer/io/color.h" #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" @@ -207,7 +206,7 @@ MapBuilderBridge::GetTrajectoryStates() { for (const auto& sensor_bridge : sensor_bridges_) { trajectory_states.insert(std::make_pair( sensor_bridge.first, - ::cartographer::mapping::PoseGraph::TrajectoryState::ACTIVE)); + ::cartographer::mapping::PoseGraphInterface::TrajectoryState::ACTIVE)); } return trajectory_states; } @@ -299,7 +298,7 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() { const auto constraints = map_builder_->pose_graph()->constraints(); for (const auto& constraint : constraints) { if (constraint.tag == - cartographer::mapping::PoseGraph::Constraint::INTER_SUBMAP) { + cartographer::mapping::PoseGraphInterface::Constraint::INTER_SUBMAP) { if (constraint.node_id.trajectory_id == constraint.submap_id.trajectory_id) { trajectory_to_last_inter_submap_constrained_node[constraint.node_id @@ -451,7 +450,7 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() { visualization_msgs::Marker *constraint_marker, *residual_marker; std_msgs::ColorRGBA color_constraint, color_residual; if (constraint.tag == - cartographer::mapping::PoseGraph::Constraint::INTRA_SUBMAP) { + cartographer::mapping::PoseGraphInterface::Constraint::INTRA_SUBMAP) { constraint_marker = &constraint_intra_marker; residual_marker = &residual_intra_marker; // Color mapping for submaps of various trajectories - add trajectory id From 2ae29b45c90cce6019293add7f42edcc253ad85a Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Fri, 6 Nov 2020 13:41:08 +0100 Subject: [PATCH 34/94] Use the MapBuilder factory function. (#1551) This follows cartographer-project/cartographer#1776. Signed-off-by: Wolfgang Hess Signed-off-by: Guillaume Doisy --- cartographer_ros/cartographer_ros/node_main.cc | 4 ++-- cartographer_ros/cartographer_ros/node_options.cc | 2 +- .../cartographer_ros/offline_node_main.cc | 11 +++++------ 3 files changed, 8 insertions(+), 9 deletions(-) diff --git a/cartographer_ros/cartographer_ros/node_main.cc b/cartographer_ros/cartographer_ros/node_main.cc index e430af24d..0a6dee342 100644 --- a/cartographer_ros/cartographer_ros/node_main.cc +++ b/cartographer_ros/cartographer_ros/node_main.cc @@ -56,8 +56,8 @@ void Run() { std::tie(node_options, trajectory_options) = LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename); - auto map_builder = absl::make_unique( - node_options.map_builder_options); + auto map_builder = + cartographer::mapping::CreateMapBuilder(node_options.map_builder_options); Node node(node_options, std::move(map_builder), &tf_buffer, FLAGS_collect_metrics); if (!FLAGS_load_state_filename.empty()) { diff --git a/cartographer_ros/cartographer_ros/node_options.cc b/cartographer_ros/cartographer_ros/node_options.cc index 17fae24ab..681754e7e 100644 --- a/cartographer_ros/cartographer_ros/node_options.cc +++ b/cartographer_ros/cartographer_ros/node_options.cc @@ -19,7 +19,7 @@ #include #include "cartographer/common/configuration_file_resolver.h" -#include "cartographer/mapping/map_builder.h" +#include "cartographer/mapping/map_builder_interface.h" #include "glog/logging.h" namespace cartographer_ros { diff --git a/cartographer_ros/cartographer_ros/offline_node_main.cc b/cartographer_ros/cartographer_ros/offline_node_main.cc index ca593bb48..7e6c3da66 100644 --- a/cartographer_ros/cartographer_ros/offline_node_main.cc +++ b/cartographer_ros/cartographer_ros/offline_node_main.cc @@ -29,12 +29,11 @@ int main(int argc, char** argv) { cartographer_ros::ScopedRosLogSink ros_log_sink; - const cartographer_ros::MapBuilderFactory map_builder_factory = - [](const ::cartographer::mapping::proto::MapBuilderOptions& - map_builder_options) { - return absl::make_unique< ::cartographer::mapping::MapBuilder>( - map_builder_options); - }; + const cartographer_ros::MapBuilderFactory map_builder_factory = []( + const ::cartographer::mapping::proto::MapBuilderOptions& + map_builder_options) { + return ::cartographer::mapping::CreateMapBuilder(map_builder_options); + }; cartographer_ros::RunOfflineNode(map_builder_factory); From 359e580310e1a9ff968741b1bef63a45a709ccf1 Mon Sep 17 00:00:00 2001 From: zhenzhenxiang <405090724@163.com> Date: Wed, 11 Nov 2020 20:46:46 +0800 Subject: [PATCH 35/94] Update algo_walkthrough.rst (#1545) Fix typos. Signed-off-by: zhenzhenxiang <405090724@163.com> Signed-off-by: Guillaume Doisy --- docs/source/algo_walkthrough.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/source/algo_walkthrough.rst b/docs/source/algo_walkthrough.rst index 483dcf59f..42b056a57 100644 --- a/docs/source/algo_walkthrough.rst +++ b/docs/source/algo_walkthrough.rst @@ -276,9 +276,9 @@ The resulting net is called the "*pose graph*". Constraints can be visualized in RViz, it is very handy to tune global SLAM. One can also toggle ``POSE_GRAPH.constraint_builder.log_matches`` to get regular reports of the constraints builder formatted as histograms. -- Non-global constraints (also known as inter submaps constraints) are built automatically between nodes that are closely following each other on a trajectory. +- Non-global constraints (also known as intra submaps constraints) are built automatically between nodes that are closely following each other on a trajectory. Intuitively, those "*non-global ropes*" keep the local structure of the trajectory coherent. -- Global constraints (also referred to as loop closure constraints or intra submaps contraints) are regularly searched between a new submap and previous nodes that are considered "*close enough*" in space (part of a certain **search window**) and a strong fit (a good match when running scan matching). +- Global constraints (also referred to as loop closure constraints or inter submaps contraints) are regularly searched between a new submap and previous nodes that are considered "*close enough*" in space (part of a certain **search window**) and a strong fit (a good match when running scan matching). Intuitively, those "*global ropes*" introduce knots in the structure and firmly bring two strands closer. .. code-block:: lua From 1dfb8a085f2cad4582245d1ff87e6069a6dea657 Mon Sep 17 00:00:00 2001 From: stribor14 Date: Fri, 13 Nov 2020 13:22:31 +0100 Subject: [PATCH 36/94] Check if we already publihed transformation at same timestamp Fix #1555 (#1556) Signed-off-by: stribor14 Co-authored-by: stribor14 Signed-off-by: Guillaume Doisy --- cartographer_ros/cartographer_ros/node.cc | 9 +++++++++ cartographer_ros/cartographer_ros/node.h | 1 + 2 files changed, 10 insertions(+) diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 3354729aa..067ab5ae0 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -261,6 +261,15 @@ void Node::PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event) { node_options_.use_pose_extrapolator ? ToRos(now) : ToRos(trajectory_data.local_slam_data->time); + + // Suppress publishing if we already published a transform at this time. + // Due to 2020-07 changes to geometry2, tf buffer will issue warnings for + // repeated transforms with the same timestamp. + if (last_published_tf_stamps_.count(entry.first) && + last_published_tf_stamps_[entry.first] == stamped_transform.header.stamp) + continue; + last_published_tf_stamps_[entry.first] = stamped_transform.header.stamp; + const Rigid3d tracking_to_local_3d = node_options_.use_pose_extrapolator ? extrapolator.ExtrapolatePose(now) diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index b02ca9635..0f7beadac 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -214,6 +214,7 @@ class Node { // These are keyed with 'trajectory_id'. std::map extrapolators_; + std::map last_published_tf_stamps_; std::unordered_map sensor_samplers_; std::unordered_map> subscribers_; std::unordered_set subscribed_topics_; From 3e0f6aaeca383147dda844148cfb4b3dbf5df4c2 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 18 Nov 2020 09:54:52 +0100 Subject: [PATCH 37/94] Improve CONTRIBUTING.md and add a pull request template. (#1564) This is similar to cartographer-project/cartographer#1780, but - we add a pull request template (there was none before) - we refer to cartographer_ros CONTRIBUTING.md - we give different instructions to execute cartographer_ros tests Signed-off-by: Wolfgang Hess Signed-off-by: Guillaume Doisy --- .github/PULL_REQUEST_TEMPLATE.md | 2 ++ CONTRIBUTING.md | 22 +++++++++++++++++++++- 2 files changed, 23 insertions(+), 1 deletion(-) create mode 100644 .github/PULL_REQUEST_TEMPLATE.md diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md new file mode 100644 index 000000000..bf6c78eae --- /dev/null +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -0,0 +1,2 @@ +Want to contribute? Great! Make sure you've read and understood +[CONTRIBUTING.md](https://github.com/cartographer-project/cartographer_ros/blob/master/CONTRIBUTING.md). diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index f123a3d31..42d286974 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -27,5 +27,25 @@ You can sign-off a commit via `git commit -s`. ### Code reviews All submissions, including submissions by project members, require review. -We use GitHub pull requests for this purpose. +We use GitHub pull requests for this purpose. Make sure you've read, +understood and considered all the points below before creating your PR. +#### Style guide + +C++ code should adhere to the +[Google C++ Style Guide](https://google.github.io/styleguide/cppguide.html). +You can handle the formatting part of the style guide via `git clang-format`. + +#### Best practices + +When preparing your PR and also during the code review make sure to follow +[best practices](https://google.github.io/eng-practices/review/developer/). +Most importantly, keep your PR under 200 lines of code and address a single +concern. + +#### Testing + +- Add unit tests and documentation (these do not count toward your 200 lines). +- Run tests as appropriate, e.g. `docker build . -t cartographer:noetic -f Dockerfile.noetic`. +- Keep rebasing (or merging) of master branch to a minimum. It triggers Travis + runs for every update which blocks merging of other changes. From 2cbe3f26c2b47248a10735fddf50cfc389b67e06 Mon Sep 17 00:00:00 2001 From: Mohamed Ahmed Date: Mon, 25 Jan 2021 12:21:50 +0300 Subject: [PATCH 38/94] Fixing typo in the docs (#1588) Signed-off-by: mohamedsayed18 Signed-off-by: Guillaume Doisy --- docs/source/assets_writer.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/source/assets_writer.rst b/docs/source/assets_writer.rst index c5e2cf14e..c0315fe42 100644 --- a/docs/source/assets_writer.rst +++ b/docs/source/assets_writer.rst @@ -62,7 +62,7 @@ When running as an online node, Cartographer doesn't know when your bag (or sens # Ask Cartographer to serialize its current state. # (press tab to quickly expand the parameter syntax) - rosservice call /write_state "{filename: '${HOME}/Downloads/b3-2016-04-05-14-14-00.bag.pbstream', include_unfinished_submaps: 'true'}" + rosservice call /write_state "{filename: '${HOME}/Downloads/b3-2016-04-05-14-14-00.bag.pbstream', include_unfinished_submaps: "true"}" Once you've retrieved your ``.pbstream`` file, you can run the assets writer with the `sample pipeline`_ for the 3D backpack: From 0d7e742e874d2471bb043612b0a647cffc1091f2 Mon Sep 17 00:00:00 2001 From: Mohamed Ahmed Date: Fri, 29 Jan 2021 13:47:57 +0300 Subject: [PATCH 39/94] typo fix (#1591) Signed-off-by: mohamedsayed18 Signed-off-by: Guillaume Doisy --- docs/source/your_bag.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/source/your_bag.rst b/docs/source/your_bag.rst index 9148e9895..8d33e0fd0 100644 --- a/docs/source/your_bag.rst +++ b/docs/source/your_bag.rst @@ -113,7 +113,7 @@ Start by copying one of the provided example: - ``demo_my_robot.launch`` is meant to be used from a development machine and expects a ``bag_filename`` argument to replay data from a recording. This launch file also spawns a rviz window configured to visualize Cartographer's state. - ``offline_my_robot.launch`` is very similar to ``demo_my_robot.launch`` but tries to execute SLAM as fast as possible. This can make map building significantly faster. This launch file can also use multiple bag files provided to the ``bag_filenames`` argument. - ``demo_my_robot_localization.launch`` is very similar to ``demo_my_robot.launch`` but expects a ``load_state_filename`` argument pointing to a ``.pbstream`` recording of a previous Cartographer execution. The previous recording will be used as a pre-computed map and Cartographer will only perform localization on this map. -- ``assets_writer_my_robot.launch`` is used to extract data out of a ``.pstream`` recording of a previous Cartographer execution. +- ``assets_writer_my_robot.launch`` is used to extract data out of a ``.pbstream`` recording of a previous Cartographer execution. Again, a few adaptations need to be made to those files to suit your robot. From 4e34fd564c438bc2941d3aa260851528c9e8ed1b Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Mon, 19 Apr 2021 14:01:18 +0200 Subject: [PATCH 40/94] Remove inactive mailing list from docs. (#1612) Signed-off-by: Michael Grupp Signed-off-by: Guillaume Doisy --- docs/source/getting_involved.rst | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/docs/source/getting_involved.rst b/docs/source/getting_involved.rst index e3334eab9..6ada0854e 100644 --- a/docs/source/getting_involved.rst +++ b/docs/source/getting_involved.rst @@ -19,11 +19,7 @@ Getting involved Cartographer is developed in the open and allows anyone to contribute to the project. There are multiple ways to get involved! -If you want to stay tuned with announcements (such as new major releases), you can join `the Cartographer mailing list`_ although you can not interact with this mailing list anymore. - -.. _the Cartographer mailing list: https://groups.google.com/forum/#!forum/google-cartographer - -If you think you've found an issue in Cartographer, you are welcome to open a `GitHub issue`_. +If you have question or think you've found an issue in Cartographer, you are welcome to open a `GitHub issue`_. .. _GitHub issue: https://github.com/cartographer-project/cartographer/issues From 30d4ac5149768a6a18b72f5b1873080349fc232a Mon Sep 17 00:00:00 2001 From: Jonathan Huber Date: Tue, 4 May 2021 13:43:20 +0200 Subject: [PATCH 41/94] [docs] Fix parameter name of publish_tracked_pose (#1620) Signed-off-by: Jonathan Huber Signed-off-by: Guillaume Doisy --- docs/source/configuration.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/source/configuration.rst b/docs/source/configuration.rst index f3b27e6ab..67a59e0a4 100644 --- a/docs/source/configuration.rst +++ b/docs/source/configuration.rst @@ -106,7 +106,7 @@ pose_publish_period_sec publish_to_tf Enable or disable providing of TF transforms. -publish_tracked_pose_msg +publish_tracked_pose Enable publishing of tracked pose as a `geometry_msgs/PoseStamped`_ to topic "tracked_pose". trajectory_publish_period_sec From a967698b4ceb35335303db412ef15eaf84aa2aed Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Wed, 5 May 2021 23:59:24 +0200 Subject: [PATCH 42/94] ros2 conversion Signed-off-by: Guillaume Doisy --- cartographer_ros/CMakeLists.txt | 227 ++++---- .../cartographer_ros/CMakeLists.txt | 156 ++--- .../cartographer_ros/assets_writer.cc | 127 ++-- .../cartographer_ros/assets_writer.h | 1 + .../cartographer_ros/assets_writer_main.cc | 14 +- .../pbstream_trajectories_to_rosbag_main.cc | 6 +- .../dev/rosbag_publisher_main.cc | 36 +- .../cartographer_ros/map_builder_bridge.cc | 131 +++-- .../cartographer_ros/map_builder_bridge.h | 32 +- .../metrics/family_factory.cc | 2 +- .../cartographer_ros/metrics/family_factory.h | 4 +- .../metrics/internal/counter.h | 8 +- .../metrics/internal/family.cc | 12 +- .../metrics/internal/family.h | 8 +- .../cartographer_ros/metrics/internal/gauge.h | 10 +- .../metrics/internal/histogram.cc | 10 +- .../metrics/internal/histogram.h | 4 +- .../cartographer_ros/msg_conversion.cc | 99 ++-- .../cartographer_ros/msg_conversion.h | 47 +- .../cartographer_ros/msg_conversion_test.cc | 8 +- cartographer_ros/cartographer_ros/node.cc | 549 +++++++++--------- cartographer_ros/cartographer_ros/node.h | 159 ++--- .../cartographer_ros/node_main.cc | 43 +- .../occupancy_grid_node_main.cc | 204 ++++--- .../cartographer_ros/offline_node.cc | 252 ++++---- .../cartographer_ros/offline_node.h | 4 +- .../cartographer_ros/offline_node_main.cc | 16 +- .../pbstream_map_publisher_main.cc | 32 +- .../pbstream_to_ros_map_main.cc | 8 +- .../cartographer_ros/playable_bag.cc | 103 ++-- .../cartographer_ros/playable_bag.h | 51 +- .../cartographer_ros/ros_log_sink.cc | 9 +- .../cartographer_ros/ros_log_sink.h | 2 + cartographer_ros/cartographer_ros/ros_map.cc | 16 +- .../cartographer_ros/rosbag_validate_main.cc | 281 +++++---- .../cartographer_ros/sensor_bridge.cc | 20 +- .../cartographer_ros/sensor_bridge.h | 38 +- cartographer_ros/cartographer_ros/submap.cc | 32 +- cartographer_ros/cartographer_ros/submap.h | 7 +- .../cartographer_ros/tf_bridge.cc | 11 +- cartographer_ros/cartographer_ros/tf_bridge.h | 3 +- .../cartographer_ros/time_conversion.cc | 15 +- .../cartographer_ros/time_conversion.h | 7 +- .../cartographer_ros/time_conversion_test.cc | 2 +- .../cartographer_ros/urdf_reader.cc | 8 +- .../cartographer_ros/urdf_reader.h | 4 +- cartographer_ros/package.xml | 15 +- cartographer_ros_msgs/CMakeLists.txt | 87 +-- cartographer_ros_msgs/package.xml | 18 +- cartographer_ros_msgs/srv/ReadMetrics.srv | 2 +- cartographer_rviz/CMakeLists.txt | 188 +++--- 51 files changed, 1670 insertions(+), 1458 deletions(-) diff --git a/cartographer_ros/CMakeLists.txt b/cartographer_ros/CMakeLists.txt index a7e483c6c..2961fad09 100644 --- a/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/CMakeLists.txt @@ -12,82 +12,62 @@ # See the License for the specific language governing permissions and # limitations under the License. -cmake_minimum_required(VERSION 2.8.12) # Ships with Ubuntu 14.04 (Trusty) +cmake_minimum_required(VERSION 3.5) project(cartographer_ros) -set(PACKAGE_DEPENDENCIES - cartographer_ros_msgs - geometry_msgs - message_runtime - nav_msgs - pcl_conversions - rosbag - roscpp - roslib - sensor_msgs - std_msgs - tf2 - tf2_eigen - tf2_ros - urdf - visualization_msgs -) +find_package(ament_cmake REQUIRED) -if(WIN32) - set(Boost_USE_STATIC_LIBS FALSE) +# Default to C++17 +set(CMAKE_CXX_STANDARD 17) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) endif() -# For yet unknown reason, if Boost is find_packaged() after find_package(cartographer), -# some Boost libraries including Thread are set to static, despite Boost_USE_STATIC_LIBS, -# which causes linking problems on windows due to shared/static Thread clashing. -# PCL also finds Boost. Work around by moving before find_package(cartographer). -find_package(Boost REQUIRED COMPONENTS system iostreams) -find_package(PCL REQUIRED COMPONENTS common) + +add_compile_options(-g) find_package(cartographer REQUIRED) include("${CARTOGRAPHER_CMAKE_DIR}/functions.cmake") +set(BUILD_SHARED_LIBS OFF) option(BUILD_GRPC "build features that require Cartographer gRPC support" false) -google_initialize_cartographer_project() -google_enable_testing() -set(CARTOGRAPHER_GMOCK_LIBRARIES ${GMOCK_LIBRARIES}) -find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES}) +google_initialize_cartographer_project() -include(FindPkgConfig) +find_package(cartographer_ros_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(tf2_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(rosbag2_compression REQUIRED) +find_package(rosbag2_cpp REQUIRED) +find_package(rosbag2_storage REQUIRED) +find_package(backward_ros REQUIRED) find_package(absl REQUIRED) find_package(LuaGoogle REQUIRED) +find_package(PCL REQUIRED COMPONENTS common) find_package(Eigen3 REQUIRED) - +find_package(urdf REQUIRED) find_package(urdfdom_headers REQUIRED) + if(DEFINED urdfdom_headers_VERSION) if(${urdfdom_headers_VERSION} GREATER 0.4.1) add_definitions(-DURDFDOM_HEADERS_HAS_SHARED_PTR_DEFS) endif() endif() -include_directories( - ${urdfdom_headers_INCLUDE_DIRS} -) -# Override Catkin's GTest configuration to use GMock. -set(GTEST_FOUND TRUE) -set(GTEST_INCLUDE_DIRS ${GMOCK_INCLUDE_DIRS}) -set(GTEST_LIBRARIES ${CARTOGRAPHER_GMOCK_LIBRARIES}) - -catkin_package( - CATKIN_DEPENDS - ${PACKAGE_DEPENDENCIES} - DEPENDS - # TODO(damonkohler): This should be here but causes Catkin to abort because - # protobuf specifies a library '-lpthread' instead of just 'pthread'. - # CARTOGRAPHER - PCL - EIGEN3 - Boost - urdfdom_headers - INCLUDE_DIRS "." - LIBRARIES ${PROJECT_NAME} +include_directories( + include + "." ) file(GLOB_RECURSE ALL_SRCS "cartographer_ros/*.cc" "cartographer_ros/*.h") @@ -101,9 +81,30 @@ if (NOT ${BUILD_GRPC}) list(REMOVE_ITEM ALL_TESTS ${ALL_GRPC_FILES}) list(REMOVE_ITEM ALL_EXECUTABLES ${ALL_GRPC_FILES}) endif() + add_library(${PROJECT_NAME} STATIC ${ALL_SRCS}) +ament_target_dependencies(${PROJECT_NAME} PUBLIC + backward_ros + cartographer + cartographer_ros_msgs + geometry_msgs + nav_msgs + pcl_conversions + rclcpp + sensor_msgs + std_msgs + tf2 + tf2_eigen + tf2_msgs + tf2_ros + visualization_msgs + rosbag2_compression + rosbag2_cpp + rosbag2_storage + urdf + urdfdom_headers +) add_subdirectory("cartographer_ros") - target_link_libraries(${PROJECT_NAME} PUBLIC cartographer) # Lua @@ -125,15 +126,17 @@ endforeach() target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC "${EIGEN3_INCLUDE_DIR}") -# Boost +## YAML in ros2 1.0 +#target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${YAMLCPP_INCLUDE_DIRS}) +#target_link_libraries(${PROJECT_NAME} PUBLIC ${YAMLCPP_LIBRARIES}) + +# Boost not in ros2 target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC "${Boost_INCLUDE_DIRS}") target_link_libraries(${PROJECT_NAME} PUBLIC ${Boost_LIBRARIES}) -# Catkin -target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS}) -target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES}) -add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +## URDFDOM in ros2 1.0 +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${urdfdom_headers_INCLUDE_DIRS}) # Add the binary directory first, so that port.h is included after it has # been generated. @@ -147,51 +150,65 @@ set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${GOOG_CXX_FLAGS}") set_target_properties(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS ${TARGET_COMPILE_FLAGS}) -if (CATKIN_ENABLE_TESTING) - foreach(TEST_SOURCE_FILENAME ${ALL_TESTS}) - get_filename_component(TEST_NAME ${TEST_SOURCE_FILENAME} NAME_WE) - catkin_add_gtest(${TEST_NAME} ${TEST_SOURCE_FILENAME}) - # catkin_add_gtest uses a plain (i.e. no PUBLIC/PRIVATE/INTERFACE) call to - # target_link_libraries. That forces us to do the same. - target_link_libraries(${TEST_NAME} ${GMOCK_LIBRARIES} ${GTEST_MAIN_LIBRARIES}) - target_include_directories(${TEST_NAME} SYSTEM PUBLIC ${LUA_INCLUDE_DIR}) - target_link_libraries(${TEST_NAME} ${LUA_LIBRARIES}) - target_include_directories(${TEST_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS}) - target_link_libraries(${TEST_NAME} ${catkin_LIBRARIES}) - add_dependencies(${TEST_NAME} ${catkin_EXPORTED_TARGETS}) - target_link_libraries(${TEST_NAME} cartographer) - target_link_libraries(${TEST_NAME} ${PROJECT_NAME}) - set_target_properties(${TEST_NAME} PROPERTIES COMPILE_FLAGS ${TARGET_COMPILE_FLAGS}) - # Needed for dynamically linked GTest on Windows. - if (WIN32) - target_compile_definitions(${TEST_NAME} PUBLIC -DGTEST_LINKED_AS_SHARED_LIBRARY) - endif() - endforeach() -endif() - -install(DIRECTORY launch urdf configuration_files - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +#if (CATKIN_ENABLE_TESTING) +# foreach(TEST_SOURCE_FILENAME ${ALL_TESTS}) +# get_filename_component(TEST_NAME ${TEST_SOURCE_FILENAME} NAME_WE) +# catkin_add_gtest(${TEST_NAME} ${TEST_SOURCE_FILENAME}) +# # catkin_add_gtest uses a plain (i.e. no PUBLIC/PRIVATE/INTERFACE) call to +# # target_link_libraries. That forces us to do the same. +# target_link_libraries(${TEST_NAME} ${GMOCK_LIBRARIES} ${GTEST_MAIN_LIBRARIES}) +# target_include_directories(${TEST_NAME} SYSTEM PUBLIC ${LUA_INCLUDE_DIR}) +# target_link_libraries(${TEST_NAME} ${LUA_LIBRARIES}) +# target_include_directories(${TEST_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS}) +# target_link_libraries(${TEST_NAME} ${catkin_LIBRARIES}) +# add_dependencies(${TEST_NAME} ${catkin_EXPORTED_TARGETS}) +# target_link_libraries(${TEST_NAME} cartographer) +# target_link_libraries(${TEST_NAME} ${PROJECT_NAME}) +# set_target_properties(${TEST_NAME} PROPERTIES COMPILE_FLAGS ${TARGET_COMPILE_FLAGS}) +# # Needed for dynamically linked GTest on Windows. +# if (WIN32) +# target_compile_definitions(${TEST_NAME} PUBLIC -DGTEST_LINKED_AS_SHARED_LIBRARY) +# endif() +# endforeach() +#endif() + +install(DIRECTORY configuration_files + DESTINATION share/${PROJECT_NAME}/ ) -install(PROGRAMS scripts/tf_remove_frames.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -) - -# Install source headers. -file(GLOB_RECURSE HDRS "cartographer_ros/*.h") -foreach(HDR ${HDRS}) - file(RELATIVE_PATH REL_FIL ${PROJECT_SOURCE_DIR} ${HDR}) - get_filename_component(INSTALL_DIR ${REL_FIL} DIRECTORY) - install( - FILES - ${HDR} - DESTINATION - include/${INSTALL_DIR} - ) -endforeach() +install(TARGETS + ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + + +# ros1 install +#install(DIRECTORY launch urdf configuration_files +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +#) + +#install(PROGRAMS scripts/tf_remove_frames.py +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +#) + +#install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +#) + +## Install source headers. +#file(GLOB_RECURSE HDRS "cartographer_ros/*.h") +#foreach(HDR ${HDRS}) +# file(RELATIVE_PATH REL_FIL ${PROJECT_SOURCE_DIR} ${HDR}) +# get_filename_component(INSTALL_DIR ${REL_FIL} DIRECTORY) +# install( +# FILES +# ${HDR} +# DESTINATION +# include/${INSTALL_DIR} +# ) +#endforeach() + +ament_package() diff --git a/cartographer_ros/cartographer_ros/CMakeLists.txt b/cartographer_ros/cartographer_ros/CMakeLists.txt index f2616d124..9252e9d0b 100644 --- a/cartographer_ros/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/cartographer_ros/CMakeLists.txt @@ -12,28 +12,14 @@ # See the License for the specific language governing permissions and # limitations under the License. -google_binary(cartographer_assets_writer - SRCS - assets_writer_main.cc - ros_map_writing_points_processor.h - ros_map_writing_points_processor.cc -) - -install(TARGETS cartographer_assets_writer - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - google_binary(cartographer_node SRCS node_main.cc ) -install(TARGETS cartographer_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +google_binary(cartographer_occupancy_grid_node + SRCS + occupancy_grid_node_main.cc ) google_binary(cartographer_offline_node @@ -41,21 +27,21 @@ google_binary(cartographer_offline_node offline_node_main.cc ) -install(TARGETS cartographer_offline_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +google_binary(cartographer_assets_writer + SRCS + assets_writer_main.cc + ros_map_writing_points_processor.h + ros_map_writing_points_processor.cc ) -google_binary(cartographer_occupancy_grid_node +google_binary(cartographer_pbstream_map_publisher SRCS - occupancy_grid_node_main.cc + pbstream_map_publisher_main.cc ) -install(TARGETS cartographer_occupancy_grid_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +google_binary(cartographer_pbstream_to_ros_map + SRCS + pbstream_to_ros_map_main.cc ) google_binary(cartographer_rosbag_validate @@ -63,93 +49,49 @@ google_binary(cartographer_rosbag_validate rosbag_validate_main.cc ) -install(TARGETS cartographer_rosbag_validate - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) +install(TARGETS + cartographer_node + cartographer_occupancy_grid_node + cartographer_offline_node + cartographer_assets_writer + cartographer_pbstream_map_publisher + cartographer_pbstream_to_ros_map + cartographer_rosbag_validate + DESTINATION lib/${PROJECT_NAME}) -google_binary(cartographer_pbstream_to_ros_map - SRCS - pbstream_to_ros_map_main.cc -) -install(TARGETS cartographer_pbstream_to_ros_map - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -google_binary(cartographer_pbstream_map_publisher - SRCS - pbstream_map_publisher_main.cc -) +#google_binary(cartographer_dev_pbstream_trajectories_to_rosbag +# SRCS +# dev/pbstream_trajectories_to_rosbag_main.cc +#) -install(TARGETS cartographer_pbstream_map_publisher - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) +#google_binary(cartographer_dev_rosbag_publisher +# SRCS +# dev/rosbag_publisher_main.cc +#) -google_binary(cartographer_dev_pbstream_trajectories_to_rosbag - SRCS - dev/pbstream_trajectories_to_rosbag_main.cc -) -install(TARGETS cartographer_dev_pbstream_trajectories_to_rosbag - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) +#google_binary(cartographer_dev_trajectory_comparison +# SRCS +# dev/trajectory_comparison_main.cc +#) -google_binary(cartographer_dev_rosbag_publisher - SRCS - dev/rosbag_publisher_main.cc -) -install(TARGETS cartographer_dev_rosbag_publisher - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) +## TODO(cschuet): Add support for shared library case. +#if (${BUILD_GRPC}) +# google_binary(cartographer_grpc_node +# SRCS +# cartographer_grpc/node_grpc_main.cc +# ) -google_binary(cartographer_dev_trajectory_comparison - SRCS - dev/trajectory_comparison_main.cc -) -install(TARGETS cartographer_dev_trajectory_comparison - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) +# google_binary(cartographer_grpc_offline_node +# SRCS +# cartographer_grpc/offline_node_grpc_main.cc +# ) -# TODO(cschuet): Add support for shared library case. -if (${BUILD_GRPC}) - google_binary(cartographer_grpc_node - SRCS - cartographer_grpc/node_grpc_main.cc - ) - - install(TARGETS cartographer_grpc_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) - - google_binary(cartographer_grpc_offline_node - SRCS - cartographer_grpc/offline_node_grpc_main.cc - ) - - install(TARGETS cartographer_grpc_offline_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) - - install(PROGRAMS - ../scripts/cartographer_grpc_server.sh - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) -endif() +# install(PROGRAMS +# ../scripts/cartographer_grpc_server.sh +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) +#endif() diff --git a/cartographer_ros/cartographer_ros/assets_writer.cc b/cartographer_ros/cartographer_ros/assets_writer.cc index 1ca3e099f..55e9a66fa 100644 --- a/cartographer_ros/cartographer_ros/assets_writer.cc +++ b/cartographer_ros/cartographer_ros/assets_writer.cc @@ -39,12 +39,11 @@ #include "cartographer_ros/urdf_reader.h" #include "gflags/gflags.h" #include "glog/logging.h" -#include "ros/ros.h" -#include "ros/time.h" -#include "rosbag/bag.h" -#include "rosbag/view.h" +#include "builtin_interfaces/msg/time.hpp" +#include +#include #include "tf2_eigen/tf2_eigen.h" -#include "tf2_msgs/TFMessage.h" +#include "tf2_msgs/msg/tf_message.hpp" #include "tf2_ros/buffer.h" #include "urdf/model.h" @@ -92,19 +91,19 @@ std::unique_ptr LoadLuaDictionary( template std::unique_ptr HandleMessage( const T& message, const std::string& tracking_frame, - const tf2_ros::Buffer& tf_buffer, + const std::shared_ptr tf_buffer, const carto::transform::TransformInterpolationBuffer& transform_interpolation_buffer) { - const carto::common::Time start_time = FromRos(message.header.stamp); + const carto::common::Time start_time = FromRos(message->header.stamp); auto points_batch = absl::make_unique(); points_batch->start_time = start_time; - points_batch->frame_id = message.header.frame_id; + points_batch->frame_id = message->header.frame_id; carto::sensor::PointCloudWithIntensities point_cloud; carto::common::Time point_cloud_time; std::tie(point_cloud, point_cloud_time) = - ToPointCloudWithIntensities(message); + ToPointCloudWithIntensities(*message); CHECK_EQ(point_cloud.intensities.size(), point_cloud.points.size()); for (size_t i = 0; i < point_cloud.points.size(); ++i) { @@ -117,8 +116,8 @@ std::unique_ptr HandleMessage( const carto::transform::Rigid3d tracking_to_map = transform_interpolation_buffer.Lookup(time); const carto::transform::Rigid3d sensor_to_tracking = - ToRigid3d(tf_buffer.lookupTransform( - tracking_frame, message.header.frame_id, ToRos(time))); + ToRigid3d(tf_buffer->lookupTransform( + tracking_frame, message->header.frame_id, ToRos(time))); const carto::transform::Rigid3f sensor_to_map = (tracking_to_map * sensor_to_tracking).cast(); points_batch->points.push_back( @@ -179,6 +178,7 @@ void AssetsWriter::Run(const std::string& configuration_directory, const std::string tracking_frame = lua_parameter_dictionary->GetString("tracking_frame"); + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); do { for (size_t trajectory_id = 0; trajectory_id < bag_filenames_.size(); ++trajectory_id) { @@ -189,73 +189,96 @@ void AssetsWriter::Run(const std::string& configuration_directory, if (trajectory_proto.node_size() == 0) { continue; } - tf2_ros::Buffer tf_buffer; + + std::shared_ptr tf_buffer = std::make_shared(clock); + tf_buffer->setUsingDedicatedThread(true); + if (!urdf_filename.empty()) { - ReadStaticTransformsFromUrdf(urdf_filename, &tf_buffer); + ReadStaticTransformsFromUrdf(urdf_filename, tf_buffer); } const carto::transform::TransformInterpolationBuffer transform_interpolation_buffer(trajectory_proto); - rosbag::Bag bag; - bag.open(bag_filename, rosbag::bagmode::Read); - rosbag::View view(bag); - const ::ros::Time begin_time = view.getBeginTime(); - const double duration_in_seconds = - (view.getEndTime() - begin_time).toSec(); + rosbag2_cpp::Reader bag_reader; + bag_reader.open(bag_filename); + rosbag2_storage::BagMetadata bag_metadata = bag_reader.get_metadata(); + const rclcpp::Time begin_time(bag_metadata.starting_time.time_since_epoch().count()); // We need to keep 'tf_buffer' small because it becomes very inefficient // otherwise. We make sure that tf_messages are published before any data // messages, so that tf lookups always work. - std::deque delayed_messages; + std::deque delayed_messages; // We publish tf messages one second earlier than other messages. Under // the assumption of higher frequency tf this should ensure that tf can // always interpolate. - const ::ros::Duration kDelay(1.); - for (const rosbag::MessageInstance& message : view) { - if (use_bag_transforms && message.isType()) { - auto tf_message = message.instantiate(); - for (const auto& transform : tf_message->transforms) { + const rclcpp::Duration kDelay(1.0,0.0); + auto serializer = rclcpp::Serialization(); + auto laser_scan_serializer = rclcpp::Serialization(); + auto multi_echo_laser_scan_serializer = rclcpp::Serialization(); + auto pcl2_serializer = rclcpp::Serialization(); + while (bag_reader.has_next()) { + auto message = bag_reader.read_next(); + if (use_bag_transforms && (message->topic_name == kTfStaticTopic || message->topic_name == "/tf")) { + tf2_msgs::msg::TFMessage tf_message; + rclcpp::SerializedMessage serialized_msg(*message->serialized_data); + serializer.deserialize_message(&serialized_msg, &tf_message); + for (const auto& transform : tf_message.transforms) { try { - tf_buffer.setTransform(transform, "unused_authority", - message.getTopic() == kTfStaticTopic); + tf_buffer->setTransform(transform, "unused_authority", + message->topic_name == kTfStaticTopic); } catch (const tf2::TransformException& ex) { LOG(WARNING) << ex.what(); } } } - while (!delayed_messages.empty() && delayed_messages.front().getTime() < - message.getTime() - kDelay) { - const rosbag::MessageInstance& delayed_message = + while (!delayed_messages.empty() && delayed_messages.front().time_stamp < + message->time_stamp - kDelay.nanoseconds()) { + const auto delayed_message = delayed_messages.front(); std::unique_ptr points_batch; - if (delayed_message.isType()) { - points_batch = HandleMessage( - *delayed_message.instantiate(), - tracking_frame, tf_buffer, transform_interpolation_buffer); - } else if (delayed_message - .isType()) { - points_batch = HandleMessage( - *delayed_message.instantiate(), - tracking_frame, tf_buffer, transform_interpolation_buffer); - } else if (delayed_message.isType()) { - points_batch = HandleMessage( - *delayed_message.instantiate(), - tracking_frame, tf_buffer, transform_interpolation_buffer); - } - if (points_batch != nullptr) { - points_batch->trajectory_id = trajectory_id; - pipeline.back()->Process(std::move(points_batch)); + + for (auto topic_info : bag_metadata.topics_with_message_count) { + if (topic_info.topic_metadata.name == delayed_message.topic_name){ + rclcpp::SerializedMessage serialized_msg(*delayed_message.serialized_data); + if (topic_info.topic_metadata.type == "sensor_msgs/msg/LaserScan") { + sensor_msgs::msg::LaserScan::SharedPtr laser_scan_msg = + std::make_shared(); + laser_scan_serializer.deserialize_message(&serialized_msg, laser_scan_msg.get()); + points_batch = HandleMessage( + laser_scan_msg, + tracking_frame, tf_buffer, transform_interpolation_buffer); + } else if (topic_info.topic_metadata.type == "sensor_msgs/msg/MultiEchoLaserScan") { + sensor_msgs::msg::MultiEchoLaserScan::SharedPtr multi_echo_laser_scan_msg = + std::make_shared(); + multi_echo_laser_scan_serializer.deserialize_message(&serialized_msg, &multi_echo_laser_scan_msg); + points_batch = HandleMessage( + multi_echo_laser_scan_msg, + tracking_frame, tf_buffer, transform_interpolation_buffer); + } + else if (topic_info.topic_metadata.type == "sensor_msgs/msg/PointCloud2") { + sensor_msgs::msg::PointCloud2::SharedPtr pcl2_scan_msg = + std::make_shared(); + pcl2_serializer.deserialize_message(&serialized_msg, &pcl2_scan_msg); + points_batch = HandleMessage( + pcl2_scan_msg, + tracking_frame, tf_buffer, transform_interpolation_buffer); + } + if (points_batch != nullptr) { + points_batch->trajectory_id = trajectory_id; + pipeline.back()->Process(std::move(points_batch)); + } + delayed_messages.pop_front(); + break; + } } - delayed_messages.pop_front(); } - delayed_messages.push_back(message); + delayed_messages.push_back(*message); LOG_EVERY_N(INFO, 10000) - << "Processed " << (message.getTime() - begin_time).toSec() - << " of " << duration_in_seconds << " bag time seconds..."; + << "Processed " << (message->time_stamp - begin_time.nanoseconds())/1e9 + << " of " << bag_metadata.duration.count()/1e9 << " bag time seconds..."; } - bag.close(); } } while (pipeline.back()->Flush() == carto::io::PointsProcessor::FlushResult::kRestartStream); diff --git a/cartographer_ros/cartographer_ros/assets_writer.h b/cartographer_ros/cartographer_ros/assets_writer.h index c12a87f07..4e3ec5149 100644 --- a/cartographer_ros/cartographer_ros/assets_writer.h +++ b/cartographer_ros/cartographer_ros/assets_writer.h @@ -21,6 +21,7 @@ #include "cartographer/io/points_processor_pipeline_builder.h" #include "cartographer/mapping/proto/pose_graph.pb.h" #include "cartographer/mapping/proto/trajectory_builder_options.pb.h" +#include #ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ASSETS_WRITER_H #define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ASSETS_WRITER_H diff --git a/cartographer_ros/cartographer_ros/assets_writer_main.cc b/cartographer_ros/cartographer_ros/assets_writer_main.cc index 238f0df7b..f9930555a 100644 --- a/cartographer_ros/cartographer_ros/assets_writer_main.cc +++ b/cartographer_ros/cartographer_ros/assets_writer_main.cc @@ -14,10 +14,10 @@ * limitations under the License. */ -#include "absl/strings/str_split.h" #include "cartographer_ros/assets_writer.h" #include "gflags/gflags.h" #include "glog/logging.h" +#include DEFINE_string(configuration_directory, "", "First directory in which configuration files are searched, " @@ -42,9 +42,13 @@ DEFINE_string(output_file_prefix, "", "will be used."); int main(int argc, char** argv) { + // Init rclcpp first because gflags reorders command line flags in argv + rclcpp::init(argc, argv); + FLAGS_alsologtostderr = true; + google::AllowCommandLineReparsing(); google::InitGoogleLogging(argv[0]); - google::ParseCommandLineFlags(&argc, &argv, true); + google::ParseCommandLineFlags(&argc, &argv, false); CHECK(!FLAGS_configuration_directory.empty()) << "-configuration_directory is missing."; @@ -54,11 +58,15 @@ int main(int argc, char** argv) { CHECK(!FLAGS_pose_graph_filename.empty()) << "-pose_graph_filename is missing."; + std::vector bag_filenames; + boost::split(bag_filenames, FLAGS_bag_filenames, boost::is_any_of(",")); + ::cartographer_ros::AssetsWriter asset_writer( FLAGS_pose_graph_filename, - absl::StrSplit(FLAGS_bag_filenames, ',', absl::SkipEmpty()), + bag_filenames, FLAGS_output_file_prefix); asset_writer.Run(FLAGS_configuration_directory, FLAGS_configuration_basename, FLAGS_urdf_filename, FLAGS_use_bag_transforms); + rclcpp::shutdown(); } diff --git a/cartographer_ros/cartographer_ros/dev/pbstream_trajectories_to_rosbag_main.cc b/cartographer_ros/cartographer_ros/dev/pbstream_trajectories_to_rosbag_main.cc index 63d2c5f24..27da714ff 100644 --- a/cartographer_ros/cartographer_ros/dev/pbstream_trajectories_to_rosbag_main.cc +++ b/cartographer_ros/cartographer_ros/dev/pbstream_trajectories_to_rosbag_main.cc @@ -35,12 +35,12 @@ DEFINE_string(parent_frame, "map", "Frame id to use as parent frame."); namespace cartographer_ros { namespace { -geometry_msgs::TransformStamped ToTransformStamped( +geometry_msgs::msg::TransformStamped ToTransformStamped( int64_t timestamp_uts, const std::string& parent_frame_id, const std::string& child_frame_id, const cartographer::transform::proto::Rigid3d& parent_T_child) { static int64_t seq = 0; - geometry_msgs::TransformStamped transform_stamped; + geometry_msgs::msg::TransformStamped transform_stamped; transform_stamped.header.seq = ++seq; transform_stamped.header.frame_id = parent_frame_id; transform_stamped.header.stamp = cartographer_ros::ToRos( @@ -67,7 +67,7 @@ void pbstream_trajectories_to_bag(const std::string& pbstream_filename, << " nodes."; for (const auto& node : trajectory.node()) { tf2_msgs::TFMessage tf_msg; - geometry_msgs::TransformStamped transform_stamped = ToTransformStamped( + geometry_msgs::msg::TransformStamped transform_stamped = ToTransformStamped( node.timestamp(), parent_frame_id, child_frame_id, node.pose()); tf_msg.transforms.push_back(transform_stamped); bag.write(child_frame_id, transform_stamped.header.stamp, diff --git a/cartographer_ros/cartographer_ros/dev/rosbag_publisher_main.cc b/cartographer_ros/cartographer_ros/dev/rosbag_publisher_main.cc index e0694820c..921710716 100644 --- a/cartographer_ros/cartographer_ros/dev/rosbag_publisher_main.cc +++ b/cartographer_ros/cartographer_ros/dev/rosbag_publisher_main.cc @@ -37,7 +37,7 @@ template void PublishWithModifiedTimestamp(MessagePtrType message, const ros::Publisher& publisher, ros::Duration bag_to_current) { - ros::Time& stamp = message->header.stamp; + rclcpp::Time& stamp = message->header.stamp; stamp += bag_to_current; publisher.publish(message); } @@ -47,7 +47,7 @@ void PublishWithModifiedTimestamp( tf2_msgs::TFMessage::Ptr message, const ros::Publisher& publisher, ros::Duration bag_to_current) { for (const auto& transform : message->transforms) { - ros::Time& stamp = const_cast(transform.header.stamp); + rclcpp::Time& stamp = const_cast(transform.header.stamp); stamp += bag_to_current; } publisher.publish(message); @@ -81,7 +81,7 @@ int main(int argc, char** argv) { node_handle.getParam("/use_sim_time", use_sim_time); if (use_sim_time) { LOG(ERROR) << "use_sim_time is true but not supported. Expect conflicting " - "ros::Time and message header times or weird behavior."; + "rclcpp::Time and message header times or weird behavior."; } std::map topic_to_publisher; for (const rosbag::ConnectionInfo* c : view.getConnections()) { @@ -95,35 +95,35 @@ int main(int argc, char** argv) { ros::Duration(1).sleep(); CHECK(ros::ok()); - ros::Time current_start = ros::Time::now(); - ros::Time bag_start = view.getBeginTime(); + rclcpp::Time current_start = rclcpp::Clock().now(); + rclcpp::Time bag_start = view.getBeginTime(); ros::Duration bag_to_current = current_start - bag_start; for (const rosbag::MessageInstance& message : view) { ros::Duration after_bag_start = message.getTime() - bag_start; if (!::ros::ok()) { break; } - ros::Time planned_publish_time = current_start + after_bag_start; - ros::Time::sleepUntil(planned_publish_time); + rclcpp::Time planned_publish_time = current_start + after_bag_start; + rclcpp::Time::sleepUntil(planned_publish_time); ros::Publisher& publisher = topic_to_publisher.at(message.getTopic()); - if (message.isType()) { + if (message.isType()) { PublishWithModifiedTimestamp( - message.instantiate(), publisher, + message.instantiate(), publisher, bag_to_current); - } else if (message.isType()) { + } else if (message.isType()) { PublishWithModifiedTimestamp( - message.instantiate(), publisher, + message.instantiate(), publisher, bag_to_current); - } else if (message.isType()) { + } else if (message.isType()) { PublishWithModifiedTimestamp( - message.instantiate(), publisher, + message.instantiate(), publisher, bag_to_current); - } else if (message.isType()) { - PublishWithModifiedTimestamp(message.instantiate(), + } else if (message.isType()) { + PublishWithModifiedTimestamp(message.instantiate(), publisher, bag_to_current); - } else if (message.isType()) { - PublishWithModifiedTimestamp(message.instantiate(), + } else if (message.isType()) { + PublishWithModifiedTimestamp(message.instantiate(), publisher, bag_to_current); } else if (message.isType()) { PublishWithModifiedTimestamp(message.instantiate(), @@ -132,7 +132,7 @@ int main(int argc, char** argv) { LOG(WARNING) << "Skipping message with type " << message.getDataType(); } - ros::Time current_time = ros::Time::now(); + rclcpp::Time current_time = rclcpp::Clock().now(); double simulation_delay = cartographer::common::ToSeconds( cartographer_ros::FromRos(current_time) - cartographer_ros::FromRos(planned_publish_time)); diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index c89a4f0f8..4ea2b191d 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -17,13 +17,12 @@ #include "cartographer_ros/map_builder_bridge.h" #include "absl/memory/memory.h" -#include "absl/strings/str_cat.h" #include "cartographer/io/color.h" #include "cartographer/io/proto_stream.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" +#include "cartographer_ros_msgs/msg/status_code.hpp" +#include "cartographer_ros_msgs/msg/status_response.hpp" namespace cartographer_ros { namespace { @@ -34,8 +33,8 @@ constexpr double kTrajectoryLineStripMarkerScale = 0.07; constexpr double kLandmarkMarkerScale = 0.2; constexpr double kConstraintMarkerScale = 0.025; -::std_msgs::ColorRGBA ToMessage(const cartographer::io::FloatColor& color) { - ::std_msgs::ColorRGBA result; +::std_msgs::msg::ColorRGBA ToMessage(const cartographer::io::FloatColor& color) { + ::std_msgs::msg::ColorRGBA result; result.r = color[0]; result.g = color[1]; result.b = color[2]; @@ -43,13 +42,14 @@ ::std_msgs::ColorRGBA ToMessage(const cartographer::io::FloatColor& color) { return result; } -visualization_msgs::Marker CreateTrajectoryMarker(const int trajectory_id, - const std::string& frame_id) { - visualization_msgs::Marker marker; - marker.ns = absl::StrCat("Trajectory ", trajectory_id); +visualization_msgs::msg::Marker CreateTrajectoryMarker(const int trajectory_id, + const std::string& frame_id, + rclcpp::Time node_time) { + visualization_msgs::msg::Marker marker; + marker.ns = "Trajectory " + std::to_string(trajectory_id); marker.id = 0; - marker.type = visualization_msgs::Marker::LINE_STRIP; - marker.header.stamp = ::ros::Time::now(); + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.header.stamp = node_time; marker.header.frame_id = frame_id; marker.color = ToMessage(cartographer::io::GetColor(trajectory_id)); marker.scale.x = kTrajectoryLineStripMarkerScale; @@ -70,14 +70,15 @@ int GetLandmarkIndex( return it->second; } -visualization_msgs::Marker CreateLandmarkMarker(int landmark_index, +visualization_msgs::msg::Marker CreateLandmarkMarker(int landmark_index, const Rigid3d& landmark_pose, - const std::string& frame_id) { - visualization_msgs::Marker marker; + const std::string& frame_id, + rclcpp::Time node_time) { + visualization_msgs::msg::Marker marker; marker.ns = "Landmarks"; marker.id = landmark_index; - marker.type = visualization_msgs::Marker::SPHERE; - marker.header.stamp = ::ros::Time::now(); + marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.header.stamp = node_time; marker.header.frame_id = frame_id; marker.scale.x = kLandmarkMarkerScale; marker.scale.y = kLandmarkMarkerScale; @@ -87,8 +88,8 @@ visualization_msgs::Marker CreateLandmarkMarker(int landmark_index, return marker; } -void PushAndResetLineMarker(visualization_msgs::Marker* marker, - std::vector* markers) { +void PushAndResetLineMarker(visualization_msgs::msg::Marker* marker, + std::vector* markers) { markers->push_back(*marker); ++marker->id; marker->points.clear(); @@ -168,24 +169,24 @@ bool MapBuilderBridge::SerializeState(const std::string& filename, } void MapBuilderBridge::HandleSubmapQuery( - cartographer_ros_msgs::SubmapQuery::Request& request, - cartographer_ros_msgs::SubmapQuery::Response& response) { + const cartographer_ros_msgs::srv::SubmapQuery::Request::SharedPtr request, + cartographer_ros_msgs::srv::SubmapQuery::Response::SharedPtr response) { cartographer::mapping::proto::SubmapQuery::Response response_proto; - cartographer::mapping::SubmapId submap_id{request.trajectory_id, - request.submap_index}; + cartographer::mapping::SubmapId submap_id{request->trajectory_id, + request->submap_index}; const std::string error = map_builder_->SubmapToProto(submap_id, &response_proto); if (!error.empty()) { LOG(ERROR) << error; - response.status.code = cartographer_ros_msgs::StatusCode::NOT_FOUND; - response.status.message = error; + response->status.code = cartographer_ros_msgs::msg::StatusCode::NOT_FOUND; + response->status.message = error; return; } - response.submap_version = response_proto.submap_version(); + response->submap_version = response_proto.submap_version(); for (const auto& texture_proto : response_proto.textures()) { - response.textures.emplace_back(); - auto& texture = response.textures.back(); + response->textures.emplace_back(); + auto& texture = response->textures.back(); texture.cells.insert(texture.cells.begin(), texture_proto.cells().begin(), texture_proto.cells().end()); texture.width = texture_proto.width(); @@ -194,8 +195,8 @@ void MapBuilderBridge::HandleSubmapQuery( texture.slice_pose = ToGeometryMsgPose( cartographer::transform::ToRigid3(texture_proto.slice_pose())); } - response.status.message = "Success."; - response.status.code = cartographer_ros_msgs::StatusCode::OK; + response->status.message = "Success."; + response->status.code = cartographer_ros_msgs::msg::StatusCode::OK; } std::map @@ -211,13 +212,13 @@ MapBuilderBridge::GetTrajectoryStates() { return trajectory_states; } -cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() { - cartographer_ros_msgs::SubmapList submap_list; - submap_list.header.stamp = ::ros::Time::now(); +cartographer_ros_msgs::msg::SubmapList MapBuilderBridge::GetSubmapList(rclcpp::Time node_time) { + cartographer_ros_msgs::msg::SubmapList submap_list; + submap_list.header.stamp = node_time; submap_list.header.frame_id = node_options_.map_frame; for (const auto& submap_id_pose : map_builder_->pose_graph()->GetAllSubmapPoses()) { - cartographer_ros_msgs::SubmapEntry submap_entry; + cartographer_ros_msgs::msg::SubmapEntry submap_entry; submap_entry.is_frozen = map_builder_->pose_graph()->IsTrajectoryFrozen( submap_id_pose.id.trajectory_id); submap_entry.trajectory_id = submap_id_pose.id.trajectory_id; @@ -259,31 +260,33 @@ MapBuilderBridge::GetLocalTrajectoryData() { } void MapBuilderBridge::HandleTrajectoryQuery( - cartographer_ros_msgs::TrajectoryQuery::Request& request, - cartographer_ros_msgs::TrajectoryQuery::Response& response) { + const cartographer_ros_msgs::srv::TrajectoryQuery::Request::SharedPtr request, + cartographer_ros_msgs::srv::TrajectoryQuery::Response::SharedPtr 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)) { + node_poses.trajectory(request->trajectory_id)) { if (!node_id_data.data.constant_pose_data.has_value()) { continue; } - geometry_msgs::PoseStamped pose_stamped; + geometry_msgs::msg::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->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, "."); + response->status.code = cartographer_ros_msgs::msg::StatusCode::OK; + response->status.message = + "Retrieved " + std::to_string(response->trajectory.size()) + + " trajectory nodes from trajectory " + std::to_string(request->trajectory_id) + "."; } -visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() { - visualization_msgs::MarkerArray trajectory_node_list; +visualization_msgs::msg::MarkerArray MapBuilderBridge::GetTrajectoryNodeList( + rclcpp::Time node_time) +{ + visualization_msgs::msg::MarkerArray trajectory_node_list; const auto node_poses = map_builder_->pose_graph()->GetTrajectoryNodePoses(); // Find the last node indices for each trajectory that have either // inter-submap or inter-trajectory constraints. @@ -317,8 +320,8 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() { } for (const int trajectory_id : node_poses.trajectory_ids()) { - visualization_msgs::Marker marker = - CreateTrajectoryMarker(trajectory_id, node_options_.map_frame); + visualization_msgs::msg::Marker marker = + CreateTrajectoryMarker(trajectory_id, node_options_.map_frame, node_time); int last_inter_submap_constrained_node = std::max( node_poses.trajectory(trajectory_id).begin()->id.node_index, trajectory_to_last_inter_submap_constrained_node.at(trajectory_id)); @@ -342,7 +345,7 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() { PushAndResetLineMarker(&marker, &trajectory_node_list.markers); continue; } - const ::geometry_msgs::Point node_point = + const ::geometry_msgs::msg::Point node_point = ToGeometryMsgPoint(node_id_data.data.global_pose.translation()); marker.points.push_back(node_point); @@ -370,7 +373,7 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() { if (trajectory_to_highest_marker_id_.count(trajectory_id) == 0) { trajectory_to_highest_marker_id_[trajectory_id] = current_last_marker_id; } else { - marker.action = visualization_msgs::Marker::DELETE; + marker.action = visualization_msgs::msg::Marker::DELETE; while (static_cast(marker.id) <= trajectory_to_highest_marker_id_[trajectory_id]) { trajectory_node_list.markers.push_back(marker); @@ -382,31 +385,33 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() { return trajectory_node_list; } -visualization_msgs::MarkerArray MapBuilderBridge::GetLandmarkPosesList() { - visualization_msgs::MarkerArray landmark_poses_list; +visualization_msgs::msg::MarkerArray MapBuilderBridge::GetLandmarkPosesList( + rclcpp::Time node_time) +{ + visualization_msgs::msg::MarkerArray landmark_poses_list; const std::map landmark_poses = map_builder_->pose_graph()->GetLandmarkPoses(); for (const auto& id_to_pose : landmark_poses) { landmark_poses_list.markers.push_back(CreateLandmarkMarker( GetLandmarkIndex(id_to_pose.first, &landmark_to_index_), - id_to_pose.second, node_options_.map_frame)); + id_to_pose.second, node_options_.map_frame, node_time)); } return landmark_poses_list; } -visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() { - visualization_msgs::MarkerArray constraint_list; +visualization_msgs::msg::MarkerArray MapBuilderBridge::GetConstraintList(rclcpp::Time node_time) { + visualization_msgs::msg::MarkerArray constraint_list; int marker_id = 0; - visualization_msgs::Marker constraint_intra_marker; + visualization_msgs::msg::Marker constraint_intra_marker; constraint_intra_marker.id = marker_id++; constraint_intra_marker.ns = "Intra constraints"; - constraint_intra_marker.type = visualization_msgs::Marker::LINE_LIST; - constraint_intra_marker.header.stamp = ros::Time::now(); + constraint_intra_marker.type = visualization_msgs::msg::Marker::LINE_LIST; + constraint_intra_marker.header.stamp = node_time; constraint_intra_marker.header.frame_id = node_options_.map_frame; constraint_intra_marker.scale.x = kConstraintMarkerScale; constraint_intra_marker.pose.orientation.w = 1.0; - visualization_msgs::Marker residual_intra_marker = constraint_intra_marker; + visualization_msgs::msg::Marker residual_intra_marker = constraint_intra_marker; residual_intra_marker.id = marker_id++; residual_intra_marker.ns = "Intra residuals"; // This and other markers which are less numerous are set to be slightly @@ -414,27 +419,27 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() { // visible. residual_intra_marker.pose.position.z = 0.1; - visualization_msgs::Marker constraint_inter_same_trajectory_marker = + visualization_msgs::msg::Marker constraint_inter_same_trajectory_marker = constraint_intra_marker; constraint_inter_same_trajectory_marker.id = marker_id++; constraint_inter_same_trajectory_marker.ns = "Inter constraints, same trajectory"; constraint_inter_same_trajectory_marker.pose.position.z = 0.1; - visualization_msgs::Marker residual_inter_same_trajectory_marker = + visualization_msgs::msg::Marker residual_inter_same_trajectory_marker = constraint_intra_marker; residual_inter_same_trajectory_marker.id = marker_id++; residual_inter_same_trajectory_marker.ns = "Inter residuals, same trajectory"; residual_inter_same_trajectory_marker.pose.position.z = 0.1; - visualization_msgs::Marker constraint_inter_diff_trajectory_marker = + visualization_msgs::msg::Marker constraint_inter_diff_trajectory_marker = constraint_intra_marker; constraint_inter_diff_trajectory_marker.id = marker_id++; constraint_inter_diff_trajectory_marker.ns = "Inter constraints, different trajectories"; constraint_inter_diff_trajectory_marker.pose.position.z = 0.1; - visualization_msgs::Marker residual_inter_diff_trajectory_marker = + visualization_msgs::msg::Marker residual_inter_diff_trajectory_marker = constraint_intra_marker; residual_inter_diff_trajectory_marker.id = marker_id++; residual_inter_diff_trajectory_marker.ns = @@ -447,8 +452,8 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() { const auto constraints = map_builder_->pose_graph()->constraints(); for (const auto& constraint : constraints) { - visualization_msgs::Marker *constraint_marker, *residual_marker; - std_msgs::ColorRGBA color_constraint, color_residual; + visualization_msgs::msg::Marker *constraint_marker, *residual_marker; + std_msgs::msg::ColorRGBA color_constraint, color_residual; if (constraint.tag == cartographer::mapping::PoseGraphInterface::Constraint::INTRA_SUBMAP) { constraint_marker = &constraint_intra_marker; diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.h b/cartographer_ros/cartographer_ros/map_builder_bridge.h index 1a54e6502..b2c00b751 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.h +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.h @@ -31,19 +31,19 @@ #include "cartographer_ros/sensor_bridge.h" #include "cartographer_ros/tf_bridge.h" #include "cartographer_ros/trajectory_options.h" -#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" +#include "cartographer_ros_msgs/msg/submap_entry.hpp" +#include "cartographer_ros_msgs/msg/submap_list.hpp" +#include "cartographer_ros_msgs/srv/submap_query.hpp" +#include "cartographer_ros_msgs/srv/trajectory_query.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" // Abseil unfortunately pulls in winnt.h, which #defines DELETE. -// Clean up to unbreak visualization_msgs::Marker::DELETE. +// Clean up to unbreak visualization_msgs::msg::Marker::DELETE. #ifdef DELETE #undef DELETE #endif -#include "visualization_msgs/MarkerArray.h" +#include "visualization_msgs/msg/marker_array.hpp" namespace cartographer_ros { @@ -84,21 +84,21 @@ class MapBuilderBridge { const bool include_unfinished_submaps); void HandleSubmapQuery( - cartographer_ros_msgs::SubmapQuery::Request& request, - cartographer_ros_msgs::SubmapQuery::Response& response); + const cartographer_ros_msgs::srv::SubmapQuery::Request::SharedPtr request, + cartographer_ros_msgs::srv::SubmapQuery::Response::SharedPtr response); void HandleTrajectoryQuery( - cartographer_ros_msgs::TrajectoryQuery::Request& request, - cartographer_ros_msgs::TrajectoryQuery::Response& response); + const cartographer_ros_msgs::srv::TrajectoryQuery::Request::SharedPtr request, + cartographer_ros_msgs::srv::TrajectoryQuery::Response::SharedPtr response); std::map GetTrajectoryStates(); - cartographer_ros_msgs::SubmapList GetSubmapList(); + cartographer_ros_msgs::msg::SubmapList GetSubmapList(rclcpp::Time node_time); std::unordered_map GetLocalTrajectoryData() LOCKS_EXCLUDED(mutex_); - visualization_msgs::MarkerArray GetTrajectoryNodeList(); - visualization_msgs::MarkerArray GetLandmarkPosesList(); - visualization_msgs::MarkerArray GetConstraintList(); + visualization_msgs::msg::MarkerArray GetTrajectoryNodeList(rclcpp::Time node_time); + visualization_msgs::msg::MarkerArray GetLandmarkPosesList(rclcpp::Time node_time); + visualization_msgs::msg::MarkerArray GetConstraintList(rclcpp::Time node_time); SensorBridge* sensor_bridge(int trajectory_id); diff --git a/cartographer_ros/cartographer_ros/metrics/family_factory.cc b/cartographer_ros/cartographer_ros/metrics/family_factory.cc index 96df38179..388c226f5 100644 --- a/cartographer_ros/cartographer_ros/metrics/family_factory.cc +++ b/cartographer_ros/cartographer_ros/metrics/family_factory.cc @@ -53,7 +53,7 @@ FamilyFactory::NewHistogramFamily(const std::string& name, } void FamilyFactory::ReadMetrics( - ::cartographer_ros_msgs::ReadMetrics::Response* response) const { + cartographer_ros_msgs::srv::ReadMetrics::Response::SharedPtr response) const { for (const auto& counter_family : counter_families_) { response->metric_families.push_back(counter_family->ToRosMessage()); } diff --git a/cartographer_ros/cartographer_ros/metrics/family_factory.h b/cartographer_ros/cartographer_ros/metrics/family_factory.h index a05fbd5a7..0c941e5a3 100644 --- a/cartographer_ros/cartographer_ros/metrics/family_factory.h +++ b/cartographer_ros/cartographer_ros/metrics/family_factory.h @@ -25,7 +25,7 @@ #include "cartographer_ros/metrics/internal/family.h" #include "cartographer_ros/metrics/internal/gauge.h" #include "cartographer_ros/metrics/internal/histogram.h" -#include "cartographer_ros_msgs/ReadMetrics.h" +#include "cartographer_ros_msgs/srv/read_metrics.hpp" namespace cartographer_ros { namespace metrics { @@ -47,7 +47,7 @@ class FamilyFactory : public ::cartographer::metrics::FamilyFactory { boundaries) override; void ReadMetrics( - ::cartographer_ros_msgs::ReadMetrics::Response* response) const; + cartographer_ros_msgs::srv::ReadMetrics::Response::SharedPtr response) const; private: std::vector> counter_families_; diff --git a/cartographer_ros/cartographer_ros/metrics/internal/counter.h b/cartographer_ros/cartographer_ros/metrics/internal/counter.h index 52f35fe88..3ef5666d9 100644 --- a/cartographer_ros/cartographer_ros/metrics/internal/counter.h +++ b/cartographer_ros/cartographer_ros/metrics/internal/counter.h @@ -19,7 +19,7 @@ #include "cartographer/metrics/counter.h" #include "cartographer_ros/metrics/internal/gauge.h" -#include "cartographer_ros_msgs/Metric.h" +#include "cartographer_ros_msgs/msg/metric.hpp" namespace cartographer_ros { namespace metrics { @@ -35,9 +35,9 @@ class Counter : public ::cartographer::metrics::Counter { double Value() { return gauge_.Value(); } - cartographer_ros_msgs::Metric ToRosMessage() { - cartographer_ros_msgs::Metric msg = gauge_.ToRosMessage(); - msg.type = cartographer_ros_msgs::Metric::TYPE_COUNTER; + cartographer_ros_msgs::msg::Metric ToRosMessage() { + cartographer_ros_msgs::msg::Metric msg = gauge_.ToRosMessage(); + msg.type = cartographer_ros_msgs::msg::Metric::TYPE_COUNTER; return msg; } diff --git a/cartographer_ros/cartographer_ros/metrics/internal/family.cc b/cartographer_ros/cartographer_ros/metrics/internal/family.cc index 42f81c65f..29572a9e8 100644 --- a/cartographer_ros/cartographer_ros/metrics/internal/family.cc +++ b/cartographer_ros/cartographer_ros/metrics/internal/family.cc @@ -33,8 +33,8 @@ Counter* CounterFamily::Add(const std::map& labels) { return ptr; } -cartographer_ros_msgs::MetricFamily CounterFamily::ToRosMessage() { - cartographer_ros_msgs::MetricFamily family_msg; +cartographer_ros_msgs::msg::MetricFamily CounterFamily::ToRosMessage() { + cartographer_ros_msgs::msg::MetricFamily family_msg; family_msg.name = name_; family_msg.description = description_; for (const auto& wrapper : wrappers_) { @@ -50,8 +50,8 @@ Gauge* GaugeFamily::Add(const std::map& labels) { return ptr; } -cartographer_ros_msgs::MetricFamily GaugeFamily::ToRosMessage() { - cartographer_ros_msgs::MetricFamily family_msg; +cartographer_ros_msgs::msg::MetricFamily GaugeFamily::ToRosMessage() { + cartographer_ros_msgs::msg::MetricFamily family_msg; family_msg.name = name_; family_msg.description = description_; for (const auto& wrapper : wrappers_) { @@ -68,8 +68,8 @@ Histogram* HistogramFamily::Add( return ptr; } -cartographer_ros_msgs::MetricFamily HistogramFamily::ToRosMessage() { - cartographer_ros_msgs::MetricFamily family_msg; +cartographer_ros_msgs::msg::MetricFamily HistogramFamily::ToRosMessage() { + cartographer_ros_msgs::msg::MetricFamily family_msg; family_msg.name = name_; family_msg.description = description_; for (const auto& wrapper : wrappers_) { diff --git a/cartographer_ros/cartographer_ros/metrics/internal/family.h b/cartographer_ros/cartographer_ros/metrics/internal/family.h index 7e21f58d6..bcab328a1 100644 --- a/cartographer_ros/cartographer_ros/metrics/internal/family.h +++ b/cartographer_ros/cartographer_ros/metrics/internal/family.h @@ -24,7 +24,7 @@ #include "cartographer_ros/metrics/internal/counter.h" #include "cartographer_ros/metrics/internal/gauge.h" #include "cartographer_ros/metrics/internal/histogram.h" -#include "cartographer_ros_msgs/MetricFamily.h" +#include "cartographer_ros_msgs/msg/metric_family.hpp" namespace cartographer_ros { namespace metrics { @@ -34,7 +34,7 @@ class CounterFamily CounterFamily(const std::string& name, const std::string& description) : name_(name), description_(description) {} Counter* Add(const std::map& labels) override; - cartographer_ros_msgs::MetricFamily ToRosMessage(); + cartographer_ros_msgs::msg::MetricFamily ToRosMessage(); private: std::string name_; @@ -49,7 +49,7 @@ class GaugeFamily : name_(name), description_(description) {} Gauge* Add(const std::map& labels) override; - cartographer_ros_msgs::MetricFamily ToRosMessage(); + cartographer_ros_msgs::msg::MetricFamily ToRosMessage(); private: std::string name_; @@ -66,7 +66,7 @@ class HistogramFamily : public ::cartographer::metrics::Family< Histogram* Add(const std::map& labels) override; - cartographer_ros_msgs::MetricFamily ToRosMessage(); + cartographer_ros_msgs::msg::MetricFamily ToRosMessage(); private: std::string name_; diff --git a/cartographer_ros/cartographer_ros/metrics/internal/gauge.h b/cartographer_ros/cartographer_ros/metrics/internal/gauge.h index fc0a6935b..f311ab133 100644 --- a/cartographer_ros/cartographer_ros/metrics/internal/gauge.h +++ b/cartographer_ros/cartographer_ros/metrics/internal/gauge.h @@ -22,7 +22,7 @@ #include "absl/synchronization/mutex.h" #include "cartographer/metrics/gauge.h" -#include "cartographer_ros_msgs/Metric.h" +#include "cartographer_ros_msgs/msg/metric.hpp" namespace cartographer_ros { namespace metrics { @@ -50,11 +50,11 @@ class Gauge : public ::cartographer::metrics::Gauge { return value_; } - cartographer_ros_msgs::Metric ToRosMessage() { - cartographer_ros_msgs::Metric msg; - msg.type = cartographer_ros_msgs::Metric::TYPE_GAUGE; + cartographer_ros_msgs::msg::Metric ToRosMessage() { + cartographer_ros_msgs::msg::Metric msg; + msg.type = cartographer_ros_msgs::msg::Metric::TYPE_GAUGE; for (const auto& label : labels_) { - cartographer_ros_msgs::MetricLabel label_msg; + cartographer_ros_msgs::msg::MetricLabel label_msg; label_msg.key = label.first; label_msg.value = label.second; msg.labels.push_back(label_msg); diff --git a/cartographer_ros/cartographer_ros/metrics/internal/histogram.cc b/cartographer_ros/cartographer_ros/metrics/internal/histogram.cc index 27646ccd0..40fe625a4 100644 --- a/cartographer_ros/cartographer_ros/metrics/internal/histogram.cc +++ b/cartographer_ros/cartographer_ros/metrics/internal/histogram.cc @@ -68,17 +68,17 @@ double Histogram::CumulativeCount() { return std::accumulate(bucket_counts_.begin(), bucket_counts_.end(), 0.); } -cartographer_ros_msgs::Metric Histogram::ToRosMessage() { - cartographer_ros_msgs::Metric msg; - msg.type = cartographer_ros_msgs::Metric::TYPE_HISTOGRAM; +cartographer_ros_msgs::msg::Metric Histogram::ToRosMessage() { + cartographer_ros_msgs::msg::Metric msg; + msg.type = cartographer_ros_msgs::msg::Metric::TYPE_HISTOGRAM; for (const auto& label : labels_) { - cartographer_ros_msgs::MetricLabel label_msg; + cartographer_ros_msgs::msg::MetricLabel label_msg; label_msg.key = label.first; label_msg.value = label.second; msg.labels.push_back(label_msg); } for (const auto& bucket : CountsByBucket()) { - cartographer_ros_msgs::HistogramBucket bucket_msg; + cartographer_ros_msgs::msg::HistogramBucket bucket_msg; bucket_msg.bucket_boundary = bucket.first; bucket_msg.count = bucket.second; msg.counts_by_bucket.push_back(bucket_msg); diff --git a/cartographer_ros/cartographer_ros/metrics/internal/histogram.h b/cartographer_ros/cartographer_ros/metrics/internal/histogram.h index ec688154e..b5d84045c 100644 --- a/cartographer_ros/cartographer_ros/metrics/internal/histogram.h +++ b/cartographer_ros/cartographer_ros/metrics/internal/histogram.h @@ -22,7 +22,7 @@ #include "absl/synchronization/mutex.h" #include "cartographer/metrics/histogram.h" -#include "cartographer_ros_msgs/Metric.h" +#include "cartographer_ros_msgs/msg/metric.hpp" namespace cartographer_ros { namespace metrics { @@ -44,7 +44,7 @@ class Histogram : public ::cartographer::metrics::Histogram { double CumulativeCount(); - cartographer_ros_msgs::Metric ToRosMessage(); + cartographer_ros_msgs::msg::Metric ToRosMessage(); private: absl::Mutex mutex_; diff --git a/cartographer_ros/cartographer_ros/msg_conversion.cc b/cartographer_ros/cartographer_ros/msg_conversion.cc index 7e19088e2..973563ee6 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion.cc +++ b/cartographer_ros/cartographer_ros/msg_conversion.cc @@ -25,22 +25,22 @@ #include "cartographer/transform/proto/transform.pb.h" #include "cartographer/transform/transform.h" #include "cartographer_ros/time_conversion.h" -#include "geometry_msgs/Pose.h" -#include "geometry_msgs/Quaternion.h" -#include "geometry_msgs/Transform.h" -#include "geometry_msgs/TransformStamped.h" -#include "geometry_msgs/Vector3.h" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/transform.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/vector3.hpp" #include "glog/logging.h" -#include "nav_msgs/OccupancyGrid.h" +#include "nav_msgs/msg/occupancy_grid.hpp" #include "pcl/point_cloud.h" #include "pcl/point_types.h" #include "pcl_conversions/pcl_conversions.h" -#include "ros/ros.h" -#include "ros/serialization.h" -#include "sensor_msgs/Imu.h" -#include "sensor_msgs/LaserScan.h" -#include "sensor_msgs/MultiEchoLaserScan.h" -#include "sensor_msgs/PointCloud2.h" +#include "builtin_interfaces/msg/time.hpp" +//#include "ros/serialization.h" +#include "sensor_msgs/msg/imu.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "sensor_msgs/msg/multi_echo_laser_scan.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" namespace { @@ -73,7 +73,7 @@ POINT_CLOUD_REGISTER_POINT_STRUCT( namespace cartographer_ros { namespace { -// The ros::sensor_msgs::PointCloud2 binary data contains 4 floats for each +// The ros::sensor_msgs::msg::PointCloud2 binary data contains 4 floats for each // point. The last one must be this value or RViz is not showing the point cloud // properly. constexpr float kPointCloudComponentFourMagic = 1.; @@ -82,13 +82,13 @@ using ::cartographer::sensor::LandmarkData; using ::cartographer::sensor::LandmarkObservation; using ::cartographer::sensor::PointCloudWithIntensities; using ::cartographer::transform::Rigid3d; -using ::cartographer_ros_msgs::LandmarkEntry; -using ::cartographer_ros_msgs::LandmarkList; +using ::cartographer_ros_msgs::msg::LandmarkEntry; +using ::cartographer_ros_msgs::msg::LandmarkList; -sensor_msgs::PointCloud2 PreparePointCloud2Message(const int64_t timestamp, +sensor_msgs::msg::PointCloud2 PreparePointCloud2Message(const int64_t timestamp, const std::string& frame_id, const int num_points) { - sensor_msgs::PointCloud2 msg; + sensor_msgs::msg::PointCloud2 msg; msg.header.stamp = ToRos(::cartographer::common::FromUniversal(timestamp)); msg.header.frame_id = frame_id; msg.height = 1; @@ -96,15 +96,15 @@ sensor_msgs::PointCloud2 PreparePointCloud2Message(const int64_t timestamp, msg.fields.resize(3); msg.fields[0].name = "x"; msg.fields[0].offset = 0; - msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32; + msg.fields[0].datatype = sensor_msgs::msg::PointField::FLOAT32; msg.fields[0].count = 1; msg.fields[1].name = "y"; msg.fields[1].offset = 4; - msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32; + msg.fields[1].datatype = sensor_msgs::msg::PointField::FLOAT32; msg.fields[1].count = 1; msg.fields[2].name = "z"; msg.fields[2].offset = 8; - msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32; + msg.fields[2].datatype = sensor_msgs::msg::PointField::FLOAT32; msg.fields[2].count = 1; msg.is_bigendian = false; msg.point_step = 16; @@ -114,21 +114,21 @@ sensor_msgs::PointCloud2 PreparePointCloud2Message(const int64_t timestamp, return msg; } -// For sensor_msgs::LaserScan. +// For sensor_msgs::msg::LaserScan. bool HasEcho(float) { return true; } float GetFirstEcho(float range) { return range; } -// For sensor_msgs::MultiEchoLaserScan. -bool HasEcho(const sensor_msgs::LaserEcho& echo) { +// For sensor_msgs::msg::MultiEchoLaserScan. +bool HasEcho(const sensor_msgs::msg::LaserEcho& echo) { return !echo.echoes.empty(); } -float GetFirstEcho(const sensor_msgs::LaserEcho& echo) { +float GetFirstEcho(const sensor_msgs::msg::LaserEcho& echo) { return echo.echoes[0]; } -// For sensor_msgs::LaserScan and sensor_msgs::MultiEchoLaserScan. +// For sensor_msgs::msg::LaserScan and sensor_msgs::msg::MultiEchoLaserScan. template std::tuple LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) { @@ -174,7 +174,7 @@ LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) { return std::make_tuple(point_cloud, timestamp); } -bool PointCloud2HasField(const sensor_msgs::PointCloud2& pc2, +bool PointCloud2HasField(const sensor_msgs::msg::PointCloud2& pc2, const std::string& field_name) { for (const auto& field : pc2.fields) { if (field.name == field_name) { @@ -186,35 +186,36 @@ bool PointCloud2HasField(const sensor_msgs::PointCloud2& pc2, } // namespace -sensor_msgs::PointCloud2 ToPointCloud2Message( +sensor_msgs::msg::PointCloud2 ToPointCloud2Message( const int64_t timestamp, const std::string& frame_id, const ::cartographer::sensor::TimedPointCloud& point_cloud) { auto msg = PreparePointCloud2Message(timestamp, frame_id, point_cloud.size()); - ::ros::serialization::OStream stream(msg.data.data(), msg.data.size()); - for (const cartographer::sensor::TimedRangefinderPoint& point : point_cloud) { - stream.next(point.position.x()); - stream.next(point.position.y()); - stream.next(point.position.z()); - stream.next(kPointCloudComponentFourMagic); + size_t offset = 0; + float * const data = reinterpret_cast(&msg.data[0]); + for (const auto& point : point_cloud) { + data[offset++] = point.position.x(); + data[offset++] = point.position.y(); + data[offset++] = point.position.z(); + data[offset++] = kPointCloudComponentFourMagic; } return msg; } std::tuple<::cartographer::sensor::PointCloudWithIntensities, ::cartographer::common::Time> -ToPointCloudWithIntensities(const sensor_msgs::LaserScan& msg) { +ToPointCloudWithIntensities(const sensor_msgs::msg::LaserScan& msg) { return LaserScanToPointCloudWithIntensities(msg); } std::tuple<::cartographer::sensor::PointCloudWithIntensities, ::cartographer::common::Time> -ToPointCloudWithIntensities(const sensor_msgs::MultiEchoLaserScan& msg) { +ToPointCloudWithIntensities(const sensor_msgs::msg::MultiEchoLaserScan& msg) { return LaserScanToPointCloudWithIntensities(msg); } std::tuple<::cartographer::sensor::PointCloudWithIntensities, ::cartographer::common::Time> -ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) { +ToPointCloudWithIntensities(const sensor_msgs::msg::PointCloud2& msg) { PointCloudWithIntensities point_cloud; // We check for intensity field here to avoid run-time warnings if we pass in // a PointCloud2 without intensity. @@ -289,27 +290,27 @@ LandmarkData ToLandmarkData(const LandmarkList& landmark_list) { return landmark_data; } -Rigid3d ToRigid3d(const geometry_msgs::TransformStamped& transform) { +Rigid3d ToRigid3d(const geometry_msgs::msg::TransformStamped& transform) { return Rigid3d(ToEigen(transform.transform.translation), ToEigen(transform.transform.rotation)); } -Rigid3d ToRigid3d(const geometry_msgs::Pose& pose) { +Rigid3d ToRigid3d(const geometry_msgs::msg::Pose& pose) { return Rigid3d({pose.position.x, pose.position.y, pose.position.z}, ToEigen(pose.orientation)); } -Eigen::Vector3d ToEigen(const geometry_msgs::Vector3& vector3) { +Eigen::Vector3d ToEigen(const geometry_msgs::msg::Vector3& vector3) { return Eigen::Vector3d(vector3.x, vector3.y, vector3.z); } -Eigen::Quaterniond ToEigen(const geometry_msgs::Quaternion& quaternion) { +Eigen::Quaterniond ToEigen(const geometry_msgs::msg::Quaternion& quaternion) { return Eigen::Quaterniond(quaternion.w, quaternion.x, quaternion.y, quaternion.z); } -geometry_msgs::Transform ToGeometryMsgTransform(const Rigid3d& rigid3d) { - geometry_msgs::Transform transform; +geometry_msgs::msg::Transform ToGeometryMsgTransform(const Rigid3d& rigid3d) { + geometry_msgs::msg::Transform transform; transform.translation.x = rigid3d.translation().x(); transform.translation.y = rigid3d.translation().y(); transform.translation.z = rigid3d.translation().z(); @@ -320,8 +321,8 @@ geometry_msgs::Transform ToGeometryMsgTransform(const Rigid3d& rigid3d) { return transform; } -geometry_msgs::Pose ToGeometryMsgPose(const Rigid3d& rigid3d) { - geometry_msgs::Pose pose; +geometry_msgs::msg::Pose ToGeometryMsgPose(const Rigid3d& rigid3d) { + geometry_msgs::msg::Pose pose; pose.position = ToGeometryMsgPoint(rigid3d.translation()); pose.orientation.w = rigid3d.rotation().w(); pose.orientation.x = rigid3d.rotation().x(); @@ -330,8 +331,8 @@ geometry_msgs::Pose ToGeometryMsgPose(const Rigid3d& rigid3d) { return pose; } -geometry_msgs::Point ToGeometryMsgPoint(const Eigen::Vector3d& vector3d) { - geometry_msgs::Point point; +geometry_msgs::msg::Point ToGeometryMsgPoint(const Eigen::Vector3d& vector3d) { + geometry_msgs::msg::Point point; point.x = vector3d.x(); point.y = vector3d.y(); point.z = vector3d.z(); @@ -370,11 +371,11 @@ cartographer::transform::Rigid3d ComputeLocalFrameFromLatLong( return cartographer::transform::Rigid3d(rotation * -translation, rotation); } -std::unique_ptr CreateOccupancyGridMsg( +std::unique_ptr CreateOccupancyGridMsg( const cartographer::io::PaintSubmapSlicesResult& painted_slices, const double resolution, const std::string& frame_id, - const ros::Time& time) { - auto occupancy_grid = absl::make_unique(); + const rclcpp::Time& time) { + auto occupancy_grid = absl::make_unique(); const int width = cairo_image_surface_get_width(painted_slices.surface.get()); const int height = diff --git a/cartographer_ros/cartographer_ros/msg_conversion.h b/cartographer_ros/cartographer_ros/msg_conversion.h index ab3fa1e6d..e895144b2 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion.h +++ b/cartographer_ros/cartographer_ros/msg_conversion.h @@ -22,56 +22,57 @@ #include "cartographer/sensor/landmark_data.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/rigid_transform.h" -#include "cartographer_ros_msgs/LandmarkList.h" -#include "geometry_msgs/Pose.h" -#include "geometry_msgs/Transform.h" -#include "geometry_msgs/TransformStamped.h" -#include "nav_msgs/OccupancyGrid.h" -#include "sensor_msgs/Imu.h" -#include "sensor_msgs/LaserScan.h" -#include "sensor_msgs/MultiEchoLaserScan.h" -#include "sensor_msgs/PointCloud2.h" +#include "cartographer_ros_msgs/msg/landmark_list.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/transform.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "sensor_msgs/msg/multi_echo_laser_scan.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include namespace cartographer_ros { -sensor_msgs::PointCloud2 ToPointCloud2Message( +sensor_msgs::msg::PointCloud2 ToPointCloud2Message( int64_t timestamp, const std::string& frame_id, const ::cartographer::sensor::TimedPointCloud& point_cloud); -geometry_msgs::Transform ToGeometryMsgTransform( +geometry_msgs::msg::Transform ToGeometryMsgTransform( const ::cartographer::transform::Rigid3d& rigid3d); -geometry_msgs::Pose ToGeometryMsgPose( +geometry_msgs::msg::Pose ToGeometryMsgPose( const ::cartographer::transform::Rigid3d& rigid3d); -geometry_msgs::Point ToGeometryMsgPoint(const Eigen::Vector3d& vector3d); +geometry_msgs::msg::Point ToGeometryMsgPoint(const Eigen::Vector3d& vector3d); // Converts ROS message to point cloud. Returns the time when the last point // was acquired (different from the ROS timestamp). Timing of points is given in // the fourth component of each point relative to `Time`. std::tuple<::cartographer::sensor::PointCloudWithIntensities, ::cartographer::common::Time> -ToPointCloudWithIntensities(const sensor_msgs::LaserScan& msg); +ToPointCloudWithIntensities(const sensor_msgs::msg::LaserScan& msg); std::tuple<::cartographer::sensor::PointCloudWithIntensities, ::cartographer::common::Time> -ToPointCloudWithIntensities(const sensor_msgs::MultiEchoLaserScan& msg); +ToPointCloudWithIntensities(const sensor_msgs::msg::MultiEchoLaserScan& msg); std::tuple<::cartographer::sensor::PointCloudWithIntensities, ::cartographer::common::Time> -ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg); +ToPointCloudWithIntensities(const sensor_msgs::msg::PointCloud2& msg); ::cartographer::sensor::LandmarkData ToLandmarkData( - const cartographer_ros_msgs::LandmarkList& landmark_list); + const cartographer_ros_msgs::msg::LandmarkList& landmark_list); ::cartographer::transform::Rigid3d ToRigid3d( - const geometry_msgs::TransformStamped& transform); + const geometry_msgs::msg::TransformStamped& transform); -::cartographer::transform::Rigid3d ToRigid3d(const geometry_msgs::Pose& pose); +::cartographer::transform::Rigid3d ToRigid3d(const geometry_msgs::msg::Pose& pose); -Eigen::Vector3d ToEigen(const geometry_msgs::Vector3& vector3); +Eigen::Vector3d ToEigen(const geometry_msgs::msg::Vector3& vector3); -Eigen::Quaterniond ToEigen(const geometry_msgs::Quaternion& quaternion); +Eigen::Quaterniond ToEigen(const geometry_msgs::msg::Quaternion& quaternion); // Converts from WGS84 (latitude, longitude, altitude) to ECEF. Eigen::Vector3d LatLongAltToEcef(double latitude, double longitude, @@ -84,10 +85,10 @@ cartographer::transform::Rigid3d ComputeLocalFrameFromLatLong(double latitude, // Points to an occupancy grid message at a specific resolution from painted // submap slices obtained via ::cartographer::io::PaintSubmapSlices(...). -std::unique_ptr CreateOccupancyGridMsg( +std::unique_ptr CreateOccupancyGridMsg( const cartographer::io::PaintSubmapSlicesResult& painted_slices, const double resolution, const std::string& frame_id, - const ros::Time& time); + const rclcpp::Time& time); } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/msg_conversion_test.cc b/cartographer_ros/cartographer_ros/msg_conversion_test.cc index 85039dd82..152fb8084 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion_test.cc +++ b/cartographer_ros/cartographer_ros/msg_conversion_test.cc @@ -38,7 +38,7 @@ using ::testing::Field; constexpr double kEps = 1e-6; TEST(MsgConversion, LaserScanToPointCloud) { - sensor_msgs::LaserScan laser_scan; + sensor_msgs::msg::LaserScan laser_scan; for (int i = 0; i < 8; ++i) { laser_scan.ranges.push_back(1.f); } @@ -73,7 +73,7 @@ TEST(MsgConversion, LaserScanToPointCloud) { } TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) { - sensor_msgs::LaserScan laser_scan; + sensor_msgs::msg::LaserScan laser_scan; laser_scan.ranges.push_back(1.f); laser_scan.ranges.push_back(std::numeric_limits::infinity()); laser_scan.ranges.push_back(2.f); @@ -110,10 +110,10 @@ ::testing::Matcher EqualsLandmark( } TEST(MsgConversion, LandmarkListToLandmarkData) { - cartographer_ros_msgs::LandmarkList message; + cartographer_ros_msgs::msg::LandmarkList message; message.header.stamp.fromSec(10); - cartographer_ros_msgs::LandmarkEntry landmark_0; + cartographer_ros_msgs::msg::LandmarkEntry landmark_0; landmark_0.id = "landmark_0"; landmark_0.tracking_from_landmark_transform.position.x = 1.0; landmark_0.tracking_from_landmark_transform.position.y = 2.0; diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 067ab5ae0..2336134b2 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -22,7 +22,6 @@ #include "Eigen/Core" #include "absl/memory/memory.h" -#include "absl/strings/str_cat.h" #include "cartographer/common/configuration_file_resolver.h" #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/port.h" @@ -38,15 +37,15 @@ #include "cartographer_ros/sensor_bridge.h" #include "cartographer_ros/tf_bridge.h" #include "cartographer_ros/time_conversion.h" -#include "cartographer_ros_msgs/StatusCode.h" -#include "cartographer_ros_msgs/StatusResponse.h" -#include "geometry_msgs/PoseStamped.h" +#include "cartographer_ros_msgs/msg/status_code.hpp" +#include "cartographer_ros_msgs/msg/status_response.hpp" +#include "geometry_msgs/msg/point_stamped.hpp" #include "glog/logging.h" -#include "nav_msgs/Odometry.h" -#include "ros/serialization.h" -#include "sensor_msgs/PointCloud2.h" +#include "nav_msgs/msg/odometry.hpp" +//#include "ros/serialization.h" +#include "sensor_msgs/msg/point_cloud2.hpp" #include "tf2_eigen/tf2_eigen.h" -#include "visualization_msgs/MarkerArray.h" +#include "visualization_msgs/msg/marker_array.hpp" namespace cartographer_ros { @@ -60,18 +59,16 @@ namespace { // Subscribes to the 'topic' for 'trajectory_id' using the 'node_handle' and // calls 'handler' on the 'node' to handle messages. Returns the subscriber. template -::ros::Subscriber SubscribeWithHandler( +::rclcpp::SubscriptionBase::SharedPtr SubscribeWithHandler( void (Node::*handler)(int, const std::string&, - const typename MessageType::ConstPtr&), + const typename MessageType::ConstSharedPtr&), const int trajectory_id, const std::string& topic, - ::ros::NodeHandle* const node_handle, Node* const node) { - return node_handle->subscribe( - topic, kInfiniteSubscriberQueueSize, - boost::function( - [node, handler, trajectory_id, - topic](const typename MessageType::ConstPtr& msg) { + ::rclcpp::Node::SharedPtr node_handle, Node* const node) { + return node_handle->create_subscription( + topic, rclcpp::SensorDataQoS(), + [node, handler, trajectory_id, topic](const typename MessageType::ConstSharedPtr msg) { (node->*handler)(trajectory_id, topic, msg); - })); + }); } std::string TrajectoryStateToString(const TrajectoryState trajectory_state) { @@ -93,9 +90,15 @@ std::string TrajectoryStateToString(const TrajectoryState trajectory_state) { Node::Node( const NodeOptions& node_options, std::unique_ptr map_builder, - tf2_ros::Buffer* const tf_buffer, const bool collect_metrics) - : node_options_(node_options), - map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer) { + std::shared_ptr tf_buffer, + rclcpp::Node::SharedPtr node, + const bool collect_metrics) + : node_options_(node_options) +{ + node_ = node; + tf_broadcaster_ = std::make_shared(node_) ; + map_builder_bridge_.reset(new cartographer_ros::MapBuilderBridge(node_options_, std::move(map_builder), tf_buffer.get())); + absl::MutexLock lock(&mutex_); if (collect_metrics) { metrics_registry_ = absl::make_unique(); @@ -103,92 +106,116 @@ Node::Node( } submap_list_publisher_ = - node_handle_.advertise<::cartographer_ros_msgs::SubmapList>( - kSubmapListTopic, kLatestOnlyPublisherQueueSize); + node_->create_publisher<::cartographer_ros_msgs::msg::SubmapList>( + kSubmapListTopic, 10); trajectory_node_list_publisher_ = - node_handle_.advertise<::visualization_msgs::MarkerArray>( - kTrajectoryNodeListTopic, kLatestOnlyPublisherQueueSize); + node_->create_publisher<::visualization_msgs::msg::MarkerArray>( + kTrajectoryNodeListTopic, 10); landmark_poses_list_publisher_ = - node_handle_.advertise<::visualization_msgs::MarkerArray>( - kLandmarkPosesListTopic, kLatestOnlyPublisherQueueSize); + node_->create_publisher<::visualization_msgs::msg::MarkerArray>( + kLandmarkPosesListTopic, 10); constraint_list_publisher_ = - node_handle_.advertise<::visualization_msgs::MarkerArray>( - kConstraintListTopic, kLatestOnlyPublisherQueueSize); + node_->create_publisher<::visualization_msgs::msg::MarkerArray>( + kConstraintListTopic, 10); if (node_options_.publish_tracked_pose) { tracked_pose_publisher_ = - node_handle_.advertise<::geometry_msgs::PoseStamped>( - kTrackedPoseTopic, 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( - kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this)); - service_servers_.push_back(node_handle_.advertiseService( - kWriteStateServiceName, &Node::HandleWriteState, this)); - service_servers_.push_back(node_handle_.advertiseService( - kGetTrajectoryStatesServiceName, &Node::HandleGetTrajectoryStates, this)); - service_servers_.push_back(node_handle_.advertiseService( - kReadMetricsServiceName, &Node::HandleReadMetrics, this)); + node_->create_publisher<::geometry_msgs::msg::PoseStamped>( + kTrackedPoseTopic, 10); + } scan_matched_point_cloud_publisher_ = - node_handle_.advertise( - kScanMatchedPointCloudTopic, kLatestOnlyPublisherQueueSize); - - wall_timers_.push_back(node_handle_.createWallTimer( - ::ros::WallDuration(node_options_.submap_publish_period_sec), - &Node::PublishSubmapList, this)); + node_->create_publisher( + kScanMatchedPointCloudTopic, 10); + + submap_query_server_ = node_->create_service( + kSubmapQueryServiceName, + std::bind( + &Node::handleSubmapQuery, this, std::placeholders::_1, std::placeholders::_2)); + trajectory_query_server = node_->create_service( + kTrajectoryQueryServiceName, + std::bind( + &Node::handleTrajectoryQuery, this, std::placeholders::_1, std::placeholders::_2)); + start_trajectory_server_ = node_->create_service( + kStartTrajectoryServiceName, + std::bind( + &Node::handleStartTrajectory, this, std::placeholders::_1, std::placeholders::_2)); + finish_trajectory_server_ = node_->create_service( + kFinishTrajectoryServiceName, + std::bind( + &Node::handleFinishTrajectory, this, std::placeholders::_1, std::placeholders::_2)); + write_state_server_ = node_->create_service( + kWriteStateServiceName, + std::bind( + &Node::handleWriteState, this, std::placeholders::_1, std::placeholders::_2)); + get_trajectory_states_server_ = node_->create_service( + kGetTrajectoryStatesServiceName, + std::bind( + &Node::handleGetTrajectoryStates, this, std::placeholders::_1, std::placeholders::_2)); + read_metrics_server_ = node_->create_service( + kReadMetricsServiceName, + std::bind( + &Node::handleReadMetrics, this, std::placeholders::_1, std::placeholders::_2)); + + + submap_list_timer_ = node_->create_wall_timer( + std::chrono::milliseconds(int(node_options_.submap_publish_period_sec * 1000)), + [this]() { + PublishSubmapList(); + }); if (node_options_.pose_publish_period_sec > 0) { - publish_local_trajectory_data_timer_ = node_handle_.createTimer( - ::ros::Duration(node_options_.pose_publish_period_sec), - &Node::PublishLocalTrajectoryData, this); - } - wall_timers_.push_back(node_handle_.createWallTimer( - ::ros::WallDuration(node_options_.trajectory_publish_period_sec), - &Node::PublishTrajectoryNodeList, this)); - wall_timers_.push_back(node_handle_.createWallTimer( - ::ros::WallDuration(node_options_.trajectory_publish_period_sec), - &Node::PublishLandmarkPosesList, this)); - wall_timers_.push_back(node_handle_.createWallTimer( - ::ros::WallDuration(kConstraintPublishPeriodSec), - &Node::PublishConstraintList, this)); + local_trajectory_data_timer_ = node_->create_wall_timer( + std::chrono::milliseconds(int(node_options_.pose_publish_period_sec * 1000)), + [this]() { + PublishLocalTrajectoryData(); + }); + } + trajectory_node_list_timer_ = node_->create_wall_timer( + std::chrono::milliseconds(int(node_options_.trajectory_publish_period_sec * 1000)), + [this]() { + PublishTrajectoryNodeList(); + }); + landmark_pose_list_timer_ = node_->create_wall_timer( + std::chrono::milliseconds(int(node_options_.trajectory_publish_period_sec * 1000)), + [this]() { + PublishLandmarkPosesList(); + }); + constrain_list_timer_ = node_->create_wall_timer( + std::chrono::milliseconds(int(kConstraintPublishPeriodSec * 1000)), + [this]() { + PublishConstraintList(); + }); } Node::~Node() { FinishAllTrajectories(); } -::ros::NodeHandle* Node::node_handle() { return &node_handle_; } - -bool Node::HandleSubmapQuery( - ::cartographer_ros_msgs::SubmapQuery::Request& request, - ::cartographer_ros_msgs::SubmapQuery::Response& response) { +bool Node::handleSubmapQuery( + const cartographer_ros_msgs::srv::SubmapQuery::Request::SharedPtr request, + cartographer_ros_msgs::srv::SubmapQuery::Response::SharedPtr response) { absl::MutexLock lock(&mutex_); - map_builder_bridge_.HandleSubmapQuery(request, response); + map_builder_bridge_->HandleSubmapQuery(request, response); return true; } -bool Node::HandleTrajectoryQuery( - ::cartographer_ros_msgs::TrajectoryQuery::Request& request, - ::cartographer_ros_msgs::TrajectoryQuery::Response& response) { +bool Node::handleTrajectoryQuery( + const cartographer_ros_msgs::srv::TrajectoryQuery::Request::SharedPtr request, + cartographer_ros_msgs::srv::TrajectoryQuery::Response::SharedPtr response) { absl::MutexLock lock(&mutex_); - response.status = TrajectoryStateToStatus( - request.trajectory_id, + response->status = TrajectoryStateToStatus( + request->trajectory_id, {TrajectoryState::ACTIVE, TrajectoryState::FINISHED, TrajectoryState::FROZEN} /* valid states */); - if (response.status.code != cartographer_ros_msgs::StatusCode::OK) { + if (response->status.code != cartographer_ros_msgs::msg::StatusCode::OK) { LOG(ERROR) << "Can't query trajectory from pose graph: " - << response.status.message; + << response->status.message; return true; } - map_builder_bridge_.HandleTrajectoryQuery(request, response); + map_builder_bridge_->HandleTrajectoryQuery(request, response); return true; } -void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) { +void Node::PublishSubmapList() { absl::MutexLock lock(&mutex_); - submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList()); + submap_list_publisher_->publish(map_builder_bridge_->GetSubmapList(node_->now())); } void Node::AddExtrapolator(const int trajectory_id, @@ -219,9 +246,9 @@ void Node::AddSensorSamplers(const int trajectory_id, options.landmarks_sampling_ratio)); } -void Node::PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event) { +void Node::PublishLocalTrajectoryData() { absl::MutexLock lock(&mutex_); - for (const auto& entry : map_builder_bridge_.GetLocalTrajectoryData()) { + for (const auto& entry : map_builder_bridge_->GetLocalTrajectoryData()) { const auto& trajectory_data = entry.second; auto& extrapolator = extrapolators_.at(entry.first); @@ -229,7 +256,7 @@ void Node::PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event) { // frequency, and republishing it would be computationally wasteful. if (trajectory_data.local_slam_data->time != extrapolator.GetLastPoseTime()) { - if (scan_matched_point_cloud_publisher_.getNumSubscribers() > 0) { + if (scan_matched_point_cloud_publisher_->get_subscription_count() > 0) { // TODO(gaschler): Consider using other message without time // information. carto::sensor::TimedPointCloud point_cloud; @@ -240,7 +267,7 @@ void Node::PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event) { point_cloud.push_back(cartographer::sensor::ToTimedRangefinderPoint( point, 0.f /* time */)); } - scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message( + scan_matched_point_cloud_publisher_->publish(ToPointCloud2Message( carto::common::ToUniversal(trajectory_data.local_slam_data->time), node_options_.map_frame, carto::sensor::TransformTimedPointCloud( @@ -250,13 +277,13 @@ void Node::PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event) { trajectory_data.local_slam_data->local_pose); } - geometry_msgs::TransformStamped stamped_transform; + geometry_msgs::msg::TransformStamped stamped_transform; // If we do not publish a new point cloud, we still allow time of the // published poses to advance. If we already know a newer pose, we use its // time instead. Since tf knows how to interpolate, providing newer // information is better. const ::cartographer::common::Time now = std::max( - FromRos(ros::Time::now()), extrapolator.GetLastExtrapolatedTime()); + FromRos(node_->now()), extrapolator.GetLastExtrapolatedTime()); stamped_transform.header.stamp = node_options_.use_pose_extrapolator ? ToRos(now) @@ -288,7 +315,7 @@ void Node::PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event) { if (trajectory_data.published_to_tracking != nullptr) { if (node_options_.publish_to_tf) { if (trajectory_data.trajectory_options.provide_odom_frame) { - std::vector stamped_transforms; + std::vector stamped_transforms; stamped_transform.header.frame_id = node_options_.map_frame; stamped_transform.child_frame_id = @@ -305,50 +332,47 @@ void Node::PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event) { tracking_to_local * (*trajectory_data.published_to_tracking)); stamped_transforms.push_back(stamped_transform); - tf_broadcaster_.sendTransform(stamped_transforms); + tf_broadcaster_->sendTransform(stamped_transforms); } else { stamped_transform.header.frame_id = node_options_.map_frame; stamped_transform.child_frame_id = trajectory_data.trajectory_options.published_frame; stamped_transform.transform = ToGeometryMsgTransform( tracking_to_map * (*trajectory_data.published_to_tracking)); - tf_broadcaster_.sendTransform(stamped_transform); + tf_broadcaster_->sendTransform(stamped_transform); } } if (node_options_.publish_tracked_pose) { - ::geometry_msgs::PoseStamped pose_msg; + ::geometry_msgs::msg::PoseStamped pose_msg; pose_msg.header.frame_id = node_options_.map_frame; pose_msg.header.stamp = stamped_transform.header.stamp; pose_msg.pose = ToGeometryMsgPose(tracking_to_map); - tracked_pose_publisher_.publish(pose_msg); + tracked_pose_publisher_->publish(pose_msg); } } } } -void Node::PublishTrajectoryNodeList( - const ::ros::WallTimerEvent& unused_timer_event) { - if (trajectory_node_list_publisher_.getNumSubscribers() > 0) { +void Node::PublishTrajectoryNodeList() { + if (trajectory_node_list_publisher_->get_subscription_count() > 0) { absl::MutexLock lock(&mutex_); - trajectory_node_list_publisher_.publish( - map_builder_bridge_.GetTrajectoryNodeList()); + trajectory_node_list_publisher_->publish( + map_builder_bridge_->GetTrajectoryNodeList(node_->now())); } } -void Node::PublishLandmarkPosesList( - const ::ros::WallTimerEvent& unused_timer_event) { - if (landmark_poses_list_publisher_.getNumSubscribers() > 0) { +void Node::PublishLandmarkPosesList() { + if (landmark_poses_list_publisher_->get_subscription_count() > 0) { absl::MutexLock lock(&mutex_); - landmark_poses_list_publisher_.publish( - map_builder_bridge_.GetLandmarkPosesList()); + landmark_poses_list_publisher_->publish( + map_builder_bridge_->GetLandmarkPosesList(node_->now())); } } -void Node::PublishConstraintList( - const ::ros::WallTimerEvent& unused_timer_event) { - if (constraint_list_publisher_.getNumSubscribers() > 0) { +void Node::PublishConstraintList() { + if (constraint_list_publisher_->get_subscription_count() > 0) { absl::MutexLock lock(&mutex_); - constraint_list_publisher_.publish(map_builder_bridge_.GetConstraintList()); + constraint_list_publisher_->publish(map_builder_bridge_->GetConstraintList(node_->now())); } } @@ -398,13 +422,15 @@ int Node::AddTrajectory(const TrajectoryOptions& options) { const std::set expected_sensor_ids = ComputeExpectedSensorIds(options); const int trajectory_id = - map_builder_bridge_.AddTrajectory(expected_sensor_ids, options); + map_builder_bridge_->AddTrajectory(expected_sensor_ids, options); AddExtrapolator(trajectory_id, options); AddSensorSamplers(trajectory_id, options); LaunchSubscribers(options, trajectory_id); - wall_timers_.push_back(node_handle_.createWallTimer( - ::ros::WallDuration(kTopicMismatchCheckDelaySec), - &Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true)); + maybe_warn_about_topic_mismatch_timer_ = node_->create_wall_timer( + std::chrono::milliseconds(int(kTopicMismatchCheckDelaySec * 1000)), + [this]() { + MaybeWarnAboutTopicMismatch(); + }); for (const auto& sensor_id : expected_sensor_ids) { subscribed_topics_.insert(sensor_id.id); } @@ -416,25 +442,22 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options, for (const std::string& topic : ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) { subscribers_[trajectory_id].push_back( - {SubscribeWithHandler( - &Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_, - this), + {SubscribeWithHandler( + &Node::HandleLaserScanMessage, trajectory_id, topic, node_, this), topic}); } for (const std::string& topic : ComputeRepeatedTopicNames( kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) { subscribers_[trajectory_id].push_back( - {SubscribeWithHandler( - &Node::HandleMultiEchoLaserScanMessage, trajectory_id, topic, - &node_handle_, this), + {SubscribeWithHandler( + &Node::HandleMultiEchoLaserScanMessage, trajectory_id, topic, node_, this), topic}); } for (const std::string& topic : ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) { subscribers_[trajectory_id].push_back( - {SubscribeWithHandler( - &Node::HandlePointCloud2Message, trajectory_id, topic, - &node_handle_, this), + {SubscribeWithHandler( + &Node::HandlePointCloud2Message, trajectory_id, topic, node_, this), topic}); } @@ -445,31 +468,31 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options, options.trajectory_builder_options.trajectory_builder_2d_options() .use_imu_data())) { subscribers_[trajectory_id].push_back( - {SubscribeWithHandler(&Node::HandleImuMessage, + {SubscribeWithHandler(&Node::HandleImuMessage, trajectory_id, kImuTopic, - &node_handle_, this), + node_, this), kImuTopic}); } if (options.use_odometry) { subscribers_[trajectory_id].push_back( - {SubscribeWithHandler(&Node::HandleOdometryMessage, + {SubscribeWithHandler(&Node::HandleOdometryMessage, trajectory_id, kOdometryTopic, - &node_handle_, this), + node_, this), kOdometryTopic}); } if (options.use_nav_sat) { subscribers_[trajectory_id].push_back( - {SubscribeWithHandler( + {SubscribeWithHandler( &Node::HandleNavSatFixMessage, trajectory_id, kNavSatFixTopic, - &node_handle_, this), + node_, this), kNavSatFixTopic}); } if (options.use_landmarks) { subscribers_[trajectory_id].push_back( - {SubscribeWithHandler( + {SubscribeWithHandler( &Node::HandleLandmarkMessage, trajectory_id, kLandmarkTopic, - &node_handle_, this), + node_, this), kLandmarkTopic}); } } @@ -497,36 +520,34 @@ bool Node::ValidateTopicNames(const TrajectoryOptions& options) { return true; } -cartographer_ros_msgs::StatusResponse Node::TrajectoryStateToStatus( +cartographer_ros_msgs::msg::StatusResponse Node::TrajectoryStateToStatus( const int trajectory_id, const std::set& valid_states) { - const auto trajectory_states = map_builder_bridge_.GetTrajectoryStates(); - cartographer_ros_msgs::StatusResponse status_response; + const auto trajectory_states = map_builder_bridge_->GetTrajectoryStates(); + cartographer_ros_msgs::msg::StatusResponse status_response; 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; + status_response.message = "Trajectory " + std::to_string(trajectory_id) + " doesn't exist."; + status_response.code = cartographer_ros_msgs::msg::StatusCode::NOT_FOUND; return status_response; } - status_response.message = - absl::StrCat("Trajectory ", trajectory_id, " is in '", - TrajectoryStateToString(it->second), "' state."); + status_response.message = "Trajectory " + std::to_string(trajectory_id) + " is in '" + + TrajectoryStateToString(it->second) + "' state."; status_response.code = valid_states.count(it->second) - ? cartographer_ros_msgs::StatusCode::OK - : cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT; + ? cartographer_ros_msgs::msg::StatusCode::OK + : cartographer_ros_msgs::msg::StatusCode::INVALID_ARGUMENT; return status_response; } -cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock( +cartographer_ros_msgs::msg::StatusResponse Node::FinishTrajectoryUnderLock( const int trajectory_id) { - cartographer_ros_msgs::StatusResponse status_response; + cartographer_ros_msgs::msg::StatusResponse status_response; if (trajectories_scheduled_for_finish_.count(trajectory_id)) { - status_response.message = absl::StrCat("Trajectory ", trajectory_id, - " already pending to finish."); - status_response.code = cartographer_ros_msgs::StatusCode::OK; + status_response.message = + "Trajectory " + std::to_string(trajectory_id) + " already pending to finish."; + status_response.code = cartographer_ros_msgs::msg::StatusCode::OK; LOG(INFO) << status_response.message; return status_response; } @@ -534,7 +555,7 @@ cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock( // First, check if we can actually finish the trajectory. status_response = TrajectoryStateToStatus( trajectory_id, {TrajectoryState::ACTIVE} /* valid states */); - if (status_response.code != cartographer_ros_msgs::StatusCode::OK) { + if (status_response.code != cartographer_ros_msgs::msg::StatusCode::OK) { LOG(ERROR) << "Can't finish trajectory: " << status_response.message; return status_response; } @@ -543,73 +564,73 @@ cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock( // A valid case with no subscribers is e.g. if we just visualize states. if (subscribers_.count(trajectory_id)) { for (auto& entry : subscribers_[trajectory_id]) { - entry.subscriber.shutdown(); + entry.subscriber.reset(); subscribed_topics_.erase(entry.topic); LOG(INFO) << "Shutdown the subscriber of [" << entry.topic << "]"; } CHECK_EQ(subscribers_.erase(trajectory_id), 1); } - map_builder_bridge_.FinishTrajectory(trajectory_id); + map_builder_bridge_->FinishTrajectory(trajectory_id); trajectories_scheduled_for_finish_.emplace(trajectory_id); status_response.message = - absl::StrCat("Finished trajectory ", trajectory_id, "."); - status_response.code = cartographer_ros_msgs::StatusCode::OK; + "Finished trajectory " + std::to_string(trajectory_id) + "."; + status_response.code = cartographer_ros_msgs::msg::StatusCode::OK; return status_response; } -bool Node::HandleStartTrajectory( - ::cartographer_ros_msgs::StartTrajectory::Request& request, - ::cartographer_ros_msgs::StartTrajectory::Response& response) { +bool Node::handleStartTrajectory( + const cartographer_ros_msgs::srv::StartTrajectory::Request::SharedPtr request, + cartographer_ros_msgs::srv::StartTrajectory::Response::SharedPtr response) { TrajectoryOptions trajectory_options; std::tie(std::ignore, trajectory_options) = LoadOptions( - request.configuration_directory, request.configuration_basename); + request->configuration_directory, request->configuration_basename); - if (request.use_initial_pose) { - const auto pose = ToRigid3d(request.initial_pose); + if (request->use_initial_pose) { + const auto pose = ToRigid3d(request->initial_pose); if (!pose.IsValid()) { - response.status.message = + response->status.message = "Invalid pose argument. Orientation quaternion must be normalized."; - LOG(ERROR) << response.status.message; - response.status.code = - cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT; + LOG(ERROR) << response->status.message; + response->status.code = + cartographer_ros_msgs::msg::StatusCode::INVALID_ARGUMENT; return true; } // Check if the requested trajectory for the relative initial pose exists. - response.status = TrajectoryStateToStatus( - request.relative_to_trajectory_id, + response->status = TrajectoryStateToStatus( + request->relative_to_trajectory_id, {TrajectoryState::ACTIVE, TrajectoryState::FROZEN, TrajectoryState::FINISHED} /* valid states */); - if (response.status.code != cartographer_ros_msgs::StatusCode::OK) { + if (response->status.code != cartographer_ros_msgs::msg::StatusCode::OK) { LOG(ERROR) << "Can't start a trajectory with initial pose: " - << response.status.message; + << response->status.message; return true; } ::cartographer::mapping::proto::InitialTrajectoryPose initial_trajectory_pose; initial_trajectory_pose.set_to_trajectory_id( - request.relative_to_trajectory_id); + request->relative_to_trajectory_id); *initial_trajectory_pose.mutable_relative_pose() = cartographer::transform::ToProto(pose); initial_trajectory_pose.set_timestamp(cartographer::common::ToUniversal( - ::cartographer_ros::FromRos(ros::Time(0)))); + ::cartographer_ros::FromRos(rclcpp::Time(0)))); *trajectory_options.trajectory_builder_options .mutable_initial_trajectory_pose() = initial_trajectory_pose; } if (!ValidateTrajectoryOptions(trajectory_options)) { - response.status.message = "Invalid trajectory options."; - LOG(ERROR) << response.status.message; - response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT; + response->status.message = "Invalid trajectory options."; + LOG(ERROR) << response->status.message; + response->status.code = cartographer_ros_msgs::msg::StatusCode::INVALID_ARGUMENT; } else if (!ValidateTopicNames(trajectory_options)) { - response.status.message = "Topics are already used by another trajectory."; - LOG(ERROR) << response.status.message; - response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT; + response->status.message = "Topics are already used by another trajectory."; + LOG(ERROR) << response->status.message; + response->status.code = cartographer_ros_msgs::msg::StatusCode::INVALID_ARGUMENT; } else { - response.status.message = "Success."; - response.trajectory_id = AddTrajectory(trajectory_options); - response.status.code = cartographer_ros_msgs::StatusCode::OK; + response->status.message = "Success."; + response->trajectory_id = AddTrajectory(trajectory_options); + response->status.code = cartographer_ros_msgs::msg::StatusCode::OK; } return true; } @@ -646,92 +667,92 @@ int Node::AddOfflineTrajectory( const TrajectoryOptions& options) { absl::MutexLock lock(&mutex_); const int trajectory_id = - map_builder_bridge_.AddTrajectory(expected_sensor_ids, options); + map_builder_bridge_->AddTrajectory(expected_sensor_ids, options); AddExtrapolator(trajectory_id, options); AddSensorSamplers(trajectory_id, options); return trajectory_id; } -bool Node::HandleGetTrajectoryStates( - ::cartographer_ros_msgs::GetTrajectoryStates::Request& request, - ::cartographer_ros_msgs::GetTrajectoryStates::Response& response) { +bool Node::handleGetTrajectoryStates( + const cartographer_ros_msgs::srv::GetTrajectoryStates::Request::SharedPtr request, + cartographer_ros_msgs::srv::GetTrajectoryStates::Response::SharedPtr response) { using TrajectoryState = ::cartographer::mapping::PoseGraphInterface::TrajectoryState; absl::MutexLock lock(&mutex_); - response.status.code = ::cartographer_ros_msgs::StatusCode::OK; - response.trajectory_states.header.stamp = ros::Time::now(); - for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) { - response.trajectory_states.trajectory_id.push_back(entry.first); + response->status.code = ::cartographer_ros_msgs::msg::StatusCode::OK; + response->trajectory_states.header.stamp = node_->now(); + for (const auto& entry : map_builder_bridge_->GetTrajectoryStates()) { + response->trajectory_states.trajectory_id.push_back(entry.first); switch (entry.second) { case TrajectoryState::ACTIVE: - response.trajectory_states.trajectory_state.push_back( - ::cartographer_ros_msgs::TrajectoryStates::ACTIVE); + response->trajectory_states.trajectory_state.push_back( + ::cartographer_ros_msgs::msg::TrajectoryStates::ACTIVE); break; case TrajectoryState::FINISHED: - response.trajectory_states.trajectory_state.push_back( - ::cartographer_ros_msgs::TrajectoryStates::FINISHED); + response->trajectory_states.trajectory_state.push_back( + ::cartographer_ros_msgs::msg::TrajectoryStates::FINISHED); break; case TrajectoryState::FROZEN: - response.trajectory_states.trajectory_state.push_back( - ::cartographer_ros_msgs::TrajectoryStates::FROZEN); + response->trajectory_states.trajectory_state.push_back( + ::cartographer_ros_msgs::msg::TrajectoryStates::FROZEN); break; case TrajectoryState::DELETED: - response.trajectory_states.trajectory_state.push_back( - ::cartographer_ros_msgs::TrajectoryStates::DELETED); + response->trajectory_states.trajectory_state.push_back( + ::cartographer_ros_msgs::msg::TrajectoryStates::DELETED); break; } } return true; } -bool Node::HandleFinishTrajectory( - ::cartographer_ros_msgs::FinishTrajectory::Request& request, - ::cartographer_ros_msgs::FinishTrajectory::Response& response) { +bool Node::handleFinishTrajectory( + const cartographer_ros_msgs::srv::FinishTrajectory::Request::SharedPtr request, + cartographer_ros_msgs::srv::FinishTrajectory::Response::SharedPtr response) { absl::MutexLock lock(&mutex_); - response.status = FinishTrajectoryUnderLock(request.trajectory_id); + response->status = FinishTrajectoryUnderLock(request->trajectory_id); return true; } -bool Node::HandleWriteState( - ::cartographer_ros_msgs::WriteState::Request& request, - ::cartographer_ros_msgs::WriteState::Response& response) { +bool Node::handleWriteState( + const cartographer_ros_msgs::srv::WriteState::Request::SharedPtr request, + cartographer_ros_msgs::srv::WriteState::Response::SharedPtr response) { absl::MutexLock lock(&mutex_); - if (map_builder_bridge_.SerializeState(request.filename, - request.include_unfinished_submaps)) { - response.status.code = cartographer_ros_msgs::StatusCode::OK; - response.status.message = - absl::StrCat("State written to '", request.filename, "'."); + if (map_builder_bridge_->SerializeState(request->filename, + request->include_unfinished_submaps)) { + response->status.code = cartographer_ros_msgs::msg::StatusCode::OK; + response->status.message = + "State written to '" + request->filename + "'."; } else { - response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT; - response.status.message = - absl::StrCat("Failed to write '", request.filename, "'."); + response->status.code = cartographer_ros_msgs::msg::StatusCode::INVALID_ARGUMENT; + response->status.message = + "Failed to write '" + request->filename + "'."; } return true; } -bool Node::HandleReadMetrics( - ::cartographer_ros_msgs::ReadMetrics::Request& request, - ::cartographer_ros_msgs::ReadMetrics::Response& response) { +bool Node::handleReadMetrics( + const cartographer_ros_msgs::srv::ReadMetrics::Request::SharedPtr request, + cartographer_ros_msgs::srv::ReadMetrics::Response::SharedPtr response) { absl::MutexLock lock(&mutex_); - response.timestamp = ros::Time::now(); + response->timestamp = node_->now(); if (!metrics_registry_) { - response.status.code = cartographer_ros_msgs::StatusCode::UNAVAILABLE; - response.status.message = "Collection of runtime metrics is not activated."; + response->status.code = cartographer_ros_msgs::msg::StatusCode::UNAVAILABLE; + response->status.message = "Collection of runtime metrics is not activated."; return true; } - metrics_registry_->ReadMetrics(&response); - response.status.code = cartographer_ros_msgs::StatusCode::OK; - response.status.message = "Successfully read metrics."; + metrics_registry_->ReadMetrics(response); + response->status.code = cartographer_ros_msgs::msg::StatusCode::OK; + response->status.message = "Successfully read metrics."; return true; } void Node::FinishAllTrajectories() { absl::MutexLock lock(&mutex_); - for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) { + for (const auto& entry : map_builder_bridge_->GetTrajectoryStates()) { if (entry.second == TrajectoryState::ACTIVE) { const int trajectory_id = entry.first; CHECK_EQ(FinishTrajectoryUnderLock(trajectory_id).code, - cartographer_ros_msgs::StatusCode::OK); + cartographer_ros_msgs::msg::StatusCode::OK); } } } @@ -739,12 +760,12 @@ void Node::FinishAllTrajectories() { bool Node::FinishTrajectory(const int trajectory_id) { absl::MutexLock lock(&mutex_); return FinishTrajectoryUnderLock(trajectory_id).code == - cartographer_ros_msgs::StatusCode::OK; + cartographer_ros_msgs::msg::StatusCode::OK; } void Node::RunFinalOptimization() { { - for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) { + for (const auto& entry : map_builder_bridge_->GetTrajectoryStates()) { const int trajectory_id = entry.first; if (entry.second == TrajectoryState::ACTIVE) { LOG(WARNING) @@ -759,17 +780,17 @@ void Node::RunFinalOptimization() { } // Assuming we are not adding new data anymore, the final optimization // can be performed without holding the mutex. - map_builder_bridge_.RunFinalOptimization(); + map_builder_bridge_->RunFinalOptimization(); } void Node::HandleOdometryMessage(const int trajectory_id, const std::string& sensor_id, - const nav_msgs::Odometry::ConstPtr& msg) { + const nav_msgs::msg::Odometry::ConstSharedPtr& msg) { absl::MutexLock lock(&mutex_); if (!sensor_samplers_.at(trajectory_id).odometry_sampler.Pulse()) { return; } - auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id); + auto sensor_bridge_ptr = map_builder_bridge_->sensor_bridge(trajectory_id); auto odometry_data_ptr = sensor_bridge_ptr->ToOdometryData(msg); if (odometry_data_ptr != nullptr) { extrapolators_.at(trajectory_id).AddOdometryData(*odometry_data_ptr); @@ -779,34 +800,34 @@ void Node::HandleOdometryMessage(const int trajectory_id, void Node::HandleNavSatFixMessage(const int trajectory_id, const std::string& sensor_id, - const sensor_msgs::NavSatFix::ConstPtr& msg) { + const sensor_msgs::msg::NavSatFix::ConstSharedPtr& msg) { absl::MutexLock lock(&mutex_); if (!sensor_samplers_.at(trajectory_id).fixed_frame_pose_sampler.Pulse()) { return; } - map_builder_bridge_.sensor_bridge(trajectory_id) + map_builder_bridge_->sensor_bridge(trajectory_id) ->HandleNavSatFixMessage(sensor_id, msg); } void Node::HandleLandmarkMessage( const int trajectory_id, const std::string& sensor_id, - const cartographer_ros_msgs::LandmarkList::ConstPtr& msg) { + const cartographer_ros_msgs::msg::LandmarkList::ConstSharedPtr& msg) { absl::MutexLock lock(&mutex_); if (!sensor_samplers_.at(trajectory_id).landmark_sampler.Pulse()) { return; } - map_builder_bridge_.sensor_bridge(trajectory_id) + map_builder_bridge_->sensor_bridge(trajectory_id) ->HandleLandmarkMessage(sensor_id, msg); } void Node::HandleImuMessage(const int trajectory_id, const std::string& sensor_id, - const sensor_msgs::Imu::ConstPtr& msg) { + const sensor_msgs::msg::Imu::ConstSharedPtr& msg) { absl::MutexLock lock(&mutex_); if (!sensor_samplers_.at(trajectory_id).imu_sampler.Pulse()) { return; } - auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id); + auto sensor_bridge_ptr = map_builder_bridge_->sensor_bridge(trajectory_id); auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg); if (imu_data_ptr != nullptr) { extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr); @@ -816,34 +837,34 @@ void Node::HandleImuMessage(const int trajectory_id, void Node::HandleLaserScanMessage(const int trajectory_id, const std::string& sensor_id, - const sensor_msgs::LaserScan::ConstPtr& msg) { + const sensor_msgs::msg::LaserScan::ConstSharedPtr& msg) { absl::MutexLock lock(&mutex_); if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) { return; } - map_builder_bridge_.sensor_bridge(trajectory_id) + map_builder_bridge_->sensor_bridge(trajectory_id) ->HandleLaserScanMessage(sensor_id, msg); } void Node::HandleMultiEchoLaserScanMessage( const int trajectory_id, const std::string& sensor_id, - const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { + const sensor_msgs::msg::MultiEchoLaserScan::ConstSharedPtr& msg) { absl::MutexLock lock(&mutex_); if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) { return; } - map_builder_bridge_.sensor_bridge(trajectory_id) + map_builder_bridge_->sensor_bridge(trajectory_id) ->HandleMultiEchoLaserScanMessage(sensor_id, msg); } void Node::HandlePointCloud2Message( const int trajectory_id, const std::string& sensor_id, - const sensor_msgs::PointCloud2::ConstPtr& msg) { + const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg) { absl::MutexLock lock(&mutex_); if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) { return; } - map_builder_bridge_.sensor_bridge(trajectory_id) + map_builder_bridge_->sensor_bridge(trajectory_id) ->HandlePointCloud2Message(sensor_id, msg); } @@ -851,45 +872,45 @@ void Node::SerializeState(const std::string& filename, const bool include_unfinished_submaps) { absl::MutexLock lock(&mutex_); CHECK( - map_builder_bridge_.SerializeState(filename, include_unfinished_submaps)) + map_builder_bridge_->SerializeState(filename, include_unfinished_submaps)) << "Could not write state."; } void Node::LoadState(const std::string& state_filename, const bool load_frozen_state) { absl::MutexLock lock(&mutex_); - map_builder_bridge_.LoadState(state_filename, load_frozen_state); -} - -void Node::MaybeWarnAboutTopicMismatch( - const ::ros::WallTimerEvent& unused_timer_event) { - ::ros::master::V_TopicInfo ros_topics; - ::ros::master::getTopics(ros_topics); - std::set published_topics; - std::stringstream published_topics_string; - for (const auto& it : ros_topics) { - std::string resolved_topic = node_handle_.resolveName(it.name, false); - published_topics.insert(resolved_topic); - published_topics_string << resolved_topic << ","; - } - bool print_topics = false; - for (const auto& entry : subscribers_) { - int trajectory_id = entry.first; - for (const auto& subscriber : entry.second) { - std::string resolved_topic = node_handle_.resolveName(subscriber.topic); - if (published_topics.count(resolved_topic) == 0) { - LOG(WARNING) << "Expected topic \"" << subscriber.topic - << "\" (trajectory " << trajectory_id << ")" - << " (resolved topic \"" << resolved_topic << "\")" - << " but no publisher is currently active."; - print_topics = true; - } - } - } - if (print_topics) { - LOG(WARNING) << "Currently available topics are: " - << published_topics_string.str(); - } + map_builder_bridge_->LoadState(state_filename, load_frozen_state); +} + +// TODO: find ROS equivalent to ros::master::getTopics +void Node::MaybeWarnAboutTopicMismatch() { +// ::ros::master::V_TopicInfo ros_topics; +// ::ros::master::getTopics(ros_topics); +// std::set published_topics; +// std::stringstream published_topics_string; +// for (const auto& it : ros_topics) { +// std::string resolved_topic = node_handle_.resolveName(it.name, false); +// published_topics.insert(resolved_topic); +// published_topics_string << resolved_topic << ","; +// } +// bool print_topics = false; +// for (const auto& entry : subscribers_) { +// int trajectory_id = entry.first; +// for (const auto& subscriber : entry.second) { +// std::string resolved_topic = node_handle_.resolveName(subscriber.topic); +// if (published_topics.count(resolved_topic) == 0) { +// LOG(WARNING) << "Expected topic \"" << subscriber.topic +// << "\" (trajectory " << trajectory_id << ")" +// << " (resolved topic \"" << resolved_topic << "\")" +// << " but no publisher is currently active."; +// print_topics = true; +// } +// } +// } +// if (print_topics) { +// LOG(WARNING) << "Currently available topics are: " +// << published_topics_string.str(); +// } } } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index 0f7beadac..4cea9e862 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -33,23 +33,25 @@ #include "cartographer_ros/node_constants.h" #include "cartographer_ros/node_options.h" #include "cartographer_ros/trajectory_options.h" -#include "cartographer_ros_msgs/FinishTrajectory.h" -#include "cartographer_ros_msgs/GetTrajectoryStates.h" -#include "cartographer_ros_msgs/ReadMetrics.h" -#include "cartographer_ros_msgs/StartTrajectory.h" -#include "cartographer_ros_msgs/StatusResponse.h" -#include "cartographer_ros_msgs/SubmapEntry.h" -#include "cartographer_ros_msgs/SubmapList.h" -#include "cartographer_ros_msgs/SubmapQuery.h" -#include "cartographer_ros_msgs/WriteState.h" -#include "nav_msgs/Odometry.h" -#include "ros/ros.h" -#include "sensor_msgs/Imu.h" -#include "sensor_msgs/LaserScan.h" -#include "sensor_msgs/MultiEchoLaserScan.h" -#include "sensor_msgs/NavSatFix.h" -#include "sensor_msgs/PointCloud2.h" -#include "tf2_ros/transform_broadcaster.h" +#include "cartographer_ros_msgs/srv/finish_trajectory.hpp" +#include "cartographer_ros_msgs/srv/get_trajectory_states.hpp" +#include "cartographer_ros_msgs/srv/read_metrics.hpp" +#include "cartographer_ros_msgs/srv/start_trajectory.hpp" +#include "cartographer_ros_msgs/msg/status_response.hpp" +#include "cartographer_ros_msgs/msg/submap_entry.hpp" +#include "cartographer_ros_msgs/msg/submap_list.hpp" +#include "cartographer_ros_msgs/srv/submap_query.hpp" +#include "cartographer_ros_msgs/srv/write_state.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include namespace cartographer_ros { @@ -58,7 +60,9 @@ class Node { public: Node(const NodeOptions& node_options, std::unique_ptr map_builder, - tf2_ros::Buffer* tf_buffer, bool collect_metrics); + std::shared_ptr tf_buffer, + rclcpp::Node::SharedPtr node, + bool collect_metrics); ~Node(); Node(const Node&) = delete; @@ -93,21 +97,21 @@ class Node { // The following functions handle adding sensor data to a trajectory. void HandleOdometryMessage(int trajectory_id, const std::string& sensor_id, - const nav_msgs::Odometry::ConstPtr& msg); + const nav_msgs::msg::Odometry::ConstSharedPtr& msg); void HandleNavSatFixMessage(int trajectory_id, const std::string& sensor_id, - const sensor_msgs::NavSatFix::ConstPtr& msg); + const sensor_msgs::msg::NavSatFix::ConstSharedPtr& msg); void HandleLandmarkMessage( int trajectory_id, const std::string& sensor_id, - const cartographer_ros_msgs::LandmarkList::ConstPtr& msg); + const cartographer_ros_msgs::msg::LandmarkList::ConstSharedPtr& msg); void HandleImuMessage(int trajectory_id, const std::string& sensor_id, - const sensor_msgs::Imu::ConstPtr& msg); + const sensor_msgs::msg::Imu::ConstSharedPtr &msg); void HandleLaserScanMessage(int trajectory_id, const std::string& sensor_id, - const sensor_msgs::LaserScan::ConstPtr& msg); + const sensor_msgs::msg::LaserScan::ConstSharedPtr& msg); void HandleMultiEchoLaserScanMessage( int trajectory_id, const std::string& sensor_id, - const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg); + const sensor_msgs::msg::MultiEchoLaserScan::ConstSharedPtr& msg); void HandlePointCloud2Message(int trajectory_id, const std::string& sensor_id, - const sensor_msgs::PointCloud2::ConstPtr& msg); + const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg); // Serializes the complete Node state. void SerializeState(const std::string& filename, @@ -116,11 +120,9 @@ class Node { // Loads a serialized SLAM state from a .pbstream file. void LoadState(const std::string& state_filename, bool load_frozen_state); - ::ros::NodeHandle* node_handle(); - private: struct Subscriber { - ::ros::Subscriber subscriber; + rclcpp::SubscriptionBase::SharedPtr subscriber; // ::ros::Subscriber::getTopic() does not necessarily return the same // std::string @@ -129,26 +131,27 @@ class Node { std::string topic; }; - 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); - bool HandleFinishTrajectory( - cartographer_ros_msgs::FinishTrajectory::Request& request, - cartographer_ros_msgs::FinishTrajectory::Response& response); - bool HandleWriteState(cartographer_ros_msgs::WriteState::Request& request, - cartographer_ros_msgs::WriteState::Response& response); - bool HandleGetTrajectoryStates( - ::cartographer_ros_msgs::GetTrajectoryStates::Request& request, - ::cartographer_ros_msgs::GetTrajectoryStates::Response& response); - bool HandleReadMetrics( - cartographer_ros_msgs::ReadMetrics::Request& request, - cartographer_ros_msgs::ReadMetrics::Response& response); + bool handleSubmapQuery( + const cartographer_ros_msgs::srv::SubmapQuery::Request::SharedPtr request, + cartographer_ros_msgs::srv::SubmapQuery::Response::SharedPtr response); + bool handleTrajectoryQuery( + const cartographer_ros_msgs::srv::TrajectoryQuery::Request::SharedPtr request, + cartographer_ros_msgs::srv::TrajectoryQuery::Response::SharedPtr response); + bool handleStartTrajectory( + const cartographer_ros_msgs::srv::StartTrajectory::Request::SharedPtr request, + cartographer_ros_msgs::srv::StartTrajectory::Response::SharedPtr response); + bool handleFinishTrajectory( + const cartographer_ros_msgs::srv::FinishTrajectory::Request::SharedPtr request, + cartographer_ros_msgs::srv::FinishTrajectory::Response::SharedPtr response); + bool handleWriteState( + const cartographer_ros_msgs::srv::WriteState::Request::SharedPtr request, + cartographer_ros_msgs::srv::WriteState::Response::SharedPtr response); + bool handleGetTrajectoryStates( + const cartographer_ros_msgs::srv::GetTrajectoryStates::Request::SharedPtr request, + cartographer_ros_msgs::srv::GetTrajectoryStates::Response::SharedPtr response); + bool handleReadMetrics( + const cartographer_ros_msgs::srv::ReadMetrics::Request::SharedPtr request, + cartographer_ros_msgs::srv::ReadMetrics::Response::SharedPtr response); // Returns the set of SensorIds expected for a trajectory. // 'SensorId::id' is the expected ROS topic name. @@ -156,42 +159,49 @@ class Node { ComputeExpectedSensorIds(const TrajectoryOptions& options) const; int AddTrajectory(const TrajectoryOptions& options); void LaunchSubscribers(const TrajectoryOptions& options, int trajectory_id); - void PublishSubmapList(const ::ros::WallTimerEvent& timer_event); + void PublishSubmapList(); void AddExtrapolator(int trajectory_id, const TrajectoryOptions& options); void AddSensorSamplers(int trajectory_id, const TrajectoryOptions& options); - void PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event); - void PublishTrajectoryNodeList(const ::ros::WallTimerEvent& timer_event); - void PublishLandmarkPosesList(const ::ros::WallTimerEvent& timer_event); - void PublishConstraintList(const ::ros::WallTimerEvent& timer_event); + void PublishLocalTrajectoryData(); + void PublishTrajectoryNodeList(); + void PublishLandmarkPosesList(); + void PublishConstraintList(); bool ValidateTrajectoryOptions(const TrajectoryOptions& options); bool ValidateTopicNames(const TrajectoryOptions& options); - cartographer_ros_msgs::StatusResponse FinishTrajectoryUnderLock( + cartographer_ros_msgs::msg::StatusResponse FinishTrajectoryUnderLock( int trajectory_id) EXCLUSIVE_LOCKS_REQUIRED(mutex_); - void MaybeWarnAboutTopicMismatch(const ::ros::WallTimerEvent&); + void MaybeWarnAboutTopicMismatch(); // Helper function for service handlers that need to check trajectory states. - cartographer_ros_msgs::StatusResponse TrajectoryStateToStatus( + cartographer_ros_msgs::msg::StatusResponse TrajectoryStateToStatus( int trajectory_id, const std::set< cartographer::mapping::PoseGraphInterface::TrajectoryState>& valid_states); const NodeOptions node_options_; - tf2_ros::TransformBroadcaster tf_broadcaster_; + std::shared_ptr tf_broadcaster_; absl::Mutex mutex_; std::unique_ptr metrics_registry_; - MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_); - - ::ros::NodeHandle node_handle_; - ::ros::Publisher submap_list_publisher_; - ::ros::Publisher trajectory_node_list_publisher_; - ::ros::Publisher landmark_poses_list_publisher_; - ::ros::Publisher constraint_list_publisher_; - ::ros::Publisher tracked_pose_publisher_; - // These ros::ServiceServers need to live for the lifetime of the node. - std::vector<::ros::ServiceServer> service_servers_; - ::ros::Publisher scan_matched_point_cloud_publisher_; + std::shared_ptr map_builder_bridge_ GUARDED_BY(mutex_); + + rclcpp::Node::SharedPtr node_; + ::rclcpp::Publisher<::cartographer_ros_msgs::msg::SubmapList>::SharedPtr submap_list_publisher_; + ::rclcpp::Publisher<::visualization_msgs::msg::MarkerArray>::SharedPtr trajectory_node_list_publisher_; + ::rclcpp::Publisher<::visualization_msgs::msg::MarkerArray>::SharedPtr landmark_poses_list_publisher_; + ::rclcpp::Publisher<::visualization_msgs::msg::MarkerArray>::SharedPtr constraint_list_publisher_; + ::rclcpp::Publisher::SharedPtr tracked_pose_publisher_; + ::rclcpp::Publisher::SharedPtr scan_matched_point_cloud_publisher_; + // These ros service servers need to live for the lifetime of the node. + ::rclcpp::Service::SharedPtr submap_query_server_; + ::rclcpp::Service::SharedPtr trajectory_query_server; + ::rclcpp::Service::SharedPtr start_trajectory_server_; + ::rclcpp::Service::SharedPtr finish_trajectory_server_; + ::rclcpp::Service::SharedPtr write_state_server_; + ::rclcpp::Service::SharedPtr get_trajectory_states_server_; + ::rclcpp::Service::SharedPtr read_metrics_server_; + struct TrajectorySensorSamplers { TrajectorySensorSamplers(const double rangefinder_sampling_ratio, @@ -214,21 +224,22 @@ class Node { // These are keyed with 'trajectory_id'. std::map extrapolators_; - std::map last_published_tf_stamps_; + std::map last_published_tf_stamps_; std::unordered_map sensor_samplers_; std::unordered_map> subscribers_; std::unordered_set subscribed_topics_; std::unordered_set trajectories_scheduled_for_finish_; - // We have to keep the timer handles of ::ros::WallTimers around, otherwise - // they do not fire. - std::vector<::ros::WallTimer> wall_timers_; - // The timer for publishing local trajectory data (i.e. pose transforms and // range data point clouds) is a regular timer which is not triggered when // simulation time is standing still. This prevents overflowing the transform // listener buffer by publishing the same transforms over and over again. - ::ros::Timer publish_local_trajectory_data_timer_; + ::rclcpp::TimerBase::SharedPtr submap_list_timer_; + ::rclcpp::TimerBase::SharedPtr local_trajectory_data_timer_; + ::rclcpp::TimerBase::SharedPtr trajectory_node_list_timer_; + ::rclcpp::TimerBase::SharedPtr landmark_pose_list_timer_; + ::rclcpp::TimerBase::SharedPtr constrain_list_timer_; + ::rclcpp::TimerBase::SharedPtr maybe_warn_about_topic_mismatch_timer_; }; } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/node_main.cc b/cartographer_ros/cartographer_ros/node_main.cc index 0a6dee342..f403be079 100644 --- a/cartographer_ros/cartographer_ros/node_main.cc +++ b/cartographer_ros/cartographer_ros/node_main.cc @@ -48,33 +48,43 @@ namespace cartographer_ros { namespace { void Run() { + rclcpp::Node::SharedPtr cartographer_node = rclcpp::Node::make_shared("cartographer_node"); constexpr double kTfBufferCacheTimeInSeconds = 10.; - tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)}; - tf2_ros::TransformListener tf(tf_buffer); + + std::shared_ptr tf_buffer = + std::make_shared( + cartographer_node->get_clock(), + tf2::durationFromSec(kTfBufferCacheTimeInSeconds), + cartographer_node); + + std::shared_ptr tf_listener = + std::make_shared(*tf_buffer); + NodeOptions node_options; TrajectoryOptions trajectory_options; std::tie(node_options, trajectory_options) = LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename); auto map_builder = - cartographer::mapping::CreateMapBuilder(node_options.map_builder_options); - Node node(node_options, std::move(map_builder), &tf_buffer, - FLAGS_collect_metrics); + cartographer::mapping::CreateMapBuilder(node_options.map_builder_options); + auto node = std::make_shared( + node_options, std::move(map_builder), tf_buffer, cartographer_node, + FLAGS_collect_metrics); if (!FLAGS_load_state_filename.empty()) { - node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state); + node->LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state); } if (FLAGS_start_trajectory_with_default_topics) { - node.StartTrajectoryWithDefaultTopics(trajectory_options); + node->StartTrajectoryWithDefaultTopics(trajectory_options); } - ::ros::spin(); + rclcpp::spin(cartographer_node); - node.FinishAllTrajectories(); - node.RunFinalOptimization(); + node->FinishAllTrajectories(); + node->RunFinalOptimization(); if (!FLAGS_save_state_filename.empty()) { - node.SerializeState(FLAGS_save_state_filename, + node->SerializeState(FLAGS_save_state_filename, true /* include_unfinished_submaps */); } } @@ -83,18 +93,19 @@ void Run() { } // namespace cartographer_ros int main(int argc, char** argv) { + // Init rclcpp first because gflags reorders command line flags in argv + rclcpp::init(argc, argv); + + google::AllowCommandLineReparsing(); google::InitGoogleLogging(argv[0]); - google::ParseCommandLineFlags(&argc, &argv, true); + google::ParseCommandLineFlags(&argc, &argv, false); CHECK(!FLAGS_configuration_directory.empty()) << "-configuration_directory is missing."; CHECK(!FLAGS_configuration_basename.empty()) << "-configuration_basename is missing."; - ::ros::init(argc, argv, "cartographer_node"); - ::ros::start(); - cartographer_ros::ScopedRosLogSink ros_log_sink; cartographer_ros::Run(); - ::ros::shutdown(); + ::rclcpp::shutdown(); } diff --git a/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc b/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc index eb0e667e6..324426bf9 100644 --- a/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc +++ b/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc @@ -31,11 +31,12 @@ #include "cartographer_ros/node_constants.h" #include "cartographer_ros/ros_log_sink.h" #include "cartographer_ros/submap.h" -#include "cartographer_ros_msgs/SubmapList.h" -#include "cartographer_ros_msgs/SubmapQuery.h" +#include "cartographer_ros_msgs/msg/submap_entry.hpp" +#include "cartographer_ros_msgs/msg/submap_list.hpp" +#include "cartographer_ros_msgs/srv/submap_query.hpp" #include "gflags/gflags.h" -#include "nav_msgs/OccupancyGrid.h" -#include "ros/ros.h" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include DEFINE_double(resolution, 0.05, "Resolution of a grid cell in the published occupancy grid."); @@ -54,7 +55,8 @@ using ::cartographer::io::PaintSubmapSlicesResult; using ::cartographer::io::SubmapSlice; using ::cartographer::mapping::SubmapId; -class Node { +class Node : public rclcpp::Node +{ public: explicit Node(double resolution, double publish_period_sec); ~Node() {} @@ -63,130 +65,144 @@ class Node { Node& operator=(const Node&) = delete; private: - void HandleSubmapList(const cartographer_ros_msgs::SubmapList::ConstPtr& msg); - void DrawAndPublish(const ::ros::WallTimerEvent& timer_event); + void HandleSubmapList(const cartographer_ros_msgs::msg::SubmapList::ConstSharedPtr& msg); + void DrawAndPublish(); - ::ros::NodeHandle node_handle_; const double resolution_; absl::Mutex mutex_; - ::ros::ServiceClient client_ GUARDED_BY(mutex_); - ::ros::Subscriber submap_list_subscriber_ GUARDED_BY(mutex_); - ::ros::Publisher occupancy_grid_publisher_ GUARDED_BY(mutex_); + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor_; + ::rclcpp::Client::SharedPtr client_ GUARDED_BY(mutex_); + ::rclcpp::Subscription::SharedPtr submap_list_subscriber_ GUARDED_BY(mutex_); + ::rclcpp::Publisher<::nav_msgs::msg::OccupancyGrid>::SharedPtr occupancy_grid_publisher_ GUARDED_BY(mutex_); std::map submap_slices_ GUARDED_BY(mutex_); - ::ros::WallTimer occupancy_grid_publisher_timer_; + rclcpp::TimerBase::SharedPtr occupancy_grid_publisher_timer_; std::string last_frame_id_; - ros::Time last_timestamp_; + rclcpp::Time last_timestamp_; }; Node::Node(const double resolution, const double publish_period_sec) - : resolution_(resolution), - client_(node_handle_.serviceClient<::cartographer_ros_msgs::SubmapQuery>( - kSubmapQueryServiceName)), - submap_list_subscriber_(node_handle_.subscribe( - kSubmapListTopic, kLatestOnlyPublisherQueueSize, - boost::function( - [this](const cartographer_ros_msgs::SubmapList::ConstPtr& msg) { - HandleSubmapList(msg); - }))), - occupancy_grid_publisher_( - node_handle_.advertise<::nav_msgs::OccupancyGrid>( - FLAGS_occupancy_grid_topic, kLatestOnlyPublisherQueueSize, - true /* latched */)), - occupancy_grid_publisher_timer_( - node_handle_.createWallTimer(::ros::WallDuration(publish_period_sec), - &Node::DrawAndPublish, this)) {} - -void Node::HandleSubmapList( - const cartographer_ros_msgs::SubmapList::ConstPtr& msg) { - absl::MutexLock locker(&mutex_); - - // We do not do any work if nobody listens. - if (occupancy_grid_publisher_.getNumSubscribers() == 0) { - return; - } - - // Keep track of submap IDs that don't appear in the message anymore. - std::set submap_ids_to_delete; - for (const auto& pair : submap_slices_) { - submap_ids_to_delete.insert(pair.first); - } + : rclcpp::Node("cartographer_occupancy_grid_node"), + resolution_(resolution) +{ + callback_group_ = this->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + callback_group_executor_ = std::make_shared(); + callback_group_executor_->add_callback_group(callback_group_, this->get_node_base_interface()); + client_ = this->create_client( + kSubmapQueryServiceName, + rmw_qos_profile_services_default, + callback_group_ + ); + + occupancy_grid_publisher_ = this->create_publisher<::nav_msgs::msg::OccupancyGrid>( + kOccupancyGridTopic, rclcpp::QoS(10).transient_local()); + + occupancy_grid_publisher_timer_ = this->create_wall_timer( + std::chrono::milliseconds(int(publish_period_sec * 1000)), + [this]() { + DrawAndPublish(); + }); + + auto handleSubmapList = + [this, publish_period_sec](const typename cartographer_ros_msgs::msg::SubmapList::SharedPtr msg) -> void + { + absl::MutexLock locker(&mutex_); + + // We do not do any work if nobody listens. + if (this->count_publishers(kSubmapListTopic) == 0){ + return; + } - for (const auto& submap_msg : msg->submap) { - const SubmapId id{submap_msg.trajectory_id, submap_msg.submap_index}; - submap_ids_to_delete.erase(id); - if ((submap_msg.is_frozen && !FLAGS_include_frozen_submaps) || - (!submap_msg.is_frozen && !FLAGS_include_unfrozen_submaps)) { - continue; + // Keep track of submap IDs that don't appear in the message anymore. + std::set submap_ids_to_delete; + for (const auto& pair : submap_slices_) { + submap_ids_to_delete.insert(pair.first); } - SubmapSlice& submap_slice = submap_slices_[id]; - submap_slice.pose = ToRigid3d(submap_msg.pose); - submap_slice.metadata_version = submap_msg.submap_version; - if (submap_slice.surface != nullptr && - submap_slice.version == submap_msg.submap_version) { - continue; + + for (const auto& submap_msg : msg->submap) { + const SubmapId id{submap_msg.trajectory_id, submap_msg.submap_index}; + submap_ids_to_delete.erase(id); + if ((submap_msg.is_frozen && !FLAGS_include_frozen_submaps) || + (!submap_msg.is_frozen && !FLAGS_include_unfrozen_submaps)) { + continue; + } + SubmapSlice& submap_slice = submap_slices_[id]; + submap_slice.pose = ToRigid3d(submap_msg.pose); + submap_slice.metadata_version = submap_msg.submap_version; + if (submap_slice.surface != nullptr && + submap_slice.version == submap_msg.submap_version) { + continue; + } + + auto fetched_textures = cartographer_ros::FetchSubmapTextures( + id, client_, callback_group_executor_, + std::chrono::milliseconds(int(publish_period_sec * 1000))); + if (fetched_textures == nullptr) { + continue; + } + CHECK(!fetched_textures->textures.empty()); + submap_slice.version = fetched_textures->version; + + // We use the first texture only. By convention this is the highest + // resolution texture and that is the one we want to use to construct the + // map for ROS. + const auto fetched_texture = fetched_textures->textures.begin(); + submap_slice.width = fetched_texture->width; + submap_slice.height = fetched_texture->height; + submap_slice.slice_pose = fetched_texture->slice_pose; + submap_slice.resolution = fetched_texture->resolution; + submap_slice.cairo_data.clear(); + submap_slice.surface = ::cartographer::io::DrawTexture( + fetched_texture->pixels.intensity, fetched_texture->pixels.alpha, + fetched_texture->width, fetched_texture->height, + &submap_slice.cairo_data); } - auto fetched_textures = - ::cartographer_ros::FetchSubmapTextures(id, &client_); - if (fetched_textures == nullptr) { - continue; + // Delete all submaps that didn't appear in the message. + for (const auto& id : submap_ids_to_delete) { + submap_slices_.erase(id); } - CHECK(!fetched_textures->textures.empty()); - submap_slice.version = fetched_textures->version; - - // We use the first texture only. By convention this is the highest - // resolution texture and that is the one we want to use to construct the - // map for ROS. - const auto fetched_texture = fetched_textures->textures.begin(); - submap_slice.width = fetched_texture->width; - submap_slice.height = fetched_texture->height; - submap_slice.slice_pose = fetched_texture->slice_pose; - submap_slice.resolution = fetched_texture->resolution; - submap_slice.cairo_data.clear(); - submap_slice.surface = ::cartographer::io::DrawTexture( - fetched_texture->pixels.intensity, fetched_texture->pixels.alpha, - fetched_texture->width, fetched_texture->height, - &submap_slice.cairo_data); - } - // Delete all submaps that didn't appear in the message. - for (const auto& id : submap_ids_to_delete) { - submap_slices_.erase(id); - } + last_timestamp_ = msg->header.stamp; + last_frame_id_ = msg->header.frame_id; + }; - last_timestamp_ = msg->header.stamp; - last_frame_id_ = msg->header.frame_id; + submap_list_subscriber_ = create_subscription( + kSubmapListTopic, rclcpp::QoS(10), handleSubmapList); } -void Node::DrawAndPublish(const ::ros::WallTimerEvent& unused_timer_event) { +void Node::DrawAndPublish() { absl::MutexLock locker(&mutex_); if (submap_slices_.empty() || last_frame_id_.empty()) { return; } auto painted_slices = PaintSubmapSlices(submap_slices_, resolution_); - std::unique_ptr msg_ptr = CreateOccupancyGridMsg( + std::unique_ptr msg_ptr = CreateOccupancyGridMsg( painted_slices, resolution_, last_frame_id_, last_timestamp_); - occupancy_grid_publisher_.publish(*msg_ptr); + occupancy_grid_publisher_->publish(*msg_ptr); } } // namespace } // namespace cartographer_ros int main(int argc, char** argv) { + // Init rclcpp first because gflags reorders command line flags in argv + rclcpp::init(argc, argv); + + google::AllowCommandLineReparsing(); google::InitGoogleLogging(argv[0]); - google::ParseCommandLineFlags(&argc, &argv, true); + google::ParseCommandLineFlags(&argc, &argv, false); CHECK(FLAGS_include_frozen_submaps || FLAGS_include_unfrozen_submaps) << "Ignoring both frozen and unfrozen submaps makes no sense."; - ::ros::init(argc, argv, "cartographer_occupancy_grid_node"); - ::ros::start(); - cartographer_ros::ScopedRosLogSink ros_log_sink; - ::cartographer_ros::Node node(FLAGS_resolution, FLAGS_publish_period_sec); + auto node = std::make_shared(FLAGS_resolution, FLAGS_publish_period_sec); - ::ros::spin(); - ::ros::shutdown(); + rclcpp::spin(node); + ::rclcpp::shutdown(); + return 0; } diff --git a/cartographer_ros/cartographer_ros/offline_node.cc b/cartographer_ros/cartographer_ros/offline_node.cc index e3a7e7abc..ed4af07ff 100644 --- a/cartographer_ros/cartographer_ros/offline_node.cc +++ b/cartographer_ros/cartographer_ros/offline_node.cc @@ -17,7 +17,7 @@ #include "cartographer_ros/offline_node.h" #include -#include +#include #ifndef WIN32 #include #endif @@ -25,15 +25,16 @@ #include -#include "absl/strings/str_split.h" #include "cartographer_ros/node.h" #include "cartographer_ros/playable_bag.h" #include "cartographer_ros/urdf_reader.h" #include "gflags/gflags.h" -#include "ros/callback_queue.h" -#include "rosgraph_msgs/Clock.h" +#include "rosgraph_msgs/msg/clock.hpp" #include "tf2_ros/static_transform_broadcaster.h" #include "urdf/model.h" +#include "rclcpp/exceptions.hpp" +//#include +#include DEFINE_bool(collect_metrics, false, "Activates the collection of runtime metrics. If activated, the " @@ -79,26 +80,29 @@ namespace cartographer_ros { constexpr char kClockTopic[] = "clock"; constexpr char kTfStaticTopic[] = "/tf_static"; -constexpr char kTfTopic[] = "tf"; +constexpr char kTfTopic[] = "/tf"; constexpr double kClockPublishFrequencySec = 1. / 30.; constexpr int kSingleThreaded = 1; // We publish tf messages one second earlier than other messages. Under // the assumption of higher frequency tf this should ensure that tf can // always interpolate. -const ::ros::Duration kDelay = ::ros::Duration(1.0); +const rclcpp::Duration kDelay(1.0, 0); -void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { +void RunOfflineNode(const MapBuilderFactory& map_builder_factory, + rclcpp::Node::SharedPtr cartographer_offline_node) { CHECK(!FLAGS_configuration_directory.empty()) << "-configuration_directory is missing."; + LOG(WARNING) << "FLAGS_configuration_directory " << FLAGS_configuration_directory; CHECK(!FLAGS_configuration_basenames.empty()) << "-configuration_basenames is missing."; + LOG(WARNING) << "FLAGS_configuration_basenames " << FLAGS_configuration_basenames; CHECK(!(FLAGS_bag_filenames.empty() && FLAGS_load_state_filename.empty())) << "-bag_filenames and -load_state_filename cannot both be unspecified."; - const std::vector bag_filenames = - absl::StrSplit(FLAGS_bag_filenames, ',', absl::SkipEmpty()); + std::vector bag_filenames; + boost::split(bag_filenames, FLAGS_bag_filenames, boost::is_any_of(",")); cartographer_ros::NodeOptions node_options; - const std::vector configuration_basenames = - absl::StrSplit(FLAGS_configuration_basenames, ',', absl::SkipEmpty()); + std::vector configuration_basenames; + boost::split(configuration_basenames, FLAGS_configuration_basenames, boost::is_any_of(",")); std::vector bag_trajectory_options(1); std::tie(node_options, bag_trajectory_options.at(0)) = LoadOptions(FLAGS_configuration_directory, configuration_basenames.at(0)); @@ -127,50 +131,48 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { const std::chrono::time_point start_time = std::chrono::steady_clock::now(); - tf2_ros::Buffer tf_buffer; - - std::vector urdf_transforms; - const std::vector urdf_filenames = - absl::StrSplit(FLAGS_urdf_filenames, ',', absl::SkipEmpty()); - for (const auto& urdf_filename : urdf_filenames) { - const auto current_urdf_transforms = - ReadStaticTransformsFromUrdf(urdf_filename, &tf_buffer); - urdf_transforms.insert(urdf_transforms.end(), - current_urdf_transforms.begin(), - current_urdf_transforms.end()); + std::shared_ptr tf_buffer = + std::make_shared( + cartographer_offline_node->get_clock(), + tf2::durationFromSec(10), + cartographer_offline_node); + + std::vector urdf_transforms; + + if (!FLAGS_urdf_filenames.empty()) { + std::vector urdf_filenames; + boost::split(urdf_filenames, FLAGS_urdf_filenames, boost::is_any_of(",")); + for (const auto& urdf_filename : urdf_filenames) { + const auto current_urdf_transforms = + ReadStaticTransformsFromUrdf(urdf_filename, tf_buffer); + urdf_transforms.insert(urdf_transforms.end(), + current_urdf_transforms.begin(), + current_urdf_transforms.end()); + } } + tf_buffer->setUsingDedicatedThread(true); - tf_buffer.setUsingDedicatedThread(true); - - Node node(node_options, std::move(map_builder), &tf_buffer, + Node node(node_options, std::move(map_builder), tf_buffer, cartographer_offline_node, FLAGS_collect_metrics); if (!FLAGS_load_state_filename.empty()) { node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state); } - ::ros::Publisher tf_publisher = - node.node_handle()->advertise( + rclcpp::Publisher::SharedPtr tf_publisher = + cartographer_offline_node->create_publisher( kTfTopic, kLatestOnlyPublisherQueueSize); - ::tf2_ros::StaticTransformBroadcaster static_tf_broadcaster; + ::tf2_ros::StaticTransformBroadcaster static_tf_broadcaster(cartographer_offline_node); - ::ros::Publisher clock_publisher = - node.node_handle()->advertise( + rclcpp::Publisher::SharedPtr clock_publisher = + cartographer_offline_node->create_publisher( kClockTopic, kLatestOnlyPublisherQueueSize); if (urdf_transforms.size() > 0) { static_tf_broadcaster.sendTransform(urdf_transforms); } - ros::AsyncSpinner async_spinner(kSingleThreaded); - async_spinner.start(); - rosgraph_msgs::Clock clock; - auto clock_republish_timer = node.node_handle()->createWallTimer( - ::ros::WallDuration(kClockPublishFrequencySec), - [&clock_publisher, &clock](const ::ros::WallTimerEvent&) { - clock_publisher.publish(clock); - }, - false /* oneshot */, false /* autostart */); + rosgraph_msgs::msg::Clock clock; std::vector< std::set> @@ -190,20 +192,22 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { std::map, cartographer::mapping::TrajectoryBuilderInterface::SensorId> bag_topic_to_sensor_id; - PlayableBagMultiplexer playable_bag_multiplexer; + PlayableBagMultiplexer playable_bag_multiplexer(cartographer_offline_node); for (size_t current_bag_index = 0; current_bag_index < bag_filenames.size(); ++current_bag_index) { const std::string& bag_filename = bag_filenames.at(current_bag_index); - if (!::ros::ok()) { + if (!rclcpp::ok()) { return; } for (const auto& expected_sensor_id : bag_expected_sensor_ids.at(current_bag_index)) { + // TODO: check resolved topic + LOG(INFO) << "expected_sensor_id.id " << expected_sensor_id.id; const auto bag_resolved_topic = std::make_pair( static_cast(current_bag_index), - node.node_handle()->resolveName(expected_sensor_id.id)); + "/"+expected_sensor_id.id); if (bag_topic_to_sensor_id.count(bag_resolved_topic) != 0) { - LOG(ERROR) << "Sensor " << expected_sensor_id.id << " of bag " + LOG(ERROR) << "Sensor /" << expected_sensor_id.id << " of bag " << current_bag_index << " resolves to topic " << bag_resolved_topic.second << " which is already used by " << " sensor " @@ -212,30 +216,37 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { bag_topic_to_sensor_id[bag_resolved_topic] = expected_sensor_id; } + auto serializer = rclcpp::Serialization(); playable_bag_multiplexer.AddPlayableBag(PlayableBag( - bag_filename, current_bag_index, ros::TIME_MIN, ros::TIME_MAX, kDelay, + bag_filename, current_bag_index, kDelay, // PlayableBag::FilteringEarlyMessageHandler is used to get an early // peek at the tf messages in the bag and insert them into 'tf_buffer'. // When a message is retrieved by GetNextMessage() further below, // we will have already inserted further 'kDelay' seconds worth of // transforms into 'tf_buffer' via this lambda. - [&tf_publisher, &tf_buffer](const rosbag::MessageInstance& msg) { - if (msg.isType()) { + [&tf_publisher, tf_buffer, cartographer_offline_node, serializer](std::shared_ptr msg) { + // TODO: filter bag msg per type ? Planned rosbag2 evolution ? + if (msg->topic_name == kTfTopic || msg->topic_name == kTfStaticTopic) { if (FLAGS_use_bag_transforms) { - const auto tf_message = msg.instantiate(); - tf_publisher.publish(tf_message); - - for (const auto& transform : tf_message->transforms) { - try { - // We need to keep 'tf_buffer' small because it becomes very - // inefficient otherwise. We make sure that tf_messages are - // published before any data messages, so that tf lookups - // always work. - tf_buffer.setTransform(transform, "unused_authority", - msg.getTopic() == kTfStaticTopic); - } catch (const tf2::TransformException& ex) { - LOG(WARNING) << ex.what(); + tf2_msgs::msg::TFMessage tf_message; + rclcpp::SerializedMessage serialized_msg(*msg->serialized_data); + try { + serializer.deserialize_message(&serialized_msg, &tf_message); + for (auto& transform : tf_message.transforms) { + try { + // We need to keep 'tf_buffer' small because it becomes very + // inefficient otherwise. We make sure that tf_messages are + // published before any data messages, so that tf lookups + // always work. + tf_buffer->setTransform(transform, "unused_authority", + msg->topic_name == kTfStaticTopic); + } catch (const tf2::TransformException& ex) { + LOG(WARNING) << ex.what(); + } } + tf_publisher->publish(tf_message); + } catch (const rclcpp::exceptions::RCLError& rcl_error) { + return true; } } // Tell 'PlayableBag' to filter the tf message since there is no @@ -250,9 +261,9 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { std::set bag_topics; std::stringstream bag_topics_string; for (const auto& topic : playable_bag_multiplexer.topics()) { - std::string resolved_topic = node.node_handle()->resolveName(topic, false); - bag_topics.insert(resolved_topic); - bag_topics_string << resolved_topic << ","; + // TODO: check resolved topic + bag_topics.insert(topic); + bag_topics_string << topic << ","; } bool print_topics = false; for (const auto& entry : bag_topic_to_sensor_id) { @@ -269,22 +280,33 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { } std::unordered_map bag_index_to_trajectory_id; - const ros::Time begin_time = + const rclcpp::Time begin_time = // If no bags were loaded, we cannot peek the time of first message. playable_bag_multiplexer.IsMessageAvailable() ? playable_bag_multiplexer.PeekMessageTime() - : ros::Time(); + : rclcpp::Time(); + + auto laser_scan_serializer = rclcpp::Serialization(); + auto multi_echo_laser_scan_serializer = rclcpp::Serialization(); + auto pcl2_serializer = rclcpp::Serialization(); + auto imu_serializer = rclcpp::Serialization(); + auto odom_serializer = rclcpp::Serialization(); + auto nav_sat_fix_serializer = rclcpp::Serialization(); + auto landmark_list_serializer = rclcpp::Serialization(); + while (playable_bag_multiplexer.IsMessageAvailable()) { - if (!::ros::ok()) { + if (!::rclcpp::ok()) { return; } const auto next_msg_tuple = playable_bag_multiplexer.GetNextMessage(); - const rosbag::MessageInstance& msg = std::get<0>(next_msg_tuple); + const rosbag2_storage::SerializedBagMessage& msg = std::get<0>(next_msg_tuple); const int bag_index = std::get<1>(next_msg_tuple); - const bool is_last_message_in_bag = std::get<2>(next_msg_tuple); + const std::string topic_type = std::get<2>(next_msg_tuple); + const bool is_last_message_in_bag = std::get<3>(next_msg_tuple); - if (msg.getTime() < (begin_time + ros::Duration(FLAGS_skip_seconds))) { + if (msg.time_stamp < + (begin_time.nanoseconds() + rclcpp::Duration(FLAGS_skip_seconds, 0).nanoseconds())) { continue; } @@ -308,44 +330,67 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { const auto bag_topic = std::make_pair( bag_index, - node.node_handle()->resolveName(msg.getTopic(), false /* resolve */)); + // TODO: check resolved topic + msg.topic_name); auto it = bag_topic_to_sensor_id.find(bag_topic); + if (it != bag_topic_to_sensor_id.end()) { const std::string& sensor_id = it->second.id; - if (msg.isType()) { + + if (topic_type == "sensor_msgs/msg/LaserScan") { + rclcpp::SerializedMessage serialized_msg(*msg.serialized_data); + sensor_msgs::msg::LaserScan::SharedPtr laser_scan_msg = + std::make_shared(); + laser_scan_serializer.deserialize_message(&serialized_msg, laser_scan_msg.get()); node.HandleLaserScanMessage(trajectory_id, sensor_id, - msg.instantiate()); - } - if (msg.isType()) { - node.HandleMultiEchoLaserScanMessage( - trajectory_id, sensor_id, - msg.instantiate()); - } - if (msg.isType()) { - node.HandlePointCloud2Message( - trajectory_id, sensor_id, - msg.instantiate()); - } - if (msg.isType()) { + laser_scan_msg); + } else if (topic_type == "sensor_msgs/msg/MultiEchoLaserScan") { + rclcpp::SerializedMessage serialized_msg(*msg.serialized_data); + sensor_msgs::msg::MultiEchoLaserScan::SharedPtr multi_echo_laser_scan_msg = + std::make_shared(); + multi_echo_laser_scan_serializer.deserialize_message(&serialized_msg, &multi_echo_laser_scan_msg); + node.HandleMultiEchoLaserScanMessage(trajectory_id, sensor_id, + multi_echo_laser_scan_msg); + } else if (topic_type == "sensor_msgs/msg/PointCloud2") { + rclcpp::SerializedMessage serialized_msg(*msg.serialized_data); + sensor_msgs::msg::PointCloud2::SharedPtr pcl2_scan_msg = + std::make_shared(); + pcl2_serializer.deserialize_message(&serialized_msg, &pcl2_scan_msg); + node.HandlePointCloud2Message(trajectory_id, sensor_id, + pcl2_scan_msg); + } else if (topic_type == "sensor_msgs/msg/Imu") { + rclcpp::SerializedMessage serialized_msg(*msg.serialized_data); + sensor_msgs::msg::Imu::SharedPtr imu_scan_msg = + std::make_shared(); + imu_serializer.deserialize_message(&serialized_msg, &imu_scan_msg); node.HandleImuMessage(trajectory_id, sensor_id, - msg.instantiate()); - } - if (msg.isType()) { + imu_scan_msg); + } else if (topic_type == "nav_msgs/msg/Odometry") { + rclcpp::SerializedMessage serialized_msg(*msg.serialized_data); + nav_msgs::msg::Odometry::SharedPtr odom_scan_msg = + std::make_shared(); + odom_serializer.deserialize_message(&serialized_msg, &odom_scan_msg); node.HandleOdometryMessage(trajectory_id, sensor_id, - msg.instantiate()); - } - if (msg.isType()) { + odom_scan_msg); + } else if (topic_type == "sensor_msgs/msg/NavSatFix") { + rclcpp::SerializedMessage serialized_msg(*msg.serialized_data); + sensor_msgs::msg::NavSatFix::SharedPtr nav_sat_fix_msg = + std::make_shared(); + nav_sat_fix_serializer.deserialize_message(&serialized_msg, &nav_sat_fix_msg); node.HandleNavSatFixMessage(trajectory_id, sensor_id, - msg.instantiate()); - } - if (msg.isType()) { - node.HandleLandmarkMessage( - trajectory_id, sensor_id, - msg.instantiate()); + nav_sat_fix_msg); + } else if (topic_type == "cartographer_ros_msgs/msg/LandmarkList") { + rclcpp::SerializedMessage serialized_msg(*msg.serialized_data); + cartographer_ros_msgs::msg::LandmarkList::SharedPtr landmark_list_msg = + std::make_shared(); + landmark_list_serializer.deserialize_message(&serialized_msg, &landmark_list_msg); + node.HandleLandmarkMessage(trajectory_id, sensor_id, + landmark_list_msg); } } - clock.clock = msg.getTime(); - clock_publisher.publish(clock); + clock.clock = rclcpp::Time(msg.time_stamp); + clock_publisher->publish(clock); + rclcpp::spin_some(cartographer_offline_node); if (is_last_message_in_bag) { node.FinishTrajectory(trajectory_id); @@ -355,7 +400,12 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { // Ensure the clock is republished after the bag has been finished, during the // final optimization, serialization, and optional indefinite spinning at the // end. - clock_republish_timer.start(); + // TODO: need a spin for the timer to tick + auto clock_republish_timer = cartographer_offline_node->create_wall_timer( + std::chrono::milliseconds(int(kClockPublishFrequencySec)), + [&clock_publisher, &clock]() { + clock_publisher->publish(clock); + }); node.RunFinalOptimization(); const std::chrono::time_point end_time = @@ -377,11 +427,11 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { #endif // Serialize unless we have neither a bagfile nor an explicit state filename. - if (::ros::ok() && + if (rclcpp::ok() && !(bag_filenames.empty() && FLAGS_save_state_filename.empty())) { const std::string state_output_filename = FLAGS_save_state_filename.empty() - ? absl::StrCat(bag_filenames.front(), ".pbstream") + ? bag_filenames.front() + ".pbstream" : FLAGS_save_state_filename; LOG(INFO) << "Writing state to '" << state_output_filename << "'..."; node.SerializeState(state_output_filename, @@ -389,7 +439,7 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { } if (FLAGS_keep_running) { LOG(INFO) << "Finished processing and waiting for shutdown."; - ::ros::waitForShutdown(); + rclcpp::spin(cartographer_offline_node); } } diff --git a/cartographer_ros/cartographer_ros/offline_node.h b/cartographer_ros/cartographer_ros/offline_node.h index 25f4f5416..1f9f9ace1 100644 --- a/cartographer_ros/cartographer_ros/offline_node.h +++ b/cartographer_ros/cartographer_ros/offline_node.h @@ -22,6 +22,7 @@ #include #include +#include #include "cartographer/mapping/map_builder_interface.h" #include "cartographer_ros/node_options.h" @@ -31,7 +32,8 @@ using MapBuilderFactory = std::function( const ::cartographer::mapping::proto::MapBuilderOptions&)>; -void RunOfflineNode(const MapBuilderFactory& map_builder_factory); +void RunOfflineNode(const MapBuilderFactory& map_builder_factory, + rclcpp::Node::SharedPtr cartographer_offline_node); } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/offline_node_main.cc b/cartographer_ros/cartographer_ros/offline_node_main.cc index 7e6c3da66..b1c2bf64e 100644 --- a/cartographer_ros/cartographer_ros/offline_node_main.cc +++ b/cartographer_ros/cartographer_ros/offline_node_main.cc @@ -18,14 +18,14 @@ #include "cartographer_ros/offline_node.h" #include "cartographer_ros/ros_log_sink.h" #include "gflags/gflags.h" -#include "ros/ros.h" +#include int main(int argc, char** argv) { - google::InitGoogleLogging(argv[0]); - google::ParseCommandLineFlags(&argc, &argv, true); + rclcpp::init(argc, argv); - ::ros::init(argc, argv, "cartographer_offline_node"); - ::ros::start(); + google::AllowCommandLineReparsing(); + google::InitGoogleLogging(argv[0]); + google::ParseCommandLineFlags(&argc, &argv, false); cartographer_ros::ScopedRosLogSink ros_log_sink; @@ -35,7 +35,9 @@ int main(int argc, char** argv) { return ::cartographer::mapping::CreateMapBuilder(map_builder_options); }; - cartographer_ros::RunOfflineNode(map_builder_factory); + rclcpp::Node::SharedPtr cartographer_offline_node = + rclcpp::Node::make_shared("cartographer_offline_node"); + cartographer_ros::RunOfflineNode(map_builder_factory, cartographer_offline_node); - ::ros::shutdown(); + rclcpp::shutdown(); } diff --git a/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc b/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc index 3ca601fd5..8d149be65 100644 --- a/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc +++ b/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc @@ -31,8 +31,8 @@ #include "cartographer_ros/ros_map.h" #include "gflags/gflags.h" #include "glog/logging.h" -#include "nav_msgs/OccupancyGrid.h" -#include "ros/ros.h" +#include +#include DEFINE_string(pbstream_filename, "", "Filename of a pbstream to draw a map from."); @@ -43,7 +43,7 @@ DEFINE_double(resolution, 0.05, "Resolution of a grid cell in the drawn map."); namespace cartographer_ros { namespace { -std::unique_ptr LoadOccupancyGridMsg( +std::unique_ptr LoadOccupancyGridMsg( const std::string& pbstream_filename, const double resolution) { ::cartographer::io::ProtoStreamReader reader(pbstream_filename); ::cartographer::io::ProtoStreamDeserializer deserializer(&reader); @@ -60,40 +60,42 @@ std::unique_ptr LoadOccupancyGridMsg( const auto painted_slices = ::cartographer::io::PaintSubmapSlices(submap_slices, resolution); return CreateOccupancyGridMsg(painted_slices, resolution, FLAGS_map_frame_id, - ros::Time::now()); + rclcpp::Clock().now()); } void Run(const std::string& pbstream_filename, const std::string& map_topic, const std::string& map_frame_id, const double resolution) { - std::unique_ptr msg_ptr = + rclcpp::Node::SharedPtr cartographer_pbstream_map_publisher_node = + rclcpp::Node::make_shared("cartographer_pbstream_map_publisher"); + std::unique_ptr msg_ptr = LoadOccupancyGridMsg(pbstream_filename, resolution); - ::ros::NodeHandle node_handle(""); - ::ros::Publisher pub = node_handle.advertise( - map_topic, kLatestOnlyPublisherQueueSize, true /*latched */); + auto pub = cartographer_pbstream_map_publisher_node->create_publisher( + map_topic, rclcpp::QoS(1).transient_local()); LOG(INFO) << "Publishing occupancy grid topic " << map_topic << " (frame_id: " << map_frame_id << ", resolution:" << std::to_string(resolution) << ")."; - pub.publish(*msg_ptr); - ::ros::spin(); - ::ros::shutdown(); + pub->publish(*msg_ptr); + rclcpp::spin(cartographer_pbstream_map_publisher_node); } } // namespace } // namespace cartographer_ros int main(int argc, char** argv) { + // Init rclcpp first because gflags reorders command line flags in argv + rclcpp::init(argc, argv); + + google::AllowCommandLineReparsing(); google::InitGoogleLogging(argv[0]); - google::ParseCommandLineFlags(&argc, &argv, true); + google::ParseCommandLineFlags(&argc, &argv, false); CHECK(!FLAGS_pbstream_filename.empty()) << "-pbstream_filename is missing."; - ::ros::init(argc, argv, "cartographer_pbstream_map_publisher"); - ::ros::start(); - cartographer_ros::ScopedRosLogSink ros_log_sink; ::cartographer_ros::Run(FLAGS_pbstream_filename, FLAGS_map_topic, FLAGS_map_frame_id, FLAGS_resolution); + ::rclcpp::shutdown(); } diff --git a/cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc b/cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc index e92f0a0d2..1ef4539a7 100644 --- a/cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc +++ b/cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc @@ -26,6 +26,7 @@ #include "cartographer_ros/ros_map.h" #include "gflags/gflags.h" #include "glog/logging.h" +#include DEFINE_string(pbstream_filename, "", "Filename of a pbstream to draw a map from."); @@ -69,13 +70,18 @@ void Run(const std::string& pbstream_filename, const std::string& map_filestem, } // namespace cartographer_ros int main(int argc, char** argv) { + // Init rclcpp first because gflags reorders command line flags in argv + rclcpp::init(argc, argv); + + google::AllowCommandLineReparsing(); FLAGS_alsologtostderr = true; google::InitGoogleLogging(argv[0]); - google::ParseCommandLineFlags(&argc, &argv, true); + google::ParseCommandLineFlags(&argc, &argv, false); CHECK(!FLAGS_pbstream_filename.empty()) << "-pbstream_filename is missing."; CHECK(!FLAGS_map_filestem.empty()) << "-map_filestem is missing."; ::cartographer_ros::Run(FLAGS_pbstream_filename, FLAGS_map_filestem, FLAGS_resolution); + rclcpp::shutdown(); } diff --git a/cartographer_ros/cartographer_ros/playable_bag.cc b/cartographer_ros/cartographer_ros/playable_bag.cc index f305ad850..f9ef8fcd4 100644 --- a/cartographer_ros/cartographer_ros/playable_bag.cc +++ b/cartographer_ros/cartographer_ros/playable_bag.cc @@ -19,49 +19,58 @@ #include "absl/memory/memory.h" #include "cartographer_ros/node_constants.h" #include "glog/logging.h" -#include "tf2_msgs/TFMessage.h" +#include "tf2_msgs/msg/tf_message.hpp" +#include "rosbag2_storage/topic_metadata.hpp" +#include "rosbag2_storage/bag_metadata.hpp" namespace cartographer_ros { PlayableBag::PlayableBag( const std::string& bag_filename, const int bag_id, - const ros::Time start_time, const ros::Time end_time, - const ros::Duration buffer_delay, + const rclcpp::Duration buffer_delay, FilteringEarlyMessageHandler filtering_early_message_handler) - : bag_(absl::make_unique(bag_filename, rosbag::bagmode::Read)), - view_(absl::make_unique(*bag_, start_time, end_time)), - view_iterator_(view_->begin()), + : bag_reader_(std::make_unique()), finished_(false), bag_id_(bag_id), bag_filename_(bag_filename), - duration_in_seconds_( - (view_->getEndTime() - view_->getBeginTime()).toSec()), message_counter_(0), buffer_delay_(buffer_delay), filtering_early_message_handler_( std::move(filtering_early_message_handler)) { + LOG(WARNING) << "Opening bag: " << bag_filename; + bag_reader_->open(bag_filename); + bag_metadata = bag_reader_->get_metadata(); + duration_in_seconds_ = bag_metadata.duration.count()/1e9; + LOG(WARNING) << "duration_in_seconds_: " << duration_in_seconds_; + LOG(WARNING) << "message_count: " << bag_metadata.message_count; + LOG(WARNING) << "compression_mode: " << bag_metadata.compression_mode; + LOG(WARNING) << "compression_format: " << bag_metadata.compression_format; AdvanceUntilMessageAvailable(); - for (const auto* connection_info : view_->getConnections()) { - topics_.insert(connection_info->topic); + for (auto topic_info : bag_metadata.topics_with_message_count) { + topics_.insert(topic_info.topic_metadata.name); } } -ros::Time PlayableBag::PeekMessageTime() const { +rclcpp::Time PlayableBag::PeekMessageTime() const { CHECK(IsMessageAvailable()); - return buffered_messages_.front().getTime(); + return rclcpp::Time(buffered_messages_.front().time_stamp); } -std::tuple PlayableBag::GetBeginEndTime() const { - return std::make_tuple(view_->getBeginTime(), view_->getEndTime()); +std::tuple PlayableBag::GetBeginEndTime() const { + return std::make_tuple( + rclcpp::Time(bag_metadata.starting_time.time_since_epoch().count()), + rclcpp::Time(bag_metadata.starting_time.time_since_epoch().count() + bag_metadata.duration.count())); } -rosbag::MessageInstance PlayableBag::GetNextMessage( - cartographer_ros_msgs::BagfileProgress* progress) { +rosbag2_storage::SerializedBagMessage PlayableBag::GetNextMessage( + cartographer_ros_msgs::msg::BagfileProgress* progress) { CHECK(IsMessageAvailable()); - const rosbag::MessageInstance msg = buffered_messages_.front(); + const rosbag2_storage::SerializedBagMessage msg = buffered_messages_.front(); buffered_messages_.pop_front(); AdvanceUntilMessageAvailable(); - double processed_seconds = (msg.getTime() - view_->getBeginTime()).toSec(); + double processed_seconds = + (rclcpp::Time(msg.time_stamp) - + rclcpp::Time(bag_metadata.starting_time.time_since_epoch().count())).seconds(); if ((message_counter_ % 10000) == 0) { LOG(INFO) << "Processed " << processed_seconds << " of " << duration_in_seconds_ << " seconds of bag " << bag_filename_; @@ -70,7 +79,7 @@ rosbag::MessageInstance PlayableBag::GetNextMessage( if (progress) { progress->current_bagfile_name = bag_filename_; progress->current_bagfile_id = bag_id_; - progress->total_messages = view_->size(); + progress->total_messages = bag_metadata.message_count; progress->processed_messages = message_counter_; progress->total_seconds = duration_in_seconds_; progress->processed_seconds = processed_seconds; @@ -81,24 +90,23 @@ rosbag::MessageInstance PlayableBag::GetNextMessage( bool PlayableBag::IsMessageAvailable() const { return !buffered_messages_.empty() && - (buffered_messages_.front().getTime() < - buffered_messages_.back().getTime() - buffer_delay_); + (buffered_messages_.front().time_stamp < + buffered_messages_.back().time_stamp - buffer_delay_.nanoseconds()); } int PlayableBag::bag_id() const { return bag_id_; } void PlayableBag::AdvanceOneMessage() { CHECK(!finished_); - if (view_iterator_ == view_->end()) { + if (!bag_reader_->has_next()) { finished_ = true; return; } - rosbag::MessageInstance& msg = *view_iterator_; + auto msg = bag_reader_->read_next(); if (!filtering_early_message_handler_ || filtering_early_message_handler_(msg)) { - buffered_messages_.push_back(msg); + buffered_messages_.push_back(*msg); } - ++view_iterator_; ++message_counter_; } @@ -111,10 +119,15 @@ void PlayableBag::AdvanceUntilMessageAvailable() { } while (!finished_ && !IsMessageAvailable()); } -PlayableBagMultiplexer::PlayableBagMultiplexer() : pnh_("~") { - bag_progress_pub_ = pnh_.advertise( +PlayableBagMultiplexer::PlayableBagMultiplexer(rclcpp::Node::SharedPtr node) { + node_ = node; + bag_progress_pub_ = node_->create_publisher( "bagfile_progress", 10); - progress_pub_interval_ = pnh_.param("bagfile_progress_pub_interval", 10.0); + if (!node_->has_parameter("bagfile_progress_pub_interval")) { + node_->declare_parameter("bagfile_progress_pub_interval", progress_pub_interval_); + } + progress_pub_interval_ = + node_->get_parameter_or("bagfile_progress_pub_interval", progress_pub_interval_, 10.0); } void PlayableBagMultiplexer::AddPlayableBag(PlayableBag playable_bag) { @@ -126,43 +139,51 @@ void PlayableBagMultiplexer::AddPlayableBag(PlayableBag playable_bag) { next_message_queue_.emplace( BagMessageItem{playable_bags_.back().PeekMessageTime(), static_cast(playable_bags_.size() - 1)}); - bag_progress_time_map_[playable_bag.bag_id()] = ros::Time::now(); + bag_progress_time_map_[playable_bag.bag_id()] = node_->now(); } bool PlayableBagMultiplexer::IsMessageAvailable() const { return !next_message_queue_.empty(); } -std::tuple -PlayableBagMultiplexer::GetNextMessage() { +std::tuple PlayableBagMultiplexer::GetNextMessage() { CHECK(IsMessageAvailable()); const int current_bag_index = next_message_queue_.top().bag_index; PlayableBag& current_bag = playable_bags_.at(current_bag_index); - cartographer_ros_msgs::BagfileProgress progress; - rosbag::MessageInstance msg = current_bag.GetNextMessage(&progress); + cartographer_ros_msgs::msg::BagfileProgress progress; + rosbag2_storage::SerializedBagMessage msg = current_bag.GetNextMessage(&progress); const bool publish_progress = current_bag.finished() || - ros::Time::now() - bag_progress_time_map_[current_bag.bag_id()] >= - ros::Duration(progress_pub_interval_); - if (bag_progress_pub_.getNumSubscribers() > 0 && publish_progress) { + node_->now() - bag_progress_time_map_[current_bag.bag_id()] >= + rclcpp::Duration(progress_pub_interval_, 0); + if (bag_progress_pub_->get_subscription_count() > 0 && publish_progress) { progress.total_bagfiles = playable_bags_.size(); if (current_bag.finished()) { progress.processed_seconds = current_bag.duration_in_seconds(); } - bag_progress_pub_.publish(progress); - bag_progress_time_map_[current_bag.bag_id()] = ros::Time::now(); + bag_progress_pub_->publish(progress); + bag_progress_time_map_[current_bag.bag_id()] = node_->now(); } - CHECK_EQ(msg.getTime(), next_message_queue_.top().message_timestamp); + CHECK_EQ(msg.time_stamp, next_message_queue_.top().message_timestamp.nanoseconds()); next_message_queue_.pop(); if (current_bag.IsMessageAvailable()) { next_message_queue_.emplace( BagMessageItem{current_bag.PeekMessageTime(), current_bag_index}); } - return std::make_tuple(std::move(msg), current_bag.bag_id(), + + std::string topic_type; + for (auto topic_info : current_bag.bag_metadata.topics_with_message_count) { + if (topic_info.topic_metadata.name == msg.topic_name){ + topic_type = topic_info.topic_metadata.type; + break; + } + } + + return std::make_tuple(std::move(msg), current_bag.bag_id(), topic_type, !current_bag.IsMessageAvailable()); } -ros::Time PlayableBagMultiplexer::PeekMessageTime() const { +rclcpp::Time PlayableBagMultiplexer::PeekMessageTime() const { CHECK(IsMessageAvailable()); return next_message_queue_.top().message_timestamp; } diff --git a/cartographer_ros/cartographer_ros/playable_bag.h b/cartographer_ros/cartographer_ros/playable_bag.h index 1ceed0619..6cdf2df60 100644 --- a/cartographer_ros/cartographer_ros/playable_bag.h +++ b/cartographer_ros/cartographer_ros/playable_bag.h @@ -20,10 +20,12 @@ #include #include -#include "cartographer_ros_msgs/BagfileProgress.h" -#include "ros/node_handle.h" -#include "rosbag/bag.h" -#include "rosbag/view.h" +#include "cartographer_ros_msgs/msg/bagfile_progress.hpp" +#include +#include +#include +#include +#include #include "tf2_ros/buffer.h" namespace cartographer_ros { @@ -34,61 +36,58 @@ class PlayableBag { // Returns a boolean indicating whether the message should enter the buffer. using FilteringEarlyMessageHandler = std::function; + std::shared_ptr)>; - PlayableBag(const std::string& bag_filename, int bag_id, ros::Time start_time, - ros::Time end_time, ros::Duration buffer_delay, + PlayableBag(const std::string& bag_filename, int bag_id, + rclcpp::Duration buffer_delay, FilteringEarlyMessageHandler filtering_early_message_handler); - ros::Time PeekMessageTime() const; - rosbag::MessageInstance GetNextMessage( - cartographer_ros_msgs::BagfileProgress* progress); + rclcpp::Time PeekMessageTime() const; + rosbag2_storage::SerializedBagMessage GetNextMessage( + cartographer_ros_msgs::msg::BagfileProgress* progress); bool IsMessageAvailable() const; - std::tuple GetBeginEndTime() const; + std::tuple GetBeginEndTime() const; int bag_id() const; std::set topics() const { return topics_; } double duration_in_seconds() const { return duration_in_seconds_; } bool finished() const { return finished_; } + rosbag2_storage::BagMetadata bag_metadata; private: void AdvanceOneMessage(); void AdvanceUntilMessageAvailable(); - std::unique_ptr bag_; - std::unique_ptr view_; - rosbag::View::const_iterator view_iterator_; + std::unique_ptr bag_reader_; bool finished_; const int bag_id_; const std::string bag_filename_; - const double duration_in_seconds_; + double duration_in_seconds_; int message_counter_; - std::deque buffered_messages_; - const ::ros::Duration buffer_delay_; + std::deque buffered_messages_; + const rclcpp::Duration buffer_delay_; FilteringEarlyMessageHandler filtering_early_message_handler_; std::set topics_; }; class PlayableBagMultiplexer { public: - PlayableBagMultiplexer(); + PlayableBagMultiplexer(rclcpp::Node::SharedPtr node); void AddPlayableBag(PlayableBag playable_bag); // Returns the next message from the multiplexed (merge-sorted) message // stream, along with the bag id corresponding to the message, and whether // this was the last message in that bag. - std::tuple - GetNextMessage(); + std::tuple GetNextMessage(); bool IsMessageAvailable() const; - ros::Time PeekMessageTime() const; + rclcpp::Time PeekMessageTime() const; std::set topics() const { return topics_; } private: struct BagMessageItem { - ros::Time message_timestamp; + rclcpp::Time message_timestamp; int bag_index; struct TimestampIsGreater { bool operator()(const BagMessageItem& l, const BagMessageItem& r) { @@ -97,11 +96,11 @@ class PlayableBagMultiplexer { }; }; - ros::NodeHandle pnh_; + rclcpp::Node::SharedPtr node_; // Publishes information about the bag-file(s) processing and its progress - ros::Publisher bag_progress_pub_; + rclcpp::Publisher::SharedPtr bag_progress_pub_; // Map between bagfile id and the last time when its progress was published - std::map bag_progress_time_map_; + std::map bag_progress_time_map_; // The time interval of publishing bag-file(s) processing in seconds double progress_pub_interval_; diff --git a/cartographer_ros/cartographer_ros/ros_log_sink.cc b/cartographer_ros/cartographer_ros/ros_log_sink.cc index f5c7bbf52..95ce0c0d3 100644 --- a/cartographer_ros/cartographer_ros/ros_log_sink.cc +++ b/cartographer_ros/cartographer_ros/ros_log_sink.cc @@ -22,7 +22,6 @@ #include #include "glog/log_severity.h" -#include "ros/console.h" namespace cartographer_ros { @@ -48,19 +47,19 @@ void ScopedRosLogSink::send(const ::google::LogSeverity severity, severity, GetBasename(filename), line, tm_time, message, message_len); switch (severity) { case ::google::GLOG_INFO: - ROS_INFO_STREAM(message_string); + RCLCPP_INFO_STREAM(logger_, message_string); break; case ::google::GLOG_WARNING: - ROS_WARN_STREAM(message_string); + RCLCPP_WARN_STREAM(logger_, message_string); break; case ::google::GLOG_ERROR: - ROS_ERROR_STREAM(message_string); + RCLCPP_ERROR_STREAM(logger_, message_string); break; case ::google::GLOG_FATAL: - ROS_FATAL_STREAM(message_string); + RCLCPP_FATAL_STREAM(logger_, message_string); will_die_ = true; break; } diff --git a/cartographer_ros/cartographer_ros/ros_log_sink.h b/cartographer_ros/cartographer_ros/ros_log_sink.h index 28538bf67..e603727ca 100644 --- a/cartographer_ros/cartographer_ros/ros_log_sink.h +++ b/cartographer_ros/cartographer_ros/ros_log_sink.h @@ -20,6 +20,7 @@ #include #include "glog/logging.h" +#include namespace cartographer_ros { @@ -38,6 +39,7 @@ class ScopedRosLogSink : public ::google::LogSink { private: bool will_die_; + rclcpp::Logger logger_{rclcpp::get_logger("cartographer logger")}; }; } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/ros_map.cc b/cartographer_ros/cartographer_ros/ros_map.cc index c5c0a1f87..a8a18d226 100644 --- a/cartographer_ros/cartographer_ros/ros_map.cc +++ b/cartographer_ros/cartographer_ros/ros_map.cc @@ -16,15 +16,14 @@ #include "cartographer_ros/ros_map.h" -#include "absl/strings/str_cat.h" - namespace cartographer_ros { +// TODO: png ? void WritePgm(const ::cartographer::io::Image& image, const double resolution, ::cartographer::io::FileWriter* file_writer) { const std::string header = - absl::StrCat("P5\n# Cartographer map; ", resolution, " m/pixel\n", - image.width(), " ", image.height(), "\n255\n"); + "P5\n# Cartographer map; " + std::to_string(resolution) + " m/pixel\n" + + std::to_string(image.width()) + " " + std::to_string(image.height()) + "\n255\n"; file_writer->Write(header.data(), header.size()); for (int y = 0; y < image.height(); ++y) { for (int x = 0; x < image.width(); ++x) { @@ -39,10 +38,11 @@ void WriteYaml(const double resolution, const Eigen::Vector2d& origin, ::cartographer::io::FileWriter* file_writer) { // Magic constants taken directly from ros map_saver code: // https://github.com/ros-planning/navigation/blob/ac41d2480c4cf1602daf39a6e9629142731d92b0/map_server/src/map_saver.cpp#L114 - const std::string output = absl::StrCat( - "image: ", pgm_filename, "\n", "resolution: ", resolution, "\n", - "origin: [", origin.x(), ", ", origin.y(), - ", 0.0]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n"); + // TODO: check ROS2 map format + const std::string output = + "image: " + pgm_filename + "\n" + "resolution: " + std::to_string(resolution) + "\n" + + "origin: [" + std::to_string(origin.x()) + ", " + std::to_string(origin.y()) + + ", 0.0]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n"; file_writer->Write(output.data(), output.size()); } diff --git a/cartographer_ros/cartographer_ros/rosbag_validate_main.cc b/cartographer_ros/cartographer_ros/rosbag_validate_main.cc index c52c70049..a89ce437d 100644 --- a/cartographer_ros/cartographer_ros/rosbag_validate_main.cc +++ b/cartographer_ros/cartographer_ros/rosbag_validate_main.cc @@ -25,19 +25,19 @@ #include "cartographer_ros/msg_conversion.h" #include "gflags/gflags.h" #include "glog/logging.h" -#include "nav_msgs/Odometry.h" -#include "ros/ros.h" -#include "ros/time.h" -#include "rosbag/bag.h" -#include "rosbag/view.h" -#include "sensor_msgs/Imu.h" -#include "sensor_msgs/LaserScan.h" -#include "sensor_msgs/MultiEchoLaserScan.h" -#include "sensor_msgs/PointCloud2.h" +#include "nav_msgs/msg/odometry.hpp" +#include +#include +#include +#include "sensor_msgs/msg/imu.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "sensor_msgs/msg/multi_echo_laser_scan.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" #include "tf2_eigen/tf2_eigen.h" -#include "tf2_msgs/TFMessage.h" +#include "tf2_msgs/msg/tf_message.hpp" #include "tf2_ros/buffer.h" #include "urdf/model.h" +#include DEFINE_string(bag_filename, "", "Bag to process."); DEFINE_bool(dump_timing, false, @@ -48,7 +48,7 @@ namespace cartographer_ros { namespace { struct FrameProperties { - ros::Time last_timestamp; + rclcpp::Time last_timestamp; std::string topic; std::vector time_deltas; std::unique_ptr timing_file; @@ -64,12 +64,9 @@ const double kMaxAverageAcceleration = 10.5; const double kMaxGapPointsData = 0.1; const double kMaxGapImuData = 0.05; const std::set kPointDataTypes = { - std::string( - ros::message_traits::DataType::value()), - std::string(ros::message_traits::DataType< - sensor_msgs::MultiEchoLaserScan>::value()), - std::string( - ros::message_traits::DataType::value())}; + std::string(rosidl_generator_traits::data_type()), + std::string(rosidl_generator_traits::data_type()), + std::string(rosidl_generator_traits::data_type())}; std::unique_ptr CreateTimingFile(const std::string& frame_id) { auto timing_file = absl::make_unique( @@ -95,14 +92,14 @@ std::unique_ptr CreateTimingFile(const std::string& frame_id) { return timing_file; } -void CheckImuMessage(const sensor_msgs::Imu& imu_message) { - auto linear_acceleration = ToEigen(imu_message.linear_acceleration); +void CheckImuMessage(const sensor_msgs::msg::Imu::ConstSharedPtr imu_message) { + auto linear_acceleration = ToEigen(imu_message->linear_acceleration); if (std::isnan(linear_acceleration.norm()) || linear_acceleration.norm() < kMinLinearAcceleration || linear_acceleration.norm() > kMaxLinearAcceleration) { LOG_FIRST_N(WARNING, 3) - << "frame_id " << imu_message.header.frame_id << " time " - << imu_message.header.stamp.toNSec() << ": IMU linear acceleration is " + << "frame_id " << imu_message->header.frame_id << " time " + << imu_message->header.stamp.nanosec << ": IMU linear acceleration is " << linear_acceleration.norm() << " m/s^2," << " expected is [" << kMinLinearAcceleration << ", " << kMaxLinearAcceleration << "] m/s^2." @@ -111,22 +108,26 @@ void CheckImuMessage(const sensor_msgs::Imu& imu_message) { } } -bool IsValidPose(const geometry_msgs::Pose& pose) { +bool IsValidPose(const geometry_msgs::msg::Pose& pose) { return ToRigid3d(pose).IsValid(); } -void CheckOdometryMessage(const nav_msgs::Odometry& message) { - const auto& pose = message.pose.pose; +void CheckOdometryMessage(nav_msgs::msg::Odometry::ConstSharedPtr message) { + const auto& pose = message->pose.pose; if (!IsValidPose(pose)) { - LOG_FIRST_N(ERROR, 3) << "frame_id " << message.header.frame_id << " time " - << message.header.stamp.toNSec() + LOG_FIRST_N(ERROR, 3) << "frame_id " << message->header.frame_id << " time " + << message->header.stamp.nanosec << ": Odometry pose is invalid." - << " pose " << pose; + << " pose.position.x: " << pose.position.x + << " pose.position.y: " << pose.position.y + << " pose.position.z: " << pose.position.z + << " pose.orientation Yaw: " << tf2::getYaw(pose.orientation) + ; } } -void CheckTfMessage(const tf2_msgs::TFMessage& message) { - for (const auto& transform : message.transforms) { +void CheckTfMessage(tf2_msgs::msg::TFMessage::ConstSharedPtr message) { + for (auto transform : message->transforms) { if (transform.header.frame_id == "map") { LOG_FIRST_N(ERROR, 1) << "Input contains transform message from frame_id \"" @@ -149,7 +150,7 @@ class RangeDataChecker { template void CheckMessage(const MessageType& message) { const std::string& frame_id = message.header.frame_id; - ros::Time current_time_stamp = message.header.stamp; + rclcpp::Time current_time_stamp = message.header.stamp; RangeChecksum current_checksum; cartographer::common::Time time_from, time_to; ReadRangeMessage(message, ¤t_checksum, &time_from, &time_to); @@ -190,7 +191,7 @@ class RangeDataChecker { LOG_FIRST_N(ERROR, 3) << "Sensor with frame_id \"" << frame_id << "\" sends exactly the same range measurements multiple times. " - << "Range data at time " << current_time_stamp + << "Range data at time " << current_time_stamp.seconds() << " equals preceding data with " << current_checksum.first << " points."; } @@ -251,111 +252,151 @@ class RangeDataChecker { }; void Run(const std::string& bag_filename, const bool dump_timing) { - rosbag::Bag bag; - bag.open(bag_filename, rosbag::bagmode::Read); - rosbag::View view(bag); + rosbag2_cpp::Reader bag_reader; + bag_reader.open(bag_filename); + rosbag2_storage::BagMetadata bag_metadata = bag_reader.get_metadata(); + + auto pcl2_serializer = rclcpp::Serialization(); + auto multi_echo_laser_scan_serializer = rclcpp::Serialization(); + auto laser_scan_serializer = rclcpp::Serialization(); + auto imu_serializer = rclcpp::Serialization(); + auto odom_serializer = rclcpp::Serialization(); + auto tf_serializer = rclcpp::Serialization(); std::map frame_id_to_properties; size_t message_index = 0; int num_imu_messages = 0; double sum_imu_acceleration = 0.; RangeDataChecker range_data_checker; - for (const rosbag::MessageInstance& message : view) { + while (bag_reader.has_next()) { + auto message = bag_reader.read_next(); ++message_index; std::string frame_id; - ros::Time time; - if (message.isType()) { - auto msg = message.instantiate(); - time = msg->header.stamp; - frame_id = msg->header.frame_id; - range_data_checker.CheckMessage(*msg); - } else if (message.isType()) { - auto msg = message.instantiate(); - time = msg->header.stamp; - frame_id = msg->header.frame_id; - range_data_checker.CheckMessage(*msg); - } else if (message.isType()) { - auto msg = message.instantiate(); - time = msg->header.stamp; - frame_id = msg->header.frame_id; - range_data_checker.CheckMessage(*msg); - } else if (message.isType()) { - auto msg = message.instantiate(); - time = msg->header.stamp; - frame_id = msg->header.frame_id; - CheckImuMessage(*msg); - num_imu_messages++; - sum_imu_acceleration += ToEigen(msg->linear_acceleration).norm(); - } else if (message.isType()) { - auto msg = message.instantiate(); - time = msg->header.stamp; - frame_id = msg->header.frame_id; - CheckOdometryMessage(*msg); - } else if (message.isType()) { - auto msg = message.instantiate(); - CheckTfMessage(*msg); - continue; - } else { - continue; + rclcpp::Time time; + bool is_data_from_sensor = true; + std::string topic_type; + for (auto topic_info : bag_metadata.topics_with_message_count) { + if (topic_info.topic_metadata.name == message->topic_name){ + if (topic_info.topic_metadata.type == "sensor_msgs/msg/PointCloud2") { + topic_type = "sensor_msgs/msg/PointCloud2"; + rclcpp::SerializedMessage serialized_msg(*message->serialized_data); + sensor_msgs::msg::PointCloud2::SharedPtr msg = + std::make_shared(); + pcl2_serializer.deserialize_message(&serialized_msg, msg.get()); + time = msg->header.stamp; + frame_id = msg->header.frame_id; + range_data_checker.CheckMessage(*msg); + } else if (topic_info.topic_metadata.type == "sensor_msgs/msg/MultiEchoLaserScan") { + topic_type = "sensor_msgs/msg/MultiEchoLaserScan"; + rclcpp::SerializedMessage serialized_msg(*message->serialized_data); + sensor_msgs::msg::MultiEchoLaserScan::SharedPtr msg = + std::make_shared(); + multi_echo_laser_scan_serializer.deserialize_message(&serialized_msg, msg.get()); + time = msg->header.stamp; + frame_id = msg->header.frame_id; + range_data_checker.CheckMessage(*msg); + } else if (topic_info.topic_metadata.type == "sensor_msgs/msg/LaserScan") { + topic_type = "sensor_msgs/msg/LaserScan"; + rclcpp::SerializedMessage serialized_msg(*message->serialized_data); + sensor_msgs::msg::LaserScan::SharedPtr msg = + std::make_shared(); + laser_scan_serializer.deserialize_message(&serialized_msg, msg.get()); + time = msg->header.stamp; + frame_id = msg->header.frame_id; + range_data_checker.CheckMessage(*msg); + } else if (topic_info.topic_metadata.type == "sensor_msgs/msg/Imu") { + topic_type = "sensor_msgs/msg/Imu"; + rclcpp::SerializedMessage serialized_msg(*message->serialized_data); + sensor_msgs::msg::Imu::SharedPtr msg = + std::make_shared(); + imu_serializer.deserialize_message(&serialized_msg, msg.get()); + time = msg->header.stamp; + frame_id = msg->header.frame_id; + CheckImuMessage(msg); + num_imu_messages++; + sum_imu_acceleration += ToEigen(msg->linear_acceleration).norm(); + } else if (topic_info.topic_metadata.type == "nav_msgs/msg/Odometry") { + topic_type = "nav_msgs/msg/Odometry"; + rclcpp::SerializedMessage serialized_msg(*message->serialized_data); + nav_msgs::msg::Odometry::SharedPtr msg = + std::make_shared(); + odom_serializer.deserialize_message(&serialized_msg, msg.get()); + time = msg->header.stamp; + frame_id = msg->header.frame_id; + CheckOdometryMessage(msg); + } else if (topic_info.topic_metadata.type == "tf2_msgs/msg/TFMessage") { + rclcpp::SerializedMessage serialized_msg(*message->serialized_data); + tf2_msgs::msg::TFMessage::SharedPtr msg + = std::make_shared();; + tf_serializer.deserialize_message(&serialized_msg, msg.get()); + CheckTfMessage(msg); + is_data_from_sensor = false; + } else { + is_data_from_sensor = false;; + } + break; + } } - bool first_packet = false; - if (!frame_id_to_properties.count(frame_id)) { - frame_id_to_properties.emplace( - frame_id, - FrameProperties{time, message.getTopic(), std::vector(), - dump_timing ? CreateTimingFile(frame_id) : nullptr, - message.getDataType()}); - first_packet = true; - } + if (is_data_from_sensor) { + bool first_packet = false; + if (!frame_id_to_properties.count(frame_id)) { + frame_id_to_properties.emplace( + frame_id, + FrameProperties{time, message->topic_name, std::vector(), + dump_timing ? CreateTimingFile(frame_id) : nullptr, + topic_type}); + first_packet = true; + } - auto& entry = frame_id_to_properties.at(frame_id); - if (!first_packet) { - const double delta_t_sec = (time - entry.last_timestamp).toSec(); - if (delta_t_sec <= 0) { - LOG_FIRST_N(ERROR, 3) - << "Sensor with frame_id \"" << frame_id - << "\" jumps backwards in time, i.e. timestamps are not strictly " - "increasing. Make sure that the bag contains the data for each " - "frame_id sorted by header.stamp, i.e. the order in which they " - "were acquired from the sensor."; + auto& entry = frame_id_to_properties.at(frame_id); + if (!first_packet) { + const double delta_t_sec = (time - entry.last_timestamp).seconds(); + if (delta_t_sec <= 0) { + LOG_FIRST_N(ERROR, 3) + << "Sensor with frame_id \"" << frame_id + << "\" jumps backwards in time, i.e. timestamps are not strictly " + "increasing. Make sure that the bag contains the data for each " + "frame_id sorted by header.stamp, i.e. the order in which they " + "were acquired from the sensor."; + } + entry.time_deltas.push_back(delta_t_sec); } - entry.time_deltas.push_back(delta_t_sec); - } - if (entry.topic != message.getTopic()) { - LOG_FIRST_N(ERROR, 3) - << "frame_id \"" << frame_id - << "\" is send on multiple topics. It was seen at least on " - << entry.topic << " and " << message.getTopic(); - } - entry.last_timestamp = time; + if (entry.topic != message->topic_name) { + LOG_FIRST_N(ERROR, 3) + << "frame_id \"" << frame_id + << "\" is send on multiple topics. It was seen at least on " + << entry.topic << " and " << message->topic_name; + } + entry.last_timestamp = time; - if (dump_timing) { - CHECK(entry.timing_file != nullptr); - (*entry.timing_file) << message_index << "\t" - << message.getTime().toNSec() << "\t" - << time.toNSec() << std::endl; - } + if (dump_timing) { + CHECK(entry.timing_file != nullptr); + (*entry.timing_file) << message_index << "\t" + << message->time_stamp << "\t" + << time.nanoseconds() << std::endl; + } - double duration_serialization_sensor = (time - message.getTime()).toSec(); - if (std::abs(duration_serialization_sensor) > - kTimeDeltaSerializationSensorWarning) { - std::stringstream stream; - stream << "frame_id \"" << frame_id << "\" on topic " - << message.getTopic() << " has serialization time " - << message.getTime() << " but sensor time " << time - << " differing by " << duration_serialization_sensor << " s."; + double duration_serialization_sensor = (time.nanoseconds() - message->time_stamp)/1e9; if (std::abs(duration_serialization_sensor) > - kTimeDeltaSerializationSensorError) { - LOG_FIRST_N(ERROR, 3) << stream.str(); - } else { - LOG_FIRST_N(WARNING, 1) << stream.str(); + kTimeDeltaSerializationSensorWarning) { + std::stringstream stream; + stream << "frame_id \"" << frame_id << "\" on topic " + << message->topic_name << " has serialization time " + << message->time_stamp << " but sensor time " << time.nanoseconds() + << " differing by " << duration_serialization_sensor << " s."; + if (std::abs(duration_serialization_sensor) > + kTimeDeltaSerializationSensorError) { + LOG_FIRST_N(ERROR, 3) << stream.str(); + } else { + LOG_FIRST_N(WARNING, 1) << stream.str(); + } } } } - bag.close(); + //end while range_data_checker.PrintReport(); if (num_imu_messages > 0) { @@ -385,7 +426,7 @@ void Run(const std::string& bag_filename, const bool dump_timing) { << " s, recommended is [0.0005, 0.05] s with no jitter."; } if (frame_properties.data_type == - ros::message_traits::DataType::value() && + rosidl_generator_traits::data_type() && max_time_delta > kMaxGapImuData) { LOG(ERROR) << "IMU data (frame_id: \"" << entry_pair.first << "\") has a large gap, largest is " << max_time_delta @@ -416,9 +457,13 @@ void Run(const std::string& bag_filename, const bool dump_timing) { } // namespace cartographer_ros int main(int argc, char** argv) { + // Init rclcpp first because gflags reorders command line flags in argv + rclcpp::init(argc, argv); + + google::AllowCommandLineReparsing(); FLAGS_alsologtostderr = true; google::InitGoogleLogging(argv[0]); - google::ParseCommandLineFlags(&argc, &argv, true); + google::ParseCommandLineFlags(&argc, &argv, false); CHECK(!FLAGS_bag_filename.empty()) << "-bag_filename is missing."; ::cartographer_ros::Run(FLAGS_bag_filename, FLAGS_dump_timing); diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.cc b/cartographer_ros/cartographer_ros/sensor_bridge.cc index 3254d98a1..f7cbb9ec4 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.cc +++ b/cartographer_ros/cartographer_ros/sensor_bridge.cc @@ -49,7 +49,7 @@ SensorBridge::SensorBridge( trajectory_builder_(trajectory_builder) {} std::unique_ptr SensorBridge::ToOdometryData( - const nav_msgs::Odometry::ConstPtr& msg) { + const nav_msgs::msg::Odometry::ConstSharedPtr& msg) { const carto::common::Time time = FromRos(msg->header.stamp); const auto sensor_to_tracking = tf_bridge_.LookupToTracking( time, CheckNoLeadingSlash(msg->child_frame_id)); @@ -62,7 +62,7 @@ std::unique_ptr SensorBridge::ToOdometryData( } void SensorBridge::HandleOdometryMessage( - const std::string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) { + const std::string& sensor_id, const nav_msgs::msg::Odometry::ConstSharedPtr& msg) { std::unique_ptr odometry_data = ToOdometryData(msg); if (odometry_data != nullptr) { @@ -73,9 +73,9 @@ void SensorBridge::HandleOdometryMessage( } void SensorBridge::HandleNavSatFixMessage( - const std::string& sensor_id, const sensor_msgs::NavSatFix::ConstPtr& msg) { + const std::string& sensor_id, const sensor_msgs::msg::NavSatFix::ConstSharedPtr& msg) { const carto::common::Time time = FromRos(msg->header.stamp); - if (msg->status.status == sensor_msgs::NavSatStatus::STATUS_NO_FIX) { + if (msg->status.status == sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX) { trajectory_builder_->AddSensorData( sensor_id, carto::sensor::FixedFramePoseData{time, absl::optional()}); @@ -99,7 +99,7 @@ void SensorBridge::HandleNavSatFixMessage( void SensorBridge::HandleLandmarkMessage( const std::string& sensor_id, - const cartographer_ros_msgs::LandmarkList::ConstPtr& msg) { + const cartographer_ros_msgs::msg::LandmarkList::ConstSharedPtr& msg) { auto landmark_data = ToLandmarkData(*msg); auto tracking_from_landmark_sensor = tf_bridge_.LookupToTracking( @@ -115,7 +115,7 @@ void SensorBridge::HandleLandmarkMessage( } std::unique_ptr SensorBridge::ToImuData( - const sensor_msgs::Imu::ConstPtr& msg) { + const sensor_msgs::msg::Imu::ConstSharedPtr& msg) { CHECK_NE(msg->linear_acceleration_covariance[0], -1) << "Your IMU data claims to not contain linear acceleration measurements " "by setting linear_acceleration_covariance[0] to -1. Cartographer " @@ -143,7 +143,7 @@ std::unique_ptr SensorBridge::ToImuData( } void SensorBridge::HandleImuMessage(const std::string& sensor_id, - const sensor_msgs::Imu::ConstPtr& msg) { + const sensor_msgs::msg::Imu::ConstSharedPtr& msg) { std::unique_ptr imu_data = ToImuData(msg); if (imu_data != nullptr) { trajectory_builder_->AddSensorData( @@ -154,7 +154,7 @@ void SensorBridge::HandleImuMessage(const std::string& sensor_id, } void SensorBridge::HandleLaserScanMessage( - const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) { + const std::string& sensor_id, const sensor_msgs::msg::LaserScan::ConstSharedPtr& msg) { carto::sensor::PointCloudWithIntensities point_cloud; carto::common::Time time; std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg); @@ -163,7 +163,7 @@ void SensorBridge::HandleLaserScanMessage( void SensorBridge::HandleMultiEchoLaserScanMessage( const std::string& sensor_id, - const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { + const sensor_msgs::msg::MultiEchoLaserScan::ConstSharedPtr& msg) { carto::sensor::PointCloudWithIntensities point_cloud; carto::common::Time time; std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg); @@ -172,7 +172,7 @@ void SensorBridge::HandleMultiEchoLaserScanMessage( void SensorBridge::HandlePointCloud2Message( const std::string& sensor_id, - const sensor_msgs::PointCloud2::ConstPtr& msg) { + const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg) { carto::sensor::PointCloudWithIntensities point_cloud; carto::common::Time time; std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg); diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.h b/cartographer_ros/cartographer_ros/sensor_bridge.h index 807d81648..e53a0c9c4 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.h +++ b/cartographer_ros/cartographer_ros/sensor_bridge.h @@ -26,16 +26,16 @@ #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" #include "cartographer_ros/tf_bridge.h" -#include "cartographer_ros_msgs/LandmarkList.h" -#include "geometry_msgs/Transform.h" -#include "geometry_msgs/TransformStamped.h" -#include "nav_msgs/OccupancyGrid.h" -#include "nav_msgs/Odometry.h" -#include "sensor_msgs/Imu.h" -#include "sensor_msgs/LaserScan.h" -#include "sensor_msgs/MultiEchoLaserScan.h" -#include "sensor_msgs/NavSatFix.h" -#include "sensor_msgs/PointCloud2.h" +#include "cartographer_ros_msgs/msg/landmark_list.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include namespace cartographer_ros { @@ -51,26 +51,26 @@ class SensorBridge { SensorBridge& operator=(const SensorBridge&) = delete; std::unique_ptr<::cartographer::sensor::OdometryData> ToOdometryData( - const nav_msgs::Odometry::ConstPtr& msg); + const nav_msgs::msg::Odometry::ConstSharedPtr& msg); void HandleOdometryMessage(const std::string& sensor_id, - const nav_msgs::Odometry::ConstPtr& msg); + const nav_msgs::msg::Odometry::ConstSharedPtr& msg); void HandleNavSatFixMessage(const std::string& sensor_id, - const sensor_msgs::NavSatFix::ConstPtr& msg); + const sensor_msgs::msg::NavSatFix::ConstSharedPtr& msg); void HandleLandmarkMessage( const std::string& sensor_id, - const cartographer_ros_msgs::LandmarkList::ConstPtr& msg); + const cartographer_ros_msgs::msg::LandmarkList::ConstSharedPtr& msg); std::unique_ptr<::cartographer::sensor::ImuData> ToImuData( - const sensor_msgs::Imu::ConstPtr& msg); + const sensor_msgs::msg::Imu::ConstSharedPtr& msg); void HandleImuMessage(const std::string& sensor_id, - const sensor_msgs::Imu::ConstPtr& msg); + const sensor_msgs::msg::Imu::ConstSharedPtr& msg); void HandleLaserScanMessage(const std::string& sensor_id, - const sensor_msgs::LaserScan::ConstPtr& msg); + const sensor_msgs::msg::LaserScan::ConstSharedPtr& msg); void HandleMultiEchoLaserScanMessage( const std::string& sensor_id, - const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg); + const sensor_msgs::msg::MultiEchoLaserScan::ConstSharedPtr& msg); void HandlePointCloud2Message(const std::string& sensor_id, - const sensor_msgs::PointCloud2::ConstPtr& msg); + const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg); const TfBridge& tf_bridge() const; diff --git a/cartographer_ros/cartographer_ros/submap.cc b/cartographer_ros/cartographer_ros/submap.cc index 111eb1654..40fa94f46 100644 --- a/cartographer_ros/cartographer_ros/submap.cc +++ b/cartographer_ros/cartographer_ros/submap.cc @@ -20,27 +20,37 @@ #include "cartographer/common/port.h" #include "cartographer/transform/transform.h" #include "cartographer_ros/msg_conversion.h" -#include "cartographer_ros_msgs/StatusCode.h" -#include "cartographer_ros_msgs/SubmapQuery.h" +#include "cartographer_ros_msgs/msg/status_code.hpp" +#include "cartographer_ros_msgs/srv/submap_query.hpp" namespace cartographer_ros { std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures( const ::cartographer::mapping::SubmapId& submap_id, - ros::ServiceClient* client) { - ::cartographer_ros_msgs::SubmapQuery srv; - srv.request.trajectory_id = submap_id.trajectory_id; - srv.request.submap_index = submap_id.submap_index; - if (!client->call(srv) || - srv.response.status.code != ::cartographer_ros_msgs::StatusCode::OK) { + rclcpp::Client::SharedPtr client, + rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor, + const std::chrono::milliseconds timeout) +{ + auto request = std::make_shared(); + request->trajectory_id = submap_id.trajectory_id; + request->submap_index = submap_id.submap_index; + auto future_result = client->async_send_request(request); + + if (callback_group_executor->spin_until_future_complete(future_result, timeout) != + rclcpp::FutureReturnCode::SUCCESS) + { return nullptr; } - if (srv.response.textures.empty()) { + auto result = future_result.get(); + + if (result->status.code != ::cartographer_ros_msgs::msg::StatusCode::OK || + result->textures.empty()) { return nullptr; } + auto response = absl::make_unique<::cartographer::io::SubmapTextures>(); - response->version = srv.response.submap_version; - for (const auto& texture : srv.response.textures) { + response->version = result->submap_version; + for (const auto& texture : result->textures) { const std::string compressed_cells(texture.cells.begin(), texture.cells.end()); response->textures.emplace_back(::cartographer::io::SubmapTexture{ diff --git a/cartographer_ros/cartographer_ros/submap.h b/cartographer_ros/cartographer_ros/submap.h index 0df22154c..48c3a425b 100644 --- a/cartographer_ros/cartographer_ros/submap.h +++ b/cartographer_ros/cartographer_ros/submap.h @@ -25,7 +25,8 @@ #include "cartographer/io/submap_painter.h" #include "cartographer/mapping/id.h" #include "cartographer/transform/rigid_transform.h" -#include "ros/ros.h" +#include "cartographer_ros_msgs/srv/submap_query.hpp" +#include namespace cartographer_ros { @@ -33,7 +34,9 @@ namespace cartographer_ros { // on error. std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures( const ::cartographer::mapping::SubmapId& submap_id, - ros::ServiceClient* client); + rclcpp::Client::SharedPtr client, + rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor, + const std::chrono::milliseconds timeout); } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/tf_bridge.cc b/cartographer_ros/cartographer_ros/tf_bridge.cc index 29f80b12c..dcd6225a8 100644 --- a/cartographer_ros/cartographer_ros/tf_bridge.cc +++ b/cartographer_ros/cartographer_ros/tf_bridge.cc @@ -31,19 +31,20 @@ TfBridge::TfBridge(const std::string& tracking_frame, std::unique_ptr<::cartographer::transform::Rigid3d> TfBridge::LookupToTracking( const ::cartographer::common::Time time, const std::string& frame_id) const { - ::ros::Duration timeout(lookup_transform_timeout_sec_); + tf2::Duration timeout(tf2::durationFromSec(lookup_transform_timeout_sec_)); std::unique_ptr<::cartographer::transform::Rigid3d> frame_id_to_tracking; try { - const ::ros::Time latest_tf_time = + const rclcpp::Time latest_tf_time = buffer_ - ->lookupTransform(tracking_frame_, frame_id, ::ros::Time(0.), + ->lookupTransform(tracking_frame_, frame_id, ::rclcpp::Time(0.), timeout) .header.stamp; - const ::ros::Time requested_time = ToRos(time); + const rclcpp::Time requested_time = ToRos(time); + if (latest_tf_time >= requested_time) { // We already have newer data, so we do not wait. Otherwise, we would wait // for the full 'timeout' even if we ask for data that is too old. - timeout = ::ros::Duration(0.); + timeout = tf2::durationFromSec(0.0); } return absl::make_unique<::cartographer::transform::Rigid3d>( ToRigid3d(buffer_->lookupTransform(tracking_frame_, frame_id, diff --git a/cartographer_ros/cartographer_ros/tf_bridge.h b/cartographer_ros/cartographer_ros/tf_bridge.h index a74f85ae7..9308790b1 100644 --- a/cartographer_ros/cartographer_ros/tf_bridge.h +++ b/cartographer_ros/cartographer_ros/tf_bridge.h @@ -21,7 +21,8 @@ #include "cartographer/transform/rigid_transform.h" #include "cartographer_ros/time_conversion.h" -#include "tf2_ros/buffer.h" +#include +#include namespace cartographer_ros { diff --git a/cartographer_ros/cartographer_ros/time_conversion.cc b/cartographer_ros/cartographer_ros/time_conversion.cc index 1075c40e8..c852aab41 100644 --- a/cartographer_ros/cartographer_ros/time_conversion.cc +++ b/cartographer_ros/cartographer_ros/time_conversion.cc @@ -17,31 +17,28 @@ #include "cartographer_ros/time_conversion.h" #include "cartographer/common/time.h" -#include "ros/ros.h" +#include namespace cartographer_ros { -::ros::Time ToRos(::cartographer::common::Time time) { +rclcpp::Time ToRos(::cartographer::common::Time time) { int64_t uts_timestamp = ::cartographer::common::ToUniversal(time); int64_t ns_since_unix_epoch = (uts_timestamp - ::cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds * 10000000ll) * 100ll; - ::ros::Time ros_time; - ros_time.fromNSec(ns_since_unix_epoch); + rclcpp::Time ros_time(ns_since_unix_epoch, rcl_clock_type_t::RCL_ROS_TIME); return ros_time; } // TODO(pedrofernandez): Write test. -::cartographer::common::Time FromRos(const ::ros::Time& time) { +::cartographer::common::Time FromRos(const rclcpp::Time& time) { // The epoch of the ICU Universal Time Scale is "0001-01-01 00:00:00.0 +0000", // exactly 719162 days before the Unix epoch. return ::cartographer::common::FromUniversal( - (time.sec + - ::cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds) * - 10000000ll + - (time.nsec + 50) / 100); // + 50 to get the rounding correct. + cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds * 10000000ll + + (time.nanoseconds() + 50) / 100); // + 50 to get the rounding correct. } } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/time_conversion.h b/cartographer_ros/cartographer_ros/time_conversion.h index dda5bb5ab..5555f4d49 100644 --- a/cartographer_ros/cartographer_ros/time_conversion.h +++ b/cartographer_ros/cartographer_ros/time_conversion.h @@ -18,13 +18,14 @@ #define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TIME_CONVERSION_H #include "cartographer/common/time.h" -#include "ros/ros.h" +#include +#include namespace cartographer_ros { -::ros::Time ToRos(::cartographer::common::Time time); +rclcpp::Time ToRos(::cartographer::common::Time time); -::cartographer::common::Time FromRos(const ::ros::Time& time); +::cartographer::common::Time FromRos(const rclcpp::Time& time); } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/time_conversion_test.cc b/cartographer_ros/cartographer_ros/time_conversion_test.cc index 3b2d57230..13337fb3a 100644 --- a/cartographer_ros/cartographer_ros/time_conversion_test.cc +++ b/cartographer_ros/cartographer_ros/time_conversion_test.cc @@ -29,7 +29,7 @@ TEST(TimeConversion, testToRos) { std::vector values = {0, 1469091375, 1466481821, 1462101382, 1468238899}; for (int64_t seconds_since_epoch : values) { - ::ros::Time ros_now; + builtin_interfaces::msg::Time ros_now; ros_now.fromSec(seconds_since_epoch); ::cartographer::common::Time cartographer_now( ::cartographer::common::FromSeconds( diff --git a/cartographer_ros/cartographer_ros/urdf_reader.cc b/cartographer_ros/cartographer_ros/urdf_reader.cc index e389e18d7..241cbbb0c 100644 --- a/cartographer_ros/cartographer_ros/urdf_reader.cc +++ b/cartographer_ros/cartographer_ros/urdf_reader.cc @@ -24,8 +24,8 @@ namespace cartographer_ros { -std::vector ReadStaticTransformsFromUrdf( - const std::string& urdf_filename, tf2_ros::Buffer* const tf_buffer) { +std::vector ReadStaticTransformsFromUrdf( + const std::string& urdf_filename, std::shared_ptr tf_buffer) { urdf::Model model; CHECK(model.initFile(urdf_filename)); #if URDFDOM_HEADERS_HAS_SHARED_PTR_DEFS @@ -34,7 +34,7 @@ std::vector ReadStaticTransformsFromUrdf( std::vector > links; #endif model.getLinks(links); - std::vector transforms; + std::vector transforms; for (const auto& link : links) { if (!link->getParent() || link->parent_joint->type != urdf::Joint::FIXED) { continue; @@ -42,7 +42,7 @@ std::vector ReadStaticTransformsFromUrdf( const urdf::Pose& pose = link->parent_joint->parent_to_joint_origin_transform; - geometry_msgs::TransformStamped transform; + geometry_msgs::msg::TransformStamped transform; transform.transform = ToGeometryMsgTransform(cartographer::transform::Rigid3d( Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z), diff --git a/cartographer_ros/cartographer_ros/urdf_reader.h b/cartographer_ros/cartographer_ros/urdf_reader.h index 93d8d11f6..ab46bfa6c 100644 --- a/cartographer_ros/cartographer_ros/urdf_reader.h +++ b/cartographer_ros/cartographer_ros/urdf_reader.h @@ -24,8 +24,8 @@ namespace cartographer_ros { -std::vector ReadStaticTransformsFromUrdf( - const std::string& urdf_filename, tf2_ros::Buffer* tf_buffer); +std::vector ReadStaticTransformsFromUrdf( + const std::string& urdf_filename, std::shared_ptr tf_buffer); } // namespace cartographer_ros diff --git a/cartographer_ros/package.xml b/cartographer_ros/package.xml index cd646489a..ff84f7001 100644 --- a/cartographer_ros/package.xml +++ b/cartographer_ros/package.xml @@ -15,9 +15,9 @@ limitations under the License. --> - + cartographer_ros - 1.0.0 + 2.0.0 Cartographer is a system that provides real-time simultaneous localization and mapping (SLAM) in 2D and 3D across multiple platforms and sensor @@ -34,15 +34,16 @@ The Cartographer Authors - catkin + ament_cmake git google-mock protobuf-dev python3-sphinx - cartographer - cartographer_ros_msgs + + cartographer + cartographer_ros_msgs geometry_msgs libgflags-dev libgoogle-glog-dev @@ -63,9 +64,7 @@ urdf visualization_msgs - rosunit - - + ament_cmake diff --git a/cartographer_ros_msgs/CMakeLists.txt b/cartographer_ros_msgs/CMakeLists.txt index 42d3c97e5..a60f711ef 100644 --- a/cartographer_ros_msgs/CMakeLists.txt +++ b/cartographer_ros_msgs/CMakeLists.txt @@ -12,54 +12,61 @@ # See the License for the specific language governing permissions and # limitations under the License. -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(cartographer_ros_msgs) -set(PACKAGE_DEPENDENCIES - geometry_msgs - std_msgs -) +# Default to C++14. +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() -find_package(catkin REQUIRED COMPONENTS message_generation ${PACKAGE_DEPENDENCIES}) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + # We don't use add_compile_options with pedantic in message packages + # because the Python C extensions don't comply with it. + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +endif() -add_message_files( - DIRECTORY msg - FILES - BagfileProgress.msg - HistogramBucket.msg - LandmarkEntry.msg - LandmarkList.msg - MetricFamily.msg - MetricLabel.msg - Metric.msg - StatusCode.msg - StatusResponse.msg - SubmapEntry.msg - SubmapList.msg - SubmapTexture.msg - TrajectoryStates.msg -) +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +find_package(builtin_interfaces REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) -add_service_files( - DIRECTORY srv - FILES - FinishTrajectory.srv - GetTrajectoryStates.srv - ReadMetrics.srv - StartTrajectory.srv - SubmapQuery.srv - TrajectoryQuery.srv - WriteState.srv +set(msg_files + "msg/BagfileProgress.msg" + "msg/HistogramBucket.msg" + "msg/LandmarkEntry.msg" + "msg/LandmarkList.msg" + "msg/MetricFamily.msg" + "msg/MetricLabel.msg" + "msg/Metric.msg" + "msg/StatusCode.msg" + "msg/StatusResponse.msg" + "msg/SubmapEntry.msg" + "msg/SubmapList.msg" + "msg/SubmapTexture.msg" + "msg/TrajectoryStates.msg" ) -generate_messages( - DEPENDENCIES - ${PACKAGE_DEPENDENCIES} +set(srv_files + "srv/FinishTrajectory.srv" + "srv/GetTrajectoryStates.srv" + "srv/ReadMetrics.srv" + "srv/StartTrajectory.srv" + "srv/TrajectoryQuery.srv" + "srv/SubmapQuery.srv" + "srv/WriteState.srv" ) -catkin_package( - CATKIN_DEPENDS - ${PACKAGE_DEPENDENCIES} - message_runtime +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + ${srv_files} + DEPENDENCIES geometry_msgs std_msgs + ADD_LINTER_TESTS ) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/cartographer_ros_msgs/package.xml b/cartographer_ros_msgs/package.xml index e26713349..1cbf10141 100644 --- a/cartographer_ros_msgs/package.xml +++ b/cartographer_ros_msgs/package.xml @@ -15,7 +15,7 @@ limitations under the License. --> - + cartographer_ros_msgs 1.0.0 @@ -32,12 +32,20 @@ The Cartographer Authors - catkin + ament_cmake + rosidl_default_generators geometry_msgs std_msgs - message_generation - - message_runtime + rosidl_default_runtime + + rosidl_interface_packages + + ament_lint_auto + ament_lint_common + + + ament_cmake + diff --git a/cartographer_ros_msgs/srv/ReadMetrics.srv b/cartographer_ros_msgs/srv/ReadMetrics.srv index 5070e3805..d6be66f07 100644 --- a/cartographer_ros_msgs/srv/ReadMetrics.srv +++ b/cartographer_ros_msgs/srv/ReadMetrics.srv @@ -15,4 +15,4 @@ --- cartographer_ros_msgs/StatusResponse status cartographer_ros_msgs/MetricFamily[] metric_families -time timestamp +builtin_interfaces/Time timestamp diff --git a/cartographer_rviz/CMakeLists.txt b/cartographer_rviz/CMakeLists.txt index d9be9729e..9f5988a7c 100644 --- a/cartographer_rviz/CMakeLists.txt +++ b/cartographer_rviz/CMakeLists.txt @@ -14,97 +14,97 @@ cmake_minimum_required(VERSION 2.8.12) # Ships with Ubuntu 14.04 (Trusty) -project(cartographer_rviz) - -set(PACKAGE_DEPENDENCIES - cartographer_ros - cartographer_ros_msgs - message_runtime - roscpp - roslib - rviz -) - -if(WIN32) - set(Boost_USE_STATIC_LIBS FALSE) -endif() -find_package(Boost REQUIRED COMPONENTS system iostreams) -find_package(cartographer REQUIRED) -include("${CARTOGRAPHER_CMAKE_DIR}/functions.cmake") -google_initialize_cartographer_project() - -find_package(absl REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES}) - -catkin_package( - CATKIN_DEPENDS - message_runtime - ${PACKAGE_DEPENDENCIES} - INCLUDE_DIRS "." -) - -file(GLOB_RECURSE ALL_SRCS "cartographer_rviz/*.cc" "cartographer_rviz/*.h") -set(CMAKE_AUTOMOC ON) - -if(rviz_QT_VERSION VERSION_LESS "5") - message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") - find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) - include(${QT_USE_FILE}) -else() - message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") - find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) - set(QT_LIBRARIES Qt5::Core Qt5::Widgets) - include_directories(${Qt5Widgets_INCLUDE_DIRS}) -endif() -add_definitions(-DQT_NO_KEYWORDS) -add_library(${PROJECT_NAME} ${ALL_SRCS}) -target_link_libraries(${PROJECT_NAME} PUBLIC ${QT_LIBRARIES}) - -# Add the binary directory first, so that port.h is included after it has -# been generated. -target_include_directories(${PROJECT_NAME} PUBLIC - $ - $ - $ -) - -target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC - "${EIGEN3_INCLUDE_DIR}") -target_link_libraries(${PROJECT_NAME} PUBLIC ${EIGEN3_LIBRARIES}) - -target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC - "${Boost_INCLUDE_DIRS}") -target_link_libraries(${PROJECT_NAME} PUBLIC ${Boost_LIBRARIES}) - -target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS}) -target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES}) -add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) - -set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${GOOG_CXX_FLAGS}") -set_target_properties(${PROJECT_NAME} PROPERTIES - COMPILE_FLAGS ${TARGET_COMPILE_FLAGS}) - -target_link_libraries(${PROJECT_NAME} PUBLIC cartographer) - -# On windows, rviz won't find the DLL in CATKIN_PACKAGE_BIN_DESTINATION, -# but it will in CATKIN_PACKAGE_LIB_DESTINATION? -if(WIN32) - set(RUNTIME_DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) -else() - set(RUNTIME_DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) -endif() - -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${RUNTIME_DESTINATION} -) - -install(FILES rviz_plugin_description.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY ogre_media - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) +#project(cartographer_rviz) + +#set(PACKAGE_DEPENDENCIES +# cartographer_ros +# cartographer_ros_msgs +# message_runtime +# roscpp +# roslib +# rviz +#) + +#if(WIN32) +# set(Boost_USE_STATIC_LIBS FALSE) +#endif() +#find_package(Boost REQUIRED COMPONENTS system iostreams) +#find_package(cartographer REQUIRED) +#include("${CARTOGRAPHER_CMAKE_DIR}/functions.cmake") +#google_initialize_cartographer_project() + +#find_package(absl REQUIRED) +#find_package(Eigen3 REQUIRED) +#find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES}) + +#catkin_package( +# CATKIN_DEPENDS +# message_runtime +# ${PACKAGE_DEPENDENCIES} +# INCLUDE_DIRS "." +#) + +#file(GLOB_RECURSE ALL_SRCS "cartographer_rviz/*.cc" "cartographer_rviz/*.h") +#set(CMAKE_AUTOMOC ON) + +#if(rviz_QT_VERSION VERSION_LESS "5") +# message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") +# find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) +# include(${QT_USE_FILE}) +#else() +# message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") +# find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) +# set(QT_LIBRARIES Qt5::Core Qt5::Widgets) +# include_directories(${Qt5Widgets_INCLUDE_DIRS}) +#endif() +#add_definitions(-DQT_NO_KEYWORDS) +#add_library(${PROJECT_NAME} ${ALL_SRCS}) +#target_link_libraries(${PROJECT_NAME} PUBLIC ${QT_LIBRARIES}) + +## Add the binary directory first, so that port.h is included after it has +## been generated. +#target_include_directories(${PROJECT_NAME} PUBLIC +# $ +# $ +# $ +#) + +#target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC +# "${EIGEN3_INCLUDE_DIR}") +#target_link_libraries(${PROJECT_NAME} PUBLIC ${EIGEN3_LIBRARIES}) + +#target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC +# "${Boost_INCLUDE_DIRS}") +#target_link_libraries(${PROJECT_NAME} PUBLIC ${Boost_LIBRARIES}) + +#target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS}) +#target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES}) +#add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) + +#set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${GOOG_CXX_FLAGS}") +#set_target_properties(${PROJECT_NAME} PROPERTIES +# COMPILE_FLAGS ${TARGET_COMPILE_FLAGS}) + +#target_link_libraries(${PROJECT_NAME} PUBLIC cartographer) + +## On windows, rviz won't find the DLL in CATKIN_PACKAGE_BIN_DESTINATION, +## but it will in CATKIN_PACKAGE_LIB_DESTINATION? +#if(WIN32) +# set(RUNTIME_DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +#else() +# set(RUNTIME_DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +#endif() + +#install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${RUNTIME_DESTINATION} +#) + +#install(FILES rviz_plugin_description.xml +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +#) + +#install(DIRECTORY ogre_media +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +#) From b2443418f3093ae4c36cc216c0985cbfca0cd4ab Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Thu, 6 May 2021 11:41:26 +0200 Subject: [PATCH 43/94] remove debug compil options Signed-off-by: Guillaume Doisy --- cartographer_ros/CMakeLists.txt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/cartographer_ros/CMakeLists.txt b/cartographer_ros/CMakeLists.txt index 2961fad09..6dbe2d52a 100644 --- a/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/CMakeLists.txt @@ -25,7 +25,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra) endif() -add_compile_options(-g) +#add_compile_options(-g) find_package(cartographer REQUIRED) include("${CARTOGRAPHER_CMAKE_DIR}/functions.cmake") @@ -49,7 +49,7 @@ find_package(visualization_msgs REQUIRED) find_package(rosbag2_compression REQUIRED) find_package(rosbag2_cpp REQUIRED) find_package(rosbag2_storage REQUIRED) -find_package(backward_ros REQUIRED) +#find_package(backward_ros REQUIRED) find_package(absl REQUIRED) find_package(LuaGoogle REQUIRED) @@ -84,7 +84,7 @@ endif() add_library(${PROJECT_NAME} STATIC ${ALL_SRCS}) ament_target_dependencies(${PROJECT_NAME} PUBLIC - backward_ros + #backward_ros cartographer cartographer_ros_msgs geometry_msgs From 9d523f5d3f444bb6cfc598ec111e59aeabb8e1e4 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Fri, 4 Jun 2021 09:29:17 +0200 Subject: [PATCH 44/94] wip not compiling Signed-off-by: Guillaume Doisy --- cartographer_rviz/CMakeLists.txt | 203 ++++++++++-------- .../cartographer_rviz/drawable_submap.cc | 4 +- .../cartographer_rviz/drawable_submap.h | 12 +- .../cartographer_rviz/submaps_display.cc | 16 +- .../cartographer_rviz/submaps_display.h | 8 +- cartographer_rviz/package.xml | 27 ++- 6 files changed, 150 insertions(+), 120 deletions(-) diff --git a/cartographer_rviz/CMakeLists.txt b/cartographer_rviz/CMakeLists.txt index 9f5988a7c..d78837780 100644 --- a/cartographer_rviz/CMakeLists.txt +++ b/cartographer_rviz/CMakeLists.txt @@ -12,99 +12,124 @@ # See the License for the specific language governing permissions and # limitations under the License. -cmake_minimum_required(VERSION 2.8.12) # Ships with Ubuntu 14.04 (Trusty) +cmake_minimum_required(VERSION 3.5) + +project(cartographer_rviz) + +find_package(ament_cmake REQUIRED) + +# Default to C++17 +set(CMAKE_CXX_STANDARD 17) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() + +find_package(Boost REQUIRED COMPONENTS system iostreams) +find_package(cartographer REQUIRED) +include("${CARTOGRAPHER_CMAKE_DIR}/functions.cmake") +set(BUILD_SHARED_LIBS OFF) +option(BUILD_GRPC "build features that require Cartographer gRPC support" false) +google_initialize_cartographer_project() + + +find_package(absl REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(cartographer_ros REQUIRED) +find_package(cartographer_ros_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rviz2 REQUIRED) +find_package(rviz_ogre_vendor REQUIRED) +find_package(rviz_assimp_vendor REQUIRED) +find_package(rviz_rendering REQUIRED) +find_package(rviz_common REQUIRED) +find_package(rviz_default_plugins REQUIRED) +#find_package(rviz REQUIRED) + +include_directories( + include + ".") + +file(GLOB_RECURSE ALL_SRCS "cartographer_rviz/*.cc" "cartographer_rviz/*.h") +set(CMAKE_AUTOMOC ON) + +add_library(${PROJECT_NAME} ${ALL_SRCS}) +ament_target_dependencies(${PROJECT_NAME} PUBLIC + rclcpp + cartographer_ros + cartographer_ros_msgs + rviz2 + rviz_ogre_vendor + rviz_assimp_vendor + rviz_rendering + rviz_common + rviz_default_plugins -#project(cartographer_rviz) - -#set(PACKAGE_DEPENDENCIES -# cartographer_ros -# cartographer_ros_msgs -# message_runtime -# roscpp -# roslib # rviz -#) +) + +ament_export_dependencies(rosidl_default_runtime) + +#if(rviz2_QT_VERSION VERSION_LESS "5") +# message(STATUS "Using Qt4 based on the rviz2_QT_VERSION: ${rviz2_QT_VERSION}") + #find_package(Qt4 ${rviz2_QT_VERSION} EXACT REQUIRED QtCore QtGui) -#if(WIN32) -# set(Boost_USE_STATIC_LIBS FALSE) -#endif() -#find_package(Boost REQUIRED COMPONENTS system iostreams) -#find_package(cartographer REQUIRED) -#include("${CARTOGRAPHER_CMAKE_DIR}/functions.cmake") -#google_initialize_cartographer_project() - -#find_package(absl REQUIRED) -#find_package(Eigen3 REQUIRED) -#find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES}) - -#catkin_package( -# CATKIN_DEPENDS -# message_runtime -# ${PACKAGE_DEPENDENCIES} -# INCLUDE_DIRS "." -#) - -#file(GLOB_RECURSE ALL_SRCS "cartographer_rviz/*.cc" "cartographer_rviz/*.h") -#set(CMAKE_AUTOMOC ON) - -#if(rviz_QT_VERSION VERSION_LESS "5") -# message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") -# find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) # include(${QT_USE_FILE}) #else() -# message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") -# find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) -# set(QT_LIBRARIES Qt5::Core Qt5::Widgets) -# include_directories(${Qt5Widgets_INCLUDE_DIRS}) -#endif() -#add_definitions(-DQT_NO_KEYWORDS) -#add_library(${PROJECT_NAME} ${ALL_SRCS}) -#target_link_libraries(${PROJECT_NAME} PUBLIC ${QT_LIBRARIES}) - -## Add the binary directory first, so that port.h is included after it has -## been generated. -#target_include_directories(${PROJECT_NAME} PUBLIC -# $ -# $ -# $ -#) - -#target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC -# "${EIGEN3_INCLUDE_DIR}") -#target_link_libraries(${PROJECT_NAME} PUBLIC ${EIGEN3_LIBRARIES}) - -#target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC -# "${Boost_INCLUDE_DIRS}") -#target_link_libraries(${PROJECT_NAME} PUBLIC ${Boost_LIBRARIES}) - -#target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS}) -#target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES}) -#add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) - -#set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${GOOG_CXX_FLAGS}") -#set_target_properties(${PROJECT_NAME} PROPERTIES -# COMPILE_FLAGS ${TARGET_COMPILE_FLAGS}) - -#target_link_libraries(${PROJECT_NAME} PUBLIC cartographer) - -## On windows, rviz won't find the DLL in CATKIN_PACKAGE_BIN_DESTINATION, -## but it will in CATKIN_PACKAGE_LIB_DESTINATION? -#if(WIN32) -# set(RUNTIME_DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) -#else() -# set(RUNTIME_DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + message(STATUS "Using Qt5 based on the rviz2_QT_VERSION: ${rviz2_QT_VERSION}") + find_package(Qt5 ${rviz2_QT_VERSION} EXACT REQUIRED Core Widgets) + set(QT_LIBRARIES Qt5::Core Qt5::Widgets) + include_directories(${Qt5Widgets_INCLUDE_DIRS}) #endif() -#install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${RUNTIME_DESTINATION} -#) - -#install(FILES rviz_plugin_description.xml -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -#) - -#install(DIRECTORY ogre_media -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -#) +add_definitions(-DQT_NO_KEYWORDS) +#add_library(${PROJECT_NAME} ${ALL_SRCS}) +target_link_libraries(${PROJECT_NAME} PUBLIC ${QT_LIBRARIES}) + +# Add the binary directory first, so that port.h is included after it has +# been generated. +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ + $ +) + +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC + "${EIGEN3_INCLUDE_DIR}") +target_link_libraries(${PROJECT_NAME} PUBLIC ${EIGEN3_LIBRARIES}) + +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC + "${CARTOGRAPHER_ROS_INCLUDE_DIR}") +target_link_libraries(${PROJECT_NAME} PUBLIC ${CARTOGRAPHER_ROS_LIBRARIES}) + +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC + "${Boost_INCLUDE_DIRS}") +target_link_libraries(${PROJECT_NAME} PUBLIC ${Boost_LIBRARIES}) + +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC + "${RVIZ_OGRE_VENDOR_INCLUDE_DIRS}") +target_link_libraries(${PROJECT_NAME} PUBLIC ${RVIZ_OGRE_VENDOR_LIBRARIES}) + +set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${GOOG_CXX_FLAGS}") +set_target_properties(${PROJECT_NAME} PROPERTIES + COMPILE_FLAGS ${TARGET_COMPILE_FLAGS}) + +target_link_libraries(${PROJECT_NAME} PUBLIC cartographer) +target_link_libraries(${PROJECT_NAME} PUBLIC rviz_ogre_vendor) +target_link_libraries(${PROJECT_NAME} PUBLIC cartographer_ros) + +install(DIRECTORY configuration_files + DESTINATION share/${PROJECT_NAME}/ +) + +install(DIRECTORY ogre_media + DESTINATION share/${PROJECT_NAME}/ +) + +install(TARGETS + ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +ament_package() diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index d2c800f4b..60e3a84fb 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -26,8 +26,8 @@ #include "absl/memory/memory.h" #include "cartographer/common/port.h" #include "cartographer_ros/msg_conversion.h" -#include "cartographer_ros_msgs/SubmapQuery.h" -#include "ros/ros.h" +#include "cartographer_ros_msgs/srv/submap_query.hpp" +#include namespace cartographer_rviz { diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.h b/cartographer_rviz/cartographer_rviz/drawable_submap.h index 21e19331d..3c3fabd40 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.h +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.h @@ -29,15 +29,15 @@ #include "cartographer/mapping/id.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer_ros/submap.h" -#include "cartographer_ros_msgs/SubmapEntry.h" -#include "cartographer_ros_msgs/SubmapQuery.h" +#include "cartographer_ros_msgs/msg/submap_entry.hpp" +#include "cartographer_ros_msgs/srv/submap_query.hpp" #include "cartographer_rviz/ogre_slice.h" -#include "ros/ros.h" -#include "rviz/display_context.h" -#include "rviz/frame_manager.h" +#include +#include +#include #include "rviz/ogre_helpers/axes.h" #include "rviz/ogre_helpers/movable_text.h" -#include "rviz/properties/bool_property.h" +#include namespace cartographer_rviz { diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc index 76f5d2d49..536f2147e 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/cartographer_rviz/submaps_display.cc @@ -20,16 +20,16 @@ #include "absl/memory/memory.h" #include "absl/synchronization/mutex.h" #include "cartographer/mapping/id.h" -#include "cartographer_ros_msgs/SubmapList.h" -#include "cartographer_ros_msgs/SubmapQuery.h" -#include "geometry_msgs/TransformStamped.h" +#include "cartographer_ros_msgs/msg/submap_list.hpp" +#include "cartographer_ros_msgs/srv/submap_query.hpp" +#include #include "pluginlib/class_list_macros.h" #include "ros/package.h" -#include "ros/ros.h" -#include "rviz/display_context.h" -#include "rviz/frame_manager.h" -#include "rviz/properties/bool_property.h" -#include "rviz/properties/string_property.h" +#include +#include +#include +#include +#include namespace cartographer_rviz { diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.h b/cartographer_rviz/cartographer_rviz/submaps_display.h index 7244b50e7..aa2570614 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.h +++ b/cartographer_rviz/cartographer_rviz/submaps_display.h @@ -24,11 +24,11 @@ #include "absl/synchronization/mutex.h" #include "cartographer/common/port.h" -#include "cartographer_ros_msgs/SubmapList.h" +#include "cartographer_ros_msgs/msg/submap_list.hpp" #include "cartographer_rviz/drawable_submap.h" -#include "rviz/message_filter_display.h" -#include "rviz/properties/bool_property.h" -#include "rviz/properties/float_property.h" +#include +#include +#include #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" diff --git a/cartographer_rviz/package.xml b/cartographer_rviz/package.xml index b43038907..cb3e4f169 100644 --- a/cartographer_rviz/package.xml +++ b/cartographer_rviz/package.xml @@ -15,9 +15,9 @@ limitations under the License. --> - + cartographer_rviz - 1.0.0 + 2.0.0 Cartographer is a system that provides real-time simultaneous localization and mapping (SLAM) in 2D and 3D across multiple platforms and sensor @@ -34,23 +34,28 @@ The Cartographer Authors - catkin + ament_cmake git - cartographer - cartographer_ros - cartographer_ros_msgs + cartographer + cartographer_ros + cartographer_ros_msgs libqt5-core libqt5-gui libqt5-widgets - message_runtime qtbase5-dev - roscpp - roslib - rviz + rclcpp + + rviz2 + rviz_ogre_vendor + rviz_assimp_vendor + rviz_rendering + rviz_common + rviz_default_plugins - + ament_cmake + From 65e1e813b52cb49eb67d6dcbc64b2c4a1d197aa4 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 15 Jun 2021 10:49:43 +0200 Subject: [PATCH 45/94] wip not compiling Signed-off-by: Guillaume Doisy --- .../cartographer_rviz/drawable_submap.cc | 18 +- .../cartographer_rviz/drawable_submap.h | 25 +- .../cartographer_rviz/msg_conversion.cc | 422 ++++++++++++++++++ .../cartographer_rviz/msg_conversion.h | 95 ++++ cartographer_rviz/cartographer_rviz/submap.cc | 65 +++ cartographer_rviz/cartographer_rviz/submap.h | 43 ++ .../cartographer_rviz/submaps_display.cc | 40 +- .../cartographer_rviz/submaps_display.h | 28 +- .../cartographer_rviz/time_conversion.cc | 44 ++ .../cartographer_rviz/time_conversion.h | 32 ++ 10 files changed, 757 insertions(+), 55 deletions(-) create mode 100644 cartographer_rviz/cartographer_rviz/msg_conversion.cc create mode 100644 cartographer_rviz/cartographer_rviz/msg_conversion.h create mode 100644 cartographer_rviz/cartographer_rviz/submap.cc create mode 100644 cartographer_rviz/cartographer_rviz/submap.h create mode 100644 cartographer_rviz/cartographer_rviz/time_conversion.cc create mode 100644 cartographer_rviz/cartographer_rviz/time_conversion.h diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index 60e3a84fb..44895d980 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -25,7 +25,7 @@ #include "Eigen/Geometry" #include "absl/memory/memory.h" #include "cartographer/common/port.h" -#include "cartographer_ros/msg_conversion.h" +#include "cartographer_rviz/msg_conversion.h" #include "cartographer_ros_msgs/srv/submap_query.hpp" #include @@ -44,9 +44,9 @@ constexpr int kNumberOfSlicesPerSubmap = 2; } // namespace DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id, - ::rviz::DisplayContext* const display_context, + ::rviz_common::DisplayContext* const display_context, Ogre::SceneNode* const map_node, - ::rviz::Property* const submap_category, + ::rviz_common::properties::Property* const submap_category, const bool visible, const bool pose_axes_visible, const float pose_axes_length, const float pose_axes_radius) @@ -71,13 +71,13 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id, // (a unique_ptr is needed because the Qt parent of the visibility // property is the submap_category object - the BoolProperty needs // to be destroyed along with the DrawableSubmap) - visibility_ = absl::make_unique<::rviz::BoolProperty>( + visibility_ = absl::make_unique<::rviz_common::properties::BoolProperty>( "" /* title */, visible, "" /* description */, submap_category, SLOT(ToggleVisibility()), this); submap_id_text_.setCharacterHeight(kSubmapIdCharHeight); submap_id_text_.setColor(kSubmapIdColor); - submap_id_text_.setTextAlignment(::rviz::MovableText::H_CENTER, - ::rviz::MovableText::V_ABOVE); + submap_id_text_.setTextAlignment(::rviz_rendering::MovableText::H_CENTER, + ::rviz_rendering::MovableText::V_ABOVE); submap_id_text_node_->setPosition(ToOgre(kSubmapIdPosition)); submap_id_text_node_->attachObject(&submap_id_text_); TogglePoseMarkerVisibility(); @@ -95,8 +95,8 @@ DrawableSubmap::~DrawableSubmap() { } void DrawableSubmap::Update( - const ::std_msgs::Header& header, - const ::cartographer_ros_msgs::SubmapEntry& metadata) { + const ::std_msgs::msg::Header& header, + const ::cartographer_ros_msgs::msg::SubmapEntry& metadata) { absl::MutexLock locker(&mutex_); metadata_version_ = metadata.submap_version; pose_ = ::cartographer_ros::ToRigid3d(metadata.pose); @@ -113,7 +113,7 @@ void DrawableSubmap::Update( .arg(metadata_version_)); } -bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) { +bool DrawableSubmap::MaybeFetchTexture(rclcpp::Client* const client) { absl::MutexLock locker(&mutex_); // Received metadata version can also be lower if we restarted Cartographer. const bool newer_version_available = diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.h b/cartographer_rviz/cartographer_rviz/drawable_submap.h index 3c3fabd40..2f4ea0f97 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.h +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.h @@ -28,15 +28,16 @@ #include "cartographer/io/submap_painter.h" #include "cartographer/mapping/id.h" #include "cartographer/transform/rigid_transform.h" -#include "cartographer_ros/submap.h" +//#include +#include "cartographer_rviz/submap.h" #include "cartographer_ros_msgs/msg/submap_entry.hpp" #include "cartographer_ros_msgs/srv/submap_query.hpp" #include "cartographer_rviz/ogre_slice.h" #include #include #include -#include "rviz/ogre_helpers/axes.h" -#include "rviz/ogre_helpers/movable_text.h" +#include "rviz_rendering/objects/axes.hpp" +#include "rviz_rendering/objects/movable_text.hpp" #include namespace cartographer_rviz { @@ -48,8 +49,8 @@ class DrawableSubmap : public QObject { public: DrawableSubmap(const ::cartographer::mapping::SubmapId& submap_id, - ::rviz::DisplayContext* display_context, - Ogre::SceneNode* map_node, ::rviz::Property* submap_category, + ::rviz_common::DisplayContext* display_context, + Ogre::SceneNode* map_node, ::rviz_common::properties::Property* submap_category, bool visible, const bool pose_axes_visible, float pose_axes_length, float pose_axes_radius); ~DrawableSubmap() override; @@ -58,12 +59,12 @@ class DrawableSubmap : public QObject { // Updates the 'metadata' for this submap. If necessary, the next call to // MaybeFetchTexture() will fetch a new submap texture. - void Update(const ::std_msgs::Header& header, - const ::cartographer_ros_msgs::SubmapEntry& metadata); + void Update(const ::std_msgs::msg::Header& header, + const ::cartographer_ros_msgs::msg::SubmapEntry& metadata); // If an update is needed, it will send an RPC using 'client' to request the // new data for the submap and returns true. - bool MaybeFetchTexture(ros::ServiceClient* client); + bool MaybeFetchTexture(rclcpp::Client* client); // Returns whether an RPC is in progress. bool QueryInProgress(); @@ -103,14 +104,14 @@ class DrawableSubmap : public QObject { const ::cartographer::mapping::SubmapId id_; absl::Mutex mutex_; - ::rviz::DisplayContext* const display_context_; + ::rviz_common::DisplayContext* const display_context_; Ogre::SceneNode* const submap_node_; Ogre::SceneNode* const submap_id_text_node_; std::vector> ogre_slices_; ::cartographer::transform::Rigid3d pose_ GUARDED_BY(mutex_); - ::rviz::Axes pose_axes_; + ::rviz_rendering::Axes pose_axes_; bool pose_axes_visible_; - ::rviz::MovableText submap_id_text_; + ::rviz_rendering::MovableText submap_id_text; std::chrono::milliseconds last_query_timestamp_ GUARDED_BY(mutex_); bool query_in_progress_ GUARDED_BY(mutex_) = false; int metadata_version_ GUARDED_BY(mutex_) = -1; @@ -118,7 +119,7 @@ class DrawableSubmap : public QObject { std::unique_ptr<::cartographer::io::SubmapTextures> submap_textures_ GUARDED_BY(mutex_); float current_alpha_ = 0.f; - std::unique_ptr<::rviz::BoolProperty> visibility_; + std::unique_ptr<::rviz_common::properties::BoolProperty> visibility_; }; } // namespace cartographer_rviz diff --git a/cartographer_rviz/cartographer_rviz/msg_conversion.cc b/cartographer_rviz/cartographer_rviz/msg_conversion.cc new file mode 100644 index 000000000..a46be5b44 --- /dev/null +++ b/cartographer_rviz/cartographer_rviz/msg_conversion.cc @@ -0,0 +1,422 @@ +/* + * Copyright 2016 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. + */ + +#include "cartographer_rviz/msg_conversion.h" + +#include + +#include "cartographer/common/math.h" +#include "cartographer/common/port.h" +#include "cartographer/common/time.h" +#include "cartographer/io/submap_painter.h" +#include "cartographer/transform/proto/transform.pb.h" +#include "cartographer/transform/transform.h" +#include "cartographer_rviz/time_conversion.h" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/transform.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/vector3.hpp" +#include "glog/logging.h" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include +//#include "pcl/point_cloud.h" +#include "pcl/point_types.h" +#include "pcl_conversions/pcl_conversions.h" +#include "builtin_interfaces/msg/time.hpp" +//#include "ros/serialization.h" +#include "sensor_msgs/msg/imu.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "sensor_msgs/msg/multi_echo_laser_scan.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" + +namespace { + +// Sizes of PCL point types have to be 4n floats for alignment, as described in +// http://pointclouds.org/documentation/tutorials/adding_custom_ptype.php +struct PointXYZT { + float x; + float y; + float z; + float time; +}; + +struct PointXYZIT { + PCL_ADD_POINT4D; + float intensity; + float time; + float unused_padding[2]; +}; + +} // namespace + +POINT_CLOUD_REGISTER_POINT_STRUCT( + PointXYZT, (float, x, x)(float, y, y)(float, z, z)(float, time, time)) + +POINT_CLOUD_REGISTER_POINT_STRUCT( + PointXYZIT, + (float, x, x)(float, y, y)(float, z, z)(float, intensity, + intensity)(float, time, time)) + +namespace cartographer_ros { +namespace { + +// The ros::sensor_msgs::msg::PointCloud2 binary data contains 4 floats for each +// point. The last one must be this value or RViz is not showing the point cloud +// properly. +constexpr float kPointCloudComponentFourMagic = 1.; + +using ::cartographer::sensor::LandmarkData; +using ::cartographer::sensor::LandmarkObservation; +using ::cartographer::sensor::PointCloudWithIntensities; +using ::cartographer::transform::Rigid3d; +using ::cartographer_ros_msgs::msg::LandmarkEntry; +using ::cartographer_ros_msgs::msg::LandmarkList; + +sensor_msgs::msg::PointCloud2 PreparePointCloud2Message(const int64_t timestamp, + const std::string& frame_id, + const int num_points) { + sensor_msgs::msg::PointCloud2 msg; + msg.header.stamp = ToRos(::cartographer::common::FromUniversal(timestamp)); + msg.header.frame_id = frame_id; + msg.height = 1; + msg.width = num_points; + msg.fields.resize(3); + msg.fields[0].name = "x"; + msg.fields[0].offset = 0; + msg.fields[0].datatype = sensor_msgs::msg::PointField::FLOAT32; + msg.fields[0].count = 1; + msg.fields[1].name = "y"; + msg.fields[1].offset = 4; + msg.fields[1].datatype = sensor_msgs::msg::PointField::FLOAT32; + msg.fields[1].count = 1; + msg.fields[2].name = "z"; + msg.fields[2].offset = 8; + msg.fields[2].datatype = sensor_msgs::msg::PointField::FLOAT32; + msg.fields[2].count = 1; + msg.is_bigendian = false; + msg.point_step = 16; + msg.row_step = 16 * msg.width; + msg.is_dense = true; + msg.data.resize(16 * num_points); + return msg; +} + +// For sensor_msgs::msg::LaserScan. +bool HasEcho(float) { return true; } + +float GetFirstEcho(float range) { return range; } + +// For sensor_msgs::msg::MultiEchoLaserScan. +bool HasEcho(const sensor_msgs::msg::LaserEcho& echo) { + return !echo.echoes.empty(); +} + +float GetFirstEcho(const sensor_msgs::msg::LaserEcho& echo) { + return echo.echoes[0]; +} + +// For sensor_msgs::msg::LaserScan and sensor_msgs::msg::MultiEchoLaserScan. +template +std::tuple +LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) { + CHECK_GE(msg.range_min, 0.f); + CHECK_GE(msg.range_max, msg.range_min); + if (msg.angle_increment > 0.f) { + CHECK_GT(msg.angle_max, msg.angle_min); + } else { + CHECK_GT(msg.angle_min, msg.angle_max); + } + PointCloudWithIntensities point_cloud; + float angle = msg.angle_min; + for (size_t i = 0; i < msg.ranges.size(); ++i) { + const auto& echoes = msg.ranges[i]; + if (HasEcho(echoes)) { + const float first_echo = GetFirstEcho(echoes); + if (msg.range_min <= first_echo && first_echo <= msg.range_max) { + const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ()); + const cartographer::sensor::TimedRangefinderPoint point{ + rotation * (first_echo * Eigen::Vector3f::UnitX()), + i * msg.time_increment}; + point_cloud.points.push_back(point); + if (msg.intensities.size() > 0) { + CHECK_EQ(msg.intensities.size(), msg.ranges.size()); + const auto& echo_intensities = msg.intensities[i]; + CHECK(HasEcho(echo_intensities)); + point_cloud.intensities.push_back(GetFirstEcho(echo_intensities)); + } else { + point_cloud.intensities.push_back(0.f); + } + } + } + angle += msg.angle_increment; + } + ::cartographer::common::Time timestamp = FromRos(msg.header.stamp); + if (!point_cloud.points.empty()) { + const double duration = point_cloud.points.back().time; + timestamp += cartographer::common::FromSeconds(duration); + for (auto& point : point_cloud.points) { + point.time -= duration; + } + } + return std::make_tuple(point_cloud, timestamp); +} + +bool PointCloud2HasField(const sensor_msgs::msg::PointCloud2& pc2, + const std::string& field_name) { + for (const auto& field : pc2.fields) { + if (field.name == field_name) { + return true; + } + } + return false; +} + +} // namespace + +sensor_msgs::msg::PointCloud2 ToPointCloud2Message( + const int64_t timestamp, const std::string& frame_id, + const ::cartographer::sensor::TimedPointCloud& point_cloud) { + auto msg = PreparePointCloud2Message(timestamp, frame_id, point_cloud.size()); + size_t offset = 0; + float * const data = reinterpret_cast(&msg.data[0]); + for (const auto& point : point_cloud) { + data[offset++] = point.position.x(); + data[offset++] = point.position.y(); + data[offset++] = point.position.z(); + data[offset++] = kPointCloudComponentFourMagic; + } + return msg; +} + +std::tuple<::cartographer::sensor::PointCloudWithIntensities, + ::cartographer::common::Time> +ToPointCloudWithIntensities(const sensor_msgs::msg::LaserScan& msg) { + return LaserScanToPointCloudWithIntensities(msg); +} + +std::tuple<::cartographer::sensor::PointCloudWithIntensities, + ::cartographer::common::Time> +ToPointCloudWithIntensities(const sensor_msgs::msg::MultiEchoLaserScan& msg) { + return LaserScanToPointCloudWithIntensities(msg); +} + +std::tuple<::cartographer::sensor::PointCloudWithIntensities, + ::cartographer::common::Time> +ToPointCloudWithIntensities(const sensor_msgs::msg::PointCloud2& msg) { + PointCloudWithIntensities point_cloud; + // We check for intensity field here to avoid run-time warnings if we pass in + // a PointCloud2 without intensity. + if (PointCloud2HasField(msg, "intensity")) { + if (PointCloud2HasField(msg, "time")) { + pcl::PointCloud pcl_point_cloud; + pcl::fromROSMsg(msg, pcl_point_cloud); + point_cloud.points.reserve(pcl_point_cloud.size()); + point_cloud.intensities.reserve(pcl_point_cloud.size()); + for (const auto& point : pcl_point_cloud) { + point_cloud.points.push_back( + {Eigen::Vector3f{point.x, point.y, point.z}, point.time}); + point_cloud.intensities.push_back(point.intensity); + } + } else { + pcl::PointCloud pcl_point_cloud; + pcl::fromROSMsg(msg, pcl_point_cloud); + point_cloud.points.reserve(pcl_point_cloud.size()); + point_cloud.intensities.reserve(pcl_point_cloud.size()); + for (const auto& point : pcl_point_cloud) { + point_cloud.points.push_back( + {Eigen::Vector3f{point.x, point.y, point.z}, 0.f}); + point_cloud.intensities.push_back(point.intensity); + } + } + } else { + // If we don't have an intensity field, just copy XYZ and fill in 1.0f. + if (PointCloud2HasField(msg, "time")) { + pcl::PointCloud pcl_point_cloud; + pcl::fromROSMsg(msg, pcl_point_cloud); + point_cloud.points.reserve(pcl_point_cloud.size()); + point_cloud.intensities.reserve(pcl_point_cloud.size()); + for (const auto& point : pcl_point_cloud) { + point_cloud.points.push_back( + {Eigen::Vector3f{point.x, point.y, point.z}, point.time}); + point_cloud.intensities.push_back(1.0f); + } + } else { + pcl::PointCloud pcl_point_cloud; + pcl::fromROSMsg(msg, pcl_point_cloud); + point_cloud.points.reserve(pcl_point_cloud.size()); + point_cloud.intensities.reserve(pcl_point_cloud.size()); + for (const auto& point : pcl_point_cloud) { + point_cloud.points.push_back( + {Eigen::Vector3f{point.x, point.y, point.z}, 0.f}); + point_cloud.intensities.push_back(1.0f); + } + } + } + ::cartographer::common::Time timestamp = FromRos(msg.header.stamp); + if (!point_cloud.points.empty()) { + const double duration = point_cloud.points.back().time; + timestamp += cartographer::common::FromSeconds(duration); + for (auto& point : point_cloud.points) { + point.time -= duration; + CHECK_LE(point.time, 0.f) + << "Encountered a point with a larger stamp than " + "the last point in the cloud."; + } + } + return std::make_tuple(point_cloud, timestamp); +} + +LandmarkData ToLandmarkData(const LandmarkList& landmark_list) { + LandmarkData landmark_data; + landmark_data.time = FromRos(landmark_list.header.stamp); + for (const LandmarkEntry& entry : landmark_list.landmarks) { + landmark_data.landmark_observations.push_back( + {entry.id, ToRigid3d(entry.tracking_from_landmark_transform), + entry.translation_weight, entry.rotation_weight}); + } + return landmark_data; +} + +Rigid3d ToRigid3d(const geometry_msgs::msg::TransformStamped& transform) { + return Rigid3d(ToEigen(transform.transform.translation), + ToEigen(transform.transform.rotation)); +} + +Rigid3d ToRigid3d(const geometry_msgs::msg::Pose& pose) { + return Rigid3d({pose.position.x, pose.position.y, pose.position.z}, + ToEigen(pose.orientation)); +} + +Eigen::Vector3d ToEigen(const geometry_msgs::msg::Vector3& vector3) { + return Eigen::Vector3d(vector3.x, vector3.y, vector3.z); +} + +Eigen::Quaterniond ToEigen(const geometry_msgs::msg::Quaternion& quaternion) { + return Eigen::Quaterniond(quaternion.w, quaternion.x, quaternion.y, + quaternion.z); +} + +geometry_msgs::msg::Transform ToGeometryMsgTransform(const Rigid3d& rigid3d) { + geometry_msgs::msg::Transform transform; + transform.translation.x = rigid3d.translation().x(); + transform.translation.y = rigid3d.translation().y(); + transform.translation.z = rigid3d.translation().z(); + transform.rotation.w = rigid3d.rotation().w(); + transform.rotation.x = rigid3d.rotation().x(); + transform.rotation.y = rigid3d.rotation().y(); + transform.rotation.z = rigid3d.rotation().z(); + return transform; +} + +geometry_msgs::msg::Pose ToGeometryMsgPose(const Rigid3d& rigid3d) { + geometry_msgs::msg::Pose pose; + pose.position = ToGeometryMsgPoint(rigid3d.translation()); + pose.orientation.w = rigid3d.rotation().w(); + pose.orientation.x = rigid3d.rotation().x(); + pose.orientation.y = rigid3d.rotation().y(); + pose.orientation.z = rigid3d.rotation().z(); + return pose; +} + +geometry_msgs::msg::Point ToGeometryMsgPoint(const Eigen::Vector3d& vector3d) { + geometry_msgs::msg::Point point; + point.x = vector3d.x(); + point.y = vector3d.y(); + point.z = vector3d.z(); + return point; +} + +Eigen::Vector3d LatLongAltToEcef(const double latitude, const double longitude, + const double altitude) { + // https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_geodetic_to_ECEF_coordinates + constexpr double a = 6378137.; // semi-major axis, equator to center. + constexpr double f = 1. / 298.257223563; + constexpr double b = a * (1. - f); // semi-minor axis, pole to center. + constexpr double a_squared = a * a; + constexpr double b_squared = b * b; + constexpr double e_squared = (a_squared - b_squared) / a_squared; + const double sin_phi = std::sin(cartographer::common::DegToRad(latitude)); + const double cos_phi = std::cos(cartographer::common::DegToRad(latitude)); + const double sin_lambda = std::sin(cartographer::common::DegToRad(longitude)); + const double cos_lambda = std::cos(cartographer::common::DegToRad(longitude)); + const double N = a / std::sqrt(1 - e_squared * sin_phi * sin_phi); + const double x = (N + altitude) * cos_phi * cos_lambda; + const double y = (N + altitude) * cos_phi * sin_lambda; + const double z = (b_squared / a_squared * N + altitude) * sin_phi; + + return Eigen::Vector3d(x, y, z); +} + +cartographer::transform::Rigid3d ComputeLocalFrameFromLatLong( + const double latitude, const double longitude) { + const Eigen::Vector3d translation = LatLongAltToEcef(latitude, longitude, 0.); + const Eigen::Quaterniond rotation = + Eigen::AngleAxisd(cartographer::common::DegToRad(latitude - 90.), + Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(cartographer::common::DegToRad(-longitude), + Eigen::Vector3d::UnitZ()); + return cartographer::transform::Rigid3d(rotation * -translation, rotation); +} + +std::unique_ptr CreateOccupancyGridMsg( + const cartographer::io::PaintSubmapSlicesResult& painted_slices, + const double resolution, const std::string& frame_id, + const rclcpp::Time& time) { + auto occupancy_grid = absl::make_unique(); + + const int width = cairo_image_surface_get_width(painted_slices.surface.get()); + const int height = + cairo_image_surface_get_height(painted_slices.surface.get()); + + occupancy_grid->header.stamp = time; + occupancy_grid->header.frame_id = frame_id; + occupancy_grid->info.map_load_time = time; + occupancy_grid->info.resolution = resolution; + occupancy_grid->info.width = width; + occupancy_grid->info.height = height; + occupancy_grid->info.origin.position.x = + -painted_slices.origin.x() * resolution; + occupancy_grid->info.origin.position.y = + (-height + painted_slices.origin.y()) * resolution; + occupancy_grid->info.origin.position.z = 0.; + occupancy_grid->info.origin.orientation.w = 1.; + occupancy_grid->info.origin.orientation.x = 0.; + occupancy_grid->info.origin.orientation.y = 0.; + occupancy_grid->info.origin.orientation.z = 0.; + + const uint32_t* pixel_data = reinterpret_cast( + cairo_image_surface_get_data(painted_slices.surface.get())); + occupancy_grid->data.reserve(width * height); + for (int y = height - 1; y >= 0; --y) { + for (int x = 0; x < width; ++x) { + const uint32_t packed = pixel_data[y * width + x]; + const unsigned char color = packed >> 16; + const unsigned char observed = packed >> 8; + const int value = + observed == 0 + ? -1 + : ::cartographer::common::RoundToInt((1. - color / 255.) * 100.); + CHECK_LE(-1, value); + CHECK_GE(100, value); + occupancy_grid->data.push_back(value); + } + } + + return occupancy_grid; +} + +} // namespace cartographer_ros diff --git a/cartographer_rviz/cartographer_rviz/msg_conversion.h b/cartographer_rviz/cartographer_rviz/msg_conversion.h new file mode 100644 index 000000000..e895144b2 --- /dev/null +++ b/cartographer_rviz/cartographer_rviz/msg_conversion.h @@ -0,0 +1,95 @@ +/* + * Copyright 2016 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. + */ + +#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MSG_CONVERSION_H +#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MSG_CONVERSION_H + +#include "cartographer/common/time.h" +#include "cartographer/io/submap_painter.h" +#include "cartographer/sensor/landmark_data.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer_ros_msgs/msg/landmark_list.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/transform.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "sensor_msgs/msg/multi_echo_laser_scan.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include + +namespace cartographer_ros { + +sensor_msgs::msg::PointCloud2 ToPointCloud2Message( + int64_t timestamp, const std::string& frame_id, + const ::cartographer::sensor::TimedPointCloud& point_cloud); + +geometry_msgs::msg::Transform ToGeometryMsgTransform( + const ::cartographer::transform::Rigid3d& rigid3d); + +geometry_msgs::msg::Pose ToGeometryMsgPose( + const ::cartographer::transform::Rigid3d& rigid3d); + +geometry_msgs::msg::Point ToGeometryMsgPoint(const Eigen::Vector3d& vector3d); + +// Converts ROS message to point cloud. Returns the time when the last point +// was acquired (different from the ROS timestamp). Timing of points is given in +// the fourth component of each point relative to `Time`. +std::tuple<::cartographer::sensor::PointCloudWithIntensities, + ::cartographer::common::Time> +ToPointCloudWithIntensities(const sensor_msgs::msg::LaserScan& msg); + +std::tuple<::cartographer::sensor::PointCloudWithIntensities, + ::cartographer::common::Time> +ToPointCloudWithIntensities(const sensor_msgs::msg::MultiEchoLaserScan& msg); + +std::tuple<::cartographer::sensor::PointCloudWithIntensities, + ::cartographer::common::Time> +ToPointCloudWithIntensities(const sensor_msgs::msg::PointCloud2& msg); + +::cartographer::sensor::LandmarkData ToLandmarkData( + const cartographer_ros_msgs::msg::LandmarkList& landmark_list); + +::cartographer::transform::Rigid3d ToRigid3d( + const geometry_msgs::msg::TransformStamped& transform); + +::cartographer::transform::Rigid3d ToRigid3d(const geometry_msgs::msg::Pose& pose); + +Eigen::Vector3d ToEigen(const geometry_msgs::msg::Vector3& vector3); + +Eigen::Quaterniond ToEigen(const geometry_msgs::msg::Quaternion& quaternion); + +// Converts from WGS84 (latitude, longitude, altitude) to ECEF. +Eigen::Vector3d LatLongAltToEcef(double latitude, double longitude, + double altitude); + +// Returns a transform that takes ECEF coordinates from nearby points to a local +// frame that has z pointing upwards. +cartographer::transform::Rigid3d ComputeLocalFrameFromLatLong(double latitude, + double longitude); + +// Points to an occupancy grid message at a specific resolution from painted +// submap slices obtained via ::cartographer::io::PaintSubmapSlices(...). +std::unique_ptr CreateOccupancyGridMsg( + const cartographer::io::PaintSubmapSlicesResult& painted_slices, + const double resolution, const std::string& frame_id, + const rclcpp::Time& time); + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MSG_CONVERSION_H diff --git a/cartographer_rviz/cartographer_rviz/submap.cc b/cartographer_rviz/cartographer_rviz/submap.cc new file mode 100644 index 000000000..467e3ebc8 --- /dev/null +++ b/cartographer_rviz/cartographer_rviz/submap.cc @@ -0,0 +1,65 @@ +/* + * Copyright 2016 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. + */ + +#include "cartographer_rviz/submap.h" + +#include "absl/memory/memory.h" +#include "cartographer/common/port.h" +#include "cartographer/transform/transform.h" +#include "cartographer_rviz/msg_conversion.h" +#include "cartographer_ros_msgs/msg/status_code.hpp" +#include "cartographer_ros_msgs/srv/submap_query.hpp" + +namespace cartographer_ros { + +std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures( + const ::cartographer::mapping::SubmapId& submap_id, + rclcpp::Client::SharedPtr client, + rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor, + const std::chrono::milliseconds timeout) +{ + auto request = std::make_shared(); + request->trajectory_id = submap_id.trajectory_id; + request->submap_index = submap_id.submap_index; + auto future_result = client->async_send_request(request); + + if (callback_group_executor->spin_until_future_complete(future_result, timeout) != + rclcpp::FutureReturnCode::SUCCESS) + { + return nullptr; + } + auto result = future_result.get(); + + if (result->status.code != ::cartographer_ros_msgs::msg::StatusCode::OK || + result->textures.empty()) { + return nullptr; + } + + auto response = absl::make_unique<::cartographer::io::SubmapTextures>(); + response->version = result->submap_version; + for (const auto& texture : result->textures) { + const std::string compressed_cells(texture.cells.begin(), + texture.cells.end()); + response->textures.emplace_back(::cartographer::io::SubmapTexture{ + ::cartographer::io::UnpackTextureData(compressed_cells, texture.width, + texture.height), + texture.width, texture.height, texture.resolution, + ToRigid3d(texture.slice_pose)}); + } + return response; +} + +} // namespace cartographer_ros diff --git a/cartographer_rviz/cartographer_rviz/submap.h b/cartographer_rviz/cartographer_rviz/submap.h new file mode 100644 index 000000000..48c3a425b --- /dev/null +++ b/cartographer_rviz/cartographer_rviz/submap.h @@ -0,0 +1,43 @@ +/* + * Copyright 2016 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. + */ + +#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SUBMAP_H +#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SUBMAP_H + +#include +#include +#include + +#include "cartographer/io/image.h" +#include "cartographer/io/submap_painter.h" +#include "cartographer/mapping/id.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer_ros_msgs/srv/submap_query.hpp" +#include + +namespace cartographer_ros { + +// Fetch 'submap_id' using the 'client' and returning the response or 'nullptr' +// on error. +std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures( + const ::cartographer::mapping::SubmapId& submap_id, + rclcpp::Client::SharedPtr client, + rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor, + const std::chrono::milliseconds timeout); + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SUBMAP_H diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc index 536f2147e..a2ff285ff 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/cartographer_rviz/submaps_display.cc @@ -24,7 +24,7 @@ #include "cartographer_ros_msgs/srv/submap_query.hpp" #include #include "pluginlib/class_list_macros.h" -#include "ros/package.h" +//#include "ros/package.h" #include #include #include @@ -45,32 +45,32 @@ constexpr char kDefaultSubmapQueryServiceName[] = "/submap_query"; } // namespace SubmapsDisplay::SubmapsDisplay() : tf_listener_(tf_buffer_) { - submap_query_service_property_ = new ::rviz::StringProperty( + submap_query_service_property_ = new ::rviz_common::properties::StringProperty( "Submap query service", kDefaultSubmapQueryServiceName, "Submap query service to connect to.", this, SLOT(Reset())); - tracking_frame_property_ = new ::rviz::StringProperty( + tracking_frame_property_ = new ::rviz_common::properties::StringProperty( "Tracking frame", kDefaultTrackingFrame, "Tracking frame, used for fading out submaps.", this); - slice_high_resolution_enabled_ = new ::rviz::BoolProperty( + slice_high_resolution_enabled_ = new ::rviz_common::properties::BoolProperty( "High Resolution", true, "Display high resolution slices.", this, SLOT(ResolutionToggled()), this); - slice_low_resolution_enabled_ = new ::rviz::BoolProperty( + slice_low_resolution_enabled_ = new ::rviz_common::properties::BoolProperty( "Low Resolution", false, "Display low resolution slices.", this, SLOT(ResolutionToggled()), this); - client_ = update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>(""); - trajectories_category_ = new ::rviz::Property( + client_ = update_nh_.serviceClient<::cartographer_ros_msgs::msg::SubmapQuery>(""); + trajectories_category_ = new ::rviz_common::properties::Property( "Submaps", QVariant(), "List of all submaps, organized by trajectories.", this); - visibility_all_enabled_ = new ::rviz::BoolProperty( + visibility_all_enabled_ = new ::rviz_common::properties::BoolProperty( "All", true, "Whether submaps from all trajectories should be displayed or not.", trajectories_category_, SLOT(AllEnabledToggled()), this); - pose_markers_all_enabled_ = new ::rviz::BoolProperty( + pose_markers_all_enabled_ = new ::rviz_common::properties::BoolProperty( "All Submap Pose Markers", true, "Whether submap pose markers should be displayed or not.", trajectories_category_, SLOT(PoseMarkersEnabledToggled()), this); fade_out_start_distance_in_meters_ = - new ::rviz::FloatProperty("Fade-out distance", 1.f, + new ::rviz_common::properties::FloatProperty("Fade-out distance", 1.f, "Distance in meters in z-direction beyond " "which submaps will start to fade out.", this); @@ -95,7 +95,7 @@ SubmapsDisplay::~SubmapsDisplay() { void SubmapsDisplay::Reset() { reset(); } void SubmapsDisplay::CreateClient() { - client_ = update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>( + client_ = update_nh_.serviceClient<::cartographer_ros_msgs::msg::SubmapQuery>( submap_query_service_property_->getStdString()); } @@ -114,12 +114,12 @@ void SubmapsDisplay::reset() { } void SubmapsDisplay::processMessage( - const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) { + const ::cartographer_ros_msgs::msg::SubmapList::ConstPtr& msg) { absl::MutexLock locker(&mutex_); map_frame_ = absl::make_unique(msg->header.frame_id); // In case Cartographer node is relaunched, destroy trajectories from the // previous instance. - for (const ::cartographer_ros_msgs::SubmapEntry& submap_entry : msg->submap) { + for (const ::cartographer_ros_msgs::msg::SubmapEntry& submap_entry : msg->submap) { const size_t trajectory_id = submap_entry.trajectory_id; if (trajectories_.count(trajectory_id) == 0) { continue; @@ -136,7 +136,7 @@ void SubmapsDisplay::processMessage( using ::cartographer::mapping::SubmapId; std::set listed_submaps; std::set listed_trajectories; - for (const ::cartographer_ros_msgs::SubmapEntry& submap_entry : msg->submap) { + for (const ::cartographer_ros_msgs::msg::SubmapEntry& submap_entry : msg->submap) { const SubmapId id{submap_entry.trajectory_id, submap_entry.submap_index}; listed_submaps.insert(id); listed_trajectories.insert(submap_entry.trajectory_id); @@ -144,7 +144,7 @@ void SubmapsDisplay::processMessage( trajectories_.insert(std::make_pair( id.trajectory_id, absl::make_unique( - absl::make_unique<::rviz::BoolProperty>( + absl::make_unique<::rviz_common::properties::BoolProperty>( QString("Trajectory %1").arg(id.trajectory_id), visibility_all_enabled_->getBool(), QString( @@ -226,7 +226,7 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) { // Update the fading by z distance. const ros::Time kLatest(0); try { - const ::geometry_msgs::TransformStamped transform_stamped = + const ::geometry_msgs::msg::TransformStamped transform_stamped = tf_buffer_.lookupTransform( *map_frame_, tracking_frame_property_->getStdString(), kLatest); for (auto& trajectory_by_id : trajectories_) { @@ -237,7 +237,7 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) { } } } catch (const tf2::TransformException& ex) { - ROS_WARN_THROTTLE(1., "Could not compute submap fading: %s", ex.what()); + //ROS_WARN_THROTTLE(1., "Could not compute submap fading: %s", ex.what()); } // Update the map frame to fixed frame transform. Ogre::Vector3 position; @@ -292,14 +292,14 @@ void Trajectory::PoseMarkersEnabledToggled() { } } -Trajectory::Trajectory(std::unique_ptr<::rviz::BoolProperty> property, +Trajectory::Trajectory(std::unique_ptr<::rviz_common::properties::BoolProperty> property, const bool pose_markers_enabled) : visibility(std::move(property)) { ::QObject::connect(visibility.get(), SIGNAL(changed()), this, SLOT(AllEnabledToggled())); // Add toggle for submap pose markers as the first entry of the visibility // property list of this trajectory. - pose_markers_visibility = absl::make_unique<::rviz::BoolProperty>( + pose_markers_visibility = absl::make_unique<::rviz_common::properties::BoolProperty>( QString("Submap Pose Markers"), pose_markers_enabled, QString("Toggles the submap pose markers of this trajectory."), visibility.get()); @@ -309,4 +309,4 @@ Trajectory::Trajectory(std::unique_ptr<::rviz::BoolProperty> property, } // namespace cartographer_rviz -PLUGINLIB_EXPORT_CLASS(cartographer_rviz::SubmapsDisplay, ::rviz::Display) +PLUGINLIB_EXPORT_CLASS(cartographer_rviz::SubmapsDisplay, ::rviz_common::Display) diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.h b/cartographer_rviz/cartographer_rviz/submaps_display.h index aa2570614..c43ccd717 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.h +++ b/cartographer_rviz/cartographer_rviz/submaps_display.h @@ -41,11 +41,11 @@ struct Trajectory : public QObject { Q_OBJECT public: - Trajectory(std::unique_ptr<::rviz::BoolProperty> property, + Trajectory(std::unique_ptr<::rviz_common::properties::BoolProperty> property, bool pose_markers_enabled); - std::unique_ptr<::rviz::BoolProperty> visibility; - std::unique_ptr<::rviz::BoolProperty> pose_markers_visibility; + std::unique_ptr<::rviz_common::properties::BoolProperty> visibility; + std::unique_ptr<::rviz_common::properties::BoolProperty> pose_markers_visibility; std::map> submaps; private Q_SLOTS: @@ -60,7 +60,7 @@ struct Trajectory : public QObject { // every submap containing pre-multiplied alpha and grayscale values, these are // then alpha blended together. class SubmapsDisplay - : public ::rviz::MessageFilterDisplay<::cartographer_ros_msgs::SubmapList> { + : public ::rviz_common::MessageFilterDisplay<::cartographer_ros_msgs::msg::SubmapList> { Q_OBJECT public: @@ -83,24 +83,24 @@ class SubmapsDisplay void onInitialize() override; void reset() override; void processMessage( - const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) override; + const ::cartographer_ros_msgs::msg::SubmapList::ConstPtr& msg) override; void update(float wall_dt, float ros_dt) override; ::tf2_ros::Buffer tf_buffer_; ::tf2_ros::TransformListener tf_listener_; - ros::ServiceClient client_; - ::rviz::StringProperty* submap_query_service_property_; + rclcpp::Client client_; + ::rviz_common::properties::StringProperty* submap_query_service_property_; std::unique_ptr map_frame_; - ::rviz::StringProperty* tracking_frame_property_; + ::rviz_common::properties::StringProperty* tracking_frame_property_; Ogre::SceneNode* map_node_ = nullptr; // Represents the map frame. std::map> trajectories_ GUARDED_BY(mutex_); absl::Mutex mutex_; - ::rviz::BoolProperty* slice_high_resolution_enabled_; - ::rviz::BoolProperty* slice_low_resolution_enabled_; - ::rviz::Property* trajectories_category_; - ::rviz::BoolProperty* visibility_all_enabled_; - ::rviz::BoolProperty* pose_markers_all_enabled_; - ::rviz::FloatProperty* fade_out_start_distance_in_meters_; + ::rviz_common::properties::BoolProperty* slice_high_resolution_enabled_; + ::rviz_common::properties::BoolProperty* slice_low_resolution_enabled_; + ::rviz_common::properties::Property* trajectories_category_; + ::rviz_common::properties::BoolProperty* visibility_all_enabled_; + ::rviz_common::properties::BoolProperty* pose_markers_all_enabled_; + ::rviz_common::properties::FloatProperty* fade_out_start_distance_in_meters_; }; } // namespace cartographer_rviz diff --git a/cartographer_rviz/cartographer_rviz/time_conversion.cc b/cartographer_rviz/cartographer_rviz/time_conversion.cc new file mode 100644 index 000000000..37afff196 --- /dev/null +++ b/cartographer_rviz/cartographer_rviz/time_conversion.cc @@ -0,0 +1,44 @@ +/* + * Copyright 2016 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. + */ + +#include "cartographer_rviz/time_conversion.h" + +#include "cartographer/common/time.h" +#include + +namespace cartographer_ros { + +rclcpp::Time ToRos(::cartographer::common::Time time) { + int64_t uts_timestamp = ::cartographer::common::ToUniversal(time); + int64_t ns_since_unix_epoch = + (uts_timestamp - + ::cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds * + 10000000ll) * + 100ll; + rclcpp::Time ros_time(ns_since_unix_epoch, rcl_clock_type_t::RCL_ROS_TIME); + return ros_time; +} + +// TODO(pedrofernandez): Write test. +::cartographer::common::Time FromRos(const rclcpp::Time& time) { + // The epoch of the ICU Universal Time Scale is "0001-01-01 00:00:00.0 +0000", + // exactly 719162 days before the Unix epoch. + return ::cartographer::common::FromUniversal( + cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds * 10000000ll + + (time.nanoseconds() + 50) / 100); // + 50 to get the rounding correct. +} + +} // namespace cartographer_ros diff --git a/cartographer_rviz/cartographer_rviz/time_conversion.h b/cartographer_rviz/cartographer_rviz/time_conversion.h new file mode 100644 index 000000000..5555f4d49 --- /dev/null +++ b/cartographer_rviz/cartographer_rviz/time_conversion.h @@ -0,0 +1,32 @@ +/* + * Copyright 2016 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. + */ + +#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TIME_CONVERSION_H +#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TIME_CONVERSION_H + +#include "cartographer/common/time.h" +#include +#include + +namespace cartographer_ros { + +rclcpp::Time ToRos(::cartographer::common::Time time); + +::cartographer::common::Time FromRos(const rclcpp::Time& time); + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TIME_CONVERSION_H From 24bd93b6805c33a3685813299c1c3bc98d2dd402 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 15 Jun 2021 11:11:09 +0200 Subject: [PATCH 46/94] more wip Signed-off-by: Guillaume Doisy --- cartographer_rviz/CMakeLists.txt | 17 ++++++++++++++++- .../cartographer_rviz/drawable_submap.h | 2 +- .../cartographer_rviz/msg_conversion.cc | 3 +-- cartographer_rviz/package.xml | 2 ++ 4 files changed, 20 insertions(+), 4 deletions(-) diff --git a/cartographer_rviz/CMakeLists.txt b/cartographer_rviz/CMakeLists.txt index d78837780..29fe5b2a9 100644 --- a/cartographer_rviz/CMakeLists.txt +++ b/cartographer_rviz/CMakeLists.txt @@ -37,9 +37,11 @@ find_package(absl REQUIRED) find_package(Eigen3 REQUIRED) find_package(cartographer_ros REQUIRED) find_package(cartographer_ros_msgs REQUIRED) +find_package(pcl_conversions REQUIRED) find_package(rclcpp REQUIRED) find_package(rviz2 REQUIRED) find_package(rviz_ogre_vendor REQUIRED) +find_package(PCL REQUIRED COMPONENTS common) find_package(rviz_assimp_vendor REQUIRED) find_package(rviz_rendering REQUIRED) find_package(rviz_common REQUIRED) @@ -64,12 +66,25 @@ ament_target_dependencies(${PROJECT_NAME} PUBLIC rviz_rendering rviz_common rviz_default_plugins - + pcl_conversions # rviz ) ament_export_dependencies(rosidl_default_runtime) +# PCL +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${PCL_INCLUDE_DIRS}) +target_link_libraries(${PROJECT_NAME} PUBLIC ${PCL_LIBRARIES}) +set(BLACKLISTED_PCL_DEFINITIONS " -march=native -msse4.2 -mfpmath=sse ") +foreach(DEFINITION ${PCL_DEFINITIONS}) + list (FIND BLACKLISTED_PCL_DEFINITIONS "${DEFINITION}" DEFINITIONS_INDEX) + if (${DEFINITIONS_INDEX} GREATER -1) + continue() + endif() + set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${DEFINITION}") +endforeach() + + #if(rviz2_QT_VERSION VERSION_LESS "5") # message(STATUS "Using Qt4 based on the rviz2_QT_VERSION: ${rviz2_QT_VERSION}") #find_package(Qt4 ${rviz2_QT_VERSION} EXACT REQUIRED QtCore QtGui) diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.h b/cartographer_rviz/cartographer_rviz/drawable_submap.h index 2f4ea0f97..24ef0699f 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.h +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.h @@ -111,7 +111,7 @@ class DrawableSubmap : public QObject { ::cartographer::transform::Rigid3d pose_ GUARDED_BY(mutex_); ::rviz_rendering::Axes pose_axes_; bool pose_axes_visible_; - ::rviz_rendering::MovableText submap_id_text; + ::rviz_rendering::MovableText submap_id_text_; std::chrono::milliseconds last_query_timestamp_ GUARDED_BY(mutex_); bool query_in_progress_ GUARDED_BY(mutex_) = false; int metadata_version_ GUARDED_BY(mutex_) = -1; diff --git a/cartographer_rviz/cartographer_rviz/msg_conversion.cc b/cartographer_rviz/cartographer_rviz/msg_conversion.cc index a46be5b44..4ccad6318 100644 --- a/cartographer_rviz/cartographer_rviz/msg_conversion.cc +++ b/cartographer_rviz/cartographer_rviz/msg_conversion.cc @@ -32,8 +32,7 @@ #include "geometry_msgs/msg/vector3.hpp" #include "glog/logging.h" #include "nav_msgs/msg/occupancy_grid.hpp" -#include -//#include "pcl/point_cloud.h" +#include "pcl/point_cloud.h" #include "pcl/point_types.h" #include "pcl_conversions/pcl_conversions.h" #include "builtin_interfaces/msg/time.hpp" diff --git a/cartographer_rviz/package.xml b/cartographer_rviz/package.xml index cb3e4f169..5b9fadc36 100644 --- a/cartographer_rviz/package.xml +++ b/cartographer_rviz/package.xml @@ -47,6 +47,8 @@ qtbase5-dev rclcpp + libpcl-all-dev + pcl_conversions rviz2 rviz_ogre_vendor rviz_assimp_vendor From c1a44d0295179206fb1f8ed8a96fdf78fddb79aa Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 15 Jun 2021 13:51:10 +0200 Subject: [PATCH 47/94] even more wip not compiling Signed-off-by: Guillaume Doisy --- cartographer_rviz/cartographer_rviz/drawable_submap.cc | 4 ++-- cartographer_rviz/cartographer_rviz/drawable_submap.h | 3 +-- cartographer_rviz/cartographer_rviz/submaps_display.cc | 3 ++- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index 44895d980..c01090d4a 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -113,7 +113,7 @@ void DrawableSubmap::Update( .arg(metadata_version_)); } -bool DrawableSubmap::MaybeFetchTexture(rclcpp::Client* const client) { +bool DrawableSubmap::MaybeFetchTexture(rclcpp::Client::SharedPtr const client) { absl::MutexLock locker(&mutex_); // Received metadata version can also be lower if we restarted Cartographer. const bool newer_version_available = @@ -131,7 +131,7 @@ bool DrawableSubmap::MaybeFetchTexture(rclcpp::Client* const client) { last_query_timestamp_ = now; rpc_request_future_ = std::async(std::launch::async, [this, client]() { std::unique_ptr<::cartographer::io::SubmapTextures> submap_textures = - ::cartographer_ros::FetchSubmapTextures(id_, client); + ::cartographer_ros::FetchSubmapTextures(id_, client); // Missing callbackgroup and timeout but this class isn't a ros node absl::MutexLock locker(&mutex_); query_in_progress_ = false; if (submap_textures != nullptr) { diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.h b/cartographer_rviz/cartographer_rviz/drawable_submap.h index 24ef0699f..15a00bc4c 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.h +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.h @@ -28,7 +28,6 @@ #include "cartographer/io/submap_painter.h" #include "cartographer/mapping/id.h" #include "cartographer/transform/rigid_transform.h" -//#include #include "cartographer_rviz/submap.h" #include "cartographer_ros_msgs/msg/submap_entry.hpp" #include "cartographer_ros_msgs/srv/submap_query.hpp" @@ -64,7 +63,7 @@ class DrawableSubmap : public QObject { // If an update is needed, it will send an RPC using 'client' to request the // new data for the submap and returns true. - bool MaybeFetchTexture(rclcpp::Client* client); + bool MaybeFetchTexture(rclcpp::Client::SharedPtr client); // Returns whether an RPC is in progress. bool QueryInProgress(); diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc index a2ff285ff..bc89b82dc 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/cartographer_rviz/submaps_display.cc @@ -30,6 +30,7 @@ #include #include #include +#include namespace cartographer_rviz { @@ -95,7 +96,7 @@ SubmapsDisplay::~SubmapsDisplay() { void SubmapsDisplay::Reset() { reset(); } void SubmapsDisplay::CreateClient() { - client_ = update_nh_.serviceClient<::cartographer_ros_msgs::msg::SubmapQuery>( + client_ = update_nh_.serviceClient<::cartographer_ros_msgs::srv::SubmapQuery>( submap_query_service_property_->getStdString()); } From 385c563a69115cac56da83e12ba1b033ee72f354 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 15 Jun 2021 15:51:35 +0200 Subject: [PATCH 48/94] wip Signed-off-by: Guillaume Doisy --- .../cartographer_rviz/drawable_submap.cc | 17 +++++++++++++++-- .../cartographer_rviz/drawable_submap.h | 2 +- .../cartographer_rviz/msg_conversion.cc | 4 ++++ .../cartographer_rviz/submaps_display.h | 2 +- 4 files changed, 21 insertions(+), 4 deletions(-) diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index c01090d4a..ce096b2de 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -43,6 +43,9 @@ constexpr int kNumberOfSlicesPerSubmap = 2; } // namespace + + +// DECLARED IT AS A ROS NODE DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id, ::rviz_common::DisplayContext* const display_context, Ogre::SceneNode* const map_node, @@ -61,7 +64,7 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id, .arg(id.trajectory_id) .arg(id.submap_index) .toStdString()), - last_query_timestamp_(0) { + last_query_timestamp_(0), Node("DrawableSubmap") { for (int slice_index = 0; slice_index < kNumberOfSlicesPerSubmap; ++slice_index) { ogre_slices_.emplace_back(absl::make_unique( @@ -129,9 +132,19 @@ bool DrawableSubmap::MaybeFetchTexture(rclcpp::Clientcreate_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); +// rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor; +// std::thread callback_group_executor_thread; + +// callback_group_executor_thread = std::thread([this]() { +// callback_group_executor.add_callback_group(sync_srv_client_callback_group, this->get_node_base_interface()); +// callback_group_executor.spin(); +// }); + rpc_request_future_ = std::async(std::launch::async, [this, client]() { std::unique_ptr<::cartographer::io::SubmapTextures> submap_textures = - ::cartographer_ros::FetchSubmapTextures(id_, client); // Missing callbackgroup and timeout but this class isn't a ros node + ::cartographer_ros::FetchSubmapTextures(id_, client,callback_group_executor,std::chrono::milliseconds(10000)); // Missing callbackgroup and timeout but this class isn't a ros node absl::MutexLock locker(&mutex_); query_in_progress_ = false; if (submap_textures != nullptr) { diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.h b/cartographer_rviz/cartographer_rviz/drawable_submap.h index 15a00bc4c..0f0b558cd 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.h +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.h @@ -43,7 +43,7 @@ namespace cartographer_rviz { // Contains all the information needed to render a submap onto the final // texture representing the whole map. -class DrawableSubmap : public QObject { +class DrawableSubmap : public QObject, rclcpp::Node{ Q_OBJECT public: diff --git a/cartographer_rviz/cartographer_rviz/msg_conversion.cc b/cartographer_rviz/cartographer_rviz/msg_conversion.cc index 4ccad6318..50b81227d 100644 --- a/cartographer_rviz/cartographer_rviz/msg_conversion.cc +++ b/cartographer_rviz/cartographer_rviz/msg_conversion.cc @@ -32,8 +32,12 @@ #include "geometry_msgs/msg/vector3.hpp" #include "glog/logging.h" #include "nav_msgs/msg/occupancy_grid.hpp" + +// WHY ARE THESE NOT COMPILING? THERE ARE BEING USED IN THE CARTO_ROS PKG WITHOUT A PROB #include "pcl/point_cloud.h" #include "pcl/point_types.h" +//#include "rviz_rendering/objects/point_cloud.hpp" + #include "pcl_conversions/pcl_conversions.h" #include "builtin_interfaces/msg/time.hpp" //#include "ros/serialization.h" diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.h b/cartographer_rviz/cartographer_rviz/submaps_display.h index c43ccd717..a9ec27b8f 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.h +++ b/cartographer_rviz/cartographer_rviz/submaps_display.h @@ -88,7 +88,7 @@ class SubmapsDisplay ::tf2_ros::Buffer tf_buffer_; ::tf2_ros::TransformListener tf_listener_; - rclcpp::Client client_; + rclcpp::Client::SharedPtr client_; ::rviz_common::properties::StringProperty* submap_query_service_property_; std::unique_ptr map_frame_; ::rviz_common::properties::StringProperty* tracking_frame_property_; From 45e6d0ce09bd49154b8f8b944eb220ed08ffd259 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 15 Jun 2021 17:04:20 +0200 Subject: [PATCH 49/94] Still not compiling but almost there Signed-off-by: Guillaume Doisy --- .../cartographer_rviz/drawable_submap.cc | 2 +- .../cartographer_rviz/submaps_display.cc | 22 ++++++++++--------- .../cartographer_rviz/submaps_display.h | 8 +++---- 3 files changed, 17 insertions(+), 15 deletions(-) diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index ce096b2de..66b49ca8c 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -144,7 +144,7 @@ bool DrawableSubmap::MaybeFetchTexture(rclcpp::Client submap_textures = - ::cartographer_ros::FetchSubmapTextures(id_, client,callback_group_executor,std::chrono::milliseconds(10000)); // Missing callbackgroup and timeout but this class isn't a ros node + ::cartographer_ros::FetchSubmapTextures(id_, client);//,callback_group_executor,std::chrono::milliseconds(10000)); // Missing callbackgroup and timeout but this class isn't a ros node absl::MutexLock locker(&mutex_); query_in_progress_ = false; if (submap_textures != nullptr) { diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc index bc89b82dc..a2f3aed99 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/cartographer_rviz/submaps_display.cc @@ -45,7 +45,7 @@ constexpr char kDefaultSubmapQueryServiceName[] = "/submap_query"; } // namespace -SubmapsDisplay::SubmapsDisplay() : tf_listener_(tf_buffer_) { +SubmapsDisplay::SubmapsDisplay() : Node("SubmapsDisplay") { submap_query_service_property_ = new ::rviz_common::properties::StringProperty( "Submap query service", kDefaultSubmapQueryServiceName, "Submap query service to connect to.", this, SLOT(Reset())); @@ -58,7 +58,7 @@ SubmapsDisplay::SubmapsDisplay() : tf_listener_(tf_buffer_) { slice_low_resolution_enabled_ = new ::rviz_common::properties::BoolProperty( "Low Resolution", false, "Display low resolution slices.", this, SLOT(ResolutionToggled()), this); - client_ = update_nh_.serviceClient<::cartographer_ros_msgs::msg::SubmapQuery>(""); + client_ = this->create_client<::cartographer_ros_msgs::srv::SubmapQuery>("");//update_nh_.serviceClient<::cartographer_ros_msgs::msg::SubmapQuery>(""); trajectories_category_ = new ::rviz_common::properties::Property( "Submaps", QVariant(), "List of all submaps, organized by trajectories.", this); @@ -85,10 +85,13 @@ SubmapsDisplay::SubmapsDisplay() : tf_listener_(tf_buffer_) { package_path + kMaterialsDirectory + kScriptsDirectory, "FileSystem", ROS_PACKAGE_NAME); Ogre::ResourceGroupManager::getSingleton().initialiseAllResourceGroups(); + + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); } SubmapsDisplay::~SubmapsDisplay() { - client_.shutdown(); + client_.reset(); trajectories_.clear(); scene_manager_->destroySceneNode(map_node_); } @@ -96,7 +99,7 @@ SubmapsDisplay::~SubmapsDisplay() { void SubmapsDisplay::Reset() { reset(); } void SubmapsDisplay::CreateClient() { - client_ = update_nh_.serviceClient<::cartographer_ros_msgs::srv::SubmapQuery>( + client_ = this->create_client<::cartographer_ros_msgs::srv::SubmapQuery>(//update_nh_.serviceClient<::cartographer_ros_msgs::srv::SubmapQuery>( submap_query_service_property_->getStdString()); } @@ -109,7 +112,7 @@ void SubmapsDisplay::onInitialize() { void SubmapsDisplay::reset() { MFDClass::reset(); absl::MutexLock locker(&mutex_); - client_.shutdown(); + client_.reset(); trajectories_.clear(); CreateClient(); } @@ -216,7 +219,7 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) { it != trajectory_by_id.second->submaps.rend() && num_ongoing_requests < kMaxOnGoingRequestsPerTrajectory; ++it) { - if (it->second->MaybeFetchTexture(&client_)) { + if (it->second->MaybeFetchTexture(client_)) { ++num_ongoing_requests; } } @@ -225,11 +228,10 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) { return; } // Update the fading by z distance. - const ros::Time kLatest(0); try { const ::geometry_msgs::msg::TransformStamped transform_stamped = - tf_buffer_.lookupTransform( - *map_frame_, tracking_frame_property_->getStdString(), kLatest); + tf_buffer_->lookupTransform( + *map_frame_, tracking_frame_property_->getStdString(), tf2::TimePointZero); for (auto& trajectory_by_id : trajectories_) { for (auto& submap_entry : trajectory_by_id.second->submaps) { submap_entry.second->SetAlpha( @@ -243,7 +245,7 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) { // Update the map frame to fixed frame transform. Ogre::Vector3 position; Ogre::Quaternion orientation; - if (context_->getFrameManager()->getTransform(*map_frame_, kLatest, position, + if (context_->getFrameManager()->getTransform(*map_frame_, this->now(), position, orientation)) { map_node_->setPosition(position); map_node_->setOrientation(orientation); diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.h b/cartographer_rviz/cartographer_rviz/submaps_display.h index a9ec27b8f..971c47dbb 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.h +++ b/cartographer_rviz/cartographer_rviz/submaps_display.h @@ -60,7 +60,7 @@ struct Trajectory : public QObject { // every submap containing pre-multiplied alpha and grayscale values, these are // then alpha blended together. class SubmapsDisplay - : public ::rviz_common::MessageFilterDisplay<::cartographer_ros_msgs::msg::SubmapList> { + : public ::rviz_common::MessageFilterDisplay<::cartographer_ros_msgs::msg::SubmapList>, rclcpp::Node { Q_OBJECT public: @@ -83,11 +83,11 @@ class SubmapsDisplay void onInitialize() override; void reset() override; void processMessage( - const ::cartographer_ros_msgs::msg::SubmapList::ConstPtr& msg) override; + const ::cartographer_ros_msgs::msg::SubmapList::ConstPtr& msg); //override; void update(float wall_dt, float ros_dt) override; - ::tf2_ros::Buffer tf_buffer_; - ::tf2_ros::TransformListener tf_listener_; + std::unique_ptr tf_buffer_; + std::shared_ptr tf_listener_; rclcpp::Client::SharedPtr client_; ::rviz_common::properties::StringProperty* submap_query_service_property_; std::unique_ptr map_frame_; From ba3544013ceff3f5244bac389e68a7557823e89e Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Fri, 18 Jun 2021 10:31:55 +0200 Subject: [PATCH 50/94] almost compiling Signed-off-by: Guillaume Doisy --- .../cartographer_rviz/drawable_submap.cc | 12 +++-------- cartographer_rviz/cartographer_rviz/submap.cc | 9 +++++---- cartographer_rviz/cartographer_rviz/submap.h | 1 - .../cartographer_rviz/submaps_display.cc | 20 ++++++++++++++----- .../cartographer_rviz/submaps_display.h | 5 +++++ 5 files changed, 28 insertions(+), 19 deletions(-) diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index 66b49ca8c..97b71f304 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -1,4 +1,4 @@ -/* +/* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); @@ -133,18 +133,12 @@ bool DrawableSubmap::MaybeFetchTexture(rclcpp::Clientcreate_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); -// rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor; -// std::thread callback_group_executor_thread; -// callback_group_executor_thread = std::thread([this]() { -// callback_group_executor.add_callback_group(sync_srv_client_callback_group, this->get_node_base_interface()); -// callback_group_executor.spin(); -// }); rpc_request_future_ = std::async(std::launch::async, [this, client]() { std::unique_ptr<::cartographer::io::SubmapTextures> submap_textures = - ::cartographer_ros::FetchSubmapTextures(id_, client);//,callback_group_executor,std::chrono::milliseconds(10000)); // Missing callbackgroup and timeout but this class isn't a ros node + ::cartographer_ros::FetchSubmapTextures(id_, client,std::chrono::milliseconds(10000)); + //,callback_group_executor,std::chrono::milliseconds(10000)); // Missing callbackgroup and timeout but this class isn't a ros node absl::MutexLock locker(&mutex_); query_in_progress_ = false; if (submap_textures != nullptr) { diff --git a/cartographer_rviz/cartographer_rviz/submap.cc b/cartographer_rviz/cartographer_rviz/submap.cc index 467e3ebc8..a526473f5 100644 --- a/cartographer_rviz/cartographer_rviz/submap.cc +++ b/cartographer_rviz/cartographer_rviz/submap.cc @@ -28,7 +28,6 @@ namespace cartographer_ros { std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures( const ::cartographer::mapping::SubmapId& submap_id, rclcpp::Client::SharedPtr client, - rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor, const std::chrono::milliseconds timeout) { auto request = std::make_shared(); @@ -36,11 +35,13 @@ std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures( request->submap_index = submap_id.submap_index; auto future_result = client->async_send_request(request); - if (callback_group_executor->spin_until_future_complete(future_result, timeout) != - rclcpp::FutureReturnCode::SUCCESS) - { + + auto status = future_result.wait_for(timeout); + if (status != std::future_status::ready){ + return nullptr; } + auto result = future_result.get(); if (result->status.code != ::cartographer_ros_msgs::msg::StatusCode::OK || diff --git a/cartographer_rviz/cartographer_rviz/submap.h b/cartographer_rviz/cartographer_rviz/submap.h index 48c3a425b..18feef3ab 100644 --- a/cartographer_rviz/cartographer_rviz/submap.h +++ b/cartographer_rviz/cartographer_rviz/submap.h @@ -35,7 +35,6 @@ namespace cartographer_ros { std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures( const ::cartographer::mapping::SubmapId& submap_id, rclcpp::Client::SharedPtr client, - rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor, const std::chrono::milliseconds timeout); } // namespace cartographer_ros diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc index a2f3aed99..e9ef80077 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/cartographer_rviz/submaps_display.cc @@ -32,6 +32,9 @@ #include #include +// Include ament_index_cpp::get_package_share_directory +#include + namespace cartographer_rviz { namespace { @@ -58,7 +61,14 @@ SubmapsDisplay::SubmapsDisplay() : Node("SubmapsDisplay") { slice_low_resolution_enabled_ = new ::rviz_common::properties::BoolProperty( "Low Resolution", false, "Display low resolution slices.", this, SLOT(ResolutionToggled()), this); - client_ = this->create_client<::cartographer_ros_msgs::srv::SubmapQuery>("");//update_nh_.serviceClient<::cartographer_ros_msgs::msg::SubmapQuery>(""); + + sync_srv_client_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + callback_group_executor_thread = std::thread([&]() { + callback_group_executor->add_callback_group(sync_srv_client_callback_group, this->get_node_base_interface()); + callback_group_executor->spin(); + }); + + client_ = this->create_client<::cartographer_ros_msgs::srv::SubmapQuery>("",rmw_qos_profile_services_default, sync_srv_client_callback_group);//update_nh_.serviceClient<::cartographer_ros_msgs::msg::SubmapQuery>(""); trajectories_category_ = new ::rviz_common::properties::Property( "Submaps", QVariant(), "List of all submaps, organized by trajectories.", this); @@ -75,15 +85,15 @@ SubmapsDisplay::SubmapsDisplay() : Node("SubmapsDisplay") { "Distance in meters in z-direction beyond " "which submaps will start to fade out.", this); - const std::string package_path = ::ros::package::getPath(ROS_PACKAGE_NAME); + const std::string package_path = ament_index_cpp::get_package_share_directory("cartographer_rviz"); Ogre::ResourceGroupManager::getSingleton().addResourceLocation( - package_path + kMaterialsDirectory, "FileSystem", ROS_PACKAGE_NAME); + package_path + kMaterialsDirectory, "FileSystem", "cartographer_rviz"); Ogre::ResourceGroupManager::getSingleton().addResourceLocation( package_path + kMaterialsDirectory + kGlsl120Directory, "FileSystem", - ROS_PACKAGE_NAME); + "cartographer_rviz"); Ogre::ResourceGroupManager::getSingleton().addResourceLocation( package_path + kMaterialsDirectory + kScriptsDirectory, "FileSystem", - ROS_PACKAGE_NAME); + "cartographer_rviz"); Ogre::ResourceGroupManager::getSingleton().initialiseAllResourceGroups(); tf_buffer_ = std::make_unique(this->get_clock()); diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.h b/cartographer_rviz/cartographer_rviz/submaps_display.h index 971c47dbb..96c176081 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.h +++ b/cartographer_rviz/cartographer_rviz/submaps_display.h @@ -101,6 +101,11 @@ class SubmapsDisplay ::rviz_common::properties::BoolProperty* visibility_all_enabled_; ::rviz_common::properties::BoolProperty* pose_markers_all_enabled_; ::rviz_common::properties::FloatProperty* fade_out_start_distance_in_meters_; + + rclcpp::CallbackGroup::SharedPtr sync_srv_client_callback_group; + rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor; + std::thread callback_group_executor_thread; + }; } // namespace cartographer_rviz From f2d4be312a8c97dfcef1a10a42541ef4c5f2a189 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Fri, 18 Jun 2021 11:23:38 +0200 Subject: [PATCH 51/94] compiling but nothing happens Signed-off-by: Guillaume Doisy --- cartographer_rviz/CMakeLists.txt | 6 +++--- cartographer_rviz/cartographer_rviz/drawable_submap.cc | 4 ++-- cartographer_rviz/cartographer_rviz/submaps_display.cc | 5 ++--- 3 files changed, 7 insertions(+), 8 deletions(-) diff --git a/cartographer_rviz/CMakeLists.txt b/cartographer_rviz/CMakeLists.txt index 29fe5b2a9..4ae895b2f 100644 --- a/cartographer_rviz/CMakeLists.txt +++ b/cartographer_rviz/CMakeLists.txt @@ -133,9 +133,9 @@ target_link_libraries(${PROJECT_NAME} PUBLIC cartographer) target_link_libraries(${PROJECT_NAME} PUBLIC rviz_ogre_vendor) target_link_libraries(${PROJECT_NAME} PUBLIC cartographer_ros) -install(DIRECTORY configuration_files - DESTINATION share/${PROJECT_NAME}/ -) +#install(DIRECTORY configuration_files +# DESTINATION share/${PROJECT_NAME}/ +#) install(DIRECTORY ogre_media DESTINATION share/${PROJECT_NAME}/ diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index 97b71f304..5f0fc6f25 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -53,7 +53,7 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id, const bool visible, const bool pose_axes_visible, const float pose_axes_length, const float pose_axes_radius) - : id_(id), + : rclcpp::Node("drawable_submap"), id_(id), display_context_(display_context), submap_node_(map_node->createChildSceneNode()), submap_id_text_node_(submap_node_->createChildSceneNode()), @@ -64,7 +64,7 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id, .arg(id.trajectory_id) .arg(id.submap_index) .toStdString()), - last_query_timestamp_(0), Node("DrawableSubmap") { + last_query_timestamp_(0){ for (int slice_index = 0; slice_index < kNumberOfSlicesPerSubmap; ++slice_index) { ogre_slices_.emplace_back(absl::make_unique( diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc index e9ef80077..2227684d5 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/cartographer_rviz/submaps_display.cc @@ -48,7 +48,7 @@ constexpr char kDefaultSubmapQueryServiceName[] = "/submap_query"; } // namespace -SubmapsDisplay::SubmapsDisplay() : Node("SubmapsDisplay") { +SubmapsDisplay::SubmapsDisplay() : rclcpp::Node("submaps_display") { submap_query_service_property_ = new ::rviz_common::properties::StringProperty( "Submap query service", kDefaultSubmapQueryServiceName, "Submap query service to connect to.", this, SLOT(Reset())); @@ -321,5 +321,4 @@ Trajectory::Trajectory(std::unique_ptr<::rviz_common::properties::BoolProperty> } } // namespace cartographer_rviz - -PLUGINLIB_EXPORT_CLASS(cartographer_rviz::SubmapsDisplay, ::rviz_common::Display) +//PLUGINLIB_EXPORT_CLASS(cartographer_rviz::SubmapsDisplay, ::rviz_common::Display) From 1599871c9cfacc971ea39fd2704ec499f8c766be Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Fri, 18 Jun 2021 16:31:28 +0200 Subject: [PATCH 52/94] not compiling again Signed-off-by: Guillaume Doisy --- cartographer_rviz/CMakeLists.txt | 7 +++++++ cartographer_rviz/cartographer_rviz/submaps_display.cc | 7 ++++--- cartographer_rviz/cartographer_rviz/submaps_display.h | 7 +++++-- cartographer_rviz/package.xml | 2 +- 4 files changed, 17 insertions(+), 6 deletions(-) diff --git a/cartographer_rviz/CMakeLists.txt b/cartographer_rviz/CMakeLists.txt index 4ae895b2f..1300fae05 100644 --- a/cartographer_rviz/CMakeLists.txt +++ b/cartographer_rviz/CMakeLists.txt @@ -46,6 +46,7 @@ find_package(rviz_assimp_vendor REQUIRED) find_package(rviz_rendering REQUIRED) find_package(rviz_common REQUIRED) find_package(rviz_default_plugins REQUIRED) +find_package(pluginlib REQUIRED) #find_package(rviz REQUIRED) include_directories( @@ -133,10 +134,16 @@ target_link_libraries(${PROJECT_NAME} PUBLIC cartographer) target_link_libraries(${PROJECT_NAME} PUBLIC rviz_ogre_vendor) target_link_libraries(${PROJECT_NAME} PUBLIC cartographer_ros) +pluginlib_export_plugin_description_file(rviz_common rviz_plugin_description.xml) + #install(DIRECTORY configuration_files # DESTINATION share/${PROJECT_NAME}/ #) +install(FILES + rviz_plugin_description.xml + DESTINATION share/${PROJECT_NAME}) + install(DIRECTORY ogre_media DESTINATION share/${PROJECT_NAME}/ ) diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc index 2227684d5..5ebe74646 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/cartographer_rviz/submaps_display.cc @@ -127,8 +127,7 @@ void SubmapsDisplay::reset() { CreateClient(); } -void SubmapsDisplay::processMessage( - const ::cartographer_ros_msgs::msg::SubmapList::ConstPtr& msg) { +void SubmapsDisplay::processMessage(::cartographer_ros_msgs::msg::SubmapList::SharedPtr msg) { absl::MutexLock locker(&mutex_); map_frame_ = absl::make_unique(msg->header.frame_id); // In case Cartographer node is relaunched, destroy trajectories from the @@ -321,4 +320,6 @@ Trajectory::Trajectory(std::unique_ptr<::rviz_common::properties::BoolProperty> } } // namespace cartographer_rviz -//PLUGINLIB_EXPORT_CLASS(cartographer_rviz::SubmapsDisplay, ::rviz_common::Display) + +#include +PLUGINLIB_EXPORT_CLASS(cartographer_rviz::SubmapsDisplay, ::rviz_common::Display) diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.h b/cartographer_rviz/cartographer_rviz/submaps_display.h index 96c176081..95d6511fe 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.h +++ b/cartographer_rviz/cartographer_rviz/submaps_display.h @@ -65,7 +65,9 @@ class SubmapsDisplay public: SubmapsDisplay(); - ~SubmapsDisplay() override; + virtual ~SubmapsDisplay() override; + + SubmapsDisplay(const SubmapsDisplay&) = delete; SubmapsDisplay& operator=(const SubmapsDisplay&) = delete; @@ -80,10 +82,11 @@ class SubmapsDisplay void CreateClient(); // These are called by RViz and therefore do not adhere to the style guide. + using ::rviz_common::MessageFilterDisplay<::cartographer_ros_msgs::msg::SubmapList>::processMessage; void onInitialize() override; void reset() override; void processMessage( - const ::cartographer_ros_msgs::msg::SubmapList::ConstPtr& msg); //override; + ::cartographer_ros_msgs::msg::SubmapList::SharedPtr msg); void update(float wall_dt, float ros_dt) override; std::unique_ptr tf_buffer_; diff --git a/cartographer_rviz/package.xml b/cartographer_rviz/package.xml index 5b9fadc36..32d0925e4 100644 --- a/cartographer_rviz/package.xml +++ b/cartographer_rviz/package.xml @@ -58,6 +58,6 @@ ament_cmake - + From aa8baf3d7ab960f1e54b11a0f23ec4d74d58c87a Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Wed, 7 Jul 2021 17:23:59 +0200 Subject: [PATCH 53/94] fix warning Signed-off-by: Guillaume Doisy --- cartographer_rviz/cartographer_rviz/submaps_display.cc | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc index 5ebe74646..b4145571b 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/cartographer_rviz/submaps_display.cc @@ -23,7 +23,7 @@ #include "cartographer_ros_msgs/msg/submap_list.hpp" #include "cartographer_ros_msgs/srv/submap_query.hpp" #include -#include "pluginlib/class_list_macros.h" +#include //#include "ros/package.h" #include #include @@ -321,5 +321,4 @@ Trajectory::Trajectory(std::unique_ptr<::rviz_common::properties::BoolProperty> } // namespace cartographer_rviz -#include PLUGINLIB_EXPORT_CLASS(cartographer_rviz::SubmapsDisplay, ::rviz_common::Display) From ddf2e316789da2c5d9233049814d4bc546b9cb15 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Fri, 9 Jul 2021 14:00:09 +0200 Subject: [PATCH 54/94] finally compiling but getting segmentation fault Signed-off-by: Guillaume Doisy --- cartographer_rviz/CMakeLists.txt | 53 +++++++++++++------ .../rviz_plugin_description.xml | 26 +++++++++ .../cartographer_rviz/submaps_display.cc | 8 +-- .../cartographer_rviz/submaps_display.h | 6 +-- cartographer_rviz/package.xml | 4 +- cartographer_rviz/rviz_plugin_description.xml | 6 +-- 6 files changed, 75 insertions(+), 28 deletions(-) create mode 100644 cartographer_rviz/cartographer_rviz/rviz_plugin_description.xml diff --git a/cartographer_rviz/CMakeLists.txt b/cartographer_rviz/CMakeLists.txt index 1300fae05..75eed0fa5 100644 --- a/cartographer_rviz/CMakeLists.txt +++ b/cartographer_rviz/CMakeLists.txt @@ -35,12 +35,12 @@ google_initialize_cartographer_project() find_package(absl REQUIRED) find_package(Eigen3 REQUIRED) -find_package(cartographer_ros REQUIRED) +#find_package(cartographer_ros REQUIRED) find_package(cartographer_ros_msgs REQUIRED) find_package(pcl_conversions REQUIRED) find_package(rclcpp REQUIRED) find_package(rviz2 REQUIRED) -find_package(rviz_ogre_vendor REQUIRED) +#find_package(rviz_ogre_vendor REQUIRED) find_package(PCL REQUIRED COMPONENTS common) find_package(rviz_assimp_vendor REQUIRED) find_package(rviz_rendering REQUIRED) @@ -56,13 +56,24 @@ include_directories( file(GLOB_RECURSE ALL_SRCS "cartographer_rviz/*.cc" "cartographer_rviz/*.h") set(CMAKE_AUTOMOC ON) -add_library(${PROJECT_NAME} ${ALL_SRCS}) +add_library(${PROJECT_NAME} SHARED ${ALL_SRCS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDING_LIBRARY") + +# prevent pluginlib from using boost +target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +pluginlib_export_plugin_description_file(rviz_common rviz_plugin_description.xml) + + ament_target_dependencies(${PROJECT_NAME} PUBLIC rclcpp - cartographer_ros +# cartographer_ros cartographer_ros_msgs rviz2 - rviz_ogre_vendor +# rviz_ogre_vendor rviz_assimp_vendor rviz_rendering rviz_common @@ -114,25 +125,25 @@ target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC "${EIGEN3_INCLUDE_DIR}") target_link_libraries(${PROJECT_NAME} PUBLIC ${EIGEN3_LIBRARIES}) -target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC - "${CARTOGRAPHER_ROS_INCLUDE_DIR}") -target_link_libraries(${PROJECT_NAME} PUBLIC ${CARTOGRAPHER_ROS_LIBRARIES}) +#target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC +# "${CARTOGRAPHER_ROS_INCLUDE_DIR}") +#target_link_libraries(${PROJECT_NAME} PUBLIC ${CARTOGRAPHER_ROS_LIBRARIES}) target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC "${Boost_INCLUDE_DIRS}") target_link_libraries(${PROJECT_NAME} PUBLIC ${Boost_LIBRARIES}) -target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC - "${RVIZ_OGRE_VENDOR_INCLUDE_DIRS}") -target_link_libraries(${PROJECT_NAME} PUBLIC ${RVIZ_OGRE_VENDOR_LIBRARIES}) +#target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC +# "${RVIZ_OGRE_VENDOR_INCLUDE_DIRS}") +#target_link_libraries(${PROJECT_NAME} PUBLIC ${RVIZ_OGRE_VENDOR_LIBRARIES}) set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${GOOG_CXX_FLAGS}") set_target_properties(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS ${TARGET_COMPILE_FLAGS}) target_link_libraries(${PROJECT_NAME} PUBLIC cartographer) -target_link_libraries(${PROJECT_NAME} PUBLIC rviz_ogre_vendor) -target_link_libraries(${PROJECT_NAME} PUBLIC cartographer_ros) +#target_link_libraries(${PROJECT_NAME} PUBLIC rviz_ogre_vendor) +#target_link_libraries(${PROJECT_NAME} PUBLIC cartographer_ros) pluginlib_export_plugin_description_file(rviz_common rviz_plugin_description.xml) @@ -140,6 +151,9 @@ pluginlib_export_plugin_description_file(rviz_common rviz_plugin_description.xml # DESTINATION share/${PROJECT_NAME}/ #) + + + install(FILES rviz_plugin_description.xml DESTINATION share/${PROJECT_NAME}) @@ -148,10 +162,19 @@ install(DIRECTORY ogre_media DESTINATION share/${PROJECT_NAME}/ ) -install(TARGETS - ${PROJECT_NAME} +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin) +# replaces catkin_package(LIBRARIES ${PROJECT_NAME}) +ament_export_libraries(${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + ament_package() diff --git a/cartographer_rviz/cartographer_rviz/rviz_plugin_description.xml b/cartographer_rviz/cartographer_rviz/rviz_plugin_description.xml new file mode 100644 index 000000000..80b5b2400 --- /dev/null +++ b/cartographer_rviz/cartographer_rviz/rviz_plugin_description.xml @@ -0,0 +1,26 @@ + + + + + + Displays submaps as a unified map in RViz. + https://github.com/cartographer-project/cartographer_ros + + + diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc index b4145571b..c2c765ba0 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/cartographer_rviz/submaps_display.cc @@ -114,20 +114,20 @@ void SubmapsDisplay::CreateClient() { } void SubmapsDisplay::onInitialize() { - MFDClass::onInitialize(); + RTDClass::onInitialize(); map_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode(); CreateClient(); } void SubmapsDisplay::reset() { - MFDClass::reset(); + RTDClass::reset(); absl::MutexLock locker(&mutex_); client_.reset(); trajectories_.clear(); CreateClient(); } -void SubmapsDisplay::processMessage(::cartographer_ros_msgs::msg::SubmapList::SharedPtr msg) { +void SubmapsDisplay::processMessage( ::cartographer_ros_msgs::msg::SubmapList::ConstSharedPtr msg) { absl::MutexLock locker(&mutex_); map_frame_ = absl::make_unique(msg->header.frame_id); // In case Cartographer node is relaunched, destroy trajectories from the @@ -321,4 +321,4 @@ Trajectory::Trajectory(std::unique_ptr<::rviz_common::properties::BoolProperty> } // namespace cartographer_rviz -PLUGINLIB_EXPORT_CLASS(cartographer_rviz::SubmapsDisplay, ::rviz_common::Display) +PLUGINLIB_EXPORT_CLASS(cartographer_rviz::SubmapsDisplay, rviz_common::Display) diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.h b/cartographer_rviz/cartographer_rviz/submaps_display.h index 95d6511fe..4762e836d 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.h +++ b/cartographer_rviz/cartographer_rviz/submaps_display.h @@ -60,7 +60,8 @@ struct Trajectory : public QObject { // every submap containing pre-multiplied alpha and grayscale values, these are // then alpha blended together. class SubmapsDisplay - : public ::rviz_common::MessageFilterDisplay<::cartographer_ros_msgs::msg::SubmapList>, rclcpp::Node { + : public ::rviz_common::RosTopicDisplay<::cartographer_ros_msgs::msg::SubmapList>, rclcpp::Node { // + //::rviz_common::MessageFilterDisplay<::cartographer_ros_msgs::msg::SubmapList>, rclcpp::Node { Q_OBJECT public: @@ -82,11 +83,10 @@ class SubmapsDisplay void CreateClient(); // These are called by RViz and therefore do not adhere to the style guide. - using ::rviz_common::MessageFilterDisplay<::cartographer_ros_msgs::msg::SubmapList>::processMessage; void onInitialize() override; void reset() override; void processMessage( - ::cartographer_ros_msgs::msg::SubmapList::SharedPtr msg); + ::cartographer_ros_msgs::msg::SubmapList::ConstSharedPtr msg) override; void update(float wall_dt, float ros_dt) override; std::unique_ptr tf_buffer_; diff --git a/cartographer_rviz/package.xml b/cartographer_rviz/package.xml index 32d0925e4..98ce79d71 100644 --- a/cartographer_rviz/package.xml +++ b/cartographer_rviz/package.xml @@ -39,7 +39,6 @@ git cartographer - cartographer_ros cartographer_ros_msgs libqt5-core libqt5-gui @@ -50,7 +49,6 @@ libpcl-all-dev pcl_conversions rviz2 - rviz_ogre_vendor rviz_assimp_vendor rviz_rendering rviz_common @@ -58,6 +56,6 @@ ament_cmake - + diff --git a/cartographer_rviz/rviz_plugin_description.xml b/cartographer_rviz/rviz_plugin_description.xml index 58db7086d..80b5b2400 100644 --- a/cartographer_rviz/rviz_plugin_description.xml +++ b/cartographer_rviz/rviz_plugin_description.xml @@ -14,10 +14,10 @@ limitations under the License. --> - - + + base_class_type="rviz_common::Display"> Displays submaps as a unified map in RViz. https://github.com/cartographer-project/cartographer_ros From c398712bca5d4ed04ea614a1c758b06d3df46dbc Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Fri, 9 Jul 2021 14:22:32 +0200 Subject: [PATCH 55/94] add backward ros Signed-off-by: Guillaume Doisy --- cartographer_rviz/CMakeLists.txt | 1 + cartographer_rviz/cartographer_rviz/submaps_display.cc | 6 +++--- cartographer_rviz/cartographer_rviz/submaps_display.h | 8 ++++---- cartographer_rviz/package.xml | 1 + 4 files changed, 9 insertions(+), 7 deletions(-) diff --git a/cartographer_rviz/CMakeLists.txt b/cartographer_rviz/CMakeLists.txt index 75eed0fa5..0e3b0260f 100644 --- a/cartographer_rviz/CMakeLists.txt +++ b/cartographer_rviz/CMakeLists.txt @@ -48,6 +48,7 @@ find_package(rviz_common REQUIRED) find_package(rviz_default_plugins REQUIRED) find_package(pluginlib REQUIRED) #find_package(rviz REQUIRED) +find_package(backward_ros REQUIRED) include_directories( include diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc index c2c765ba0..110f002d8 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/cartographer_rviz/submaps_display.cc @@ -114,20 +114,20 @@ void SubmapsDisplay::CreateClient() { } void SubmapsDisplay::onInitialize() { - RTDClass::onInitialize(); + MFDClass::onInitialize(); map_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode(); CreateClient(); } void SubmapsDisplay::reset() { - RTDClass::reset(); + MFDClass::reset(); absl::MutexLock locker(&mutex_); client_.reset(); trajectories_.clear(); CreateClient(); } -void SubmapsDisplay::processMessage( ::cartographer_ros_msgs::msg::SubmapList::ConstSharedPtr msg) { +void SubmapsDisplay::processMessage( const ::cartographer_ros_msgs::msg::SubmapList::ConstSharedPtr msg) { absl::MutexLock locker(&mutex_); map_frame_ = absl::make_unique(msg->header.frame_id); // In case Cartographer node is relaunched, destroy trajectories from the diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.h b/cartographer_rviz/cartographer_rviz/submaps_display.h index 4762e836d..357772725 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.h +++ b/cartographer_rviz/cartographer_rviz/submaps_display.h @@ -60,13 +60,13 @@ struct Trajectory : public QObject { // every submap containing pre-multiplied alpha and grayscale values, these are // then alpha blended together. class SubmapsDisplay - : public ::rviz_common::RosTopicDisplay<::cartographer_ros_msgs::msg::SubmapList>, rclcpp::Node { // - //::rviz_common::MessageFilterDisplay<::cartographer_ros_msgs::msg::SubmapList>, rclcpp::Node { + : public ::rviz_common::MessageFilterDisplay<::cartographer_ros_msgs::msg::SubmapList>, rclcpp::Node { //::rviz_common::RosTopicDisplay<::cartographer_ros_msgs::msg::SubmapList>, rclcpp::Node { // + // Q_OBJECT public: SubmapsDisplay(); - virtual ~SubmapsDisplay() override; + ~SubmapsDisplay() override; @@ -86,7 +86,7 @@ class SubmapsDisplay void onInitialize() override; void reset() override; void processMessage( - ::cartographer_ros_msgs::msg::SubmapList::ConstSharedPtr msg) override; + const ::cartographer_ros_msgs::msg::SubmapList::ConstSharedPtr msg) override; void update(float wall_dt, float ros_dt) override; std::unique_ptr tf_buffer_; diff --git a/cartographer_rviz/package.xml b/cartographer_rviz/package.xml index 98ce79d71..38e714101 100644 --- a/cartographer_rviz/package.xml +++ b/cartographer_rviz/package.xml @@ -53,6 +53,7 @@ rviz_rendering rviz_common rviz_default_plugins + backward_ros ament_cmake From 46d6f4ab0bc26b3ba4d771ca39bb3341f8bff923 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Fri, 9 Jul 2021 16:56:38 +0200 Subject: [PATCH 56/94] backward changes temp Signed-off-by: Guillaume Doisy --- .../cartographer_rviz/backward.cpp | 42 + .../cartographer_rviz/backward.hpp | 4457 +++++++++++++++++ .../cartographer_rviz/drawable_submap.cc | 9 + .../rviz_plugin_description.xml | 2 +- .../cartographer_rviz/submaps_display.cc | 9 + cartographer_rviz/package.xml | 2 +- cartographer_rviz/rviz_plugin_description.xml | 2 +- 7 files changed, 4520 insertions(+), 3 deletions(-) create mode 100644 cartographer_rviz/cartographer_rviz/backward.cpp create mode 100644 cartographer_rviz/cartographer_rviz/backward.hpp diff --git a/cartographer_rviz/cartographer_rviz/backward.cpp b/cartographer_rviz/cartographer_rviz/backward.cpp new file mode 100644 index 000000000..110441cba --- /dev/null +++ b/cartographer_rviz/cartographer_rviz/backward.cpp @@ -0,0 +1,42 @@ +// Pick your poison. +// +// On GNU/Linux, you have few choices to get the most out of your stack trace. +// +// By default you get: +// - object filename +// - function name +// +// In order to add: +// - source filename +// - line and column numbers +// - source code snippet (assuming the file is accessible) + +// Install one of the following libraries then uncomment one of the macro (or +// better, add the detection of the lib and the macro definition in your build +// system) + +// - apt-get install libdw-dev ... +// - g++/clang++ -ldw ... +// #define BACKWARD_HAS_DW 1 + +// - apt-get install binutils-dev ... +// - g++/clang++ -lbfd ... +// #define BACKWARD_HAS_BFD 1 + +// - apt-get install libdwarf-dev ... +// - g++/clang++ -ldwarf ... +// #define BACKWARD_HAS_DWARF 1 + +// Regardless of the library you choose to read the debug information, +// for potentially more detailed stack traces you can use libunwind +// - apt-get install libunwind-dev +// - g++/clang++ -lunwind +// #define BACKWARD_HAS_LIBUNWIND 1 + +#include "backward.hpp" + +namespace backward { + +backward::SignalHandling sh; + +} // namespace backward diff --git a/cartographer_rviz/cartographer_rviz/backward.hpp b/cartographer_rviz/cartographer_rviz/backward.hpp new file mode 100644 index 000000000..93037d5dc --- /dev/null +++ b/cartographer_rviz/cartographer_rviz/backward.hpp @@ -0,0 +1,4457 @@ +/* + * backward.hpp + * Copyright 2013 Google Inc. All Rights Reserved. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#ifndef H_6B9572DA_A64B_49E6_B234_051480991C89 +#define H_6B9572DA_A64B_49E6_B234_051480991C89 + +#ifndef __cplusplus +#error "It's not going to compile without a C++ compiler..." +#endif + +#if defined(BACKWARD_CXX11) +#elif defined(BACKWARD_CXX98) +#else +#if __cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1800) +#define BACKWARD_CXX11 +#define BACKWARD_ATLEAST_CXX11 +#define BACKWARD_ATLEAST_CXX98 +#if __cplusplus >= 201703L || (defined(_MSVC_LANG) && _MSVC_LANG >= 201703L) +#define BACKWARD_ATLEAST_CXX17 +#endif +#else +#define BACKWARD_CXX98 +#define BACKWARD_ATLEAST_CXX98 +#endif +#endif + +// You can define one of the following (or leave it to the auto-detection): +// +// #define BACKWARD_SYSTEM_LINUX +// - specialization for linux +// +// #define BACKWARD_SYSTEM_DARWIN +// - specialization for Mac OS X 10.5 and later. +// +// #define BACKWARD_SYSTEM_WINDOWS +// - specialization for Windows (Clang 9 and MSVC2017) +// +// #define BACKWARD_SYSTEM_UNKNOWN +// - placebo implementation, does nothing. +// +#if defined(BACKWARD_SYSTEM_LINUX) +#elif defined(BACKWARD_SYSTEM_DARWIN) +#elif defined(BACKWARD_SYSTEM_UNKNOWN) +#elif defined(BACKWARD_SYSTEM_WINDOWS) +#else +#if defined(__linux) || defined(__linux__) +#define BACKWARD_SYSTEM_LINUX +#elif defined(__APPLE__) +#define BACKWARD_SYSTEM_DARWIN +#elif defined(_WIN32) +#define BACKWARD_SYSTEM_WINDOWS +#else +#define BACKWARD_SYSTEM_UNKNOWN +#endif +#endif + +#define NOINLINE __attribute__((noinline)) + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if defined(BACKWARD_SYSTEM_LINUX) + +// On linux, backtrace can back-trace or "walk" the stack using the following +// libraries: +// +// #define BACKWARD_HAS_UNWIND 1 +// - unwind comes from libgcc, but I saw an equivalent inside clang itself. +// - with unwind, the stacktrace is as accurate as it can possibly be, since +// this is used by the C++ runtine in gcc/clang for stack unwinding on +// exception. +// - normally libgcc is already linked to your program by default. +// +// #define BACKWARD_HAS_LIBUNWIND 1 +// - libunwind provides, in some cases, a more accurate stacktrace as it knows +// to decode signal handler frames and lets us edit the context registers when +// unwinding, allowing stack traces over bad function references. +// +// #define BACKWARD_HAS_BACKTRACE == 1 +// - backtrace seems to be a little bit more portable than libunwind, but on +// linux, it uses unwind anyway, but abstract away a tiny information that is +// sadly really important in order to get perfectly accurate stack traces. +// - backtrace is part of the (e)glib library. +// +// The default is: +// #define BACKWARD_HAS_UNWIND == 1 +// +// Note that only one of the define should be set to 1 at a time. +// +#if BACKWARD_HAS_UNWIND == 1 +#elif BACKWARD_HAS_LIBUNWIND == 1 +#elif BACKWARD_HAS_BACKTRACE == 1 +#else +#undef BACKWARD_HAS_UNWIND +#define BACKWARD_HAS_UNWIND 1 +#undef BACKWARD_HAS_LIBUNWIND +#define BACKWARD_HAS_LIBUNWIND 0 +#undef BACKWARD_HAS_BACKTRACE +#define BACKWARD_HAS_BACKTRACE 0 +#endif + +// On linux, backward can extract detailed information about a stack trace +// using one of the following libraries: +// +// #define BACKWARD_HAS_DW 1 +// - libdw gives you the most juicy details out of your stack traces: +// - object filename +// - function name +// - source filename +// - line and column numbers +// - source code snippet (assuming the file is accessible) +// - variables name and values (if not optimized out) +// - You need to link with the lib "dw": +// - apt-get install libdw-dev +// - g++/clang++ -ldw ... +// +// #define BACKWARD_HAS_BFD 1 +// - With libbfd, you get a fair amount of details: +// - object filename +// - function name +// - source filename +// - line numbers +// - source code snippet (assuming the file is accessible) +// - You need to link with the lib "bfd": +// - apt-get install binutils-dev +// - g++/clang++ -lbfd ... +// +// #define BACKWARD_HAS_DWARF 1 +// - libdwarf gives you the most juicy details out of your stack traces: +// - object filename +// - function name +// - source filename +// - line and column numbers +// - source code snippet (assuming the file is accessible) +// - variables name and values (if not optimized out) +// - You need to link with the lib "dwarf": +// - apt-get install libdwarf-dev +// - g++/clang++ -ldwarf ... +// +// #define BACKWARD_HAS_BACKTRACE_SYMBOL 1 +// - backtrace provides minimal details for a stack trace: +// - object filename +// - function name +// - backtrace is part of the (e)glib library. +// +// The default is: +// #define BACKWARD_HAS_BACKTRACE_SYMBOL == 1 +// +// Note that only one of the define should be set to 1 at a time. +// +#if BACKWARD_HAS_DW == 1 +#elif BACKWARD_HAS_BFD == 1 +#elif BACKWARD_HAS_DWARF == 1 +#elif BACKWARD_HAS_BACKTRACE_SYMBOL == 1 +#else +#undef BACKWARD_HAS_DW +#define BACKWARD_HAS_DW 0 +#undef BACKWARD_HAS_BFD +#define BACKWARD_HAS_BFD 0 +#undef BACKWARD_HAS_DWARF +#define BACKWARD_HAS_DWARF 0 +#undef BACKWARD_HAS_BACKTRACE_SYMBOL +#define BACKWARD_HAS_BACKTRACE_SYMBOL 1 +#endif + +#include +#include +#ifdef __ANDROID__ +// Old Android API levels define _Unwind_Ptr in both link.h and +// unwind.h Rename the one in link.h as we are not going to be using +// it +#define _Unwind_Ptr _Unwind_Ptr_Custom +#include +#undef _Unwind_Ptr +#else +#include +#endif +#include +#include +#include +#include + +#if BACKWARD_HAS_BFD == 1 +// NOTE: defining PACKAGE{,_VERSION} is required before including +// bfd.h on some platforms, see also: +// https://sourceware.org/bugzilla/show_bug.cgi?id=14243 +#ifndef PACKAGE +#define PACKAGE +#endif +#ifndef PACKAGE_VERSION +#define PACKAGE_VERSION +#endif +#include +#ifndef _GNU_SOURCE +#define _GNU_SOURCE +#include +#undef _GNU_SOURCE +#else +#include +#endif +#endif + +#if BACKWARD_HAS_DW == 1 +#include +#include +#include +#endif + +#if BACKWARD_HAS_DWARF == 1 +#include +#include +#include +#include +#include +#ifndef _GNU_SOURCE +#define _GNU_SOURCE +#include +#undef _GNU_SOURCE +#else +#include +#endif +#endif + +#if (BACKWARD_HAS_BACKTRACE == 1) || (BACKWARD_HAS_BACKTRACE_SYMBOL == 1) +// then we shall rely on backtrace +#include +#endif + +#endif // defined(BACKWARD_SYSTEM_LINUX) + +#if defined(BACKWARD_SYSTEM_DARWIN) +// On Darwin, backtrace can back-trace or "walk" the stack using the following +// libraries: +// +// #define BACKWARD_HAS_UNWIND 1 +// - unwind comes from libgcc, but I saw an equivalent inside clang itself. +// - with unwind, the stacktrace is as accurate as it can possibly be, since +// this is used by the C++ runtine in gcc/clang for stack unwinding on +// exception. +// - normally libgcc is already linked to your program by default. +// +// #define BACKWARD_HAS_LIBUNWIND 1 +// - libunwind comes from clang, which implements an API compatible version. +// - libunwind provides, in some cases, a more accurate stacktrace as it knows +// to decode signal handler frames and lets us edit the context registers when +// unwinding, allowing stack traces over bad function references. +// +// #define BACKWARD_HAS_BACKTRACE == 1 +// - backtrace is available by default, though it does not produce as much +// information as another library might. +// +// The default is: +// #define BACKWARD_HAS_UNWIND == 1 +// +// Note that only one of the define should be set to 1 at a time. +// +#if BACKWARD_HAS_UNWIND == 1 +#elif BACKWARD_HAS_BACKTRACE == 1 +#elif BACKWARD_HAS_LIBUNWIND == 1 +#else +#undef BACKWARD_HAS_UNWIND +#define BACKWARD_HAS_UNWIND 1 +#undef BACKWARD_HAS_BACKTRACE +#define BACKWARD_HAS_BACKTRACE 0 +#undef BACKWARD_HAS_LIBUNWIND +#define BACKWARD_HAS_LIBUNWIND 0 +#endif + +// On Darwin, backward can extract detailed information about a stack trace +// using one of the following libraries: +// +// #define BACKWARD_HAS_BACKTRACE_SYMBOL 1 +// - backtrace provides minimal details for a stack trace: +// - object filename +// - function name +// +// The default is: +// #define BACKWARD_HAS_BACKTRACE_SYMBOL == 1 +// +#if BACKWARD_HAS_BACKTRACE_SYMBOL == 1 +#else +#undef BACKWARD_HAS_BACKTRACE_SYMBOL +#define BACKWARD_HAS_BACKTRACE_SYMBOL 1 +#endif + +#include +#include +#include +#include +#include +#include + +#if (BACKWARD_HAS_BACKTRACE == 1) || (BACKWARD_HAS_BACKTRACE_SYMBOL == 1) +#include +#endif +#endif // defined(BACKWARD_SYSTEM_DARWIN) + +#if defined(BACKWARD_SYSTEM_WINDOWS) + +#include +#include +#include + +#include +typedef SSIZE_T ssize_t; + +#define NOMINMAX +#include +#include + +#include +#include + +#ifndef __clang__ +#undef NOINLINE +#define NOINLINE __declspec(noinline) +#endif + +#pragma comment(lib, "psapi.lib") +#pragma comment(lib, "dbghelp.lib") + +// Comment / packing is from stackoverflow: +// https://stackoverflow.com/questions/6205981/windows-c-stack-trace-from-a-running-app/28276227#28276227 +// Some versions of imagehlp.dll lack the proper packing directives themselves +// so we need to do it. +#pragma pack(push, before_imagehlp, 8) +#include +#pragma pack(pop, before_imagehlp) + +// TODO maybe these should be undefined somewhere else? +#undef BACKWARD_HAS_UNWIND +#undef BACKWARD_HAS_BACKTRACE +#if BACKWARD_HAS_PDB_SYMBOL == 1 +#else +#undef BACKWARD_HAS_PDB_SYMBOL +#define BACKWARD_HAS_PDB_SYMBOL 1 +#endif + +#endif + +#if BACKWARD_HAS_UNWIND == 1 + +#include +// while gcc's unwind.h defines something like that: +// extern _Unwind_Ptr _Unwind_GetIP (struct _Unwind_Context *); +// extern _Unwind_Ptr _Unwind_GetIPInfo (struct _Unwind_Context *, int *); +// +// clang's unwind.h defines something like this: +// uintptr_t _Unwind_GetIP(struct _Unwind_Context* __context); +// +// Even if the _Unwind_GetIPInfo can be linked to, it is not declared, worse we +// cannot just redeclare it because clang's unwind.h doesn't define _Unwind_Ptr +// anyway. +// +// Luckily we can play on the fact that the guard macros have a different name: +#ifdef __CLANG_UNWIND_H +// In fact, this function still comes from libgcc (on my different linux boxes, +// clang links against libgcc). +#include +extern "C" uintptr_t _Unwind_GetIPInfo(_Unwind_Context *, int *); +#endif + +#endif // BACKWARD_HAS_UNWIND == 1 + +#if BACKWARD_HAS_LIBUNWIND == 1 +#define UNW_LOCAL_ONLY +#include +#endif // BACKWARD_HAS_LIBUNWIND == 1 + +#ifdef BACKWARD_ATLEAST_CXX11 +#include +#include // for std::swap +namespace backward { +namespace details { +template struct hashtable { + typedef std::unordered_map type; +}; +using std::move; +} // namespace details +} // namespace backward +#else // NOT BACKWARD_ATLEAST_CXX11 +#define nullptr NULL +#define override +#include +namespace backward { +namespace details { +template struct hashtable { + typedef std::map type; +}; +template const T &move(const T &v) { return v; } +template T &move(T &v) { return v; } +} // namespace details +} // namespace backward +#endif // BACKWARD_ATLEAST_CXX11 + +namespace backward { +namespace details { +#if defined(BACKWARD_SYSTEM_WINDOWS) +const char kBackwardPathDelimiter[] = ";"; +#else +const char kBackwardPathDelimiter[] = ":"; +#endif +} // namespace details +} // namespace backward + +namespace backward { + +namespace system_tag { +struct linux_tag; // seems that I cannot call that "linux" because the name +// is already defined... so I am adding _tag everywhere. +struct darwin_tag; +struct windows_tag; +struct unknown_tag; + +#if defined(BACKWARD_SYSTEM_LINUX) +typedef linux_tag current_tag; +#elif defined(BACKWARD_SYSTEM_DARWIN) +typedef darwin_tag current_tag; +#elif defined(BACKWARD_SYSTEM_WINDOWS) +typedef windows_tag current_tag; +#elif defined(BACKWARD_SYSTEM_UNKNOWN) +typedef unknown_tag current_tag; +#else +#error "May I please get my system defines?" +#endif +} // namespace system_tag + +namespace trace_resolver_tag { +#if defined(BACKWARD_SYSTEM_LINUX) +struct libdw; +struct libbfd; +struct libdwarf; +struct backtrace_symbol; + +#if BACKWARD_HAS_DW == 1 +typedef libdw current; +#elif BACKWARD_HAS_BFD == 1 +typedef libbfd current; +#elif BACKWARD_HAS_DWARF == 1 +typedef libdwarf current; +#elif BACKWARD_HAS_BACKTRACE_SYMBOL == 1 +typedef backtrace_symbol current; +#else +#error "You shall not pass, until you know what you want." +#endif +#elif defined(BACKWARD_SYSTEM_DARWIN) +struct backtrace_symbol; + +#if BACKWARD_HAS_BACKTRACE_SYMBOL == 1 +typedef backtrace_symbol current; +#else +#error "You shall not pass, until you know what you want." +#endif +#elif defined(BACKWARD_SYSTEM_WINDOWS) +struct pdb_symbol; +#if BACKWARD_HAS_PDB_SYMBOL == 1 +typedef pdb_symbol current; +#else +#error "You shall not pass, until you know what you want." +#endif +#endif +} // namespace trace_resolver_tag + +namespace details { + +template struct rm_ptr { typedef T type; }; + +template struct rm_ptr { typedef T type; }; + +template struct rm_ptr { typedef const T type; }; + +template struct deleter { + template void operator()(U &ptr) const { (*F)(ptr); } +}; + +template struct default_delete { + void operator()(T &ptr) const { delete ptr; } +}; + +template > +class handle { + struct dummy; + T _val; + bool _empty; + +#ifdef BACKWARD_ATLEAST_CXX11 + handle(const handle &) = delete; + handle &operator=(const handle &) = delete; +#endif + +public: + ~handle() { + if (!_empty) { + Deleter()(_val); + } + } + + explicit handle() : _val(), _empty(true) {} + explicit handle(T val) : _val(val), _empty(false) { + if (!_val) + _empty = true; + } + +#ifdef BACKWARD_ATLEAST_CXX11 + handle(handle &&from) : _empty(true) { swap(from); } + handle &operator=(handle &&from) { + swap(from); + return *this; + } +#else + explicit handle(const handle &from) : _empty(true) { + // some sort of poor man's move semantic. + swap(const_cast(from)); + } + handle &operator=(const handle &from) { + // some sort of poor man's move semantic. + swap(const_cast(from)); + return *this; + } +#endif + + void reset(T new_val) { + handle tmp(new_val); + swap(tmp); + } + + void update(T new_val) { + _val = new_val; + _empty = !static_cast(new_val); + } + + operator const dummy *() const { + if (_empty) { + return nullptr; + } + return reinterpret_cast(_val); + } + T get() { return _val; } + T release() { + _empty = true; + return _val; + } + void swap(handle &b) { + using std::swap; + swap(b._val, _val); // can throw, we are safe here. + swap(b._empty, _empty); // should not throw: if you cannot swap two + // bools without throwing... It's a lost cause anyway! + } + + T &operator->() { return _val; } + const T &operator->() const { return _val; } + + typedef typename rm_ptr::type &ref_t; + typedef const typename rm_ptr::type &const_ref_t; + ref_t operator*() { return *_val; } + const_ref_t operator*() const { return *_val; } + ref_t operator[](size_t idx) { return _val[idx]; } + + // Watch out, we've got a badass over here + T *operator&() { + _empty = false; + return &_val; + } +}; + +// Default demangler implementation (do nothing). +template struct demangler_impl { + static std::string demangle(const char *funcname) { return funcname; } +}; + +#if defined(BACKWARD_SYSTEM_LINUX) || defined(BACKWARD_SYSTEM_DARWIN) + +template <> struct demangler_impl { + demangler_impl() : _demangle_buffer_length(0) {} + + std::string demangle(const char *funcname) { + using namespace details; + char *result = abi::__cxa_demangle(funcname, _demangle_buffer.get(), + &_demangle_buffer_length, nullptr); + if (result) { + _demangle_buffer.update(result); + return result; + } + return funcname; + } + +private: + details::handle _demangle_buffer; + size_t _demangle_buffer_length; +}; + +#endif // BACKWARD_SYSTEM_LINUX || BACKWARD_SYSTEM_DARWIN + +struct demangler : public demangler_impl {}; + +// Split a string on the platform's PATH delimiter. Example: if delimiter +// is ":" then: +// "" --> [] +// ":" --> ["",""] +// "::" --> ["","",""] +// "/a/b/c" --> ["/a/b/c"] +// "/a/b/c:/d/e/f" --> ["/a/b/c","/d/e/f"] +// etc. +inline std::vector split_source_prefixes(const std::string &s) { + std::vector out; + size_t last = 0; + size_t next = 0; + size_t delimiter_size = sizeof(kBackwardPathDelimiter) - 1; + while ((next = s.find(kBackwardPathDelimiter, last)) != std::string::npos) { + out.push_back(s.substr(last, next - last)); + last = next + delimiter_size; + } + if (last <= s.length()) { + out.push_back(s.substr(last)); + } + return out; +} + +} // namespace details + +/*************** A TRACE ***************/ + +struct Trace { + void *addr; + size_t idx; + + Trace() : addr(nullptr), idx(0) {} + + explicit Trace(void *_addr, size_t _idx) : addr(_addr), idx(_idx) {} +}; + +struct ResolvedTrace : public Trace { + + struct SourceLoc { + std::string function; + std::string filename; + unsigned line; + unsigned col; + + SourceLoc() : line(0), col(0) {} + + bool operator==(const SourceLoc &b) const { + return function == b.function && filename == b.filename && + line == b.line && col == b.col; + } + + bool operator!=(const SourceLoc &b) const { return !(*this == b); } + }; + + // In which binary object this trace is located. + std::string object_filename; + + // The function in the object that contain the trace. This is not the same + // as source.function which can be an function inlined in object_function. + std::string object_function; + + // The source location of this trace. It is possible for filename to be + // empty and for line/col to be invalid (value 0) if this information + // couldn't be deduced, for example if there is no debug information in the + // binary object. + SourceLoc source; + + // An optionals list of "inliners". All the successive sources location + // from where the source location of the trace (the attribute right above) + // is inlined. It is especially useful when you compiled with optimization. + typedef std::vector source_locs_t; + source_locs_t inliners; + + ResolvedTrace() : Trace() {} + ResolvedTrace(const Trace &mini_trace) : Trace(mini_trace) {} +}; + +/*************** STACK TRACE ***************/ + +// default implemention. +template class StackTraceImpl { +public: + size_t size() const { return 0; } + Trace operator[](size_t) const { return Trace(); } + size_t load_here(size_t = 0) { return 0; } + size_t load_from(void *, size_t = 0, void * = nullptr, void * = nullptr) { + return 0; + } + size_t thread_id() const { return 0; } + void skip_n_firsts(size_t) {} +}; + +class StackTraceImplBase { +public: + StackTraceImplBase() + : _thread_id(0), _skip(0), _context(nullptr), _error_addr(nullptr) {} + + size_t thread_id() const { return _thread_id; } + + void skip_n_firsts(size_t n) { _skip = n; } + +protected: + void load_thread_info() { +#ifdef BACKWARD_SYSTEM_LINUX +#ifndef __ANDROID__ + _thread_id = static_cast(syscall(SYS_gettid)); +#else + _thread_id = static_cast(gettid()); +#endif + if (_thread_id == static_cast(getpid())) { + // If the thread is the main one, let's hide that. + // I like to keep little secret sometimes. + _thread_id = 0; + } +#elif defined(BACKWARD_SYSTEM_DARWIN) + _thread_id = reinterpret_cast(pthread_self()); + if (pthread_main_np() == 1) { + // If the thread is the main one, let's hide that. + _thread_id = 0; + } +#endif + } + + void set_context(void *context) { _context = context; } + void *context() const { return _context; } + + void set_error_addr(void *error_addr) { _error_addr = error_addr; } + void *error_addr() const { return _error_addr; } + + size_t skip_n_firsts() const { return _skip; } + +private: + size_t _thread_id; + size_t _skip; + void *_context; + void *_error_addr; +}; + +class StackTraceImplHolder : public StackTraceImplBase { +public: + size_t size() const { + return (_stacktrace.size() >= skip_n_firsts()) + ? _stacktrace.size() - skip_n_firsts() + : 0; + } + Trace operator[](size_t idx) const { + if (idx >= size()) { + return Trace(); + } + return Trace(_stacktrace[idx + skip_n_firsts()], idx); + } + void *const *begin() const { + if (size()) { + return &_stacktrace[skip_n_firsts()]; + } + return nullptr; + } + +protected: + std::vector _stacktrace; +}; + +#if BACKWARD_HAS_UNWIND == 1 + +namespace details { + +template class Unwinder { +public: + size_t operator()(F &f, size_t depth) { + _f = &f; + _index = -1; + _depth = depth; + _Unwind_Backtrace(&this->backtrace_trampoline, this); + return static_cast(_index); + } + +private: + F *_f; + ssize_t _index; + size_t _depth; + + static _Unwind_Reason_Code backtrace_trampoline(_Unwind_Context *ctx, + void *self) { + return (static_cast(self))->backtrace(ctx); + } + + _Unwind_Reason_Code backtrace(_Unwind_Context *ctx) { + if (_index >= 0 && static_cast(_index) >= _depth) + return _URC_END_OF_STACK; + + int ip_before_instruction = 0; + uintptr_t ip = _Unwind_GetIPInfo(ctx, &ip_before_instruction); + + if (!ip_before_instruction) { + // calculating 0-1 for unsigned, looks like a possible bug to sanitiziers, + // so let's do it explicitly: + if (ip == 0) { + ip = std::numeric_limits::max(); // set it to 0xffff... (as + // from casting 0-1) + } else { + ip -= 1; // else just normally decrement it (no overflow/underflow will + // happen) + } + } + + if (_index >= 0) { // ignore first frame. + (*_f)(static_cast(_index), reinterpret_cast(ip)); + } + _index += 1; + return _URC_NO_REASON; + } +}; + +template size_t unwind(F f, size_t depth) { + Unwinder unwinder; + return unwinder(f, depth); +} + +} // namespace details + +template <> +class StackTraceImpl : public StackTraceImplHolder { +public: + NOINLINE + size_t load_here(size_t depth = 32, void *context = nullptr, + void *error_addr = nullptr) { + load_thread_info(); + set_context(context); + set_error_addr(error_addr); + if (depth == 0) { + return 0; + } + _stacktrace.resize(depth); + size_t trace_cnt = details::unwind(callback(*this), depth); + _stacktrace.resize(trace_cnt); + skip_n_firsts(0); + return size(); + } + size_t load_from(void *addr, size_t depth = 32, void *context = nullptr, + void *error_addr = nullptr) { + load_here(depth + 8, context, error_addr); + + for (size_t i = 0; i < _stacktrace.size(); ++i) { + if (_stacktrace[i] == addr) { + skip_n_firsts(i); + break; + } + } + + _stacktrace.resize(std::min(_stacktrace.size(), skip_n_firsts() + depth)); + return size(); + } + +private: + struct callback { + StackTraceImpl &self; + callback(StackTraceImpl &_self) : self(_self) {} + + void operator()(size_t idx, void *addr) { self._stacktrace[idx] = addr; } + }; +}; + +#elif BACKWARD_HAS_LIBUNWIND == 1 + +template <> +class StackTraceImpl : public StackTraceImplHolder { +public: + __attribute__((noinline)) size_t load_here(size_t depth = 32, + void *_context = nullptr, + void *_error_addr = nullptr) { + set_context(_context); + set_error_addr(_error_addr); + load_thread_info(); + if (depth == 0) { + return 0; + } + _stacktrace.resize(depth + 1); + + int result = 0; + + unw_context_t ctx; + size_t index = 0; + + // Add the tail call. If the Instruction Pointer is the crash address it + // means we got a bad function pointer dereference, so we "unwind" the + // bad pointer manually by using the return address pointed to by the + // Stack Pointer as the Instruction Pointer and letting libunwind do + // the rest + + if (context()) { + ucontext_t *uctx = reinterpret_cast(context()); +#ifdef REG_RIP // x86_64 + if (uctx->uc_mcontext.gregs[REG_RIP] == + reinterpret_cast(error_addr())) { + uctx->uc_mcontext.gregs[REG_RIP] = + *reinterpret_cast(uctx->uc_mcontext.gregs[REG_RSP]); + } + _stacktrace[index] = + reinterpret_cast(uctx->uc_mcontext.gregs[REG_RIP]); + ++index; + ctx = *reinterpret_cast(uctx); +#elif defined(REG_EIP) // x86_32 + if (uctx->uc_mcontext.gregs[REG_EIP] == + reinterpret_cast(error_addr())) { + uctx->uc_mcontext.gregs[REG_EIP] = + *reinterpret_cast(uctx->uc_mcontext.gregs[REG_ESP]); + } + _stacktrace[index] = + reinterpret_cast(uctx->uc_mcontext.gregs[REG_EIP]); + ++index; + ctx = *reinterpret_cast(uctx); +#elif defined(__arm__) + // libunwind uses its own context type for ARM unwinding. + // Copy the registers from the signal handler's context so we can + // unwind + unw_getcontext(&ctx); + ctx.regs[UNW_ARM_R0] = uctx->uc_mcontext.arm_r0; + ctx.regs[UNW_ARM_R1] = uctx->uc_mcontext.arm_r1; + ctx.regs[UNW_ARM_R2] = uctx->uc_mcontext.arm_r2; + ctx.regs[UNW_ARM_R3] = uctx->uc_mcontext.arm_r3; + ctx.regs[UNW_ARM_R4] = uctx->uc_mcontext.arm_r4; + ctx.regs[UNW_ARM_R5] = uctx->uc_mcontext.arm_r5; + ctx.regs[UNW_ARM_R6] = uctx->uc_mcontext.arm_r6; + ctx.regs[UNW_ARM_R7] = uctx->uc_mcontext.arm_r7; + ctx.regs[UNW_ARM_R8] = uctx->uc_mcontext.arm_r8; + ctx.regs[UNW_ARM_R9] = uctx->uc_mcontext.arm_r9; + ctx.regs[UNW_ARM_R10] = uctx->uc_mcontext.arm_r10; + ctx.regs[UNW_ARM_R11] = uctx->uc_mcontext.arm_fp; + ctx.regs[UNW_ARM_R12] = uctx->uc_mcontext.arm_ip; + ctx.regs[UNW_ARM_R13] = uctx->uc_mcontext.arm_sp; + ctx.regs[UNW_ARM_R14] = uctx->uc_mcontext.arm_lr; + ctx.regs[UNW_ARM_R15] = uctx->uc_mcontext.arm_pc; + + // If we have crashed in the PC use the LR instead, as this was + // a bad function dereference + if (reinterpret_cast(error_addr()) == + uctx->uc_mcontext.arm_pc) { + ctx.regs[UNW_ARM_R15] = + uctx->uc_mcontext.arm_lr - sizeof(unsigned long); + } + _stacktrace[index] = reinterpret_cast(ctx.regs[UNW_ARM_R15]); + ++index; +#elif defined(__APPLE__) && defined(__x86_64__) + unw_getcontext(&ctx); + // OS X's implementation of libunwind uses its own context object + // so we need to convert the passed context to libunwind's format + // (information about the data layout taken from unw_getcontext.s + // in Apple's libunwind source + ctx.data[0] = uctx->uc_mcontext->__ss.__rax; + ctx.data[1] = uctx->uc_mcontext->__ss.__rbx; + ctx.data[2] = uctx->uc_mcontext->__ss.__rcx; + ctx.data[3] = uctx->uc_mcontext->__ss.__rdx; + ctx.data[4] = uctx->uc_mcontext->__ss.__rdi; + ctx.data[5] = uctx->uc_mcontext->__ss.__rsi; + ctx.data[6] = uctx->uc_mcontext->__ss.__rbp; + ctx.data[7] = uctx->uc_mcontext->__ss.__rsp; + ctx.data[8] = uctx->uc_mcontext->__ss.__r8; + ctx.data[9] = uctx->uc_mcontext->__ss.__r9; + ctx.data[10] = uctx->uc_mcontext->__ss.__r10; + ctx.data[11] = uctx->uc_mcontext->__ss.__r11; + ctx.data[12] = uctx->uc_mcontext->__ss.__r12; + ctx.data[13] = uctx->uc_mcontext->__ss.__r13; + ctx.data[14] = uctx->uc_mcontext->__ss.__r14; + ctx.data[15] = uctx->uc_mcontext->__ss.__r15; + ctx.data[16] = uctx->uc_mcontext->__ss.__rip; + + // If the IP is the same as the crash address we have a bad function + // dereference The caller's address is pointed to by %rsp, so we + // dereference that value and set it to be the next frame's IP. + if (uctx->uc_mcontext->__ss.__rip == + reinterpret_cast<__uint64_t>(error_addr())) { + ctx.data[16] = + *reinterpret_cast<__uint64_t *>(uctx->uc_mcontext->__ss.__rsp); + } + _stacktrace[index] = reinterpret_cast(ctx.data[16]); + ++index; +#elif defined(__APPLE__) + unw_getcontext(&ctx) + // TODO: Convert the ucontext_t to libunwind's unw_context_t like + // we do in 64 bits + if (ctx.uc_mcontext->__ss.__eip == + reinterpret_cast(error_addr())) { + ctx.uc_mcontext->__ss.__eip = ctx.uc_mcontext->__ss.__esp; + } + _stacktrace[index] = + reinterpret_cast(ctx.uc_mcontext->__ss.__eip); + ++index; +#endif + } + + unw_cursor_t cursor; + if (context()) { +#if defined(UNW_INIT_SIGNAL_FRAME) + result = unw_init_local2(&cursor, &ctx, UNW_INIT_SIGNAL_FRAME); +#else + result = unw_init_local(&cursor, &ctx); +#endif + } else { + unw_getcontext(&ctx); + ; + result = unw_init_local(&cursor, &ctx); + } + + if (result != 0) + return 1; + + unw_word_t ip = 0; + + while (index <= depth && unw_step(&cursor) > 0) { + result = unw_get_reg(&cursor, UNW_REG_IP, &ip); + if (result == 0) { + _stacktrace[index] = reinterpret_cast(--ip); + ++index; + } + } + --index; + + _stacktrace.resize(index + 1); + skip_n_firsts(0); + return size(); + } + + size_t load_from(void *addr, size_t depth = 32, void *context = nullptr, + void *error_addr = nullptr) { + load_here(depth + 8, context, error_addr); + + for (size_t i = 0; i < _stacktrace.size(); ++i) { + if (_stacktrace[i] == addr) { + skip_n_firsts(i); + _stacktrace[i] = (void *)((uintptr_t)_stacktrace[i]); + break; + } + } + + _stacktrace.resize(std::min(_stacktrace.size(), skip_n_firsts() + depth)); + return size(); + } +}; + +#elif defined(BACKWARD_HAS_BACKTRACE) + +template <> +class StackTraceImpl : public StackTraceImplHolder { +public: + NOINLINE + size_t load_here(size_t depth = 32, void *context = nullptr, + void *error_addr = nullptr) { + set_context(context); + set_error_addr(error_addr); + load_thread_info(); + if (depth == 0) { + return 0; + } + _stacktrace.resize(depth + 1); + size_t trace_cnt = backtrace(&_stacktrace[0], _stacktrace.size()); + _stacktrace.resize(trace_cnt); + skip_n_firsts(1); + return size(); + } + + size_t load_from(void *addr, size_t depth = 32, void *context = nullptr, + void *error_addr = nullptr) { + load_here(depth + 8, context, error_addr); + + for (size_t i = 0; i < _stacktrace.size(); ++i) { + if (_stacktrace[i] == addr) { + skip_n_firsts(i); + _stacktrace[i] = (void *)((uintptr_t)_stacktrace[i] + 1); + break; + } + } + + _stacktrace.resize(std::min(_stacktrace.size(), skip_n_firsts() + depth)); + return size(); + } +}; + +#elif defined(BACKWARD_SYSTEM_WINDOWS) + +template <> +class StackTraceImpl : public StackTraceImplHolder { +public: + // We have to load the machine type from the image info + // So we first initialize the resolver, and it tells us this info + void set_machine_type(DWORD machine_type) { machine_type_ = machine_type; } + void set_context(CONTEXT *ctx) { ctx_ = ctx; } + void set_thread_handle(HANDLE handle) { thd_ = handle; } + + NOINLINE + size_t load_here(size_t depth = 32, void *context = nullptr, + void *error_addr = nullptr) { + set_context(static_cast(context)); + set_error_addr(error_addr); + CONTEXT localCtx; // used when no context is provided + + if (depth == 0) { + return 0; + } + + if (!ctx_) { + ctx_ = &localCtx; + RtlCaptureContext(ctx_); + } + + if (!thd_) { + thd_ = GetCurrentThread(); + } + + HANDLE process = GetCurrentProcess(); + + STACKFRAME64 s; + memset(&s, 0, sizeof(STACKFRAME64)); + + // TODO: 32 bit context capture + s.AddrStack.Mode = AddrModeFlat; + s.AddrFrame.Mode = AddrModeFlat; + s.AddrPC.Mode = AddrModeFlat; +#ifdef _M_X64 + s.AddrPC.Offset = ctx_->Rip; + s.AddrStack.Offset = ctx_->Rsp; + s.AddrFrame.Offset = ctx_->Rbp; +#else + s.AddrPC.Offset = ctx_->Eip; + s.AddrStack.Offset = ctx_->Esp; + s.AddrFrame.Offset = ctx_->Ebp; +#endif + + if (!machine_type_) { +#ifdef _M_X64 + machine_type_ = IMAGE_FILE_MACHINE_AMD64; +#else + machine_type_ = IMAGE_FILE_MACHINE_I386; +#endif + } + + for (;;) { + // NOTE: this only works if PDBs are already loaded! + SetLastError(0); + if (!StackWalk64(machine_type_, process, thd_, &s, ctx_, NULL, + SymFunctionTableAccess64, SymGetModuleBase64, NULL)) + break; + + if (s.AddrReturn.Offset == 0) + break; + + _stacktrace.push_back(reinterpret_cast(s.AddrPC.Offset)); + + if (size() >= depth) + break; + } + + return size(); + } + + size_t load_from(void *addr, size_t depth = 32, void *context = nullptr, + void *error_addr = nullptr) { + load_here(depth + 8, context, error_addr); + + for (size_t i = 0; i < _stacktrace.size(); ++i) { + if (_stacktrace[i] == addr) { + skip_n_firsts(i); + break; + } + } + + _stacktrace.resize(std::min(_stacktrace.size(), skip_n_firsts() + depth)); + return size(); + } + +private: + DWORD machine_type_ = 0; + HANDLE thd_ = 0; + CONTEXT *ctx_ = nullptr; +}; + +#endif + +class StackTrace : public StackTraceImpl {}; + +/*************** TRACE RESOLVER ***************/ + +class TraceResolverImplBase { +public: + virtual ~TraceResolverImplBase() {} + + virtual void load_addresses(void *const*addresses, int address_count) { + (void)addresses; + (void)address_count; + } + + template void load_stacktrace(ST &st) { + load_addresses(st.begin(), (int)st.size()); + } + + virtual ResolvedTrace resolve(ResolvedTrace t) { return t; } + +protected: + std::string demangle(const char *funcname) { + return _demangler.demangle(funcname); + } + +private: + details::demangler _demangler; +}; + +template class TraceResolverImpl; + +#ifdef BACKWARD_SYSTEM_UNKNOWN + +template <> class TraceResolverImpl + : public TraceResolverImplBase {}; + +#endif + +#ifdef BACKWARD_SYSTEM_LINUX + +class TraceResolverLinuxBase : public TraceResolverImplBase { +public: + TraceResolverLinuxBase() + : argv0_(get_argv0()), exec_path_(read_symlink("/proc/self/exe")) {} + std::string resolve_exec_path(Dl_info &symbol_info) const { + // mutates symbol_info.dli_fname to be filename to open and returns filename + // to display + if (symbol_info.dli_fname == argv0_) { + // dladdr returns argv[0] in dli_fname for symbols contained in + // the main executable, which is not a valid path if the + // executable was found by a search of the PATH environment + // variable; In that case, we actually open /proc/self/exe, which + // is always the actual executable (even if it was deleted/replaced!) + // but display the path that /proc/self/exe links to. + // However, this right away reduces probability of successful symbol + // resolution, because libbfd may try to find *.debug files in the + // same dir, in case symbols are stripped. As a result, it may try + // to find a file /proc/self/.debug, which obviously does + // not exist. /proc/self/exe is a last resort. First load attempt + // should go for the original executable file path. + symbol_info.dli_fname = "/proc/self/exe"; + return exec_path_; + } else { + return symbol_info.dli_fname; + } + } + +private: + std::string argv0_; + std::string exec_path_; + + static std::string get_argv0() { + std::string argv0; + std::ifstream ifs("/proc/self/cmdline"); + std::getline(ifs, argv0, '\0'); + return argv0; + } + + static std::string read_symlink(std::string const &symlink_path) { + std::string path; + path.resize(100); + + while (true) { + ssize_t len = + ::readlink(symlink_path.c_str(), &*path.begin(), path.size()); + if (len < 0) { + return ""; + } + if (static_cast(len) == path.size()) { + path.resize(path.size() * 2); + } else { + path.resize(static_cast(len)); + break; + } + } + + return path; + } +}; + +template class TraceResolverLinuxImpl; + +#if BACKWARD_HAS_BACKTRACE_SYMBOL == 1 + +template <> +class TraceResolverLinuxImpl + : public TraceResolverLinuxBase { +public: + void load_addresses(void *const*addresses, int address_count) override { + if (address_count == 0) { + return; + } + _symbols.reset(backtrace_symbols(addresses, address_count)); + } + + ResolvedTrace resolve(ResolvedTrace trace) override { + char *filename = _symbols[trace.idx]; + char *funcname = filename; + while (*funcname && *funcname != '(') { + funcname += 1; + } + trace.object_filename.assign(filename, + funcname); // ok even if funcname is the ending + // \0 (then we assign entire string) + + if (*funcname) { // if it's not end of string (e.g. from last frame ip==0) + funcname += 1; + char *funcname_end = funcname; + while (*funcname_end && *funcname_end != ')' && *funcname_end != '+') { + funcname_end += 1; + } + *funcname_end = '\0'; + trace.object_function = this->demangle(funcname); + trace.source.function = trace.object_function; // we cannot do better. + } + return trace; + } + +private: + details::handle _symbols; +}; + +#endif // BACKWARD_HAS_BACKTRACE_SYMBOL == 1 + +#if BACKWARD_HAS_BFD == 1 + +template <> +class TraceResolverLinuxImpl + : public TraceResolverLinuxBase { +public: + TraceResolverLinuxImpl() : _bfd_loaded(false) {} + + ResolvedTrace resolve(ResolvedTrace trace) override { + Dl_info symbol_info; + + // trace.addr is a virtual address in memory pointing to some code. + // Let's try to find from which loaded object it comes from. + // The loaded object can be yourself btw. + if (!dladdr(trace.addr, &symbol_info)) { + return trace; // dat broken trace... + } + + // Now we get in symbol_info: + // .dli_fname: + // pathname of the shared object that contains the address. + // .dli_fbase: + // where the object is loaded in memory. + // .dli_sname: + // the name of the nearest symbol to trace.addr, we expect a + // function name. + // .dli_saddr: + // the exact address corresponding to .dli_sname. + + if (symbol_info.dli_sname) { + trace.object_function = demangle(symbol_info.dli_sname); + } + + if (!symbol_info.dli_fname) { + return trace; + } + + trace.object_filename = resolve_exec_path(symbol_info); + bfd_fileobject *fobj; + // Before rushing to resolution need to ensure the executable + // file still can be used. For that compare inode numbers of + // what is stored by the executable's file path, and in the + // dli_fname, which not necessarily equals to the executable. + // It can be a shared library, or /proc/self/exe, and in the + // latter case has drawbacks. See the exec path resolution for + // details. In short - the dli object should be used only as + // the last resort. + // If inode numbers are equal, it is known dli_fname and the + // executable file are the same. This is guaranteed by Linux, + // because if the executable file is changed/deleted, it will + // be done in a new inode. The old file will be preserved in + // /proc/self/exe, and may even have inode 0. The latter can + // happen if the inode was actually reused, and the file was + // kept only in the main memory. + // + struct stat obj_stat; + struct stat dli_stat; + if (stat(trace.object_filename.c_str(), &obj_stat) == 0 && + stat(symbol_info.dli_fname, &dli_stat) == 0 && + obj_stat.st_ino == dli_stat.st_ino) { + // The executable file, and the shared object containing the + // address are the same file. Safe to use the original path. + // this is preferable. Libbfd will search for stripped debug + // symbols in the same directory. + fobj = load_object_with_bfd(trace.object_filename); + } else{ + // The original object file was *deleted*! The only hope is + // that the debug symbols are either inside the shared + // object file, or are in the same directory, and this is + // not /proc/self/exe. + fobj = nullptr; + } + if (fobj == nullptr || !fobj->handle) { + fobj = load_object_with_bfd(symbol_info.dli_fname); + if (!fobj->handle) { + return trace; + } + } + + find_sym_result *details_selected; // to be filled. + + // trace.addr is the next instruction to be executed after returning + // from the nested stack frame. In C++ this usually relate to the next + // statement right after the function call that leaded to a new stack + // frame. This is not usually what you want to see when printing out a + // stacktrace... + find_sym_result details_call_site = + find_symbol_details(fobj, trace.addr, symbol_info.dli_fbase); + details_selected = &details_call_site; + +#if BACKWARD_HAS_UNWIND == 0 + // ...this is why we also try to resolve the symbol that is right + // before the return address. If we are lucky enough, we will get the + // line of the function that was called. But if the code is optimized, + // we might get something absolutely not related since the compiler + // can reschedule the return address with inline functions and + // tail-call optimisation (among other things that I don't even know + // or cannot even dream about with my tiny limited brain). + find_sym_result details_adjusted_call_site = find_symbol_details( + fobj, (void *)(uintptr_t(trace.addr) - 1), symbol_info.dli_fbase); + + // In debug mode, we should always get the right thing(TM). + if (details_call_site.found && details_adjusted_call_site.found) { + // Ok, we assume that details_adjusted_call_site is a better estimation. + details_selected = &details_adjusted_call_site; + trace.addr = (void *)(uintptr_t(trace.addr) - 1); + } + + if (details_selected == &details_call_site && details_call_site.found) { + // we have to re-resolve the symbol in order to reset some + // internal state in BFD... so we can call backtrace_inliners + // thereafter... + details_call_site = + find_symbol_details(fobj, trace.addr, symbol_info.dli_fbase); + } +#endif // BACKWARD_HAS_UNWIND + + if (details_selected->found) { + if (details_selected->filename) { + trace.source.filename = details_selected->filename; + } + trace.source.line = details_selected->line; + + if (details_selected->funcname) { + // this time we get the name of the function where the code is + // located, instead of the function were the address is + // located. In short, if the code was inlined, we get the + // function correspoding to the code. Else we already got in + // trace.function. + trace.source.function = demangle(details_selected->funcname); + + if (!symbol_info.dli_sname) { + // for the case dladdr failed to find the symbol name of + // the function, we might as well try to put something + // here. + trace.object_function = trace.source.function; + } + } + + // Maybe the source of the trace got inlined inside the function + // (trace.source.function). Let's see if we can get all the inlined + // calls along the way up to the initial call site. + trace.inliners = backtrace_inliners(fobj, *details_selected); + +#if 0 + if (trace.inliners.size() == 0) { + // Maybe the trace was not inlined... or maybe it was and we + // are lacking the debug information. Let's try to make the + // world better and see if we can get the line number of the + // function (trace.source.function) now. + // + // We will get the location of where the function start (to be + // exact: the first instruction that really start the + // function), not where the name of the function is defined. + // This can be quite far away from the name of the function + // btw. + // + // If the source of the function is the same as the source of + // the trace, we cannot say if the trace was really inlined or + // not. However, if the filename of the source is different + // between the function and the trace... we can declare it as + // an inliner. This is not 100% accurate, but better than + // nothing. + + if (symbol_info.dli_saddr) { + find_sym_result details = find_symbol_details(fobj, + symbol_info.dli_saddr, + symbol_info.dli_fbase); + + if (details.found) { + ResolvedTrace::SourceLoc diy_inliner; + diy_inliner.line = details.line; + if (details.filename) { + diy_inliner.filename = details.filename; + } + if (details.funcname) { + diy_inliner.function = demangle(details.funcname); + } else { + diy_inliner.function = trace.source.function; + } + if (diy_inliner != trace.source) { + trace.inliners.push_back(diy_inliner); + } + } + } + } +#endif + } + + return trace; + } + +private: + bool _bfd_loaded; + + typedef details::handle> + bfd_handle_t; + + typedef details::handle bfd_symtab_t; + + struct bfd_fileobject { + bfd_handle_t handle; + bfd_vma base_addr; + bfd_symtab_t symtab; + bfd_symtab_t dynamic_symtab; + }; + + typedef details::hashtable::type fobj_bfd_map_t; + fobj_bfd_map_t _fobj_bfd_map; + + bfd_fileobject *load_object_with_bfd(const std::string &filename_object) { + using namespace details; + + if (!_bfd_loaded) { + using namespace details; + bfd_init(); + _bfd_loaded = true; + } + + fobj_bfd_map_t::iterator it = _fobj_bfd_map.find(filename_object); + if (it != _fobj_bfd_map.end()) { + return &it->second; + } + + // this new object is empty for now. + bfd_fileobject *r = &_fobj_bfd_map[filename_object]; + + // we do the work temporary in this one; + bfd_handle_t bfd_handle; + + int fd = open(filename_object.c_str(), O_RDONLY); + bfd_handle.reset(bfd_fdopenr(filename_object.c_str(), "default", fd)); + if (!bfd_handle) { + close(fd); + return r; + } + + if (!bfd_check_format(bfd_handle.get(), bfd_object)) { + return r; // not an object? You lose. + } + + if ((bfd_get_file_flags(bfd_handle.get()) & HAS_SYMS) == 0) { + return r; // that's what happen when you forget to compile in debug. + } + + ssize_t symtab_storage_size = bfd_get_symtab_upper_bound(bfd_handle.get()); + + ssize_t dyn_symtab_storage_size = + bfd_get_dynamic_symtab_upper_bound(bfd_handle.get()); + + if (symtab_storage_size <= 0 && dyn_symtab_storage_size <= 0) { + return r; // weird, is the file is corrupted? + } + + bfd_symtab_t symtab, dynamic_symtab; + ssize_t symcount = 0, dyn_symcount = 0; + + if (symtab_storage_size > 0) { + symtab.reset(static_cast( + malloc(static_cast(symtab_storage_size)))); + symcount = bfd_canonicalize_symtab(bfd_handle.get(), symtab.get()); + } + + if (dyn_symtab_storage_size > 0) { + dynamic_symtab.reset(static_cast( + malloc(static_cast(dyn_symtab_storage_size)))); + dyn_symcount = bfd_canonicalize_dynamic_symtab(bfd_handle.get(), + dynamic_symtab.get()); + } + + if (symcount <= 0 && dyn_symcount <= 0) { + return r; // damned, that's a stripped file that you got there! + } + + r->handle = move(bfd_handle); + r->symtab = move(symtab); + r->dynamic_symtab = move(dynamic_symtab); + return r; + } + + struct find_sym_result { + bool found; + const char *filename; + const char *funcname; + unsigned int line; + }; + + struct find_sym_context { + TraceResolverLinuxImpl *self; + bfd_fileobject *fobj; + void *addr; + void *base_addr; + find_sym_result result; + }; + + find_sym_result find_symbol_details(bfd_fileobject *fobj, void *addr, + void *base_addr) { + find_sym_context context; + context.self = this; + context.fobj = fobj; + context.addr = addr; + context.base_addr = base_addr; + context.result.found = false; + bfd_map_over_sections(fobj->handle.get(), &find_in_section_trampoline, + static_cast(&context)); + return context.result; + } + + static void find_in_section_trampoline(bfd *, asection *section, void *data) { + find_sym_context *context = static_cast(data); + context->self->find_in_section( + reinterpret_cast(context->addr), + reinterpret_cast(context->base_addr), context->fobj, section, + context->result); + } + + void find_in_section(bfd_vma addr, bfd_vma base_addr, bfd_fileobject *fobj, + asection *section, find_sym_result &result) { + if (result.found) + return; + +#ifdef bfd_get_section_flags + if ((bfd_get_section_flags(fobj->handle.get(), section) & SEC_ALLOC) == 0) +#else + if ((bfd_section_flags(section) & SEC_ALLOC) == 0) +#endif + return; // a debug section is never loaded automatically. + +#ifdef bfd_get_section_vma + bfd_vma sec_addr = bfd_get_section_vma(fobj->handle.get(), section); +#else + bfd_vma sec_addr = bfd_section_vma(section); +#endif +#ifdef bfd_get_section_size + bfd_size_type size = bfd_get_section_size(section); +#else + bfd_size_type size = bfd_section_size(section); +#endif + + // are we in the boundaries of the section? + if (addr < sec_addr || addr >= sec_addr + size) { + addr -= base_addr; // oups, a relocated object, lets try again... + if (addr < sec_addr || addr >= sec_addr + size) { + return; + } + } + +#if defined(__clang__) +#pragma clang diagnostic push +#pragma clang diagnostic ignored "-Wzero-as-null-pointer-constant" +#endif + if (!result.found && fobj->symtab) { + result.found = bfd_find_nearest_line( + fobj->handle.get(), section, fobj->symtab.get(), addr - sec_addr, + &result.filename, &result.funcname, &result.line); + } + + if (!result.found && fobj->dynamic_symtab) { + result.found = bfd_find_nearest_line( + fobj->handle.get(), section, fobj->dynamic_symtab.get(), + addr - sec_addr, &result.filename, &result.funcname, &result.line); + } +#if defined(__clang__) +#pragma clang diagnostic pop +#endif + } + + ResolvedTrace::source_locs_t + backtrace_inliners(bfd_fileobject *fobj, find_sym_result previous_result) { + // This function can be called ONLY after a SUCCESSFUL call to + // find_symbol_details. The state is global to the bfd_handle. + ResolvedTrace::source_locs_t results; + while (previous_result.found) { + find_sym_result result; + result.found = bfd_find_inliner_info(fobj->handle.get(), &result.filename, + &result.funcname, &result.line); + + if (result + .found) /* and not ( + cstrings_eq(previous_result.filename, + result.filename) and + cstrings_eq(previous_result.funcname, result.funcname) + and result.line == previous_result.line + )) */ + { + ResolvedTrace::SourceLoc src_loc; + src_loc.line = result.line; + if (result.filename) { + src_loc.filename = result.filename; + } + if (result.funcname) { + src_loc.function = demangle(result.funcname); + } + results.push_back(src_loc); + } + previous_result = result; + } + return results; + } + + bool cstrings_eq(const char *a, const char *b) { + if (!a || !b) { + return false; + } + return strcmp(a, b) == 0; + } +}; +#endif // BACKWARD_HAS_BFD == 1 + +#if BACKWARD_HAS_DW == 1 + +template <> +class TraceResolverLinuxImpl + : public TraceResolverLinuxBase { +public: + TraceResolverLinuxImpl() : _dwfl_handle_initialized(false) {} + + ResolvedTrace resolve(ResolvedTrace trace) override { + using namespace details; + + Dwarf_Addr trace_addr = (Dwarf_Addr)trace.addr; + + if (!_dwfl_handle_initialized) { + // initialize dwfl... + _dwfl_cb.reset(new Dwfl_Callbacks); + _dwfl_cb->find_elf = &dwfl_linux_proc_find_elf; + _dwfl_cb->find_debuginfo = &dwfl_standard_find_debuginfo; + _dwfl_cb->debuginfo_path = 0; + + _dwfl_handle.reset(dwfl_begin(_dwfl_cb.get())); + _dwfl_handle_initialized = true; + + if (!_dwfl_handle) { + return trace; + } + + // ...from the current process. + dwfl_report_begin(_dwfl_handle.get()); + int r = dwfl_linux_proc_report(_dwfl_handle.get(), getpid()); + dwfl_report_end(_dwfl_handle.get(), NULL, NULL); + if (r < 0) { + return trace; + } + } + + if (!_dwfl_handle) { + return trace; + } + + // find the module (binary object) that contains the trace's address. + // This is not using any debug information, but the addresses ranges of + // all the currently loaded binary object. + Dwfl_Module *mod = dwfl_addrmodule(_dwfl_handle.get(), trace_addr); + if (mod) { + // now that we found it, lets get the name of it, this will be the + // full path to the running binary or one of the loaded library. + const char *module_name = dwfl_module_info(mod, 0, 0, 0, 0, 0, 0, 0); + if (module_name) { + trace.object_filename = module_name; + } + // We also look after the name of the symbol, equal or before this + // address. This is found by walking the symtab. We should get the + // symbol corresponding to the function (mangled) containing the + // address. If the code corresponding to the address was inlined, + // this is the name of the out-most inliner function. + const char *sym_name = dwfl_module_addrname(mod, trace_addr); + if (sym_name) { + trace.object_function = demangle(sym_name); + } + } + + // now let's get serious, and find out the source location (file and + // line number) of the address. + + // This function will look in .debug_aranges for the address and map it + // to the location of the compilation unit DIE in .debug_info and + // return it. + Dwarf_Addr mod_bias = 0; + Dwarf_Die *cudie = dwfl_module_addrdie(mod, trace_addr, &mod_bias); + +#if 1 + if (!cudie) { + // Sadly clang does not generate the section .debug_aranges, thus + // dwfl_module_addrdie will fail early. Clang doesn't either set + // the lowpc/highpc/range info for every compilation unit. + // + // So in order to save the world: + // for every compilation unit, we will iterate over every single + // DIEs. Normally functions should have a lowpc/highpc/range, which + // we will use to infer the compilation unit. + + // note that this is probably badly inefficient. + while ((cudie = dwfl_module_nextcu(mod, cudie, &mod_bias))) { + Dwarf_Die die_mem; + Dwarf_Die *fundie = + find_fundie_by_pc(cudie, trace_addr - mod_bias, &die_mem); + if (fundie) { + break; + } + } + } +#endif + +//#define BACKWARD_I_DO_NOT_RECOMMEND_TO_ENABLE_THIS_HORRIBLE_PIECE_OF_CODE +#ifdef BACKWARD_I_DO_NOT_RECOMMEND_TO_ENABLE_THIS_HORRIBLE_PIECE_OF_CODE + if (!cudie) { + // If it's still not enough, lets dive deeper in the shit, and try + // to save the world again: for every compilation unit, we will + // load the corresponding .debug_line section, and see if we can + // find our address in it. + + Dwarf_Addr cfi_bias; + Dwarf_CFI *cfi_cache = dwfl_module_eh_cfi(mod, &cfi_bias); + + Dwarf_Addr bias; + while ((cudie = dwfl_module_nextcu(mod, cudie, &bias))) { + if (dwarf_getsrc_die(cudie, trace_addr - bias)) { + + // ...but if we get a match, it might be a false positive + // because our (address - bias) might as well be valid in a + // different compilation unit. So we throw our last card on + // the table and lookup for the address into the .eh_frame + // section. + + handle frame; + dwarf_cfi_addrframe(cfi_cache, trace_addr - cfi_bias, &frame); + if (frame) { + break; + } + } + } + } +#endif + + if (!cudie) { + return trace; // this time we lost the game :/ + } + + // Now that we have a compilation unit DIE, this function will be able + // to load the corresponding section in .debug_line (if not already + // loaded) and hopefully find the source location mapped to our + // address. + Dwarf_Line *srcloc = dwarf_getsrc_die(cudie, trace_addr - mod_bias); + + if (srcloc) { + const char *srcfile = dwarf_linesrc(srcloc, 0, 0); + if (srcfile) { + trace.source.filename = srcfile; + } + int line = 0, col = 0; + dwarf_lineno(srcloc, &line); + dwarf_linecol(srcloc, &col); + trace.source.line = line; + trace.source.col = col; + } + + deep_first_search_by_pc(cudie, trace_addr - mod_bias, + inliners_search_cb(trace)); + if (trace.source.function.size() == 0) { + // fallback. + trace.source.function = trace.object_function; + } + + return trace; + } + +private: + typedef details::handle> + dwfl_handle_t; + details::handle> + _dwfl_cb; + dwfl_handle_t _dwfl_handle; + bool _dwfl_handle_initialized; + + // defined here because in C++98, template function cannot take locally + // defined types... grrr. + struct inliners_search_cb { + void operator()(Dwarf_Die *die) { + switch (dwarf_tag(die)) { + const char *name; + case DW_TAG_subprogram: + if ((name = dwarf_diename(die))) { + trace.source.function = name; + } + break; + + case DW_TAG_inlined_subroutine: + ResolvedTrace::SourceLoc sloc; + Dwarf_Attribute attr_mem; + + if ((name = dwarf_diename(die))) { + sloc.function = name; + } + if ((name = die_call_file(die))) { + sloc.filename = name; + } + + Dwarf_Word line = 0, col = 0; + dwarf_formudata(dwarf_attr(die, DW_AT_call_line, &attr_mem), &line); + dwarf_formudata(dwarf_attr(die, DW_AT_call_column, &attr_mem), &col); + sloc.line = (unsigned)line; + sloc.col = (unsigned)col; + + trace.inliners.push_back(sloc); + break; + }; + } + ResolvedTrace &trace; + inliners_search_cb(ResolvedTrace &t) : trace(t) {} + }; + + static bool die_has_pc(Dwarf_Die *die, Dwarf_Addr pc) { + Dwarf_Addr low, high; + + // continuous range + if (dwarf_hasattr(die, DW_AT_low_pc) && dwarf_hasattr(die, DW_AT_high_pc)) { + if (dwarf_lowpc(die, &low) != 0) { + return false; + } + if (dwarf_highpc(die, &high) != 0) { + Dwarf_Attribute attr_mem; + Dwarf_Attribute *attr = dwarf_attr(die, DW_AT_high_pc, &attr_mem); + Dwarf_Word value; + if (dwarf_formudata(attr, &value) != 0) { + return false; + } + high = low + value; + } + return pc >= low && pc < high; + } + + // non-continuous range. + Dwarf_Addr base; + ptrdiff_t offset = 0; + while ((offset = dwarf_ranges(die, offset, &base, &low, &high)) > 0) { + if (pc >= low && pc < high) { + return true; + } + } + return false; + } + + static Dwarf_Die *find_fundie_by_pc(Dwarf_Die *parent_die, Dwarf_Addr pc, + Dwarf_Die *result) { + if (dwarf_child(parent_die, result) != 0) { + return 0; + } + + Dwarf_Die *die = result; + do { + switch (dwarf_tag(die)) { + case DW_TAG_subprogram: + case DW_TAG_inlined_subroutine: + if (die_has_pc(die, pc)) { + return result; + } + }; + bool declaration = false; + Dwarf_Attribute attr_mem; + dwarf_formflag(dwarf_attr(die, DW_AT_declaration, &attr_mem), + &declaration); + if (!declaration) { + // let's be curious and look deeper in the tree, + // function are not necessarily at the first level, but + // might be nested inside a namespace, structure etc. + Dwarf_Die die_mem; + Dwarf_Die *indie = find_fundie_by_pc(die, pc, &die_mem); + if (indie) { + *result = die_mem; + return result; + } + } + } while (dwarf_siblingof(die, result) == 0); + return 0; + } + + template + static bool deep_first_search_by_pc(Dwarf_Die *parent_die, Dwarf_Addr pc, + CB cb) { + Dwarf_Die die_mem; + if (dwarf_child(parent_die, &die_mem) != 0) { + return false; + } + + bool branch_has_pc = false; + Dwarf_Die *die = &die_mem; + do { + bool declaration = false; + Dwarf_Attribute attr_mem; + dwarf_formflag(dwarf_attr(die, DW_AT_declaration, &attr_mem), + &declaration); + if (!declaration) { + // let's be curious and look deeper in the tree, function are + // not necessarily at the first level, but might be nested + // inside a namespace, structure, a function, an inlined + // function etc. + branch_has_pc = deep_first_search_by_pc(die, pc, cb); + } + if (!branch_has_pc) { + branch_has_pc = die_has_pc(die, pc); + } + if (branch_has_pc) { + cb(die); + } + } while (dwarf_siblingof(die, &die_mem) == 0); + return branch_has_pc; + } + + static const char *die_call_file(Dwarf_Die *die) { + Dwarf_Attribute attr_mem; + Dwarf_Word file_idx = 0; + + dwarf_formudata(dwarf_attr(die, DW_AT_call_file, &attr_mem), &file_idx); + + if (file_idx == 0) { + return 0; + } + + Dwarf_Die die_mem; + Dwarf_Die *cudie = dwarf_diecu(die, &die_mem, 0, 0); + if (!cudie) { + return 0; + } + + Dwarf_Files *files = 0; + size_t nfiles; + dwarf_getsrcfiles(cudie, &files, &nfiles); + if (!files) { + return 0; + } + + return dwarf_filesrc(files, file_idx, 0, 0); + } +}; +#endif // BACKWARD_HAS_DW == 1 + +#if BACKWARD_HAS_DWARF == 1 + +template <> +class TraceResolverLinuxImpl + : public TraceResolverLinuxBase { +public: + TraceResolverLinuxImpl() : _dwarf_loaded(false) {} + + ResolvedTrace resolve(ResolvedTrace trace) override { + // trace.addr is a virtual address in memory pointing to some code. + // Let's try to find from which loaded object it comes from. + // The loaded object can be yourself btw. + + Dl_info symbol_info; + int dladdr_result = 0; +#if defined(__GLIBC__) + link_map *link_map; + // We request the link map so we can get information about offsets + dladdr_result = + dladdr1(trace.addr, &symbol_info, reinterpret_cast(&link_map), + RTLD_DL_LINKMAP); +#else + // Android doesn't have dladdr1. Don't use the linker map. + dladdr_result = dladdr(trace.addr, &symbol_info); +#endif + if (!dladdr_result) { + return trace; // dat broken trace... + } + + // Now we get in symbol_info: + // .dli_fname: + // pathname of the shared object that contains the address. + // .dli_fbase: + // where the object is loaded in memory. + // .dli_sname: + // the name of the nearest symbol to trace.addr, we expect a + // function name. + // .dli_saddr: + // the exact address corresponding to .dli_sname. + // + // And in link_map: + // .l_addr: + // difference between the address in the ELF file and the address + // in memory + // l_name: + // absolute pathname where the object was found + + if (symbol_info.dli_sname) { + trace.object_function = demangle(symbol_info.dli_sname); + } + + if (!symbol_info.dli_fname) { + return trace; + } + + trace.object_filename = resolve_exec_path(symbol_info); + dwarf_fileobject &fobj = load_object_with_dwarf(symbol_info.dli_fname); + if (!fobj.dwarf_handle) { + return trace; // sad, we couldn't load the object :( + } + +#if defined(__GLIBC__) + // Convert the address to a module relative one by looking at + // the module's loading address in the link map + Dwarf_Addr address = reinterpret_cast(trace.addr) - + reinterpret_cast(link_map->l_addr); +#else + Dwarf_Addr address = reinterpret_cast(trace.addr); +#endif + + if (trace.object_function.empty()) { + symbol_cache_t::iterator it = fobj.symbol_cache.lower_bound(address); + + if (it != fobj.symbol_cache.end()) { + if (it->first != address) { + if (it != fobj.symbol_cache.begin()) { + --it; + } + } + trace.object_function = demangle(it->second.c_str()); + } + } + + // Get the Compilation Unit DIE for the address + Dwarf_Die die = find_die(fobj, address); + + if (!die) { + return trace; // this time we lost the game :/ + } + + // libdwarf doesn't give us direct access to its objects, it always + // allocates a copy for the caller. We keep that copy alive in a cache + // and we deallocate it later when it's no longer required. + die_cache_entry &die_object = get_die_cache(fobj, die); + if (die_object.isEmpty()) + return trace; // We have no line section for this DIE + + die_linemap_t::iterator it = die_object.line_section.lower_bound(address); + + if (it != die_object.line_section.end()) { + if (it->first != address) { + if (it == die_object.line_section.begin()) { + // If we are on the first item of the line section + // but the address does not match it means that + // the address is below the range of the DIE. Give up. + return trace; + } else { + --it; + } + } + } else { + return trace; // We didn't find the address. + } + + // Get the Dwarf_Line that the address points to and call libdwarf + // to get source file, line and column info. + Dwarf_Line line = die_object.line_buffer[it->second]; + Dwarf_Error error = DW_DLE_NE; + + char *filename; + if (dwarf_linesrc(line, &filename, &error) == DW_DLV_OK) { + trace.source.filename = std::string(filename); + dwarf_dealloc(fobj.dwarf_handle.get(), filename, DW_DLA_STRING); + } + + Dwarf_Unsigned number = 0; + if (dwarf_lineno(line, &number, &error) == DW_DLV_OK) { + trace.source.line = number; + } else { + trace.source.line = 0; + } + + if (dwarf_lineoff_b(line, &number, &error) == DW_DLV_OK) { + trace.source.col = number; + } else { + trace.source.col = 0; + } + + std::vector namespace_stack; + deep_first_search_by_pc(fobj, die, address, namespace_stack, + inliners_search_cb(trace, fobj, die)); + + dwarf_dealloc(fobj.dwarf_handle.get(), die, DW_DLA_DIE); + + return trace; + } + +public: + static int close_dwarf(Dwarf_Debug dwarf) { + return dwarf_finish(dwarf, NULL); + } + +private: + bool _dwarf_loaded; + + typedef details::handle> + dwarf_file_t; + + typedef details::handle> + dwarf_elf_t; + + typedef details::handle> + dwarf_handle_t; + + typedef std::map die_linemap_t; + + typedef std::map die_specmap_t; + + struct die_cache_entry { + die_specmap_t spec_section; + die_linemap_t line_section; + Dwarf_Line *line_buffer; + Dwarf_Signed line_count; + Dwarf_Line_Context line_context; + + inline bool isEmpty() { + return line_buffer == NULL || line_count == 0 || line_context == NULL || + line_section.empty(); + } + + die_cache_entry() : line_buffer(0), line_count(0), line_context(0) {} + + ~die_cache_entry() { + if (line_context) { + dwarf_srclines_dealloc_b(line_context); + } + } + }; + + typedef std::map die_cache_t; + + typedef std::map symbol_cache_t; + + struct dwarf_fileobject { + dwarf_file_t file_handle; + dwarf_elf_t elf_handle; + dwarf_handle_t dwarf_handle; + symbol_cache_t symbol_cache; + + // Die cache + die_cache_t die_cache; + die_cache_entry *current_cu; + }; + + typedef details::hashtable::type + fobj_dwarf_map_t; + fobj_dwarf_map_t _fobj_dwarf_map; + + static bool cstrings_eq(const char *a, const char *b) { + if (!a || !b) { + return false; + } + return strcmp(a, b) == 0; + } + + dwarf_fileobject &load_object_with_dwarf(const std::string &filename_object) { + + if (!_dwarf_loaded) { + // Set the ELF library operating version + // If that fails there's nothing we can do + _dwarf_loaded = elf_version(EV_CURRENT) != EV_NONE; + } + + fobj_dwarf_map_t::iterator it = _fobj_dwarf_map.find(filename_object); + if (it != _fobj_dwarf_map.end()) { + return it->second; + } + + // this new object is empty for now + dwarf_fileobject &r = _fobj_dwarf_map[filename_object]; + + dwarf_file_t file_handle; + file_handle.reset(open(filename_object.c_str(), O_RDONLY)); + if (file_handle.get() < 0) { + return r; + } + + // Try to get an ELF handle. We need to read the ELF sections + // because we want to see if there is a .gnu_debuglink section + // that points to a split debug file + dwarf_elf_t elf_handle; + elf_handle.reset(elf_begin(file_handle.get(), ELF_C_READ, NULL)); + if (!elf_handle) { + return r; + } + + const char *e_ident = elf_getident(elf_handle.get(), 0); + if (!e_ident) { + return r; + } + + // Get the number of sections + // We use the new APIs as elf_getshnum is deprecated + size_t shdrnum = 0; + if (elf_getshdrnum(elf_handle.get(), &shdrnum) == -1) { + return r; + } + + // Get the index to the string section + size_t shdrstrndx = 0; + if (elf_getshdrstrndx(elf_handle.get(), &shdrstrndx) == -1) { + return r; + } + + std::string debuglink; + // Iterate through the ELF sections to try to get a gnu_debuglink + // note and also to cache the symbol table. + // We go the preprocessor way to avoid having to create templated + // classes or using gelf (which might throw a compiler error if 64 bit + // is not supported +#define ELF_GET_DATA(ARCH) \ + Elf_Scn *elf_section = 0; \ + Elf_Data *elf_data = 0; \ + Elf##ARCH##_Shdr *section_header = 0; \ + Elf_Scn *symbol_section = 0; \ + size_t symbol_count = 0; \ + size_t symbol_strings = 0; \ + Elf##ARCH##_Sym *symbol = 0; \ + const char *section_name = 0; \ + \ + while ((elf_section = elf_nextscn(elf_handle.get(), elf_section)) != NULL) { \ + section_header = elf##ARCH##_getshdr(elf_section); \ + if (section_header == NULL) { \ + return r; \ + } \ + \ + if ((section_name = elf_strptr(elf_handle.get(), shdrstrndx, \ + section_header->sh_name)) == NULL) { \ + return r; \ + } \ + \ + if (cstrings_eq(section_name, ".gnu_debuglink")) { \ + elf_data = elf_getdata(elf_section, NULL); \ + if (elf_data && elf_data->d_size > 0) { \ + debuglink = \ + std::string(reinterpret_cast(elf_data->d_buf)); \ + } \ + } \ + \ + switch (section_header->sh_type) { \ + case SHT_SYMTAB: \ + symbol_section = elf_section; \ + symbol_count = section_header->sh_size / section_header->sh_entsize; \ + symbol_strings = section_header->sh_link; \ + break; \ + \ + /* We use .dynsyms as a last resort, we prefer .symtab */ \ + case SHT_DYNSYM: \ + if (!symbol_section) { \ + symbol_section = elf_section; \ + symbol_count = section_header->sh_size / section_header->sh_entsize; \ + symbol_strings = section_header->sh_link; \ + } \ + break; \ + } \ + } \ + \ + if (symbol_section && symbol_count && symbol_strings) { \ + elf_data = elf_getdata(symbol_section, NULL); \ + symbol = reinterpret_cast(elf_data->d_buf); \ + for (size_t i = 0; i < symbol_count; ++i) { \ + int type = ELF##ARCH##_ST_TYPE(symbol->st_info); \ + if (type == STT_FUNC && symbol->st_value > 0) { \ + r.symbol_cache[symbol->st_value] = std::string( \ + elf_strptr(elf_handle.get(), symbol_strings, symbol->st_name)); \ + } \ + ++symbol; \ + } \ + } + + if (e_ident[EI_CLASS] == ELFCLASS32) { + ELF_GET_DATA(32) + } else if (e_ident[EI_CLASS] == ELFCLASS64) { + // libelf might have been built without 64 bit support +#if __LIBELF64 + ELF_GET_DATA(64) +#endif + } + + if (!debuglink.empty()) { + // We have a debuglink section! Open an elf instance on that + // file instead. If we can't open the file, then return + // the elf handle we had already opened. + dwarf_file_t debuglink_file; + debuglink_file.reset(open(debuglink.c_str(), O_RDONLY)); + if (debuglink_file.get() > 0) { + dwarf_elf_t debuglink_elf; + debuglink_elf.reset(elf_begin(debuglink_file.get(), ELF_C_READ, NULL)); + + // If we have a valid elf handle, return the new elf handle + // and file handle and discard the original ones + if (debuglink_elf) { + elf_handle = move(debuglink_elf); + file_handle = move(debuglink_file); + } + } + } + + // Ok, we have a valid ELF handle, let's try to get debug symbols + Dwarf_Debug dwarf_debug; + Dwarf_Error error = DW_DLE_NE; + dwarf_handle_t dwarf_handle; + + int dwarf_result = dwarf_elf_init(elf_handle.get(), DW_DLC_READ, NULL, NULL, + &dwarf_debug, &error); + + // We don't do any special handling for DW_DLV_NO_ENTRY specially. + // If we get an error, or the file doesn't have debug information + // we just return. + if (dwarf_result != DW_DLV_OK) { + return r; + } + + dwarf_handle.reset(dwarf_debug); + + r.file_handle = move(file_handle); + r.elf_handle = move(elf_handle); + r.dwarf_handle = move(dwarf_handle); + + return r; + } + + die_cache_entry &get_die_cache(dwarf_fileobject &fobj, Dwarf_Die die) { + Dwarf_Error error = DW_DLE_NE; + + // Get the die offset, we use it as the cache key + Dwarf_Off die_offset; + if (dwarf_dieoffset(die, &die_offset, &error) != DW_DLV_OK) { + die_offset = 0; + } + + die_cache_t::iterator it = fobj.die_cache.find(die_offset); + + if (it != fobj.die_cache.end()) { + fobj.current_cu = &it->second; + return it->second; + } + + die_cache_entry &de = fobj.die_cache[die_offset]; + fobj.current_cu = &de; + + Dwarf_Addr line_addr; + Dwarf_Small table_count; + + // The addresses in the line section are not fully sorted (they might + // be sorted by block of code belonging to the same file), which makes + // it necessary to do so before searching is possible. + // + // As libdwarf allocates a copy of everything, let's get the contents + // of the line section and keep it around. We also create a map of + // program counter to line table indices so we can search by address + // and get the line buffer index. + // + // To make things more difficult, the same address can span more than + // one line, so we need to keep the index pointing to the first line + // by using insert instead of the map's [ operator. + + // Get the line context for the DIE + if (dwarf_srclines_b(die, 0, &table_count, &de.line_context, &error) == + DW_DLV_OK) { + // Get the source lines for this line context, to be deallocated + // later + if (dwarf_srclines_from_linecontext(de.line_context, &de.line_buffer, + &de.line_count, + &error) == DW_DLV_OK) { + + // Add all the addresses to our map + for (int i = 0; i < de.line_count; i++) { + if (dwarf_lineaddr(de.line_buffer[i], &line_addr, &error) != + DW_DLV_OK) { + line_addr = 0; + } + de.line_section.insert(std::pair(line_addr, i)); + } + } + } + + // For each CU, cache the function DIEs that contain the + // DW_AT_specification attribute. When building with -g3 the function + // DIEs are separated in declaration and specification, with the + // declaration containing only the name and parameters and the + // specification the low/high pc and other compiler attributes. + // + // We cache those specifications so we don't skip over the declarations, + // because they have no pc, and we can do namespace resolution for + // DWARF function names. + Dwarf_Debug dwarf = fobj.dwarf_handle.get(); + Dwarf_Die current_die = 0; + if (dwarf_child(die, ¤t_die, &error) == DW_DLV_OK) { + for (;;) { + Dwarf_Die sibling_die = 0; + + Dwarf_Half tag_value; + dwarf_tag(current_die, &tag_value, &error); + + if (tag_value == DW_TAG_subprogram || + tag_value == DW_TAG_inlined_subroutine) { + + Dwarf_Bool has_attr = 0; + if (dwarf_hasattr(current_die, DW_AT_specification, &has_attr, + &error) == DW_DLV_OK) { + if (has_attr) { + Dwarf_Attribute attr_mem; + if (dwarf_attr(current_die, DW_AT_specification, &attr_mem, + &error) == DW_DLV_OK) { + Dwarf_Off spec_offset = 0; + if (dwarf_formref(attr_mem, &spec_offset, &error) == + DW_DLV_OK) { + Dwarf_Off spec_die_offset; + if (dwarf_dieoffset(current_die, &spec_die_offset, &error) == + DW_DLV_OK) { + de.spec_section[spec_offset] = spec_die_offset; + } + } + } + dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR); + } + } + } + + int result = dwarf_siblingof(dwarf, current_die, &sibling_die, &error); + if (result == DW_DLV_ERROR) { + break; + } else if (result == DW_DLV_NO_ENTRY) { + break; + } + + if (current_die != die) { + dwarf_dealloc(dwarf, current_die, DW_DLA_DIE); + current_die = 0; + } + + current_die = sibling_die; + } + } + return de; + } + + static Dwarf_Die get_referenced_die(Dwarf_Debug dwarf, Dwarf_Die die, + Dwarf_Half attr, bool global) { + Dwarf_Error error = DW_DLE_NE; + Dwarf_Attribute attr_mem; + + Dwarf_Die found_die = NULL; + if (dwarf_attr(die, attr, &attr_mem, &error) == DW_DLV_OK) { + Dwarf_Off offset; + int result = 0; + if (global) { + result = dwarf_global_formref(attr_mem, &offset, &error); + } else { + result = dwarf_formref(attr_mem, &offset, &error); + } + + if (result == DW_DLV_OK) { + if (dwarf_offdie(dwarf, offset, &found_die, &error) != DW_DLV_OK) { + found_die = NULL; + } + } + dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR); + } + return found_die; + } + + static std::string get_referenced_die_name(Dwarf_Debug dwarf, Dwarf_Die die, + Dwarf_Half attr, bool global) { + Dwarf_Error error = DW_DLE_NE; + std::string value; + + Dwarf_Die found_die = get_referenced_die(dwarf, die, attr, global); + + if (found_die) { + char *name; + if (dwarf_diename(found_die, &name, &error) == DW_DLV_OK) { + if (name) { + value = std::string(name); + } + dwarf_dealloc(dwarf, name, DW_DLA_STRING); + } + dwarf_dealloc(dwarf, found_die, DW_DLA_DIE); + } + + return value; + } + + // Returns a spec DIE linked to the passed one. The caller should + // deallocate the DIE + static Dwarf_Die get_spec_die(dwarf_fileobject &fobj, Dwarf_Die die) { + Dwarf_Debug dwarf = fobj.dwarf_handle.get(); + Dwarf_Error error = DW_DLE_NE; + Dwarf_Off die_offset; + if (fobj.current_cu && + dwarf_die_CU_offset(die, &die_offset, &error) == DW_DLV_OK) { + die_specmap_t::iterator it = + fobj.current_cu->spec_section.find(die_offset); + + // If we have a DIE that completes the current one, check if + // that one has the pc we are looking for + if (it != fobj.current_cu->spec_section.end()) { + Dwarf_Die spec_die = 0; + if (dwarf_offdie(dwarf, it->second, &spec_die, &error) == DW_DLV_OK) { + return spec_die; + } + } + } + + // Maybe we have an abstract origin DIE with the function information? + return get_referenced_die(fobj.dwarf_handle.get(), die, + DW_AT_abstract_origin, true); + } + + static bool die_has_pc(dwarf_fileobject &fobj, Dwarf_Die die, Dwarf_Addr pc) { + Dwarf_Addr low_pc = 0, high_pc = 0; + Dwarf_Half high_pc_form = 0; + Dwarf_Form_Class return_class; + Dwarf_Error error = DW_DLE_NE; + Dwarf_Debug dwarf = fobj.dwarf_handle.get(); + bool has_lowpc = false; + bool has_highpc = false; + bool has_ranges = false; + + if (dwarf_lowpc(die, &low_pc, &error) == DW_DLV_OK) { + // If we have a low_pc check if there is a high pc. + // If we don't have a high pc this might mean we have a base + // address for the ranges list or just an address. + has_lowpc = true; + + if (dwarf_highpc_b(die, &high_pc, &high_pc_form, &return_class, &error) == + DW_DLV_OK) { + // We do have a high pc. In DWARF 4+ this is an offset from the + // low pc, but in earlier versions it's an absolute address. + + has_highpc = true; + // In DWARF 2/3 this would be a DW_FORM_CLASS_ADDRESS + if (return_class == DW_FORM_CLASS_CONSTANT) { + high_pc = low_pc + high_pc; + } + + // We have low and high pc, check if our address + // is in that range + return pc >= low_pc && pc < high_pc; + } + } else { + // Reset the low_pc, in case dwarf_lowpc failing set it to some + // undefined value. + low_pc = 0; + } + + // Check if DW_AT_ranges is present and search for the PC in the + // returned ranges list. We always add the low_pc, as it not set it will + // be 0, in case we had a DW_AT_low_pc and DW_AT_ranges pair + bool result = false; + + Dwarf_Attribute attr; + if (dwarf_attr(die, DW_AT_ranges, &attr, &error) == DW_DLV_OK) { + + Dwarf_Off offset; + if (dwarf_global_formref(attr, &offset, &error) == DW_DLV_OK) { + Dwarf_Ranges *ranges; + Dwarf_Signed ranges_count = 0; + Dwarf_Unsigned byte_count = 0; + + if (dwarf_get_ranges_a(dwarf, offset, die, &ranges, &ranges_count, + &byte_count, &error) == DW_DLV_OK) { + has_ranges = ranges_count != 0; + for (int i = 0; i < ranges_count; i++) { + if (ranges[i].dwr_addr1 != 0 && + pc >= ranges[i].dwr_addr1 + low_pc && + pc < ranges[i].dwr_addr2 + low_pc) { + result = true; + break; + } + } + dwarf_ranges_dealloc(dwarf, ranges, ranges_count); + } + } + } + + // Last attempt. We might have a single address set as low_pc. + if (!result && low_pc != 0 && pc == low_pc) { + result = true; + } + + // If we don't have lowpc, highpc and ranges maybe this DIE is a + // declaration that relies on a DW_AT_specification DIE that happens + // later. Use the specification cache we filled when we loaded this CU. + if (!result && (!has_lowpc && !has_highpc && !has_ranges)) { + Dwarf_Die spec_die = get_spec_die(fobj, die); + if (spec_die) { + result = die_has_pc(fobj, spec_die, pc); + dwarf_dealloc(dwarf, spec_die, DW_DLA_DIE); + } + } + + return result; + } + + static void get_type(Dwarf_Debug dwarf, Dwarf_Die die, std::string &type) { + Dwarf_Error error = DW_DLE_NE; + + Dwarf_Die child = 0; + if (dwarf_child(die, &child, &error) == DW_DLV_OK) { + get_type(dwarf, child, type); + } + + if (child) { + type.insert(0, "::"); + dwarf_dealloc(dwarf, child, DW_DLA_DIE); + } + + char *name; + if (dwarf_diename(die, &name, &error) == DW_DLV_OK) { + type.insert(0, std::string(name)); + dwarf_dealloc(dwarf, name, DW_DLA_STRING); + } else { + type.insert(0, ""); + } + } + + static std::string get_type_by_signature(Dwarf_Debug dwarf, Dwarf_Die die) { + Dwarf_Error error = DW_DLE_NE; + + Dwarf_Sig8 signature; + Dwarf_Bool has_attr = 0; + if (dwarf_hasattr(die, DW_AT_signature, &has_attr, &error) == DW_DLV_OK) { + if (has_attr) { + Dwarf_Attribute attr_mem; + if (dwarf_attr(die, DW_AT_signature, &attr_mem, &error) == DW_DLV_OK) { + if (dwarf_formsig8(attr_mem, &signature, &error) != DW_DLV_OK) { + return std::string(""); + } + } + dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR); + } + } + + Dwarf_Unsigned next_cu_header; + Dwarf_Sig8 tu_signature; + std::string result; + bool found = false; + + while (dwarf_next_cu_header_d(dwarf, 0, 0, 0, 0, 0, 0, 0, &tu_signature, 0, + &next_cu_header, 0, &error) == DW_DLV_OK) { + + if (strncmp(signature.signature, tu_signature.signature, 8) == 0) { + Dwarf_Die type_cu_die = 0; + if (dwarf_siblingof_b(dwarf, 0, 0, &type_cu_die, &error) == DW_DLV_OK) { + Dwarf_Die child_die = 0; + if (dwarf_child(type_cu_die, &child_die, &error) == DW_DLV_OK) { + get_type(dwarf, child_die, result); + found = !result.empty(); + dwarf_dealloc(dwarf, child_die, DW_DLA_DIE); + } + dwarf_dealloc(dwarf, type_cu_die, DW_DLA_DIE); + } + } + } + + if (found) { + while (dwarf_next_cu_header_d(dwarf, 0, 0, 0, 0, 0, 0, 0, 0, 0, + &next_cu_header, 0, &error) == DW_DLV_OK) { + // Reset the cu header state. Unfortunately, libdwarf's + // next_cu_header API keeps its own iterator per Dwarf_Debug + // that can't be reset. We need to keep fetching elements until + // the end. + } + } else { + // If we couldn't resolve the type just print out the signature + std::ostringstream string_stream; + string_stream << "<0x" << std::hex << std::setfill('0'); + for (int i = 0; i < 8; ++i) { + string_stream << std::setw(2) << std::hex + << (int)(unsigned char)(signature.signature[i]); + } + string_stream << ">"; + result = string_stream.str(); + } + return result; + } + + struct type_context_t { + bool is_const; + bool is_typedef; + bool has_type; + bool has_name; + std::string text; + + type_context_t() + : is_const(false), is_typedef(false), has_type(false), has_name(false) { + } + }; + + // Types are resolved from right to left: we get the variable name first + // and then all specifiers (like const or pointer) in a chain of DW_AT_type + // DIEs. Call this function recursively until we get a complete type + // string. + static void set_parameter_string(dwarf_fileobject &fobj, Dwarf_Die die, + type_context_t &context) { + char *name; + Dwarf_Error error = DW_DLE_NE; + + // typedefs contain also the base type, so we skip it and only + // print the typedef name + if (!context.is_typedef) { + if (dwarf_diename(die, &name, &error) == DW_DLV_OK) { + if (!context.text.empty()) { + context.text.insert(0, " "); + } + context.text.insert(0, std::string(name)); + dwarf_dealloc(fobj.dwarf_handle.get(), name, DW_DLA_STRING); + } + } else { + context.is_typedef = false; + context.has_type = true; + if (context.is_const) { + context.text.insert(0, "const "); + context.is_const = false; + } + } + + bool next_type_is_const = false; + bool is_keyword = true; + + Dwarf_Half tag = 0; + Dwarf_Bool has_attr = 0; + if (dwarf_tag(die, &tag, &error) == DW_DLV_OK) { + switch (tag) { + case DW_TAG_structure_type: + case DW_TAG_union_type: + case DW_TAG_class_type: + case DW_TAG_enumeration_type: + context.has_type = true; + if (dwarf_hasattr(die, DW_AT_signature, &has_attr, &error) == + DW_DLV_OK) { + // If we have a signature it means the type is defined + // in .debug_types, so we need to load the DIE pointed + // at by the signature and resolve it + if (has_attr) { + std::string type = + get_type_by_signature(fobj.dwarf_handle.get(), die); + if (context.is_const) + type.insert(0, "const "); + + if (!context.text.empty()) + context.text.insert(0, " "); + context.text.insert(0, type); + } + + // Treat enums like typedefs, and skip printing its + // base type + context.is_typedef = (tag == DW_TAG_enumeration_type); + } + break; + case DW_TAG_const_type: + next_type_is_const = true; + break; + case DW_TAG_pointer_type: + context.text.insert(0, "*"); + break; + case DW_TAG_reference_type: + context.text.insert(0, "&"); + break; + case DW_TAG_restrict_type: + context.text.insert(0, "restrict "); + break; + case DW_TAG_rvalue_reference_type: + context.text.insert(0, "&&"); + break; + case DW_TAG_volatile_type: + context.text.insert(0, "volatile "); + break; + case DW_TAG_typedef: + // Propagate the const-ness to the next type + // as typedefs are linked to its base type + next_type_is_const = context.is_const; + context.is_typedef = true; + context.has_type = true; + break; + case DW_TAG_base_type: + context.has_type = true; + break; + case DW_TAG_formal_parameter: + context.has_name = true; + break; + default: + is_keyword = false; + break; + } + } + + if (!is_keyword && context.is_const) { + context.text.insert(0, "const "); + } + + context.is_const = next_type_is_const; + + Dwarf_Die ref = + get_referenced_die(fobj.dwarf_handle.get(), die, DW_AT_type, true); + if (ref) { + set_parameter_string(fobj, ref, context); + dwarf_dealloc(fobj.dwarf_handle.get(), ref, DW_DLA_DIE); + } + + if (!context.has_type && context.has_name) { + context.text.insert(0, "void "); + context.has_type = true; + } + } + + // Resolve the function return type and parameters + static void set_function_parameters(std::string &function_name, + std::vector &ns, + dwarf_fileobject &fobj, Dwarf_Die die) { + Dwarf_Debug dwarf = fobj.dwarf_handle.get(); + Dwarf_Error error = DW_DLE_NE; + Dwarf_Die current_die = 0; + std::string parameters; + bool has_spec = true; + // Check if we have a spec DIE. If we do we use it as it contains + // more information, like parameter names. + Dwarf_Die spec_die = get_spec_die(fobj, die); + if (!spec_die) { + has_spec = false; + spec_die = die; + } + + std::vector::const_iterator it = ns.begin(); + std::string ns_name; + for (it = ns.begin(); it < ns.end(); ++it) { + ns_name.append(*it).append("::"); + } + + if (!ns_name.empty()) { + function_name.insert(0, ns_name); + } + + // See if we have a function return type. It can be either on the + // current die or in its spec one (usually true for inlined functions) + std::string return_type = + get_referenced_die_name(dwarf, die, DW_AT_type, true); + if (return_type.empty()) { + return_type = get_referenced_die_name(dwarf, spec_die, DW_AT_type, true); + } + if (!return_type.empty()) { + return_type.append(" "); + function_name.insert(0, return_type); + } + + if (dwarf_child(spec_die, ¤t_die, &error) == DW_DLV_OK) { + for (;;) { + Dwarf_Die sibling_die = 0; + + Dwarf_Half tag_value; + dwarf_tag(current_die, &tag_value, &error); + + if (tag_value == DW_TAG_formal_parameter) { + // Ignore artificial (ie, compiler generated) parameters + bool is_artificial = false; + Dwarf_Attribute attr_mem; + if (dwarf_attr(current_die, DW_AT_artificial, &attr_mem, &error) == + DW_DLV_OK) { + Dwarf_Bool flag = 0; + if (dwarf_formflag(attr_mem, &flag, &error) == DW_DLV_OK) { + is_artificial = flag != 0; + } + dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR); + } + + if (!is_artificial) { + type_context_t context; + set_parameter_string(fobj, current_die, context); + + if (parameters.empty()) { + parameters.append("("); + } else { + parameters.append(", "); + } + parameters.append(context.text); + } + } + + int result = dwarf_siblingof(dwarf, current_die, &sibling_die, &error); + if (result == DW_DLV_ERROR) { + break; + } else if (result == DW_DLV_NO_ENTRY) { + break; + } + + if (current_die != die) { + dwarf_dealloc(dwarf, current_die, DW_DLA_DIE); + current_die = 0; + } + + current_die = sibling_die; + } + } + if (parameters.empty()) + parameters = "("; + parameters.append(")"); + + // If we got a spec DIE we need to deallocate it + if (has_spec) + dwarf_dealloc(dwarf, spec_die, DW_DLA_DIE); + + function_name.append(parameters); + } + + // defined here because in C++98, template function cannot take locally + // defined types... grrr. + struct inliners_search_cb { + void operator()(Dwarf_Die die, std::vector &ns) { + Dwarf_Error error = DW_DLE_NE; + Dwarf_Half tag_value; + Dwarf_Attribute attr_mem; + Dwarf_Debug dwarf = fobj.dwarf_handle.get(); + + dwarf_tag(die, &tag_value, &error); + + switch (tag_value) { + char *name; + case DW_TAG_subprogram: + if (!trace.source.function.empty()) + break; + if (dwarf_diename(die, &name, &error) == DW_DLV_OK) { + trace.source.function = std::string(name); + dwarf_dealloc(dwarf, name, DW_DLA_STRING); + } else { + // We don't have a function name in this DIE. + // Check if there is a referenced non-defining + // declaration. + trace.source.function = + get_referenced_die_name(dwarf, die, DW_AT_abstract_origin, true); + if (trace.source.function.empty()) { + trace.source.function = + get_referenced_die_name(dwarf, die, DW_AT_specification, true); + } + } + + // Append the function parameters, if available + set_function_parameters(trace.source.function, ns, fobj, die); + + // If the object function name is empty, it's possible that + // there is no dynamic symbol table (maybe the executable + // was stripped or not built with -rdynamic). See if we have + // a DWARF linkage name to use instead. We try both + // linkage_name and MIPS_linkage_name because the MIPS tag + // was the unofficial one until it was adopted in DWARF4. + // Old gcc versions generate MIPS_linkage_name + if (trace.object_function.empty()) { + details::demangler demangler; + + if (dwarf_attr(die, DW_AT_linkage_name, &attr_mem, &error) != + DW_DLV_OK) { + if (dwarf_attr(die, DW_AT_MIPS_linkage_name, &attr_mem, &error) != + DW_DLV_OK) { + break; + } + } + + char *linkage; + if (dwarf_formstring(attr_mem, &linkage, &error) == DW_DLV_OK) { + trace.object_function = demangler.demangle(linkage); + dwarf_dealloc(dwarf, linkage, DW_DLA_STRING); + } + dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR); + } + break; + + case DW_TAG_inlined_subroutine: + ResolvedTrace::SourceLoc sloc; + + if (dwarf_diename(die, &name, &error) == DW_DLV_OK) { + sloc.function = std::string(name); + dwarf_dealloc(dwarf, name, DW_DLA_STRING); + } else { + // We don't have a name for this inlined DIE, it could + // be that there is an abstract origin instead. + // Get the DW_AT_abstract_origin value, which is a + // reference to the source DIE and try to get its name + sloc.function = + get_referenced_die_name(dwarf, die, DW_AT_abstract_origin, true); + } + + set_function_parameters(sloc.function, ns, fobj, die); + + std::string file = die_call_file(dwarf, die, cu_die); + if (!file.empty()) + sloc.filename = file; + + Dwarf_Unsigned number = 0; + if (dwarf_attr(die, DW_AT_call_line, &attr_mem, &error) == DW_DLV_OK) { + if (dwarf_formudata(attr_mem, &number, &error) == DW_DLV_OK) { + sloc.line = number; + } + dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR); + } + + if (dwarf_attr(die, DW_AT_call_column, &attr_mem, &error) == + DW_DLV_OK) { + if (dwarf_formudata(attr_mem, &number, &error) == DW_DLV_OK) { + sloc.col = number; + } + dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR); + } + + trace.inliners.push_back(sloc); + break; + }; + } + ResolvedTrace &trace; + dwarf_fileobject &fobj; + Dwarf_Die cu_die; + inliners_search_cb(ResolvedTrace &t, dwarf_fileobject &f, Dwarf_Die c) + : trace(t), fobj(f), cu_die(c) {} + }; + + static Dwarf_Die find_fundie_by_pc(dwarf_fileobject &fobj, + Dwarf_Die parent_die, Dwarf_Addr pc, + Dwarf_Die result) { + Dwarf_Die current_die = 0; + Dwarf_Error error = DW_DLE_NE; + Dwarf_Debug dwarf = fobj.dwarf_handle.get(); + + if (dwarf_child(parent_die, ¤t_die, &error) != DW_DLV_OK) { + return NULL; + } + + for (;;) { + Dwarf_Die sibling_die = 0; + Dwarf_Half tag_value; + dwarf_tag(current_die, &tag_value, &error); + + switch (tag_value) { + case DW_TAG_subprogram: + case DW_TAG_inlined_subroutine: + if (die_has_pc(fobj, current_die, pc)) { + return current_die; + } + }; + bool declaration = false; + Dwarf_Attribute attr_mem; + if (dwarf_attr(current_die, DW_AT_declaration, &attr_mem, &error) == + DW_DLV_OK) { + Dwarf_Bool flag = 0; + if (dwarf_formflag(attr_mem, &flag, &error) == DW_DLV_OK) { + declaration = flag != 0; + } + dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR); + } + + if (!declaration) { + // let's be curious and look deeper in the tree, functions are + // not necessarily at the first level, but might be nested + // inside a namespace, structure, a function, an inlined + // function etc. + Dwarf_Die die_mem = 0; + Dwarf_Die indie = find_fundie_by_pc(fobj, current_die, pc, die_mem); + if (indie) { + result = die_mem; + return result; + } + } + + int res = dwarf_siblingof(dwarf, current_die, &sibling_die, &error); + if (res == DW_DLV_ERROR) { + return NULL; + } else if (res == DW_DLV_NO_ENTRY) { + break; + } + + if (current_die != parent_die) { + dwarf_dealloc(dwarf, current_die, DW_DLA_DIE); + current_die = 0; + } + + current_die = sibling_die; + } + return NULL; + } + + template + static bool deep_first_search_by_pc(dwarf_fileobject &fobj, + Dwarf_Die parent_die, Dwarf_Addr pc, + std::vector &ns, CB cb) { + Dwarf_Die current_die = 0; + Dwarf_Debug dwarf = fobj.dwarf_handle.get(); + Dwarf_Error error = DW_DLE_NE; + + if (dwarf_child(parent_die, ¤t_die, &error) != DW_DLV_OK) { + return false; + } + + bool branch_has_pc = false; + bool has_namespace = false; + for (;;) { + Dwarf_Die sibling_die = 0; + + Dwarf_Half tag; + if (dwarf_tag(current_die, &tag, &error) == DW_DLV_OK) { + if (tag == DW_TAG_namespace || tag == DW_TAG_class_type) { + char *ns_name = NULL; + if (dwarf_diename(current_die, &ns_name, &error) == DW_DLV_OK) { + if (ns_name) { + ns.push_back(std::string(ns_name)); + } else { + ns.push_back(""); + } + dwarf_dealloc(dwarf, ns_name, DW_DLA_STRING); + } else { + ns.push_back(""); + } + has_namespace = true; + } + } + + bool declaration = false; + Dwarf_Attribute attr_mem; + if (tag != DW_TAG_class_type && + dwarf_attr(current_die, DW_AT_declaration, &attr_mem, &error) == + DW_DLV_OK) { + Dwarf_Bool flag = 0; + if (dwarf_formflag(attr_mem, &flag, &error) == DW_DLV_OK) { + declaration = flag != 0; + } + dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR); + } + + if (!declaration) { + // let's be curious and look deeper in the tree, function are + // not necessarily at the first level, but might be nested + // inside a namespace, structure, a function, an inlined + // function etc. + branch_has_pc = deep_first_search_by_pc(fobj, current_die, pc, ns, cb); + } + + if (!branch_has_pc) { + branch_has_pc = die_has_pc(fobj, current_die, pc); + } + + if (branch_has_pc) { + cb(current_die, ns); + } + + int result = dwarf_siblingof(dwarf, current_die, &sibling_die, &error); + if (result == DW_DLV_ERROR) { + return false; + } else if (result == DW_DLV_NO_ENTRY) { + break; + } + + if (current_die != parent_die) { + dwarf_dealloc(dwarf, current_die, DW_DLA_DIE); + current_die = 0; + } + + if (has_namespace) { + has_namespace = false; + ns.pop_back(); + } + current_die = sibling_die; + } + + if (has_namespace) { + ns.pop_back(); + } + return branch_has_pc; + } + + static std::string die_call_file(Dwarf_Debug dwarf, Dwarf_Die die, + Dwarf_Die cu_die) { + Dwarf_Attribute attr_mem; + Dwarf_Error error = DW_DLE_NE; + Dwarf_Unsigned file_index; + + std::string file; + + if (dwarf_attr(die, DW_AT_call_file, &attr_mem, &error) == DW_DLV_OK) { + if (dwarf_formudata(attr_mem, &file_index, &error) != DW_DLV_OK) { + file_index = 0; + } + dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR); + + if (file_index == 0) { + return file; + } + + char **srcfiles = 0; + Dwarf_Signed file_count = 0; + if (dwarf_srcfiles(cu_die, &srcfiles, &file_count, &error) == DW_DLV_OK) { + if (file_count > 0 && file_index <= static_cast(file_count)) { + file = std::string(srcfiles[file_index - 1]); + } + + // Deallocate all strings! + for (int i = 0; i < file_count; ++i) { + dwarf_dealloc(dwarf, srcfiles[i], DW_DLA_STRING); + } + dwarf_dealloc(dwarf, srcfiles, DW_DLA_LIST); + } + } + return file; + } + + Dwarf_Die find_die(dwarf_fileobject &fobj, Dwarf_Addr addr) { + // Let's get to work! First see if we have a debug_aranges section so + // we can speed up the search + + Dwarf_Debug dwarf = fobj.dwarf_handle.get(); + Dwarf_Error error = DW_DLE_NE; + Dwarf_Arange *aranges; + Dwarf_Signed arange_count; + + Dwarf_Die returnDie; + bool found = false; + if (dwarf_get_aranges(dwarf, &aranges, &arange_count, &error) != + DW_DLV_OK) { + aranges = NULL; + } + + if (aranges) { + // We have aranges. Get the one where our address is. + Dwarf_Arange arange; + if (dwarf_get_arange(aranges, arange_count, addr, &arange, &error) == + DW_DLV_OK) { + + // We found our address. Get the compilation-unit DIE offset + // represented by the given address range. + Dwarf_Off cu_die_offset; + if (dwarf_get_cu_die_offset(arange, &cu_die_offset, &error) == + DW_DLV_OK) { + // Get the DIE at the offset returned by the aranges search. + // We set is_info to 1 to specify that the offset is from + // the .debug_info section (and not .debug_types) + int dwarf_result = + dwarf_offdie_b(dwarf, cu_die_offset, 1, &returnDie, &error); + + found = dwarf_result == DW_DLV_OK; + } + dwarf_dealloc(dwarf, arange, DW_DLA_ARANGE); + } + } + + if (found) + return returnDie; // The caller is responsible for freeing the die + + // The search for aranges failed. Try to find our address by scanning + // all compilation units. + Dwarf_Unsigned next_cu_header; + Dwarf_Half tag = 0; + returnDie = 0; + + while (!found && + dwarf_next_cu_header_d(dwarf, 1, 0, 0, 0, 0, 0, 0, 0, 0, + &next_cu_header, 0, &error) == DW_DLV_OK) { + + if (returnDie) + dwarf_dealloc(dwarf, returnDie, DW_DLA_DIE); + + if (dwarf_siblingof(dwarf, 0, &returnDie, &error) == DW_DLV_OK) { + if ((dwarf_tag(returnDie, &tag, &error) == DW_DLV_OK) && + tag == DW_TAG_compile_unit) { + if (die_has_pc(fobj, returnDie, addr)) { + found = true; + } + } + } + } + + if (found) { + while (dwarf_next_cu_header_d(dwarf, 1, 0, 0, 0, 0, 0, 0, 0, 0, + &next_cu_header, 0, &error) == DW_DLV_OK) { + // Reset the cu header state. Libdwarf's next_cu_header API + // keeps its own iterator per Dwarf_Debug that can't be reset. + // We need to keep fetching elements until the end. + } + } + + if (found) + return returnDie; + + // We couldn't find any compilation units with ranges or a high/low pc. + // Try again by looking at all DIEs in all compilation units. + Dwarf_Die cudie; + while (dwarf_next_cu_header_d(dwarf, 1, 0, 0, 0, 0, 0, 0, 0, 0, + &next_cu_header, 0, &error) == DW_DLV_OK) { + if (dwarf_siblingof(dwarf, 0, &cudie, &error) == DW_DLV_OK) { + Dwarf_Die die_mem = 0; + Dwarf_Die resultDie = find_fundie_by_pc(fobj, cudie, addr, die_mem); + + if (resultDie) { + found = true; + break; + } + } + } + + if (found) { + while (dwarf_next_cu_header_d(dwarf, 1, 0, 0, 0, 0, 0, 0, 0, 0, + &next_cu_header, 0, &error) == DW_DLV_OK) { + // Reset the cu header state. Libdwarf's next_cu_header API + // keeps its own iterator per Dwarf_Debug that can't be reset. + // We need to keep fetching elements until the end. + } + } + + if (found) + return cudie; + + // We failed. + return NULL; + } +}; +#endif // BACKWARD_HAS_DWARF == 1 + +template <> +class TraceResolverImpl + : public TraceResolverLinuxImpl {}; + +#endif // BACKWARD_SYSTEM_LINUX + +#ifdef BACKWARD_SYSTEM_DARWIN + +template class TraceResolverDarwinImpl; + +template <> +class TraceResolverDarwinImpl + : public TraceResolverImplBase { +public: + void load_addresses(void *const*addresses, int address_count) override { + if (address_count == 0) { + return; + } + _symbols.reset(backtrace_symbols(addresses, address_count)); + } + + ResolvedTrace resolve(ResolvedTrace trace) override { + // parse: + // + + char *filename = _symbols[trace.idx]; + + // skip " " + while (*filename && *filename != ' ') + filename++; + while (*filename == ' ') + filename++; + + // find start of from end ( may contain a space) + char *p = filename + strlen(filename) - 1; + // skip to start of " + " + while (p > filename && *p != ' ') + p--; + while (p > filename && *p == ' ') + p--; + while (p > filename && *p != ' ') + p--; + while (p > filename && *p == ' ') + p--; + char *funcname_end = p + 1; + + // skip to start of "" + while (p > filename && *p != ' ') + p--; + char *funcname = p + 1; + + // skip to start of " " + while (p > filename && *p == ' ') + p--; + while (p > filename && *p != ' ') + p--; + while (p > filename && *p == ' ') + p--; + + // skip "", handling the case where it contains a + char *filename_end = p + 1; + if (p == filename) { + // something went wrong, give up + filename_end = filename + strlen(filename); + funcname = filename_end; + } + trace.object_filename.assign( + filename, filename_end); // ok even if filename_end is the ending \0 + // (then we assign entire string) + + if (*funcname) { // if it's not end of string + *funcname_end = '\0'; + + trace.object_function = this->demangle(funcname); + trace.object_function += " "; + trace.object_function += (funcname_end + 1); + trace.source.function = trace.object_function; // we cannot do better. + } + return trace; + } + +private: + details::handle _symbols; +}; + +template <> +class TraceResolverImpl + : public TraceResolverDarwinImpl {}; + +#endif // BACKWARD_SYSTEM_DARWIN + +#ifdef BACKWARD_SYSTEM_WINDOWS + +// Load all symbol info +// Based on: +// https://stackoverflow.com/questions/6205981/windows-c-stack-trace-from-a-running-app/28276227#28276227 + +struct module_data { + std::string image_name; + std::string module_name; + void *base_address; + DWORD load_size; +}; + +class get_mod_info { + HANDLE process; + static const int buffer_length = 4096; + +public: + get_mod_info(HANDLE h) : process(h) {} + + module_data operator()(HMODULE module) { + module_data ret; + char temp[buffer_length]; + MODULEINFO mi; + + GetModuleInformation(process, module, &mi, sizeof(mi)); + ret.base_address = mi.lpBaseOfDll; + ret.load_size = mi.SizeOfImage; + + GetModuleFileNameExA(process, module, temp, sizeof(temp)); + ret.image_name = temp; + GetModuleBaseNameA(process, module, temp, sizeof(temp)); + ret.module_name = temp; + std::vector img(ret.image_name.begin(), ret.image_name.end()); + std::vector mod(ret.module_name.begin(), ret.module_name.end()); + SymLoadModule64(process, 0, &img[0], &mod[0], (DWORD64)ret.base_address, + ret.load_size); + return ret; + } +}; + +template <> class TraceResolverImpl + : public TraceResolverImplBase { +public: + TraceResolverImpl() { + + HANDLE process = GetCurrentProcess(); + + std::vector modules; + DWORD cbNeeded; + std::vector module_handles(1); + SymInitialize(process, NULL, false); + DWORD symOptions = SymGetOptions(); + symOptions |= SYMOPT_LOAD_LINES | SYMOPT_UNDNAME; + SymSetOptions(symOptions); + EnumProcessModules(process, &module_handles[0], + module_handles.size() * sizeof(HMODULE), &cbNeeded); + module_handles.resize(cbNeeded / sizeof(HMODULE)); + EnumProcessModules(process, &module_handles[0], + module_handles.size() * sizeof(HMODULE), &cbNeeded); + std::transform(module_handles.begin(), module_handles.end(), + std::back_inserter(modules), get_mod_info(process)); + void *base = modules[0].base_address; + IMAGE_NT_HEADERS *h = ImageNtHeader(base); + image_type = h->FileHeader.Machine; + } + + static const int max_sym_len = 255; + struct symbol_t { + SYMBOL_INFO sym; + char buffer[max_sym_len]; + } sym; + + DWORD64 displacement; + + ResolvedTrace resolve(ResolvedTrace t) override { + HANDLE process = GetCurrentProcess(); + + char name[256]; + + memset(&sym, 0, sizeof(sym)); + sym.sym.SizeOfStruct = sizeof(SYMBOL_INFO); + sym.sym.MaxNameLen = max_sym_len; + + if (!SymFromAddr(process, (ULONG64)t.addr, &displacement, &sym.sym)) { + // TODO: error handling everywhere + char* lpMsgBuf; + DWORD dw = GetLastError(); + + FormatMessageA(FORMAT_MESSAGE_ALLOCATE_BUFFER | + FORMAT_MESSAGE_FROM_SYSTEM | + FORMAT_MESSAGE_IGNORE_INSERTS, + NULL, dw, MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), + (char*)&lpMsgBuf, 0, NULL); + + printf(lpMsgBuf); + + // abort(); + } + UnDecorateSymbolName(sym.sym.Name, (PSTR)name, 256, UNDNAME_COMPLETE); + + DWORD offset = 0; + IMAGEHLP_LINE line; + if (SymGetLineFromAddr(process, (ULONG64)t.addr, &offset, &line)) { + t.object_filename = line.FileName; + t.source.filename = line.FileName; + t.source.line = line.LineNumber; + t.source.col = offset; + } + + t.source.function = name; + t.object_filename = ""; + t.object_function = name; + + return t; + } + + DWORD machine_type() const { return image_type; } + +private: + DWORD image_type; +}; + +#endif + +class TraceResolver : public TraceResolverImpl {}; + +/*************** CODE SNIPPET ***************/ + +class SourceFile { +public: + typedef std::vector> lines_t; + + SourceFile() {} + SourceFile(const std::string &path) { + // 1. If BACKWARD_CXX_SOURCE_PREFIXES is set then assume it contains + // a colon-separated list of path prefixes. Try prepending each + // to the given path until a valid file is found. + const std::vector &prefixes = get_paths_from_env_variable(); + for (size_t i = 0; i < prefixes.size(); ++i) { + // Double slashes (//) should not be a problem. + std::string new_path = prefixes[i] + '/' + path; + _file.reset(new std::ifstream(new_path.c_str())); + if (is_open()) + break; + } + // 2. If no valid file found then fallback to opening the path as-is. + if (!_file || !is_open()) { + _file.reset(new std::ifstream(path.c_str())); + } + } + bool is_open() const { return _file->is_open(); } + + lines_t &get_lines(unsigned line_start, unsigned line_count, lines_t &lines) { + using namespace std; + // This function make uses of the dumbest algo ever: + // 1) seek(0) + // 2) read lines one by one and discard until line_start + // 3) read line one by one until line_start + line_count + // + // If you are getting snippets many time from the same file, it is + // somewhat a waste of CPU, feel free to benchmark and propose a + // better solution ;) + + _file->clear(); + _file->seekg(0); + string line; + unsigned line_idx; + + for (line_idx = 1; line_idx < line_start; ++line_idx) { + std::getline(*_file, line); + if (!*_file) { + return lines; + } + } + + // think of it like a lambda in C++98 ;) + // but look, I will reuse it two times! + // What a good boy am I. + struct isspace { + bool operator()(char c) { return std::isspace(c); } + }; + + bool started = false; + for (; line_idx < line_start + line_count; ++line_idx) { + getline(*_file, line); + if (!*_file) { + return lines; + } + if (!started) { + if (std::find_if(line.begin(), line.end(), not_isspace()) == line.end()) + continue; + started = true; + } + lines.push_back(make_pair(line_idx, line)); + } + + lines.erase( + std::find_if(lines.rbegin(), lines.rend(), not_isempty()).base(), + lines.end()); + return lines; + } + + lines_t get_lines(unsigned line_start, unsigned line_count) { + lines_t lines; + return get_lines(line_start, line_count, lines); + } + + // there is no find_if_not in C++98, lets do something crappy to + // workaround. + struct not_isspace { + bool operator()(char c) { return !std::isspace(c); } + }; + // and define this one here because C++98 is not happy with local defined + // struct passed to template functions, fuuuu. + struct not_isempty { + bool operator()(const lines_t::value_type &p) { + return !(std::find_if(p.second.begin(), p.second.end(), not_isspace()) == + p.second.end()); + } + }; + + void swap(SourceFile &b) { _file.swap(b._file); } + +#ifdef BACKWARD_ATLEAST_CXX11 + SourceFile(SourceFile &&from) : _file(nullptr) { swap(from); } + SourceFile &operator=(SourceFile &&from) { + swap(from); + return *this; + } +#else + explicit SourceFile(const SourceFile &from) { + // some sort of poor man's move semantic. + swap(const_cast(from)); + } + SourceFile &operator=(const SourceFile &from) { + // some sort of poor man's move semantic. + swap(const_cast(from)); + return *this; + } +#endif + +private: + details::handle> + _file; + + std::vector get_paths_from_env_variable_impl() { + std::vector paths; + const char *prefixes_str = std::getenv("BACKWARD_CXX_SOURCE_PREFIXES"); + if (prefixes_str && prefixes_str[0]) { + paths = details::split_source_prefixes(prefixes_str); + } + return paths; + } + + const std::vector &get_paths_from_env_variable() { + static std::vector paths = get_paths_from_env_variable_impl(); + return paths; + } + +#ifdef BACKWARD_ATLEAST_CXX11 + SourceFile(const SourceFile &) = delete; + SourceFile &operator=(const SourceFile &) = delete; +#endif +}; + +class SnippetFactory { +public: + typedef SourceFile::lines_t lines_t; + + lines_t get_snippet(const std::string &filename, unsigned line_start, + unsigned context_size) { + + SourceFile &src_file = get_src_file(filename); + unsigned start = line_start - context_size / 2; + return src_file.get_lines(start, context_size); + } + + lines_t get_combined_snippet(const std::string &filename_a, unsigned line_a, + const std::string &filename_b, unsigned line_b, + unsigned context_size) { + SourceFile &src_file_a = get_src_file(filename_a); + SourceFile &src_file_b = get_src_file(filename_b); + + lines_t lines = + src_file_a.get_lines(line_a - context_size / 4, context_size / 2); + src_file_b.get_lines(line_b - context_size / 4, context_size / 2, lines); + return lines; + } + + lines_t get_coalesced_snippet(const std::string &filename, unsigned line_a, + unsigned line_b, unsigned context_size) { + SourceFile &src_file = get_src_file(filename); + + using std::max; + using std::min; + unsigned a = min(line_a, line_b); + unsigned b = max(line_a, line_b); + + if ((b - a) < (context_size / 3)) { + return src_file.get_lines((a + b - context_size + 1) / 2, context_size); + } + + lines_t lines = src_file.get_lines(a - context_size / 4, context_size / 2); + src_file.get_lines(b - context_size / 4, context_size / 2, lines); + return lines; + } + +private: + typedef details::hashtable::type src_files_t; + src_files_t _src_files; + + SourceFile &get_src_file(const std::string &filename) { + src_files_t::iterator it = _src_files.find(filename); + if (it != _src_files.end()) { + return it->second; + } + SourceFile &new_src_file = _src_files[filename]; + new_src_file = SourceFile(filename); + return new_src_file; + } +}; + +/*************** PRINTER ***************/ + +namespace ColorMode { +enum type { automatic, never, always }; +} + +class cfile_streambuf : public std::streambuf { +public: + cfile_streambuf(FILE *_sink) : sink(_sink) {} + int_type underflow() override { return traits_type::eof(); } + int_type overflow(int_type ch) override { + if (traits_type::not_eof(ch) && fputc(ch, sink) != EOF) { + return ch; + } + return traits_type::eof(); + } + + std::streamsize xsputn(const char_type *s, std::streamsize count) override { + return static_cast( + fwrite(s, sizeof *s, static_cast(count), sink)); + } + +#ifdef BACKWARD_ATLEAST_CXX11 +public: + cfile_streambuf(const cfile_streambuf &) = delete; + cfile_streambuf &operator=(const cfile_streambuf &) = delete; +#else +private: + cfile_streambuf(const cfile_streambuf &); + cfile_streambuf &operator=(const cfile_streambuf &); +#endif + +private: + FILE *sink; + std::vector buffer; +}; + +#ifdef BACKWARD_SYSTEM_LINUX + +namespace Color { +enum type { yellow = 33, purple = 35, reset = 39 }; +} // namespace Color + +class Colorize { +public: + Colorize(std::ostream &os) : _os(os), _reset(false), _enabled(false) {} + + void activate(ColorMode::type mode) { _enabled = mode == ColorMode::always; } + + void activate(ColorMode::type mode, FILE *fp) { activate(mode, fileno(fp)); } + + void set_color(Color::type ccode) { + if (!_enabled) + return; + + // I assume that the terminal can handle basic colors. Seriously I + // don't want to deal with all the termcap shit. + _os << "\033[" << static_cast(ccode) << "m"; + _reset = (ccode != Color::reset); + } + + ~Colorize() { + if (_reset) { + set_color(Color::reset); + } + } + +private: + void activate(ColorMode::type mode, int fd) { + activate(mode == ColorMode::automatic && isatty(fd) ? ColorMode::always + : mode); + } + + std::ostream &_os; + bool _reset; + bool _enabled; +}; + +#else // ndef BACKWARD_SYSTEM_LINUX + +namespace Color { +enum type { yellow = 0, purple = 0, reset = 0 }; +} // namespace Color + +class Colorize { +public: + Colorize(std::ostream &) {} + void activate(ColorMode::type) {} + void activate(ColorMode::type, FILE *) {} + void set_color(Color::type) {} +}; + +#endif // BACKWARD_SYSTEM_LINUX + +class Printer { +public: + bool snippet; + ColorMode::type color_mode; + bool address; + bool object; + int inliner_context_size; + int trace_context_size; + + Printer() + : snippet(true), color_mode(ColorMode::automatic), address(false), + object(false), inliner_context_size(5), trace_context_size(7) {} + + template FILE *print(ST &st, FILE *fp = stderr) { + cfile_streambuf obuf(fp); + std::ostream os(&obuf); + Colorize colorize(os); + colorize.activate(color_mode, fp); + print_stacktrace(st, os, colorize); + return fp; + } + + template std::ostream &print(ST &st, std::ostream &os) { + Colorize colorize(os); + colorize.activate(color_mode); + print_stacktrace(st, os, colorize); + return os; + } + + template + FILE *print(IT begin, IT end, FILE *fp = stderr, size_t thread_id = 0) { + cfile_streambuf obuf(fp); + std::ostream os(&obuf); + Colorize colorize(os); + colorize.activate(color_mode, fp); + print_stacktrace(begin, end, os, thread_id, colorize); + return fp; + } + + template + std::ostream &print(IT begin, IT end, std::ostream &os, + size_t thread_id = 0) { + Colorize colorize(os); + colorize.activate(color_mode); + print_stacktrace(begin, end, os, thread_id, colorize); + return os; + } + + TraceResolver const &resolver() const { return _resolver; } + +private: + TraceResolver _resolver; + SnippetFactory _snippets; + + template + void print_stacktrace(ST &st, std::ostream &os, Colorize &colorize) { + print_header(os, st.thread_id()); + _resolver.load_stacktrace(st); + for (size_t trace_idx = st.size(); trace_idx > 0; --trace_idx) { + print_trace(os, _resolver.resolve(st[trace_idx - 1]), colorize); + } + } + + template + void print_stacktrace(IT begin, IT end, std::ostream &os, size_t thread_id, + Colorize &colorize) { + print_header(os, thread_id); + for (; begin != end; ++begin) { + print_trace(os, *begin, colorize); + } + } + + void print_header(std::ostream &os, size_t thread_id) { + os << "Stack trace (most recent call last)"; + if (thread_id) { + os << " in thread " << thread_id; + } + os << ":\n"; + } + + void print_trace(std::ostream &os, const ResolvedTrace &trace, + Colorize &colorize) { + os << "#" << std::left << std::setw(2) << trace.idx << std::right; + bool already_indented = true; + + if (!trace.source.filename.size() || object) { + os << " Object \"" << trace.object_filename << "\", at " << trace.addr + << ", in " << trace.object_function << "\n"; + already_indented = false; + } + + for (size_t inliner_idx = trace.inliners.size(); inliner_idx > 0; + --inliner_idx) { + if (!already_indented) { + os << " "; + } + const ResolvedTrace::SourceLoc &inliner_loc = + trace.inliners[inliner_idx - 1]; + print_source_loc(os, " | ", inliner_loc); + if (snippet) { + print_snippet(os, " | ", inliner_loc, colorize, Color::purple, + inliner_context_size); + } + already_indented = false; + } + + if (trace.source.filename.size()) { + if (!already_indented) { + os << " "; + } + print_source_loc(os, " ", trace.source, trace.addr); + if (snippet) { + print_snippet(os, " ", trace.source, colorize, Color::yellow, + trace_context_size); + } + } + } + + void print_snippet(std::ostream &os, const char *indent, + const ResolvedTrace::SourceLoc &source_loc, + Colorize &colorize, Color::type color_code, + int context_size) { + using namespace std; + typedef SnippetFactory::lines_t lines_t; + + lines_t lines = _snippets.get_snippet(source_loc.filename, source_loc.line, + static_cast(context_size)); + + for (lines_t::const_iterator it = lines.begin(); it != lines.end(); ++it) { + if (it->first == source_loc.line) { + colorize.set_color(color_code); + os << indent << ">"; + } else { + os << indent << " "; + } + os << std::setw(4) << it->first << ": " << it->second << "\n"; + if (it->first == source_loc.line) { + colorize.set_color(Color::reset); + } + } + } + + void print_source_loc(std::ostream &os, const char *indent, + const ResolvedTrace::SourceLoc &source_loc, + void *addr = nullptr) { + os << indent << "Source \"" << source_loc.filename << "\", line " + << source_loc.line << ", in " << source_loc.function; + + if (address && addr != nullptr) { + os << " [" << addr << "]"; + } + os << "\n"; + } +}; + +/*************** SIGNALS HANDLING ***************/ + +#if defined(BACKWARD_SYSTEM_LINUX) || defined(BACKWARD_SYSTEM_DARWIN) + +class SignalHandling { +public: + static std::vector make_default_signals() { + const int posix_signals[] = { + // Signals for which the default action is "Core". + SIGABRT, // Abort signal from abort(3) + SIGBUS, // Bus error (bad memory access) + SIGFPE, // Floating point exception + SIGILL, // Illegal Instruction + SIGIOT, // IOT trap. A synonym for SIGABRT + SIGQUIT, // Quit from keyboard + SIGSEGV, // Invalid memory reference + SIGSYS, // Bad argument to routine (SVr4) + SIGTRAP, // Trace/breakpoint trap + SIGXCPU, // CPU time limit exceeded (4.2BSD) + SIGXFSZ, // File size limit exceeded (4.2BSD) +#if defined(BACKWARD_SYSTEM_DARWIN) + SIGEMT, // emulation instruction executed +#endif + }; + return std::vector(posix_signals, + posix_signals + + sizeof posix_signals / sizeof posix_signals[0]); + } + + SignalHandling(const std::vector &posix_signals = make_default_signals()) + : _loaded(false) { + bool success = true; + + const size_t stack_size = 1024 * 1024 * 8; + _stack_content.reset(static_cast(malloc(stack_size))); + if (_stack_content) { + stack_t ss; + ss.ss_sp = _stack_content.get(); + ss.ss_size = stack_size; + ss.ss_flags = 0; + if (sigaltstack(&ss, nullptr) < 0) { + success = false; + } + } else { + success = false; + } + + for (size_t i = 0; i < posix_signals.size(); ++i) { + struct sigaction action; + memset(&action, 0, sizeof action); + action.sa_flags = + static_cast(SA_SIGINFO | SA_ONSTACK | SA_NODEFER | SA_RESETHAND); + sigfillset(&action.sa_mask); + sigdelset(&action.sa_mask, posix_signals[i]); +#if defined(__clang__) +#pragma clang diagnostic push +#pragma clang diagnostic ignored "-Wdisabled-macro-expansion" +#endif + action.sa_sigaction = &sig_handler; +#if defined(__clang__) +#pragma clang diagnostic pop +#endif + + int r = sigaction(posix_signals[i], &action, nullptr); + if (r < 0) + success = false; + } + + _loaded = success; + } + + bool loaded() const { return _loaded; } + + static void handleSignal(int, siginfo_t *info, void *_ctx) { + ucontext_t *uctx = static_cast(_ctx); + + StackTrace st; + void *error_addr = nullptr; +#ifdef REG_RIP // x86_64 + error_addr = reinterpret_cast(uctx->uc_mcontext.gregs[REG_RIP]); +#elif defined(REG_EIP) // x86_32 + error_addr = reinterpret_cast(uctx->uc_mcontext.gregs[REG_EIP]); +#elif defined(__arm__) + error_addr = reinterpret_cast(uctx->uc_mcontext.arm_pc); +#elif defined(__aarch64__) + #if defined(__APPLE__) + error_addr = reinterpret_cast(uctx->uc_mcontext->__ss.__pc); + #else + error_addr = reinterpret_cast(uctx->uc_mcontext.pc); + #endif +#elif defined(__mips__) + error_addr = reinterpret_cast( + reinterpret_cast(&uctx->uc_mcontext)->sc_pc); +#elif defined(__ppc__) || defined(__powerpc) || defined(__powerpc__) || \ + defined(__POWERPC__) + error_addr = reinterpret_cast(uctx->uc_mcontext.regs->nip); +#elif defined(__riscv) + error_addr = reinterpret_cast(uctx->uc_mcontext.__gregs[REG_PC]); +#elif defined(__s390x__) + error_addr = reinterpret_cast(uctx->uc_mcontext.psw.addr); +#elif defined(__APPLE__) && defined(__x86_64__) + error_addr = reinterpret_cast(uctx->uc_mcontext->__ss.__rip); +#elif defined(__APPLE__) + error_addr = reinterpret_cast(uctx->uc_mcontext->__ss.__eip); +#else +#warning ":/ sorry, ain't know no nothing none not of your architecture!" +#endif + if (error_addr) { + st.load_from(error_addr, 32, reinterpret_cast(uctx), + info->si_addr); + } else { + st.load_here(32, reinterpret_cast(uctx), info->si_addr); + } + + Printer printer; + printer.address = true; + printer.print(st, stderr); + +#if _XOPEN_SOURCE >= 700 || _POSIX_C_SOURCE >= 200809L + psiginfo(info, nullptr); +#else + (void)info; +#endif + } + +private: + details::handle _stack_content; + bool _loaded; + +#ifdef __GNUC__ + __attribute__((noreturn)) +#endif + static void + sig_handler(int signo, siginfo_t *info, void *_ctx) { + handleSignal(signo, info, _ctx); + + // try to forward the signal. + raise(info->si_signo); + + // terminate the process immediately. + puts("watf? exit"); + _exit(EXIT_FAILURE); + } +}; + +#endif // BACKWARD_SYSTEM_LINUX || BACKWARD_SYSTEM_DARWIN + +#ifdef BACKWARD_SYSTEM_WINDOWS + +class SignalHandling { +public: + SignalHandling(const std::vector & = std::vector()) + : reporter_thread_([]() { + /* We handle crashes in a utility thread: + backward structures and some Windows functions called here + need stack space, which we do not have when we encounter a + stack overflow. + To support reporting stack traces during a stack overflow, + we create a utility thread at startup, which waits until a + crash happens or the program exits normally. */ + + { + std::unique_lock lk(mtx()); + cv().wait(lk, [] { return crashed() != crash_status::running; }); + } + if (crashed() == crash_status::crashed) { + handle_stacktrace(skip_recs()); + } + { + std::unique_lock lk(mtx()); + crashed() = crash_status::ending; + } + cv().notify_one(); + }) { + SetUnhandledExceptionFilter(crash_handler); + + signal(SIGABRT, signal_handler); + _set_abort_behavior(0, _WRITE_ABORT_MSG | _CALL_REPORTFAULT); + + std::set_terminate(&terminator); +#ifndef BACKWARD_ATLEAST_CXX17 + std::set_unexpected(&terminator); +#endif + _set_purecall_handler(&terminator); + _set_invalid_parameter_handler(&invalid_parameter_handler); + } + bool loaded() const { return true; } + + ~SignalHandling() { + { + std::unique_lock lk(mtx()); + crashed() = crash_status::normal_exit; + } + + cv().notify_one(); + + reporter_thread_.join(); + } + +private: + static CONTEXT *ctx() { + static CONTEXT data; + return &data; + } + + enum class crash_status { running, crashed, normal_exit, ending }; + + static crash_status &crashed() { + static crash_status data; + return data; + } + + static std::mutex &mtx() { + static std::mutex data; + return data; + } + + static std::condition_variable &cv() { + static std::condition_variable data; + return data; + } + + static HANDLE &thread_handle() { + static HANDLE handle; + return handle; + } + + std::thread reporter_thread_; + + // TODO: how not to hardcode these? + static const constexpr int signal_skip_recs = +#ifdef __clang__ + // With clang, RtlCaptureContext also captures the stack frame of the + // current function Below that, there ar 3 internal Windows functions + 4 +#else + // With MSVC cl, RtlCaptureContext misses the stack frame of the current + // function The first entries during StackWalk are the 3 internal Windows + // functions + 3 +#endif + ; + + static int &skip_recs() { + static int data; + return data; + } + + static inline void terminator() { + crash_handler(signal_skip_recs); + abort(); + } + + static inline void signal_handler(int) { + crash_handler(signal_skip_recs); + abort(); + } + + static inline void __cdecl invalid_parameter_handler(const wchar_t *, + const wchar_t *, + const wchar_t *, + unsigned int, + uintptr_t) { + crash_handler(signal_skip_recs); + abort(); + } + + NOINLINE static LONG WINAPI crash_handler(EXCEPTION_POINTERS *info) { + // The exception info supplies a trace from exactly where the issue was, + // no need to skip records + crash_handler(0, info->ContextRecord); + return EXCEPTION_CONTINUE_SEARCH; + } + + NOINLINE static void crash_handler(int skip, CONTEXT *ct = nullptr) { + + if (ct == nullptr) { + RtlCaptureContext(ctx()); + } else { + memcpy(ctx(), ct, sizeof(CONTEXT)); + } + DuplicateHandle(GetCurrentProcess(), GetCurrentThread(), + GetCurrentProcess(), &thread_handle(), 0, FALSE, + DUPLICATE_SAME_ACCESS); + + skip_recs() = skip; + + { + std::unique_lock lk(mtx()); + crashed() = crash_status::crashed; + } + + cv().notify_one(); + + { + std::unique_lock lk(mtx()); + cv().wait(lk, [] { return crashed() != crash_status::crashed; }); + } + } + + static void handle_stacktrace(int skip_frames = 0) { + // printer creates the TraceResolver, which can supply us a machine type + // for stack walking. Without this, StackTrace can only guess using some + // macros. + // StackTrace also requires that the PDBs are already loaded, which is done + // in the constructor of TraceResolver + Printer printer; + + StackTrace st; + st.set_machine_type(printer.resolver().machine_type()); + st.set_thread_handle(thread_handle()); + st.load_here(32 + skip_frames, ctx()); + st.skip_n_firsts(skip_frames); + + printer.address = true; + printer.print(st, std::cerr); + } +}; + +#endif // BACKWARD_SYSTEM_WINDOWS + +#ifdef BACKWARD_SYSTEM_UNKNOWN + +class SignalHandling { +public: + SignalHandling(const std::vector & = std::vector()) {} + bool init() { return false; } + bool loaded() { return false; } +}; + +#endif // BACKWARD_SYSTEM_UNKNOWN + +} // namespace backward + +#endif /* H_GUARD */ diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index 5f0fc6f25..858ef8974 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -29,6 +29,15 @@ #include "cartographer_ros_msgs/srv/submap_query.hpp" #include +#include "backward.hpp" + +namespace backward { + +backward::SignalHandling sh; + +} // namespace backward + + namespace cartographer_rviz { namespace { diff --git a/cartographer_rviz/cartographer_rviz/rviz_plugin_description.xml b/cartographer_rviz/cartographer_rviz/rviz_plugin_description.xml index 80b5b2400..fc7cb9e90 100644 --- a/cartographer_rviz/cartographer_rviz/rviz_plugin_description.xml +++ b/cartographer_rviz/cartographer_rviz/rviz_plugin_description.xml @@ -15,7 +15,7 @@ --> - diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc index 110f002d8..e07469fdc 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/cartographer_rviz/submaps_display.cc @@ -35,6 +35,15 @@ // Include ament_index_cpp::get_package_share_directory #include +#include "backward.hpp" + +namespace backward { + +backward::SignalHandling sh2; + +} // namespace backward + + namespace cartographer_rviz { namespace { diff --git a/cartographer_rviz/package.xml b/cartographer_rviz/package.xml index 38e714101..03207a6c8 100644 --- a/cartographer_rviz/package.xml +++ b/cartographer_rviz/package.xml @@ -57,6 +57,6 @@ ament_cmake - + diff --git a/cartographer_rviz/rviz_plugin_description.xml b/cartographer_rviz/rviz_plugin_description.xml index 80b5b2400..fc7cb9e90 100644 --- a/cartographer_rviz/rviz_plugin_description.xml +++ b/cartographer_rviz/rviz_plugin_description.xml @@ -15,7 +15,7 @@ --> - From ebf144c229b075c7de6ab853ce2842d8f50003e3 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Thu, 15 Jul 2021 16:55:27 +0200 Subject: [PATCH 57/94] some tries Signed-off-by: Guillaume Doisy --- cartographer_ros/package.xml | 6 +----- cartographer_rviz/CMakeLists.txt | 16 ++++++++++++++++ .../cartographer_rviz/drawable_submap.h | 2 +- .../cartographer_rviz/submaps_display.h | 2 +- cartographer_rviz/package.xml | 4 ++-- 5 files changed, 21 insertions(+), 9 deletions(-) diff --git a/cartographer_ros/package.xml b/cartographer_ros/package.xml index ff84f7001..20afb2d45 100644 --- a/cartographer_ros/package.xml +++ b/cartographer_ros/package.xml @@ -48,14 +48,10 @@ libgflags-dev libgoogle-glog-dev libpcl-all-dev - message_runtime nav_msgs pcl_conversions robot_state_publisher - rosbag - roscpp - roslaunch - roslib + rclcpp sensor_msgs std_msgs tf2 diff --git a/cartographer_rviz/CMakeLists.txt b/cartographer_rviz/CMakeLists.txt index 0e3b0260f..9742e0a98 100644 --- a/cartographer_rviz/CMakeLists.txt +++ b/cartographer_rviz/CMakeLists.txt @@ -47,6 +47,7 @@ find_package(rviz_rendering REQUIRED) find_package(rviz_common REQUIRED) find_package(rviz_default_plugins REQUIRED) find_package(pluginlib REQUIRED) +find_package(rcutils REQUIRED) #find_package(rviz REQUIRED) find_package(backward_ros REQUIRED) @@ -71,6 +72,7 @@ pluginlib_export_plugin_description_file(rviz_common rviz_plugin_description.xml ament_target_dependencies(${PROJECT_NAME} PUBLIC rclcpp + cartographer # cartographer_ros cartographer_ros_msgs rviz2 @@ -80,9 +82,22 @@ ament_target_dependencies(${PROJECT_NAME} PUBLIC rviz_common rviz_default_plugins pcl_conversions + libdw + rcutils # rviz ) +target_link_libraries(${PROJECT_NAME} PUBLIC +# usb-1.0 + -lstdc++fs + -ldl +# -lexiv2 + -lboost_thread) +# exiv2lib +# nlohmann_json::nlohmann_json +# ${gphoto2_LIBRARIES} +# ${Boost_SYSTEM_LIBRARY}) +ament_export_include_directories(include) ament_export_dependencies(rosidl_default_runtime) # PCL @@ -173,6 +188,7 @@ install( # replaces catkin_package(LIBRARIES ${PROJECT_NAME}) ament_export_libraries(${PROJECT_NAME}) +#LD_LIBRARY_PATH() if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.h b/cartographer_rviz/cartographer_rviz/drawable_submap.h index 0f0b558cd..2f6322f98 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.h +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.h @@ -19,7 +19,7 @@ #include #include - +#include #include "Eigen/Core" #include "Eigen/Geometry" #include "OgreSceneManager.h" diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.h b/cartographer_rviz/cartographer_rviz/submaps_display.h index 357772725..babc80d41 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.h +++ b/cartographer_rviz/cartographer_rviz/submaps_display.h @@ -21,7 +21,7 @@ #include #include #include - +#include #include "absl/synchronization/mutex.h" #include "cartographer/common/port.h" #include "cartographer_ros_msgs/msg/submap_list.hpp" diff --git a/cartographer_rviz/package.xml b/cartographer_rviz/package.xml index 03207a6c8..265a8f8eb 100644 --- a/cartographer_rviz/package.xml +++ b/cartographer_rviz/package.xml @@ -53,10 +53,10 @@ rviz_rendering rviz_common rviz_default_plugins - backward_ros + rcutils ament_cmake - + From 7154b7a5535efa4c1f8f7114e08e6d02b06bf1fa Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Fri, 16 Jul 2021 13:43:50 +0200 Subject: [PATCH 58/94] some other tries. i think the prob is in the linking to abseilcpp Signed-off-by: Guillaume Doisy --- cartographer_rviz/CMakeLists.txt | 21 +++++++++++++++++-- .../cartographer_rviz/drawable_submap.cc | 6 +++--- .../cartographer_rviz/submaps_display.cc | 7 ++++--- cartographer_rviz/package.xml | 1 + 4 files changed, 27 insertions(+), 8 deletions(-) diff --git a/cartographer_rviz/CMakeLists.txt b/cartographer_rviz/CMakeLists.txt index 9742e0a98..797545eca 100644 --- a/cartographer_rviz/CMakeLists.txt +++ b/cartographer_rviz/CMakeLists.txt @@ -15,11 +15,13 @@ cmake_minimum_required(VERSION 3.5) project(cartographer_rviz) +set(CMAKE_POSITION_INDEPENDENT_CODE TRUE) +#add_subdirectory(../../../../../../abseil-cpp/) find_package(ament_cmake REQUIRED) # Default to C++17 -set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD 11) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra) @@ -71,6 +73,7 @@ pluginlib_export_plugin_description_file(rviz_common rviz_plugin_description.xml ament_target_dependencies(${PROJECT_NAME} PUBLIC + absl rclcpp cartographer # cartographer_ros @@ -92,7 +95,21 @@ target_link_libraries(${PROJECT_NAME} PUBLIC -lstdc++fs -ldl # -lexiv2 - -lboost_thread) + -lboost_thread + absl::algorithm + absl::base + absl::debugging + absl::flat_hash_map + absl::flags + absl::memory + absl::meta + absl::numeric + absl::random_random + absl::strings + absl::synchronization + absl::time + absl::utility + ) # exiv2lib # nlohmann_json::nlohmann_json # ${gphoto2_LIBRARIES} diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index 858ef8974..d1822d194 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -31,11 +31,11 @@ #include "backward.hpp" -namespace backward { +//namespace backward { -backward::SignalHandling sh; +//backward::SignalHandling sh; -} // namespace backward +//} // namespace backward namespace cartographer_rviz { diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc index e07469fdc..7cde85038 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/cartographer_rviz/submaps_display.cc @@ -19,6 +19,7 @@ #include "OgreResourceGroupManager.h" #include "absl/memory/memory.h" #include "absl/synchronization/mutex.h" + #include "cartographer/mapping/id.h" #include "cartographer_ros_msgs/msg/submap_list.hpp" #include "cartographer_ros_msgs/srv/submap_query.hpp" @@ -37,11 +38,11 @@ #include "backward.hpp" -namespace backward { +//namespace backward { -backward::SignalHandling sh2; +//backward::SignalHandling sh2; -} // namespace backward +//} // namespace backward namespace cartographer_rviz { diff --git a/cartographer_rviz/package.xml b/cartographer_rviz/package.xml index 265a8f8eb..fea142fda 100644 --- a/cartographer_rviz/package.xml +++ b/cartographer_rviz/package.xml @@ -45,6 +45,7 @@ libqt5-widgets qtbase5-dev rclcpp + absl libpcl-all-dev pcl_conversions From c5f73decc806e0659031220ced55988f43c7f5a2 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Thu, 16 Sep 2021 13:57:35 +0200 Subject: [PATCH 59/94] solved one issue, got another one Signed-off-by: Guillaume Doisy --- cartographer_rviz/CMakeLists.txt | 16 ++++++++-------- .../cartographer_rviz/submaps_display.cc | 6 +++--- .../cartographer_rviz/submaps_display.h | 3 +-- 3 files changed, 12 insertions(+), 13 deletions(-) diff --git a/cartographer_rviz/CMakeLists.txt b/cartographer_rviz/CMakeLists.txt index 797545eca..4366e283c 100644 --- a/cartographer_rviz/CMakeLists.txt +++ b/cartographer_rviz/CMakeLists.txt @@ -21,7 +21,7 @@ set(CMAKE_POSITION_INDEPENDENT_CODE TRUE) find_package(ament_cmake REQUIRED) # Default to C++17 -set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_STANDARD 17) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra) @@ -51,7 +51,7 @@ find_package(rviz_default_plugins REQUIRED) find_package(pluginlib REQUIRED) find_package(rcutils REQUIRED) #find_package(rviz REQUIRED) -find_package(backward_ros REQUIRED) +#find_package(backward_ros REQUIRED) include_directories( include @@ -70,7 +70,8 @@ target_compile_definitions(${PROJECT_NAME} PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDIN target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") pluginlib_export_plugin_description_file(rviz_common rviz_plugin_description.xml) - +register_rviz_ogre_media_exports(DIRECTORIES "ogre_media/materials/scripts") +register_rviz_ogre_media_exports(DIRECTORIES "ogre_media/materials/glsl120") ament_target_dependencies(${PROJECT_NAME} PUBLIC absl @@ -85,7 +86,7 @@ ament_target_dependencies(${PROJECT_NAME} PUBLIC rviz_common rviz_default_plugins pcl_conversions - libdw +# libdw rcutils # rviz ) @@ -100,11 +101,11 @@ target_link_libraries(${PROJECT_NAME} PUBLIC absl::base absl::debugging absl::flat_hash_map - absl::flags +# absl::flags absl::memory absl::meta absl::numeric - absl::random_random +# absl::random_random absl::strings absl::synchronization absl::time @@ -192,8 +193,7 @@ install(FILES DESTINATION share/${PROJECT_NAME}) install(DIRECTORY ogre_media - DESTINATION share/${PROJECT_NAME}/ -) + DESTINATION share/${PROJECT_NAME}) install( TARGETS ${PROJECT_NAME} diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc index 7cde85038..4fea32281 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/cartographer_rviz/submaps_display.cc @@ -78,7 +78,7 @@ SubmapsDisplay::SubmapsDisplay() : rclcpp::Node("submaps_display") { callback_group_executor->spin(); }); - client_ = this->create_client<::cartographer_ros_msgs::srv::SubmapQuery>("",rmw_qos_profile_services_default, sync_srv_client_callback_group);//update_nh_.serviceClient<::cartographer_ros_msgs::msg::SubmapQuery>(""); + client_ = this->create_client<::cartographer_ros_msgs::srv::SubmapQuery>(kDefaultSubmapQueryServiceName,rmw_qos_profile_services_default, sync_srv_client_callback_group);//update_nh_.serviceClient<::cartographer_ros_msgs::msg::SubmapQuery>(""); trajectories_category_ = new ::rviz_common::properties::Property( "Submaps", QVariant(), "List of all submaps, organized by trajectories.", this); @@ -124,13 +124,13 @@ void SubmapsDisplay::CreateClient() { } void SubmapsDisplay::onInitialize() { - MFDClass::onInitialize(); + RTDClass::onInitialize(); map_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode(); CreateClient(); } void SubmapsDisplay::reset() { - MFDClass::reset(); + RTDClass::reset(); absl::MutexLock locker(&mutex_); client_.reset(); trajectories_.clear(); diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.h b/cartographer_rviz/cartographer_rviz/submaps_display.h index babc80d41..7f9ef758a 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.h +++ b/cartographer_rviz/cartographer_rviz/submaps_display.h @@ -60,8 +60,7 @@ struct Trajectory : public QObject { // every submap containing pre-multiplied alpha and grayscale values, these are // then alpha blended together. class SubmapsDisplay - : public ::rviz_common::MessageFilterDisplay<::cartographer_ros_msgs::msg::SubmapList>, rclcpp::Node { //::rviz_common::RosTopicDisplay<::cartographer_ros_msgs::msg::SubmapList>, rclcpp::Node { // - // + : public ::rviz_common::RosTopicDisplay<::cartographer_ros_msgs::msg::SubmapList>, rclcpp::Node { //::rviz_common::MessageFilterDisplay<::cartographer_ros_msgs::msg::SubmapList>, rclcpp::Node { Q_OBJECT public: From 7b9adbf7439db8ddc414954778ac807692353f1d Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Thu, 16 Sep 2021 14:21:55 +0200 Subject: [PATCH 60/94] backward gbd Signed-off-by: Guillaume Doisy --- cartographer_rviz/CMakeLists.txt | 7 +- .../cartographer_rviz/backward.cpp | 42 - .../cartographer_rviz/backward.hpp | 4457 ----------------- .../cartographer_rviz/drawable_submap.cc | 8 - .../cartographer_rviz/submaps_display.cc | 9 - cartographer_rviz/package.xml | 1 + 6 files changed, 7 insertions(+), 4517 deletions(-) delete mode 100644 cartographer_rviz/cartographer_rviz/backward.cpp delete mode 100644 cartographer_rviz/cartographer_rviz/backward.hpp diff --git a/cartographer_rviz/CMakeLists.txt b/cartographer_rviz/CMakeLists.txt index 4366e283c..dde7aa1c4 100644 --- a/cartographer_rviz/CMakeLists.txt +++ b/cartographer_rviz/CMakeLists.txt @@ -34,6 +34,7 @@ set(BUILD_SHARED_LIBS OFF) option(BUILD_GRPC "build features that require Cartographer gRPC support" false) google_initialize_cartographer_project() +add_compile_options(-g) find_package(absl REQUIRED) find_package(Eigen3 REQUIRED) @@ -51,7 +52,7 @@ find_package(rviz_default_plugins REQUIRED) find_package(pluginlib REQUIRED) find_package(rcutils REQUIRED) #find_package(rviz REQUIRED) -#find_package(backward_ros REQUIRED) +find_package(backward_ros REQUIRED) include_directories( include @@ -180,6 +181,10 @@ target_link_libraries(${PROJECT_NAME} PUBLIC cartographer) #target_link_libraries(${PROJECT_NAME} PUBLIC cartographer_ros) pluginlib_export_plugin_description_file(rviz_common rviz_plugin_description.xml) +register_rviz_ogre_media_exports(DIRECTORIES "ogre_media/materials/scripts") +register_rviz_ogre_media_exports(DIRECTORIES "ogre_media/materials/glsl120") + + #install(DIRECTORY configuration_files # DESTINATION share/${PROJECT_NAME}/ diff --git a/cartographer_rviz/cartographer_rviz/backward.cpp b/cartographer_rviz/cartographer_rviz/backward.cpp deleted file mode 100644 index 110441cba..000000000 --- a/cartographer_rviz/cartographer_rviz/backward.cpp +++ /dev/null @@ -1,42 +0,0 @@ -// Pick your poison. -// -// On GNU/Linux, you have few choices to get the most out of your stack trace. -// -// By default you get: -// - object filename -// - function name -// -// In order to add: -// - source filename -// - line and column numbers -// - source code snippet (assuming the file is accessible) - -// Install one of the following libraries then uncomment one of the macro (or -// better, add the detection of the lib and the macro definition in your build -// system) - -// - apt-get install libdw-dev ... -// - g++/clang++ -ldw ... -// #define BACKWARD_HAS_DW 1 - -// - apt-get install binutils-dev ... -// - g++/clang++ -lbfd ... -// #define BACKWARD_HAS_BFD 1 - -// - apt-get install libdwarf-dev ... -// - g++/clang++ -ldwarf ... -// #define BACKWARD_HAS_DWARF 1 - -// Regardless of the library you choose to read the debug information, -// for potentially more detailed stack traces you can use libunwind -// - apt-get install libunwind-dev -// - g++/clang++ -lunwind -// #define BACKWARD_HAS_LIBUNWIND 1 - -#include "backward.hpp" - -namespace backward { - -backward::SignalHandling sh; - -} // namespace backward diff --git a/cartographer_rviz/cartographer_rviz/backward.hpp b/cartographer_rviz/cartographer_rviz/backward.hpp deleted file mode 100644 index 93037d5dc..000000000 --- a/cartographer_rviz/cartographer_rviz/backward.hpp +++ /dev/null @@ -1,4457 +0,0 @@ -/* - * backward.hpp - * Copyright 2013 Google Inc. All Rights Reserved. - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#ifndef H_6B9572DA_A64B_49E6_B234_051480991C89 -#define H_6B9572DA_A64B_49E6_B234_051480991C89 - -#ifndef __cplusplus -#error "It's not going to compile without a C++ compiler..." -#endif - -#if defined(BACKWARD_CXX11) -#elif defined(BACKWARD_CXX98) -#else -#if __cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1800) -#define BACKWARD_CXX11 -#define BACKWARD_ATLEAST_CXX11 -#define BACKWARD_ATLEAST_CXX98 -#if __cplusplus >= 201703L || (defined(_MSVC_LANG) && _MSVC_LANG >= 201703L) -#define BACKWARD_ATLEAST_CXX17 -#endif -#else -#define BACKWARD_CXX98 -#define BACKWARD_ATLEAST_CXX98 -#endif -#endif - -// You can define one of the following (or leave it to the auto-detection): -// -// #define BACKWARD_SYSTEM_LINUX -// - specialization for linux -// -// #define BACKWARD_SYSTEM_DARWIN -// - specialization for Mac OS X 10.5 and later. -// -// #define BACKWARD_SYSTEM_WINDOWS -// - specialization for Windows (Clang 9 and MSVC2017) -// -// #define BACKWARD_SYSTEM_UNKNOWN -// - placebo implementation, does nothing. -// -#if defined(BACKWARD_SYSTEM_LINUX) -#elif defined(BACKWARD_SYSTEM_DARWIN) -#elif defined(BACKWARD_SYSTEM_UNKNOWN) -#elif defined(BACKWARD_SYSTEM_WINDOWS) -#else -#if defined(__linux) || defined(__linux__) -#define BACKWARD_SYSTEM_LINUX -#elif defined(__APPLE__) -#define BACKWARD_SYSTEM_DARWIN -#elif defined(_WIN32) -#define BACKWARD_SYSTEM_WINDOWS -#else -#define BACKWARD_SYSTEM_UNKNOWN -#endif -#endif - -#define NOINLINE __attribute__((noinline)) - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#if defined(BACKWARD_SYSTEM_LINUX) - -// On linux, backtrace can back-trace or "walk" the stack using the following -// libraries: -// -// #define BACKWARD_HAS_UNWIND 1 -// - unwind comes from libgcc, but I saw an equivalent inside clang itself. -// - with unwind, the stacktrace is as accurate as it can possibly be, since -// this is used by the C++ runtine in gcc/clang for stack unwinding on -// exception. -// - normally libgcc is already linked to your program by default. -// -// #define BACKWARD_HAS_LIBUNWIND 1 -// - libunwind provides, in some cases, a more accurate stacktrace as it knows -// to decode signal handler frames and lets us edit the context registers when -// unwinding, allowing stack traces over bad function references. -// -// #define BACKWARD_HAS_BACKTRACE == 1 -// - backtrace seems to be a little bit more portable than libunwind, but on -// linux, it uses unwind anyway, but abstract away a tiny information that is -// sadly really important in order to get perfectly accurate stack traces. -// - backtrace is part of the (e)glib library. -// -// The default is: -// #define BACKWARD_HAS_UNWIND == 1 -// -// Note that only one of the define should be set to 1 at a time. -// -#if BACKWARD_HAS_UNWIND == 1 -#elif BACKWARD_HAS_LIBUNWIND == 1 -#elif BACKWARD_HAS_BACKTRACE == 1 -#else -#undef BACKWARD_HAS_UNWIND -#define BACKWARD_HAS_UNWIND 1 -#undef BACKWARD_HAS_LIBUNWIND -#define BACKWARD_HAS_LIBUNWIND 0 -#undef BACKWARD_HAS_BACKTRACE -#define BACKWARD_HAS_BACKTRACE 0 -#endif - -// On linux, backward can extract detailed information about a stack trace -// using one of the following libraries: -// -// #define BACKWARD_HAS_DW 1 -// - libdw gives you the most juicy details out of your stack traces: -// - object filename -// - function name -// - source filename -// - line and column numbers -// - source code snippet (assuming the file is accessible) -// - variables name and values (if not optimized out) -// - You need to link with the lib "dw": -// - apt-get install libdw-dev -// - g++/clang++ -ldw ... -// -// #define BACKWARD_HAS_BFD 1 -// - With libbfd, you get a fair amount of details: -// - object filename -// - function name -// - source filename -// - line numbers -// - source code snippet (assuming the file is accessible) -// - You need to link with the lib "bfd": -// - apt-get install binutils-dev -// - g++/clang++ -lbfd ... -// -// #define BACKWARD_HAS_DWARF 1 -// - libdwarf gives you the most juicy details out of your stack traces: -// - object filename -// - function name -// - source filename -// - line and column numbers -// - source code snippet (assuming the file is accessible) -// - variables name and values (if not optimized out) -// - You need to link with the lib "dwarf": -// - apt-get install libdwarf-dev -// - g++/clang++ -ldwarf ... -// -// #define BACKWARD_HAS_BACKTRACE_SYMBOL 1 -// - backtrace provides minimal details for a stack trace: -// - object filename -// - function name -// - backtrace is part of the (e)glib library. -// -// The default is: -// #define BACKWARD_HAS_BACKTRACE_SYMBOL == 1 -// -// Note that only one of the define should be set to 1 at a time. -// -#if BACKWARD_HAS_DW == 1 -#elif BACKWARD_HAS_BFD == 1 -#elif BACKWARD_HAS_DWARF == 1 -#elif BACKWARD_HAS_BACKTRACE_SYMBOL == 1 -#else -#undef BACKWARD_HAS_DW -#define BACKWARD_HAS_DW 0 -#undef BACKWARD_HAS_BFD -#define BACKWARD_HAS_BFD 0 -#undef BACKWARD_HAS_DWARF -#define BACKWARD_HAS_DWARF 0 -#undef BACKWARD_HAS_BACKTRACE_SYMBOL -#define BACKWARD_HAS_BACKTRACE_SYMBOL 1 -#endif - -#include -#include -#ifdef __ANDROID__ -// Old Android API levels define _Unwind_Ptr in both link.h and -// unwind.h Rename the one in link.h as we are not going to be using -// it -#define _Unwind_Ptr _Unwind_Ptr_Custom -#include -#undef _Unwind_Ptr -#else -#include -#endif -#include -#include -#include -#include - -#if BACKWARD_HAS_BFD == 1 -// NOTE: defining PACKAGE{,_VERSION} is required before including -// bfd.h on some platforms, see also: -// https://sourceware.org/bugzilla/show_bug.cgi?id=14243 -#ifndef PACKAGE -#define PACKAGE -#endif -#ifndef PACKAGE_VERSION -#define PACKAGE_VERSION -#endif -#include -#ifndef _GNU_SOURCE -#define _GNU_SOURCE -#include -#undef _GNU_SOURCE -#else -#include -#endif -#endif - -#if BACKWARD_HAS_DW == 1 -#include -#include -#include -#endif - -#if BACKWARD_HAS_DWARF == 1 -#include -#include -#include -#include -#include -#ifndef _GNU_SOURCE -#define _GNU_SOURCE -#include -#undef _GNU_SOURCE -#else -#include -#endif -#endif - -#if (BACKWARD_HAS_BACKTRACE == 1) || (BACKWARD_HAS_BACKTRACE_SYMBOL == 1) -// then we shall rely on backtrace -#include -#endif - -#endif // defined(BACKWARD_SYSTEM_LINUX) - -#if defined(BACKWARD_SYSTEM_DARWIN) -// On Darwin, backtrace can back-trace or "walk" the stack using the following -// libraries: -// -// #define BACKWARD_HAS_UNWIND 1 -// - unwind comes from libgcc, but I saw an equivalent inside clang itself. -// - with unwind, the stacktrace is as accurate as it can possibly be, since -// this is used by the C++ runtine in gcc/clang for stack unwinding on -// exception. -// - normally libgcc is already linked to your program by default. -// -// #define BACKWARD_HAS_LIBUNWIND 1 -// - libunwind comes from clang, which implements an API compatible version. -// - libunwind provides, in some cases, a more accurate stacktrace as it knows -// to decode signal handler frames and lets us edit the context registers when -// unwinding, allowing stack traces over bad function references. -// -// #define BACKWARD_HAS_BACKTRACE == 1 -// - backtrace is available by default, though it does not produce as much -// information as another library might. -// -// The default is: -// #define BACKWARD_HAS_UNWIND == 1 -// -// Note that only one of the define should be set to 1 at a time. -// -#if BACKWARD_HAS_UNWIND == 1 -#elif BACKWARD_HAS_BACKTRACE == 1 -#elif BACKWARD_HAS_LIBUNWIND == 1 -#else -#undef BACKWARD_HAS_UNWIND -#define BACKWARD_HAS_UNWIND 1 -#undef BACKWARD_HAS_BACKTRACE -#define BACKWARD_HAS_BACKTRACE 0 -#undef BACKWARD_HAS_LIBUNWIND -#define BACKWARD_HAS_LIBUNWIND 0 -#endif - -// On Darwin, backward can extract detailed information about a stack trace -// using one of the following libraries: -// -// #define BACKWARD_HAS_BACKTRACE_SYMBOL 1 -// - backtrace provides minimal details for a stack trace: -// - object filename -// - function name -// -// The default is: -// #define BACKWARD_HAS_BACKTRACE_SYMBOL == 1 -// -#if BACKWARD_HAS_BACKTRACE_SYMBOL == 1 -#else -#undef BACKWARD_HAS_BACKTRACE_SYMBOL -#define BACKWARD_HAS_BACKTRACE_SYMBOL 1 -#endif - -#include -#include -#include -#include -#include -#include - -#if (BACKWARD_HAS_BACKTRACE == 1) || (BACKWARD_HAS_BACKTRACE_SYMBOL == 1) -#include -#endif -#endif // defined(BACKWARD_SYSTEM_DARWIN) - -#if defined(BACKWARD_SYSTEM_WINDOWS) - -#include -#include -#include - -#include -typedef SSIZE_T ssize_t; - -#define NOMINMAX -#include -#include - -#include -#include - -#ifndef __clang__ -#undef NOINLINE -#define NOINLINE __declspec(noinline) -#endif - -#pragma comment(lib, "psapi.lib") -#pragma comment(lib, "dbghelp.lib") - -// Comment / packing is from stackoverflow: -// https://stackoverflow.com/questions/6205981/windows-c-stack-trace-from-a-running-app/28276227#28276227 -// Some versions of imagehlp.dll lack the proper packing directives themselves -// so we need to do it. -#pragma pack(push, before_imagehlp, 8) -#include -#pragma pack(pop, before_imagehlp) - -// TODO maybe these should be undefined somewhere else? -#undef BACKWARD_HAS_UNWIND -#undef BACKWARD_HAS_BACKTRACE -#if BACKWARD_HAS_PDB_SYMBOL == 1 -#else -#undef BACKWARD_HAS_PDB_SYMBOL -#define BACKWARD_HAS_PDB_SYMBOL 1 -#endif - -#endif - -#if BACKWARD_HAS_UNWIND == 1 - -#include -// while gcc's unwind.h defines something like that: -// extern _Unwind_Ptr _Unwind_GetIP (struct _Unwind_Context *); -// extern _Unwind_Ptr _Unwind_GetIPInfo (struct _Unwind_Context *, int *); -// -// clang's unwind.h defines something like this: -// uintptr_t _Unwind_GetIP(struct _Unwind_Context* __context); -// -// Even if the _Unwind_GetIPInfo can be linked to, it is not declared, worse we -// cannot just redeclare it because clang's unwind.h doesn't define _Unwind_Ptr -// anyway. -// -// Luckily we can play on the fact that the guard macros have a different name: -#ifdef __CLANG_UNWIND_H -// In fact, this function still comes from libgcc (on my different linux boxes, -// clang links against libgcc). -#include -extern "C" uintptr_t _Unwind_GetIPInfo(_Unwind_Context *, int *); -#endif - -#endif // BACKWARD_HAS_UNWIND == 1 - -#if BACKWARD_HAS_LIBUNWIND == 1 -#define UNW_LOCAL_ONLY -#include -#endif // BACKWARD_HAS_LIBUNWIND == 1 - -#ifdef BACKWARD_ATLEAST_CXX11 -#include -#include // for std::swap -namespace backward { -namespace details { -template struct hashtable { - typedef std::unordered_map type; -}; -using std::move; -} // namespace details -} // namespace backward -#else // NOT BACKWARD_ATLEAST_CXX11 -#define nullptr NULL -#define override -#include -namespace backward { -namespace details { -template struct hashtable { - typedef std::map type; -}; -template const T &move(const T &v) { return v; } -template T &move(T &v) { return v; } -} // namespace details -} // namespace backward -#endif // BACKWARD_ATLEAST_CXX11 - -namespace backward { -namespace details { -#if defined(BACKWARD_SYSTEM_WINDOWS) -const char kBackwardPathDelimiter[] = ";"; -#else -const char kBackwardPathDelimiter[] = ":"; -#endif -} // namespace details -} // namespace backward - -namespace backward { - -namespace system_tag { -struct linux_tag; // seems that I cannot call that "linux" because the name -// is already defined... so I am adding _tag everywhere. -struct darwin_tag; -struct windows_tag; -struct unknown_tag; - -#if defined(BACKWARD_SYSTEM_LINUX) -typedef linux_tag current_tag; -#elif defined(BACKWARD_SYSTEM_DARWIN) -typedef darwin_tag current_tag; -#elif defined(BACKWARD_SYSTEM_WINDOWS) -typedef windows_tag current_tag; -#elif defined(BACKWARD_SYSTEM_UNKNOWN) -typedef unknown_tag current_tag; -#else -#error "May I please get my system defines?" -#endif -} // namespace system_tag - -namespace trace_resolver_tag { -#if defined(BACKWARD_SYSTEM_LINUX) -struct libdw; -struct libbfd; -struct libdwarf; -struct backtrace_symbol; - -#if BACKWARD_HAS_DW == 1 -typedef libdw current; -#elif BACKWARD_HAS_BFD == 1 -typedef libbfd current; -#elif BACKWARD_HAS_DWARF == 1 -typedef libdwarf current; -#elif BACKWARD_HAS_BACKTRACE_SYMBOL == 1 -typedef backtrace_symbol current; -#else -#error "You shall not pass, until you know what you want." -#endif -#elif defined(BACKWARD_SYSTEM_DARWIN) -struct backtrace_symbol; - -#if BACKWARD_HAS_BACKTRACE_SYMBOL == 1 -typedef backtrace_symbol current; -#else -#error "You shall not pass, until you know what you want." -#endif -#elif defined(BACKWARD_SYSTEM_WINDOWS) -struct pdb_symbol; -#if BACKWARD_HAS_PDB_SYMBOL == 1 -typedef pdb_symbol current; -#else -#error "You shall not pass, until you know what you want." -#endif -#endif -} // namespace trace_resolver_tag - -namespace details { - -template struct rm_ptr { typedef T type; }; - -template struct rm_ptr { typedef T type; }; - -template struct rm_ptr { typedef const T type; }; - -template struct deleter { - template void operator()(U &ptr) const { (*F)(ptr); } -}; - -template struct default_delete { - void operator()(T &ptr) const { delete ptr; } -}; - -template > -class handle { - struct dummy; - T _val; - bool _empty; - -#ifdef BACKWARD_ATLEAST_CXX11 - handle(const handle &) = delete; - handle &operator=(const handle &) = delete; -#endif - -public: - ~handle() { - if (!_empty) { - Deleter()(_val); - } - } - - explicit handle() : _val(), _empty(true) {} - explicit handle(T val) : _val(val), _empty(false) { - if (!_val) - _empty = true; - } - -#ifdef BACKWARD_ATLEAST_CXX11 - handle(handle &&from) : _empty(true) { swap(from); } - handle &operator=(handle &&from) { - swap(from); - return *this; - } -#else - explicit handle(const handle &from) : _empty(true) { - // some sort of poor man's move semantic. - swap(const_cast(from)); - } - handle &operator=(const handle &from) { - // some sort of poor man's move semantic. - swap(const_cast(from)); - return *this; - } -#endif - - void reset(T new_val) { - handle tmp(new_val); - swap(tmp); - } - - void update(T new_val) { - _val = new_val; - _empty = !static_cast(new_val); - } - - operator const dummy *() const { - if (_empty) { - return nullptr; - } - return reinterpret_cast(_val); - } - T get() { return _val; } - T release() { - _empty = true; - return _val; - } - void swap(handle &b) { - using std::swap; - swap(b._val, _val); // can throw, we are safe here. - swap(b._empty, _empty); // should not throw: if you cannot swap two - // bools without throwing... It's a lost cause anyway! - } - - T &operator->() { return _val; } - const T &operator->() const { return _val; } - - typedef typename rm_ptr::type &ref_t; - typedef const typename rm_ptr::type &const_ref_t; - ref_t operator*() { return *_val; } - const_ref_t operator*() const { return *_val; } - ref_t operator[](size_t idx) { return _val[idx]; } - - // Watch out, we've got a badass over here - T *operator&() { - _empty = false; - return &_val; - } -}; - -// Default demangler implementation (do nothing). -template struct demangler_impl { - static std::string demangle(const char *funcname) { return funcname; } -}; - -#if defined(BACKWARD_SYSTEM_LINUX) || defined(BACKWARD_SYSTEM_DARWIN) - -template <> struct demangler_impl { - demangler_impl() : _demangle_buffer_length(0) {} - - std::string demangle(const char *funcname) { - using namespace details; - char *result = abi::__cxa_demangle(funcname, _demangle_buffer.get(), - &_demangle_buffer_length, nullptr); - if (result) { - _demangle_buffer.update(result); - return result; - } - return funcname; - } - -private: - details::handle _demangle_buffer; - size_t _demangle_buffer_length; -}; - -#endif // BACKWARD_SYSTEM_LINUX || BACKWARD_SYSTEM_DARWIN - -struct demangler : public demangler_impl {}; - -// Split a string on the platform's PATH delimiter. Example: if delimiter -// is ":" then: -// "" --> [] -// ":" --> ["",""] -// "::" --> ["","",""] -// "/a/b/c" --> ["/a/b/c"] -// "/a/b/c:/d/e/f" --> ["/a/b/c","/d/e/f"] -// etc. -inline std::vector split_source_prefixes(const std::string &s) { - std::vector out; - size_t last = 0; - size_t next = 0; - size_t delimiter_size = sizeof(kBackwardPathDelimiter) - 1; - while ((next = s.find(kBackwardPathDelimiter, last)) != std::string::npos) { - out.push_back(s.substr(last, next - last)); - last = next + delimiter_size; - } - if (last <= s.length()) { - out.push_back(s.substr(last)); - } - return out; -} - -} // namespace details - -/*************** A TRACE ***************/ - -struct Trace { - void *addr; - size_t idx; - - Trace() : addr(nullptr), idx(0) {} - - explicit Trace(void *_addr, size_t _idx) : addr(_addr), idx(_idx) {} -}; - -struct ResolvedTrace : public Trace { - - struct SourceLoc { - std::string function; - std::string filename; - unsigned line; - unsigned col; - - SourceLoc() : line(0), col(0) {} - - bool operator==(const SourceLoc &b) const { - return function == b.function && filename == b.filename && - line == b.line && col == b.col; - } - - bool operator!=(const SourceLoc &b) const { return !(*this == b); } - }; - - // In which binary object this trace is located. - std::string object_filename; - - // The function in the object that contain the trace. This is not the same - // as source.function which can be an function inlined in object_function. - std::string object_function; - - // The source location of this trace. It is possible for filename to be - // empty and for line/col to be invalid (value 0) if this information - // couldn't be deduced, for example if there is no debug information in the - // binary object. - SourceLoc source; - - // An optionals list of "inliners". All the successive sources location - // from where the source location of the trace (the attribute right above) - // is inlined. It is especially useful when you compiled with optimization. - typedef std::vector source_locs_t; - source_locs_t inliners; - - ResolvedTrace() : Trace() {} - ResolvedTrace(const Trace &mini_trace) : Trace(mini_trace) {} -}; - -/*************** STACK TRACE ***************/ - -// default implemention. -template class StackTraceImpl { -public: - size_t size() const { return 0; } - Trace operator[](size_t) const { return Trace(); } - size_t load_here(size_t = 0) { return 0; } - size_t load_from(void *, size_t = 0, void * = nullptr, void * = nullptr) { - return 0; - } - size_t thread_id() const { return 0; } - void skip_n_firsts(size_t) {} -}; - -class StackTraceImplBase { -public: - StackTraceImplBase() - : _thread_id(0), _skip(0), _context(nullptr), _error_addr(nullptr) {} - - size_t thread_id() const { return _thread_id; } - - void skip_n_firsts(size_t n) { _skip = n; } - -protected: - void load_thread_info() { -#ifdef BACKWARD_SYSTEM_LINUX -#ifndef __ANDROID__ - _thread_id = static_cast(syscall(SYS_gettid)); -#else - _thread_id = static_cast(gettid()); -#endif - if (_thread_id == static_cast(getpid())) { - // If the thread is the main one, let's hide that. - // I like to keep little secret sometimes. - _thread_id = 0; - } -#elif defined(BACKWARD_SYSTEM_DARWIN) - _thread_id = reinterpret_cast(pthread_self()); - if (pthread_main_np() == 1) { - // If the thread is the main one, let's hide that. - _thread_id = 0; - } -#endif - } - - void set_context(void *context) { _context = context; } - void *context() const { return _context; } - - void set_error_addr(void *error_addr) { _error_addr = error_addr; } - void *error_addr() const { return _error_addr; } - - size_t skip_n_firsts() const { return _skip; } - -private: - size_t _thread_id; - size_t _skip; - void *_context; - void *_error_addr; -}; - -class StackTraceImplHolder : public StackTraceImplBase { -public: - size_t size() const { - return (_stacktrace.size() >= skip_n_firsts()) - ? _stacktrace.size() - skip_n_firsts() - : 0; - } - Trace operator[](size_t idx) const { - if (idx >= size()) { - return Trace(); - } - return Trace(_stacktrace[idx + skip_n_firsts()], idx); - } - void *const *begin() const { - if (size()) { - return &_stacktrace[skip_n_firsts()]; - } - return nullptr; - } - -protected: - std::vector _stacktrace; -}; - -#if BACKWARD_HAS_UNWIND == 1 - -namespace details { - -template class Unwinder { -public: - size_t operator()(F &f, size_t depth) { - _f = &f; - _index = -1; - _depth = depth; - _Unwind_Backtrace(&this->backtrace_trampoline, this); - return static_cast(_index); - } - -private: - F *_f; - ssize_t _index; - size_t _depth; - - static _Unwind_Reason_Code backtrace_trampoline(_Unwind_Context *ctx, - void *self) { - return (static_cast(self))->backtrace(ctx); - } - - _Unwind_Reason_Code backtrace(_Unwind_Context *ctx) { - if (_index >= 0 && static_cast(_index) >= _depth) - return _URC_END_OF_STACK; - - int ip_before_instruction = 0; - uintptr_t ip = _Unwind_GetIPInfo(ctx, &ip_before_instruction); - - if (!ip_before_instruction) { - // calculating 0-1 for unsigned, looks like a possible bug to sanitiziers, - // so let's do it explicitly: - if (ip == 0) { - ip = std::numeric_limits::max(); // set it to 0xffff... (as - // from casting 0-1) - } else { - ip -= 1; // else just normally decrement it (no overflow/underflow will - // happen) - } - } - - if (_index >= 0) { // ignore first frame. - (*_f)(static_cast(_index), reinterpret_cast(ip)); - } - _index += 1; - return _URC_NO_REASON; - } -}; - -template size_t unwind(F f, size_t depth) { - Unwinder unwinder; - return unwinder(f, depth); -} - -} // namespace details - -template <> -class StackTraceImpl : public StackTraceImplHolder { -public: - NOINLINE - size_t load_here(size_t depth = 32, void *context = nullptr, - void *error_addr = nullptr) { - load_thread_info(); - set_context(context); - set_error_addr(error_addr); - if (depth == 0) { - return 0; - } - _stacktrace.resize(depth); - size_t trace_cnt = details::unwind(callback(*this), depth); - _stacktrace.resize(trace_cnt); - skip_n_firsts(0); - return size(); - } - size_t load_from(void *addr, size_t depth = 32, void *context = nullptr, - void *error_addr = nullptr) { - load_here(depth + 8, context, error_addr); - - for (size_t i = 0; i < _stacktrace.size(); ++i) { - if (_stacktrace[i] == addr) { - skip_n_firsts(i); - break; - } - } - - _stacktrace.resize(std::min(_stacktrace.size(), skip_n_firsts() + depth)); - return size(); - } - -private: - struct callback { - StackTraceImpl &self; - callback(StackTraceImpl &_self) : self(_self) {} - - void operator()(size_t idx, void *addr) { self._stacktrace[idx] = addr; } - }; -}; - -#elif BACKWARD_HAS_LIBUNWIND == 1 - -template <> -class StackTraceImpl : public StackTraceImplHolder { -public: - __attribute__((noinline)) size_t load_here(size_t depth = 32, - void *_context = nullptr, - void *_error_addr = nullptr) { - set_context(_context); - set_error_addr(_error_addr); - load_thread_info(); - if (depth == 0) { - return 0; - } - _stacktrace.resize(depth + 1); - - int result = 0; - - unw_context_t ctx; - size_t index = 0; - - // Add the tail call. If the Instruction Pointer is the crash address it - // means we got a bad function pointer dereference, so we "unwind" the - // bad pointer manually by using the return address pointed to by the - // Stack Pointer as the Instruction Pointer and letting libunwind do - // the rest - - if (context()) { - ucontext_t *uctx = reinterpret_cast(context()); -#ifdef REG_RIP // x86_64 - if (uctx->uc_mcontext.gregs[REG_RIP] == - reinterpret_cast(error_addr())) { - uctx->uc_mcontext.gregs[REG_RIP] = - *reinterpret_cast(uctx->uc_mcontext.gregs[REG_RSP]); - } - _stacktrace[index] = - reinterpret_cast(uctx->uc_mcontext.gregs[REG_RIP]); - ++index; - ctx = *reinterpret_cast(uctx); -#elif defined(REG_EIP) // x86_32 - if (uctx->uc_mcontext.gregs[REG_EIP] == - reinterpret_cast(error_addr())) { - uctx->uc_mcontext.gregs[REG_EIP] = - *reinterpret_cast(uctx->uc_mcontext.gregs[REG_ESP]); - } - _stacktrace[index] = - reinterpret_cast(uctx->uc_mcontext.gregs[REG_EIP]); - ++index; - ctx = *reinterpret_cast(uctx); -#elif defined(__arm__) - // libunwind uses its own context type for ARM unwinding. - // Copy the registers from the signal handler's context so we can - // unwind - unw_getcontext(&ctx); - ctx.regs[UNW_ARM_R0] = uctx->uc_mcontext.arm_r0; - ctx.regs[UNW_ARM_R1] = uctx->uc_mcontext.arm_r1; - ctx.regs[UNW_ARM_R2] = uctx->uc_mcontext.arm_r2; - ctx.regs[UNW_ARM_R3] = uctx->uc_mcontext.arm_r3; - ctx.regs[UNW_ARM_R4] = uctx->uc_mcontext.arm_r4; - ctx.regs[UNW_ARM_R5] = uctx->uc_mcontext.arm_r5; - ctx.regs[UNW_ARM_R6] = uctx->uc_mcontext.arm_r6; - ctx.regs[UNW_ARM_R7] = uctx->uc_mcontext.arm_r7; - ctx.regs[UNW_ARM_R8] = uctx->uc_mcontext.arm_r8; - ctx.regs[UNW_ARM_R9] = uctx->uc_mcontext.arm_r9; - ctx.regs[UNW_ARM_R10] = uctx->uc_mcontext.arm_r10; - ctx.regs[UNW_ARM_R11] = uctx->uc_mcontext.arm_fp; - ctx.regs[UNW_ARM_R12] = uctx->uc_mcontext.arm_ip; - ctx.regs[UNW_ARM_R13] = uctx->uc_mcontext.arm_sp; - ctx.regs[UNW_ARM_R14] = uctx->uc_mcontext.arm_lr; - ctx.regs[UNW_ARM_R15] = uctx->uc_mcontext.arm_pc; - - // If we have crashed in the PC use the LR instead, as this was - // a bad function dereference - if (reinterpret_cast(error_addr()) == - uctx->uc_mcontext.arm_pc) { - ctx.regs[UNW_ARM_R15] = - uctx->uc_mcontext.arm_lr - sizeof(unsigned long); - } - _stacktrace[index] = reinterpret_cast(ctx.regs[UNW_ARM_R15]); - ++index; -#elif defined(__APPLE__) && defined(__x86_64__) - unw_getcontext(&ctx); - // OS X's implementation of libunwind uses its own context object - // so we need to convert the passed context to libunwind's format - // (information about the data layout taken from unw_getcontext.s - // in Apple's libunwind source - ctx.data[0] = uctx->uc_mcontext->__ss.__rax; - ctx.data[1] = uctx->uc_mcontext->__ss.__rbx; - ctx.data[2] = uctx->uc_mcontext->__ss.__rcx; - ctx.data[3] = uctx->uc_mcontext->__ss.__rdx; - ctx.data[4] = uctx->uc_mcontext->__ss.__rdi; - ctx.data[5] = uctx->uc_mcontext->__ss.__rsi; - ctx.data[6] = uctx->uc_mcontext->__ss.__rbp; - ctx.data[7] = uctx->uc_mcontext->__ss.__rsp; - ctx.data[8] = uctx->uc_mcontext->__ss.__r8; - ctx.data[9] = uctx->uc_mcontext->__ss.__r9; - ctx.data[10] = uctx->uc_mcontext->__ss.__r10; - ctx.data[11] = uctx->uc_mcontext->__ss.__r11; - ctx.data[12] = uctx->uc_mcontext->__ss.__r12; - ctx.data[13] = uctx->uc_mcontext->__ss.__r13; - ctx.data[14] = uctx->uc_mcontext->__ss.__r14; - ctx.data[15] = uctx->uc_mcontext->__ss.__r15; - ctx.data[16] = uctx->uc_mcontext->__ss.__rip; - - // If the IP is the same as the crash address we have a bad function - // dereference The caller's address is pointed to by %rsp, so we - // dereference that value and set it to be the next frame's IP. - if (uctx->uc_mcontext->__ss.__rip == - reinterpret_cast<__uint64_t>(error_addr())) { - ctx.data[16] = - *reinterpret_cast<__uint64_t *>(uctx->uc_mcontext->__ss.__rsp); - } - _stacktrace[index] = reinterpret_cast(ctx.data[16]); - ++index; -#elif defined(__APPLE__) - unw_getcontext(&ctx) - // TODO: Convert the ucontext_t to libunwind's unw_context_t like - // we do in 64 bits - if (ctx.uc_mcontext->__ss.__eip == - reinterpret_cast(error_addr())) { - ctx.uc_mcontext->__ss.__eip = ctx.uc_mcontext->__ss.__esp; - } - _stacktrace[index] = - reinterpret_cast(ctx.uc_mcontext->__ss.__eip); - ++index; -#endif - } - - unw_cursor_t cursor; - if (context()) { -#if defined(UNW_INIT_SIGNAL_FRAME) - result = unw_init_local2(&cursor, &ctx, UNW_INIT_SIGNAL_FRAME); -#else - result = unw_init_local(&cursor, &ctx); -#endif - } else { - unw_getcontext(&ctx); - ; - result = unw_init_local(&cursor, &ctx); - } - - if (result != 0) - return 1; - - unw_word_t ip = 0; - - while (index <= depth && unw_step(&cursor) > 0) { - result = unw_get_reg(&cursor, UNW_REG_IP, &ip); - if (result == 0) { - _stacktrace[index] = reinterpret_cast(--ip); - ++index; - } - } - --index; - - _stacktrace.resize(index + 1); - skip_n_firsts(0); - return size(); - } - - size_t load_from(void *addr, size_t depth = 32, void *context = nullptr, - void *error_addr = nullptr) { - load_here(depth + 8, context, error_addr); - - for (size_t i = 0; i < _stacktrace.size(); ++i) { - if (_stacktrace[i] == addr) { - skip_n_firsts(i); - _stacktrace[i] = (void *)((uintptr_t)_stacktrace[i]); - break; - } - } - - _stacktrace.resize(std::min(_stacktrace.size(), skip_n_firsts() + depth)); - return size(); - } -}; - -#elif defined(BACKWARD_HAS_BACKTRACE) - -template <> -class StackTraceImpl : public StackTraceImplHolder { -public: - NOINLINE - size_t load_here(size_t depth = 32, void *context = nullptr, - void *error_addr = nullptr) { - set_context(context); - set_error_addr(error_addr); - load_thread_info(); - if (depth == 0) { - return 0; - } - _stacktrace.resize(depth + 1); - size_t trace_cnt = backtrace(&_stacktrace[0], _stacktrace.size()); - _stacktrace.resize(trace_cnt); - skip_n_firsts(1); - return size(); - } - - size_t load_from(void *addr, size_t depth = 32, void *context = nullptr, - void *error_addr = nullptr) { - load_here(depth + 8, context, error_addr); - - for (size_t i = 0; i < _stacktrace.size(); ++i) { - if (_stacktrace[i] == addr) { - skip_n_firsts(i); - _stacktrace[i] = (void *)((uintptr_t)_stacktrace[i] + 1); - break; - } - } - - _stacktrace.resize(std::min(_stacktrace.size(), skip_n_firsts() + depth)); - return size(); - } -}; - -#elif defined(BACKWARD_SYSTEM_WINDOWS) - -template <> -class StackTraceImpl : public StackTraceImplHolder { -public: - // We have to load the machine type from the image info - // So we first initialize the resolver, and it tells us this info - void set_machine_type(DWORD machine_type) { machine_type_ = machine_type; } - void set_context(CONTEXT *ctx) { ctx_ = ctx; } - void set_thread_handle(HANDLE handle) { thd_ = handle; } - - NOINLINE - size_t load_here(size_t depth = 32, void *context = nullptr, - void *error_addr = nullptr) { - set_context(static_cast(context)); - set_error_addr(error_addr); - CONTEXT localCtx; // used when no context is provided - - if (depth == 0) { - return 0; - } - - if (!ctx_) { - ctx_ = &localCtx; - RtlCaptureContext(ctx_); - } - - if (!thd_) { - thd_ = GetCurrentThread(); - } - - HANDLE process = GetCurrentProcess(); - - STACKFRAME64 s; - memset(&s, 0, sizeof(STACKFRAME64)); - - // TODO: 32 bit context capture - s.AddrStack.Mode = AddrModeFlat; - s.AddrFrame.Mode = AddrModeFlat; - s.AddrPC.Mode = AddrModeFlat; -#ifdef _M_X64 - s.AddrPC.Offset = ctx_->Rip; - s.AddrStack.Offset = ctx_->Rsp; - s.AddrFrame.Offset = ctx_->Rbp; -#else - s.AddrPC.Offset = ctx_->Eip; - s.AddrStack.Offset = ctx_->Esp; - s.AddrFrame.Offset = ctx_->Ebp; -#endif - - if (!machine_type_) { -#ifdef _M_X64 - machine_type_ = IMAGE_FILE_MACHINE_AMD64; -#else - machine_type_ = IMAGE_FILE_MACHINE_I386; -#endif - } - - for (;;) { - // NOTE: this only works if PDBs are already loaded! - SetLastError(0); - if (!StackWalk64(machine_type_, process, thd_, &s, ctx_, NULL, - SymFunctionTableAccess64, SymGetModuleBase64, NULL)) - break; - - if (s.AddrReturn.Offset == 0) - break; - - _stacktrace.push_back(reinterpret_cast(s.AddrPC.Offset)); - - if (size() >= depth) - break; - } - - return size(); - } - - size_t load_from(void *addr, size_t depth = 32, void *context = nullptr, - void *error_addr = nullptr) { - load_here(depth + 8, context, error_addr); - - for (size_t i = 0; i < _stacktrace.size(); ++i) { - if (_stacktrace[i] == addr) { - skip_n_firsts(i); - break; - } - } - - _stacktrace.resize(std::min(_stacktrace.size(), skip_n_firsts() + depth)); - return size(); - } - -private: - DWORD machine_type_ = 0; - HANDLE thd_ = 0; - CONTEXT *ctx_ = nullptr; -}; - -#endif - -class StackTrace : public StackTraceImpl {}; - -/*************** TRACE RESOLVER ***************/ - -class TraceResolverImplBase { -public: - virtual ~TraceResolverImplBase() {} - - virtual void load_addresses(void *const*addresses, int address_count) { - (void)addresses; - (void)address_count; - } - - template void load_stacktrace(ST &st) { - load_addresses(st.begin(), (int)st.size()); - } - - virtual ResolvedTrace resolve(ResolvedTrace t) { return t; } - -protected: - std::string demangle(const char *funcname) { - return _demangler.demangle(funcname); - } - -private: - details::demangler _demangler; -}; - -template class TraceResolverImpl; - -#ifdef BACKWARD_SYSTEM_UNKNOWN - -template <> class TraceResolverImpl - : public TraceResolverImplBase {}; - -#endif - -#ifdef BACKWARD_SYSTEM_LINUX - -class TraceResolverLinuxBase : public TraceResolverImplBase { -public: - TraceResolverLinuxBase() - : argv0_(get_argv0()), exec_path_(read_symlink("/proc/self/exe")) {} - std::string resolve_exec_path(Dl_info &symbol_info) const { - // mutates symbol_info.dli_fname to be filename to open and returns filename - // to display - if (symbol_info.dli_fname == argv0_) { - // dladdr returns argv[0] in dli_fname for symbols contained in - // the main executable, which is not a valid path if the - // executable was found by a search of the PATH environment - // variable; In that case, we actually open /proc/self/exe, which - // is always the actual executable (even if it was deleted/replaced!) - // but display the path that /proc/self/exe links to. - // However, this right away reduces probability of successful symbol - // resolution, because libbfd may try to find *.debug files in the - // same dir, in case symbols are stripped. As a result, it may try - // to find a file /proc/self/.debug, which obviously does - // not exist. /proc/self/exe is a last resort. First load attempt - // should go for the original executable file path. - symbol_info.dli_fname = "/proc/self/exe"; - return exec_path_; - } else { - return symbol_info.dli_fname; - } - } - -private: - std::string argv0_; - std::string exec_path_; - - static std::string get_argv0() { - std::string argv0; - std::ifstream ifs("/proc/self/cmdline"); - std::getline(ifs, argv0, '\0'); - return argv0; - } - - static std::string read_symlink(std::string const &symlink_path) { - std::string path; - path.resize(100); - - while (true) { - ssize_t len = - ::readlink(symlink_path.c_str(), &*path.begin(), path.size()); - if (len < 0) { - return ""; - } - if (static_cast(len) == path.size()) { - path.resize(path.size() * 2); - } else { - path.resize(static_cast(len)); - break; - } - } - - return path; - } -}; - -template class TraceResolverLinuxImpl; - -#if BACKWARD_HAS_BACKTRACE_SYMBOL == 1 - -template <> -class TraceResolverLinuxImpl - : public TraceResolverLinuxBase { -public: - void load_addresses(void *const*addresses, int address_count) override { - if (address_count == 0) { - return; - } - _symbols.reset(backtrace_symbols(addresses, address_count)); - } - - ResolvedTrace resolve(ResolvedTrace trace) override { - char *filename = _symbols[trace.idx]; - char *funcname = filename; - while (*funcname && *funcname != '(') { - funcname += 1; - } - trace.object_filename.assign(filename, - funcname); // ok even if funcname is the ending - // \0 (then we assign entire string) - - if (*funcname) { // if it's not end of string (e.g. from last frame ip==0) - funcname += 1; - char *funcname_end = funcname; - while (*funcname_end && *funcname_end != ')' && *funcname_end != '+') { - funcname_end += 1; - } - *funcname_end = '\0'; - trace.object_function = this->demangle(funcname); - trace.source.function = trace.object_function; // we cannot do better. - } - return trace; - } - -private: - details::handle _symbols; -}; - -#endif // BACKWARD_HAS_BACKTRACE_SYMBOL == 1 - -#if BACKWARD_HAS_BFD == 1 - -template <> -class TraceResolverLinuxImpl - : public TraceResolverLinuxBase { -public: - TraceResolverLinuxImpl() : _bfd_loaded(false) {} - - ResolvedTrace resolve(ResolvedTrace trace) override { - Dl_info symbol_info; - - // trace.addr is a virtual address in memory pointing to some code. - // Let's try to find from which loaded object it comes from. - // The loaded object can be yourself btw. - if (!dladdr(trace.addr, &symbol_info)) { - return trace; // dat broken trace... - } - - // Now we get in symbol_info: - // .dli_fname: - // pathname of the shared object that contains the address. - // .dli_fbase: - // where the object is loaded in memory. - // .dli_sname: - // the name of the nearest symbol to trace.addr, we expect a - // function name. - // .dli_saddr: - // the exact address corresponding to .dli_sname. - - if (symbol_info.dli_sname) { - trace.object_function = demangle(symbol_info.dli_sname); - } - - if (!symbol_info.dli_fname) { - return trace; - } - - trace.object_filename = resolve_exec_path(symbol_info); - bfd_fileobject *fobj; - // Before rushing to resolution need to ensure the executable - // file still can be used. For that compare inode numbers of - // what is stored by the executable's file path, and in the - // dli_fname, which not necessarily equals to the executable. - // It can be a shared library, or /proc/self/exe, and in the - // latter case has drawbacks. See the exec path resolution for - // details. In short - the dli object should be used only as - // the last resort. - // If inode numbers are equal, it is known dli_fname and the - // executable file are the same. This is guaranteed by Linux, - // because if the executable file is changed/deleted, it will - // be done in a new inode. The old file will be preserved in - // /proc/self/exe, and may even have inode 0. The latter can - // happen if the inode was actually reused, and the file was - // kept only in the main memory. - // - struct stat obj_stat; - struct stat dli_stat; - if (stat(trace.object_filename.c_str(), &obj_stat) == 0 && - stat(symbol_info.dli_fname, &dli_stat) == 0 && - obj_stat.st_ino == dli_stat.st_ino) { - // The executable file, and the shared object containing the - // address are the same file. Safe to use the original path. - // this is preferable. Libbfd will search for stripped debug - // symbols in the same directory. - fobj = load_object_with_bfd(trace.object_filename); - } else{ - // The original object file was *deleted*! The only hope is - // that the debug symbols are either inside the shared - // object file, or are in the same directory, and this is - // not /proc/self/exe. - fobj = nullptr; - } - if (fobj == nullptr || !fobj->handle) { - fobj = load_object_with_bfd(symbol_info.dli_fname); - if (!fobj->handle) { - return trace; - } - } - - find_sym_result *details_selected; // to be filled. - - // trace.addr is the next instruction to be executed after returning - // from the nested stack frame. In C++ this usually relate to the next - // statement right after the function call that leaded to a new stack - // frame. This is not usually what you want to see when printing out a - // stacktrace... - find_sym_result details_call_site = - find_symbol_details(fobj, trace.addr, symbol_info.dli_fbase); - details_selected = &details_call_site; - -#if BACKWARD_HAS_UNWIND == 0 - // ...this is why we also try to resolve the symbol that is right - // before the return address. If we are lucky enough, we will get the - // line of the function that was called. But if the code is optimized, - // we might get something absolutely not related since the compiler - // can reschedule the return address with inline functions and - // tail-call optimisation (among other things that I don't even know - // or cannot even dream about with my tiny limited brain). - find_sym_result details_adjusted_call_site = find_symbol_details( - fobj, (void *)(uintptr_t(trace.addr) - 1), symbol_info.dli_fbase); - - // In debug mode, we should always get the right thing(TM). - if (details_call_site.found && details_adjusted_call_site.found) { - // Ok, we assume that details_adjusted_call_site is a better estimation. - details_selected = &details_adjusted_call_site; - trace.addr = (void *)(uintptr_t(trace.addr) - 1); - } - - if (details_selected == &details_call_site && details_call_site.found) { - // we have to re-resolve the symbol in order to reset some - // internal state in BFD... so we can call backtrace_inliners - // thereafter... - details_call_site = - find_symbol_details(fobj, trace.addr, symbol_info.dli_fbase); - } -#endif // BACKWARD_HAS_UNWIND - - if (details_selected->found) { - if (details_selected->filename) { - trace.source.filename = details_selected->filename; - } - trace.source.line = details_selected->line; - - if (details_selected->funcname) { - // this time we get the name of the function where the code is - // located, instead of the function were the address is - // located. In short, if the code was inlined, we get the - // function correspoding to the code. Else we already got in - // trace.function. - trace.source.function = demangle(details_selected->funcname); - - if (!symbol_info.dli_sname) { - // for the case dladdr failed to find the symbol name of - // the function, we might as well try to put something - // here. - trace.object_function = trace.source.function; - } - } - - // Maybe the source of the trace got inlined inside the function - // (trace.source.function). Let's see if we can get all the inlined - // calls along the way up to the initial call site. - trace.inliners = backtrace_inliners(fobj, *details_selected); - -#if 0 - if (trace.inliners.size() == 0) { - // Maybe the trace was not inlined... or maybe it was and we - // are lacking the debug information. Let's try to make the - // world better and see if we can get the line number of the - // function (trace.source.function) now. - // - // We will get the location of where the function start (to be - // exact: the first instruction that really start the - // function), not where the name of the function is defined. - // This can be quite far away from the name of the function - // btw. - // - // If the source of the function is the same as the source of - // the trace, we cannot say if the trace was really inlined or - // not. However, if the filename of the source is different - // between the function and the trace... we can declare it as - // an inliner. This is not 100% accurate, but better than - // nothing. - - if (symbol_info.dli_saddr) { - find_sym_result details = find_symbol_details(fobj, - symbol_info.dli_saddr, - symbol_info.dli_fbase); - - if (details.found) { - ResolvedTrace::SourceLoc diy_inliner; - diy_inliner.line = details.line; - if (details.filename) { - diy_inliner.filename = details.filename; - } - if (details.funcname) { - diy_inliner.function = demangle(details.funcname); - } else { - diy_inliner.function = trace.source.function; - } - if (diy_inliner != trace.source) { - trace.inliners.push_back(diy_inliner); - } - } - } - } -#endif - } - - return trace; - } - -private: - bool _bfd_loaded; - - typedef details::handle> - bfd_handle_t; - - typedef details::handle bfd_symtab_t; - - struct bfd_fileobject { - bfd_handle_t handle; - bfd_vma base_addr; - bfd_symtab_t symtab; - bfd_symtab_t dynamic_symtab; - }; - - typedef details::hashtable::type fobj_bfd_map_t; - fobj_bfd_map_t _fobj_bfd_map; - - bfd_fileobject *load_object_with_bfd(const std::string &filename_object) { - using namespace details; - - if (!_bfd_loaded) { - using namespace details; - bfd_init(); - _bfd_loaded = true; - } - - fobj_bfd_map_t::iterator it = _fobj_bfd_map.find(filename_object); - if (it != _fobj_bfd_map.end()) { - return &it->second; - } - - // this new object is empty for now. - bfd_fileobject *r = &_fobj_bfd_map[filename_object]; - - // we do the work temporary in this one; - bfd_handle_t bfd_handle; - - int fd = open(filename_object.c_str(), O_RDONLY); - bfd_handle.reset(bfd_fdopenr(filename_object.c_str(), "default", fd)); - if (!bfd_handle) { - close(fd); - return r; - } - - if (!bfd_check_format(bfd_handle.get(), bfd_object)) { - return r; // not an object? You lose. - } - - if ((bfd_get_file_flags(bfd_handle.get()) & HAS_SYMS) == 0) { - return r; // that's what happen when you forget to compile in debug. - } - - ssize_t symtab_storage_size = bfd_get_symtab_upper_bound(bfd_handle.get()); - - ssize_t dyn_symtab_storage_size = - bfd_get_dynamic_symtab_upper_bound(bfd_handle.get()); - - if (symtab_storage_size <= 0 && dyn_symtab_storage_size <= 0) { - return r; // weird, is the file is corrupted? - } - - bfd_symtab_t symtab, dynamic_symtab; - ssize_t symcount = 0, dyn_symcount = 0; - - if (symtab_storage_size > 0) { - symtab.reset(static_cast( - malloc(static_cast(symtab_storage_size)))); - symcount = bfd_canonicalize_symtab(bfd_handle.get(), symtab.get()); - } - - if (dyn_symtab_storage_size > 0) { - dynamic_symtab.reset(static_cast( - malloc(static_cast(dyn_symtab_storage_size)))); - dyn_symcount = bfd_canonicalize_dynamic_symtab(bfd_handle.get(), - dynamic_symtab.get()); - } - - if (symcount <= 0 && dyn_symcount <= 0) { - return r; // damned, that's a stripped file that you got there! - } - - r->handle = move(bfd_handle); - r->symtab = move(symtab); - r->dynamic_symtab = move(dynamic_symtab); - return r; - } - - struct find_sym_result { - bool found; - const char *filename; - const char *funcname; - unsigned int line; - }; - - struct find_sym_context { - TraceResolverLinuxImpl *self; - bfd_fileobject *fobj; - void *addr; - void *base_addr; - find_sym_result result; - }; - - find_sym_result find_symbol_details(bfd_fileobject *fobj, void *addr, - void *base_addr) { - find_sym_context context; - context.self = this; - context.fobj = fobj; - context.addr = addr; - context.base_addr = base_addr; - context.result.found = false; - bfd_map_over_sections(fobj->handle.get(), &find_in_section_trampoline, - static_cast(&context)); - return context.result; - } - - static void find_in_section_trampoline(bfd *, asection *section, void *data) { - find_sym_context *context = static_cast(data); - context->self->find_in_section( - reinterpret_cast(context->addr), - reinterpret_cast(context->base_addr), context->fobj, section, - context->result); - } - - void find_in_section(bfd_vma addr, bfd_vma base_addr, bfd_fileobject *fobj, - asection *section, find_sym_result &result) { - if (result.found) - return; - -#ifdef bfd_get_section_flags - if ((bfd_get_section_flags(fobj->handle.get(), section) & SEC_ALLOC) == 0) -#else - if ((bfd_section_flags(section) & SEC_ALLOC) == 0) -#endif - return; // a debug section is never loaded automatically. - -#ifdef bfd_get_section_vma - bfd_vma sec_addr = bfd_get_section_vma(fobj->handle.get(), section); -#else - bfd_vma sec_addr = bfd_section_vma(section); -#endif -#ifdef bfd_get_section_size - bfd_size_type size = bfd_get_section_size(section); -#else - bfd_size_type size = bfd_section_size(section); -#endif - - // are we in the boundaries of the section? - if (addr < sec_addr || addr >= sec_addr + size) { - addr -= base_addr; // oups, a relocated object, lets try again... - if (addr < sec_addr || addr >= sec_addr + size) { - return; - } - } - -#if defined(__clang__) -#pragma clang diagnostic push -#pragma clang diagnostic ignored "-Wzero-as-null-pointer-constant" -#endif - if (!result.found && fobj->symtab) { - result.found = bfd_find_nearest_line( - fobj->handle.get(), section, fobj->symtab.get(), addr - sec_addr, - &result.filename, &result.funcname, &result.line); - } - - if (!result.found && fobj->dynamic_symtab) { - result.found = bfd_find_nearest_line( - fobj->handle.get(), section, fobj->dynamic_symtab.get(), - addr - sec_addr, &result.filename, &result.funcname, &result.line); - } -#if defined(__clang__) -#pragma clang diagnostic pop -#endif - } - - ResolvedTrace::source_locs_t - backtrace_inliners(bfd_fileobject *fobj, find_sym_result previous_result) { - // This function can be called ONLY after a SUCCESSFUL call to - // find_symbol_details. The state is global to the bfd_handle. - ResolvedTrace::source_locs_t results; - while (previous_result.found) { - find_sym_result result; - result.found = bfd_find_inliner_info(fobj->handle.get(), &result.filename, - &result.funcname, &result.line); - - if (result - .found) /* and not ( - cstrings_eq(previous_result.filename, - result.filename) and - cstrings_eq(previous_result.funcname, result.funcname) - and result.line == previous_result.line - )) */ - { - ResolvedTrace::SourceLoc src_loc; - src_loc.line = result.line; - if (result.filename) { - src_loc.filename = result.filename; - } - if (result.funcname) { - src_loc.function = demangle(result.funcname); - } - results.push_back(src_loc); - } - previous_result = result; - } - return results; - } - - bool cstrings_eq(const char *a, const char *b) { - if (!a || !b) { - return false; - } - return strcmp(a, b) == 0; - } -}; -#endif // BACKWARD_HAS_BFD == 1 - -#if BACKWARD_HAS_DW == 1 - -template <> -class TraceResolverLinuxImpl - : public TraceResolverLinuxBase { -public: - TraceResolverLinuxImpl() : _dwfl_handle_initialized(false) {} - - ResolvedTrace resolve(ResolvedTrace trace) override { - using namespace details; - - Dwarf_Addr trace_addr = (Dwarf_Addr)trace.addr; - - if (!_dwfl_handle_initialized) { - // initialize dwfl... - _dwfl_cb.reset(new Dwfl_Callbacks); - _dwfl_cb->find_elf = &dwfl_linux_proc_find_elf; - _dwfl_cb->find_debuginfo = &dwfl_standard_find_debuginfo; - _dwfl_cb->debuginfo_path = 0; - - _dwfl_handle.reset(dwfl_begin(_dwfl_cb.get())); - _dwfl_handle_initialized = true; - - if (!_dwfl_handle) { - return trace; - } - - // ...from the current process. - dwfl_report_begin(_dwfl_handle.get()); - int r = dwfl_linux_proc_report(_dwfl_handle.get(), getpid()); - dwfl_report_end(_dwfl_handle.get(), NULL, NULL); - if (r < 0) { - return trace; - } - } - - if (!_dwfl_handle) { - return trace; - } - - // find the module (binary object) that contains the trace's address. - // This is not using any debug information, but the addresses ranges of - // all the currently loaded binary object. - Dwfl_Module *mod = dwfl_addrmodule(_dwfl_handle.get(), trace_addr); - if (mod) { - // now that we found it, lets get the name of it, this will be the - // full path to the running binary or one of the loaded library. - const char *module_name = dwfl_module_info(mod, 0, 0, 0, 0, 0, 0, 0); - if (module_name) { - trace.object_filename = module_name; - } - // We also look after the name of the symbol, equal or before this - // address. This is found by walking the symtab. We should get the - // symbol corresponding to the function (mangled) containing the - // address. If the code corresponding to the address was inlined, - // this is the name of the out-most inliner function. - const char *sym_name = dwfl_module_addrname(mod, trace_addr); - if (sym_name) { - trace.object_function = demangle(sym_name); - } - } - - // now let's get serious, and find out the source location (file and - // line number) of the address. - - // This function will look in .debug_aranges for the address and map it - // to the location of the compilation unit DIE in .debug_info and - // return it. - Dwarf_Addr mod_bias = 0; - Dwarf_Die *cudie = dwfl_module_addrdie(mod, trace_addr, &mod_bias); - -#if 1 - if (!cudie) { - // Sadly clang does not generate the section .debug_aranges, thus - // dwfl_module_addrdie will fail early. Clang doesn't either set - // the lowpc/highpc/range info for every compilation unit. - // - // So in order to save the world: - // for every compilation unit, we will iterate over every single - // DIEs. Normally functions should have a lowpc/highpc/range, which - // we will use to infer the compilation unit. - - // note that this is probably badly inefficient. - while ((cudie = dwfl_module_nextcu(mod, cudie, &mod_bias))) { - Dwarf_Die die_mem; - Dwarf_Die *fundie = - find_fundie_by_pc(cudie, trace_addr - mod_bias, &die_mem); - if (fundie) { - break; - } - } - } -#endif - -//#define BACKWARD_I_DO_NOT_RECOMMEND_TO_ENABLE_THIS_HORRIBLE_PIECE_OF_CODE -#ifdef BACKWARD_I_DO_NOT_RECOMMEND_TO_ENABLE_THIS_HORRIBLE_PIECE_OF_CODE - if (!cudie) { - // If it's still not enough, lets dive deeper in the shit, and try - // to save the world again: for every compilation unit, we will - // load the corresponding .debug_line section, and see if we can - // find our address in it. - - Dwarf_Addr cfi_bias; - Dwarf_CFI *cfi_cache = dwfl_module_eh_cfi(mod, &cfi_bias); - - Dwarf_Addr bias; - while ((cudie = dwfl_module_nextcu(mod, cudie, &bias))) { - if (dwarf_getsrc_die(cudie, trace_addr - bias)) { - - // ...but if we get a match, it might be a false positive - // because our (address - bias) might as well be valid in a - // different compilation unit. So we throw our last card on - // the table and lookup for the address into the .eh_frame - // section. - - handle frame; - dwarf_cfi_addrframe(cfi_cache, trace_addr - cfi_bias, &frame); - if (frame) { - break; - } - } - } - } -#endif - - if (!cudie) { - return trace; // this time we lost the game :/ - } - - // Now that we have a compilation unit DIE, this function will be able - // to load the corresponding section in .debug_line (if not already - // loaded) and hopefully find the source location mapped to our - // address. - Dwarf_Line *srcloc = dwarf_getsrc_die(cudie, trace_addr - mod_bias); - - if (srcloc) { - const char *srcfile = dwarf_linesrc(srcloc, 0, 0); - if (srcfile) { - trace.source.filename = srcfile; - } - int line = 0, col = 0; - dwarf_lineno(srcloc, &line); - dwarf_linecol(srcloc, &col); - trace.source.line = line; - trace.source.col = col; - } - - deep_first_search_by_pc(cudie, trace_addr - mod_bias, - inliners_search_cb(trace)); - if (trace.source.function.size() == 0) { - // fallback. - trace.source.function = trace.object_function; - } - - return trace; - } - -private: - typedef details::handle> - dwfl_handle_t; - details::handle> - _dwfl_cb; - dwfl_handle_t _dwfl_handle; - bool _dwfl_handle_initialized; - - // defined here because in C++98, template function cannot take locally - // defined types... grrr. - struct inliners_search_cb { - void operator()(Dwarf_Die *die) { - switch (dwarf_tag(die)) { - const char *name; - case DW_TAG_subprogram: - if ((name = dwarf_diename(die))) { - trace.source.function = name; - } - break; - - case DW_TAG_inlined_subroutine: - ResolvedTrace::SourceLoc sloc; - Dwarf_Attribute attr_mem; - - if ((name = dwarf_diename(die))) { - sloc.function = name; - } - if ((name = die_call_file(die))) { - sloc.filename = name; - } - - Dwarf_Word line = 0, col = 0; - dwarf_formudata(dwarf_attr(die, DW_AT_call_line, &attr_mem), &line); - dwarf_formudata(dwarf_attr(die, DW_AT_call_column, &attr_mem), &col); - sloc.line = (unsigned)line; - sloc.col = (unsigned)col; - - trace.inliners.push_back(sloc); - break; - }; - } - ResolvedTrace &trace; - inliners_search_cb(ResolvedTrace &t) : trace(t) {} - }; - - static bool die_has_pc(Dwarf_Die *die, Dwarf_Addr pc) { - Dwarf_Addr low, high; - - // continuous range - if (dwarf_hasattr(die, DW_AT_low_pc) && dwarf_hasattr(die, DW_AT_high_pc)) { - if (dwarf_lowpc(die, &low) != 0) { - return false; - } - if (dwarf_highpc(die, &high) != 0) { - Dwarf_Attribute attr_mem; - Dwarf_Attribute *attr = dwarf_attr(die, DW_AT_high_pc, &attr_mem); - Dwarf_Word value; - if (dwarf_formudata(attr, &value) != 0) { - return false; - } - high = low + value; - } - return pc >= low && pc < high; - } - - // non-continuous range. - Dwarf_Addr base; - ptrdiff_t offset = 0; - while ((offset = dwarf_ranges(die, offset, &base, &low, &high)) > 0) { - if (pc >= low && pc < high) { - return true; - } - } - return false; - } - - static Dwarf_Die *find_fundie_by_pc(Dwarf_Die *parent_die, Dwarf_Addr pc, - Dwarf_Die *result) { - if (dwarf_child(parent_die, result) != 0) { - return 0; - } - - Dwarf_Die *die = result; - do { - switch (dwarf_tag(die)) { - case DW_TAG_subprogram: - case DW_TAG_inlined_subroutine: - if (die_has_pc(die, pc)) { - return result; - } - }; - bool declaration = false; - Dwarf_Attribute attr_mem; - dwarf_formflag(dwarf_attr(die, DW_AT_declaration, &attr_mem), - &declaration); - if (!declaration) { - // let's be curious and look deeper in the tree, - // function are not necessarily at the first level, but - // might be nested inside a namespace, structure etc. - Dwarf_Die die_mem; - Dwarf_Die *indie = find_fundie_by_pc(die, pc, &die_mem); - if (indie) { - *result = die_mem; - return result; - } - } - } while (dwarf_siblingof(die, result) == 0); - return 0; - } - - template - static bool deep_first_search_by_pc(Dwarf_Die *parent_die, Dwarf_Addr pc, - CB cb) { - Dwarf_Die die_mem; - if (dwarf_child(parent_die, &die_mem) != 0) { - return false; - } - - bool branch_has_pc = false; - Dwarf_Die *die = &die_mem; - do { - bool declaration = false; - Dwarf_Attribute attr_mem; - dwarf_formflag(dwarf_attr(die, DW_AT_declaration, &attr_mem), - &declaration); - if (!declaration) { - // let's be curious and look deeper in the tree, function are - // not necessarily at the first level, but might be nested - // inside a namespace, structure, a function, an inlined - // function etc. - branch_has_pc = deep_first_search_by_pc(die, pc, cb); - } - if (!branch_has_pc) { - branch_has_pc = die_has_pc(die, pc); - } - if (branch_has_pc) { - cb(die); - } - } while (dwarf_siblingof(die, &die_mem) == 0); - return branch_has_pc; - } - - static const char *die_call_file(Dwarf_Die *die) { - Dwarf_Attribute attr_mem; - Dwarf_Word file_idx = 0; - - dwarf_formudata(dwarf_attr(die, DW_AT_call_file, &attr_mem), &file_idx); - - if (file_idx == 0) { - return 0; - } - - Dwarf_Die die_mem; - Dwarf_Die *cudie = dwarf_diecu(die, &die_mem, 0, 0); - if (!cudie) { - return 0; - } - - Dwarf_Files *files = 0; - size_t nfiles; - dwarf_getsrcfiles(cudie, &files, &nfiles); - if (!files) { - return 0; - } - - return dwarf_filesrc(files, file_idx, 0, 0); - } -}; -#endif // BACKWARD_HAS_DW == 1 - -#if BACKWARD_HAS_DWARF == 1 - -template <> -class TraceResolverLinuxImpl - : public TraceResolverLinuxBase { -public: - TraceResolverLinuxImpl() : _dwarf_loaded(false) {} - - ResolvedTrace resolve(ResolvedTrace trace) override { - // trace.addr is a virtual address in memory pointing to some code. - // Let's try to find from which loaded object it comes from. - // The loaded object can be yourself btw. - - Dl_info symbol_info; - int dladdr_result = 0; -#if defined(__GLIBC__) - link_map *link_map; - // We request the link map so we can get information about offsets - dladdr_result = - dladdr1(trace.addr, &symbol_info, reinterpret_cast(&link_map), - RTLD_DL_LINKMAP); -#else - // Android doesn't have dladdr1. Don't use the linker map. - dladdr_result = dladdr(trace.addr, &symbol_info); -#endif - if (!dladdr_result) { - return trace; // dat broken trace... - } - - // Now we get in symbol_info: - // .dli_fname: - // pathname of the shared object that contains the address. - // .dli_fbase: - // where the object is loaded in memory. - // .dli_sname: - // the name of the nearest symbol to trace.addr, we expect a - // function name. - // .dli_saddr: - // the exact address corresponding to .dli_sname. - // - // And in link_map: - // .l_addr: - // difference between the address in the ELF file and the address - // in memory - // l_name: - // absolute pathname where the object was found - - if (symbol_info.dli_sname) { - trace.object_function = demangle(symbol_info.dli_sname); - } - - if (!symbol_info.dli_fname) { - return trace; - } - - trace.object_filename = resolve_exec_path(symbol_info); - dwarf_fileobject &fobj = load_object_with_dwarf(symbol_info.dli_fname); - if (!fobj.dwarf_handle) { - return trace; // sad, we couldn't load the object :( - } - -#if defined(__GLIBC__) - // Convert the address to a module relative one by looking at - // the module's loading address in the link map - Dwarf_Addr address = reinterpret_cast(trace.addr) - - reinterpret_cast(link_map->l_addr); -#else - Dwarf_Addr address = reinterpret_cast(trace.addr); -#endif - - if (trace.object_function.empty()) { - symbol_cache_t::iterator it = fobj.symbol_cache.lower_bound(address); - - if (it != fobj.symbol_cache.end()) { - if (it->first != address) { - if (it != fobj.symbol_cache.begin()) { - --it; - } - } - trace.object_function = demangle(it->second.c_str()); - } - } - - // Get the Compilation Unit DIE for the address - Dwarf_Die die = find_die(fobj, address); - - if (!die) { - return trace; // this time we lost the game :/ - } - - // libdwarf doesn't give us direct access to its objects, it always - // allocates a copy for the caller. We keep that copy alive in a cache - // and we deallocate it later when it's no longer required. - die_cache_entry &die_object = get_die_cache(fobj, die); - if (die_object.isEmpty()) - return trace; // We have no line section for this DIE - - die_linemap_t::iterator it = die_object.line_section.lower_bound(address); - - if (it != die_object.line_section.end()) { - if (it->first != address) { - if (it == die_object.line_section.begin()) { - // If we are on the first item of the line section - // but the address does not match it means that - // the address is below the range of the DIE. Give up. - return trace; - } else { - --it; - } - } - } else { - return trace; // We didn't find the address. - } - - // Get the Dwarf_Line that the address points to and call libdwarf - // to get source file, line and column info. - Dwarf_Line line = die_object.line_buffer[it->second]; - Dwarf_Error error = DW_DLE_NE; - - char *filename; - if (dwarf_linesrc(line, &filename, &error) == DW_DLV_OK) { - trace.source.filename = std::string(filename); - dwarf_dealloc(fobj.dwarf_handle.get(), filename, DW_DLA_STRING); - } - - Dwarf_Unsigned number = 0; - if (dwarf_lineno(line, &number, &error) == DW_DLV_OK) { - trace.source.line = number; - } else { - trace.source.line = 0; - } - - if (dwarf_lineoff_b(line, &number, &error) == DW_DLV_OK) { - trace.source.col = number; - } else { - trace.source.col = 0; - } - - std::vector namespace_stack; - deep_first_search_by_pc(fobj, die, address, namespace_stack, - inliners_search_cb(trace, fobj, die)); - - dwarf_dealloc(fobj.dwarf_handle.get(), die, DW_DLA_DIE); - - return trace; - } - -public: - static int close_dwarf(Dwarf_Debug dwarf) { - return dwarf_finish(dwarf, NULL); - } - -private: - bool _dwarf_loaded; - - typedef details::handle> - dwarf_file_t; - - typedef details::handle> - dwarf_elf_t; - - typedef details::handle> - dwarf_handle_t; - - typedef std::map die_linemap_t; - - typedef std::map die_specmap_t; - - struct die_cache_entry { - die_specmap_t spec_section; - die_linemap_t line_section; - Dwarf_Line *line_buffer; - Dwarf_Signed line_count; - Dwarf_Line_Context line_context; - - inline bool isEmpty() { - return line_buffer == NULL || line_count == 0 || line_context == NULL || - line_section.empty(); - } - - die_cache_entry() : line_buffer(0), line_count(0), line_context(0) {} - - ~die_cache_entry() { - if (line_context) { - dwarf_srclines_dealloc_b(line_context); - } - } - }; - - typedef std::map die_cache_t; - - typedef std::map symbol_cache_t; - - struct dwarf_fileobject { - dwarf_file_t file_handle; - dwarf_elf_t elf_handle; - dwarf_handle_t dwarf_handle; - symbol_cache_t symbol_cache; - - // Die cache - die_cache_t die_cache; - die_cache_entry *current_cu; - }; - - typedef details::hashtable::type - fobj_dwarf_map_t; - fobj_dwarf_map_t _fobj_dwarf_map; - - static bool cstrings_eq(const char *a, const char *b) { - if (!a || !b) { - return false; - } - return strcmp(a, b) == 0; - } - - dwarf_fileobject &load_object_with_dwarf(const std::string &filename_object) { - - if (!_dwarf_loaded) { - // Set the ELF library operating version - // If that fails there's nothing we can do - _dwarf_loaded = elf_version(EV_CURRENT) != EV_NONE; - } - - fobj_dwarf_map_t::iterator it = _fobj_dwarf_map.find(filename_object); - if (it != _fobj_dwarf_map.end()) { - return it->second; - } - - // this new object is empty for now - dwarf_fileobject &r = _fobj_dwarf_map[filename_object]; - - dwarf_file_t file_handle; - file_handle.reset(open(filename_object.c_str(), O_RDONLY)); - if (file_handle.get() < 0) { - return r; - } - - // Try to get an ELF handle. We need to read the ELF sections - // because we want to see if there is a .gnu_debuglink section - // that points to a split debug file - dwarf_elf_t elf_handle; - elf_handle.reset(elf_begin(file_handle.get(), ELF_C_READ, NULL)); - if (!elf_handle) { - return r; - } - - const char *e_ident = elf_getident(elf_handle.get(), 0); - if (!e_ident) { - return r; - } - - // Get the number of sections - // We use the new APIs as elf_getshnum is deprecated - size_t shdrnum = 0; - if (elf_getshdrnum(elf_handle.get(), &shdrnum) == -1) { - return r; - } - - // Get the index to the string section - size_t shdrstrndx = 0; - if (elf_getshdrstrndx(elf_handle.get(), &shdrstrndx) == -1) { - return r; - } - - std::string debuglink; - // Iterate through the ELF sections to try to get a gnu_debuglink - // note and also to cache the symbol table. - // We go the preprocessor way to avoid having to create templated - // classes or using gelf (which might throw a compiler error if 64 bit - // is not supported -#define ELF_GET_DATA(ARCH) \ - Elf_Scn *elf_section = 0; \ - Elf_Data *elf_data = 0; \ - Elf##ARCH##_Shdr *section_header = 0; \ - Elf_Scn *symbol_section = 0; \ - size_t symbol_count = 0; \ - size_t symbol_strings = 0; \ - Elf##ARCH##_Sym *symbol = 0; \ - const char *section_name = 0; \ - \ - while ((elf_section = elf_nextscn(elf_handle.get(), elf_section)) != NULL) { \ - section_header = elf##ARCH##_getshdr(elf_section); \ - if (section_header == NULL) { \ - return r; \ - } \ - \ - if ((section_name = elf_strptr(elf_handle.get(), shdrstrndx, \ - section_header->sh_name)) == NULL) { \ - return r; \ - } \ - \ - if (cstrings_eq(section_name, ".gnu_debuglink")) { \ - elf_data = elf_getdata(elf_section, NULL); \ - if (elf_data && elf_data->d_size > 0) { \ - debuglink = \ - std::string(reinterpret_cast(elf_data->d_buf)); \ - } \ - } \ - \ - switch (section_header->sh_type) { \ - case SHT_SYMTAB: \ - symbol_section = elf_section; \ - symbol_count = section_header->sh_size / section_header->sh_entsize; \ - symbol_strings = section_header->sh_link; \ - break; \ - \ - /* We use .dynsyms as a last resort, we prefer .symtab */ \ - case SHT_DYNSYM: \ - if (!symbol_section) { \ - symbol_section = elf_section; \ - symbol_count = section_header->sh_size / section_header->sh_entsize; \ - symbol_strings = section_header->sh_link; \ - } \ - break; \ - } \ - } \ - \ - if (symbol_section && symbol_count && symbol_strings) { \ - elf_data = elf_getdata(symbol_section, NULL); \ - symbol = reinterpret_cast(elf_data->d_buf); \ - for (size_t i = 0; i < symbol_count; ++i) { \ - int type = ELF##ARCH##_ST_TYPE(symbol->st_info); \ - if (type == STT_FUNC && symbol->st_value > 0) { \ - r.symbol_cache[symbol->st_value] = std::string( \ - elf_strptr(elf_handle.get(), symbol_strings, symbol->st_name)); \ - } \ - ++symbol; \ - } \ - } - - if (e_ident[EI_CLASS] == ELFCLASS32) { - ELF_GET_DATA(32) - } else if (e_ident[EI_CLASS] == ELFCLASS64) { - // libelf might have been built without 64 bit support -#if __LIBELF64 - ELF_GET_DATA(64) -#endif - } - - if (!debuglink.empty()) { - // We have a debuglink section! Open an elf instance on that - // file instead. If we can't open the file, then return - // the elf handle we had already opened. - dwarf_file_t debuglink_file; - debuglink_file.reset(open(debuglink.c_str(), O_RDONLY)); - if (debuglink_file.get() > 0) { - dwarf_elf_t debuglink_elf; - debuglink_elf.reset(elf_begin(debuglink_file.get(), ELF_C_READ, NULL)); - - // If we have a valid elf handle, return the new elf handle - // and file handle and discard the original ones - if (debuglink_elf) { - elf_handle = move(debuglink_elf); - file_handle = move(debuglink_file); - } - } - } - - // Ok, we have a valid ELF handle, let's try to get debug symbols - Dwarf_Debug dwarf_debug; - Dwarf_Error error = DW_DLE_NE; - dwarf_handle_t dwarf_handle; - - int dwarf_result = dwarf_elf_init(elf_handle.get(), DW_DLC_READ, NULL, NULL, - &dwarf_debug, &error); - - // We don't do any special handling for DW_DLV_NO_ENTRY specially. - // If we get an error, or the file doesn't have debug information - // we just return. - if (dwarf_result != DW_DLV_OK) { - return r; - } - - dwarf_handle.reset(dwarf_debug); - - r.file_handle = move(file_handle); - r.elf_handle = move(elf_handle); - r.dwarf_handle = move(dwarf_handle); - - return r; - } - - die_cache_entry &get_die_cache(dwarf_fileobject &fobj, Dwarf_Die die) { - Dwarf_Error error = DW_DLE_NE; - - // Get the die offset, we use it as the cache key - Dwarf_Off die_offset; - if (dwarf_dieoffset(die, &die_offset, &error) != DW_DLV_OK) { - die_offset = 0; - } - - die_cache_t::iterator it = fobj.die_cache.find(die_offset); - - if (it != fobj.die_cache.end()) { - fobj.current_cu = &it->second; - return it->second; - } - - die_cache_entry &de = fobj.die_cache[die_offset]; - fobj.current_cu = &de; - - Dwarf_Addr line_addr; - Dwarf_Small table_count; - - // The addresses in the line section are not fully sorted (they might - // be sorted by block of code belonging to the same file), which makes - // it necessary to do so before searching is possible. - // - // As libdwarf allocates a copy of everything, let's get the contents - // of the line section and keep it around. We also create a map of - // program counter to line table indices so we can search by address - // and get the line buffer index. - // - // To make things more difficult, the same address can span more than - // one line, so we need to keep the index pointing to the first line - // by using insert instead of the map's [ operator. - - // Get the line context for the DIE - if (dwarf_srclines_b(die, 0, &table_count, &de.line_context, &error) == - DW_DLV_OK) { - // Get the source lines for this line context, to be deallocated - // later - if (dwarf_srclines_from_linecontext(de.line_context, &de.line_buffer, - &de.line_count, - &error) == DW_DLV_OK) { - - // Add all the addresses to our map - for (int i = 0; i < de.line_count; i++) { - if (dwarf_lineaddr(de.line_buffer[i], &line_addr, &error) != - DW_DLV_OK) { - line_addr = 0; - } - de.line_section.insert(std::pair(line_addr, i)); - } - } - } - - // For each CU, cache the function DIEs that contain the - // DW_AT_specification attribute. When building with -g3 the function - // DIEs are separated in declaration and specification, with the - // declaration containing only the name and parameters and the - // specification the low/high pc and other compiler attributes. - // - // We cache those specifications so we don't skip over the declarations, - // because they have no pc, and we can do namespace resolution for - // DWARF function names. - Dwarf_Debug dwarf = fobj.dwarf_handle.get(); - Dwarf_Die current_die = 0; - if (dwarf_child(die, ¤t_die, &error) == DW_DLV_OK) { - for (;;) { - Dwarf_Die sibling_die = 0; - - Dwarf_Half tag_value; - dwarf_tag(current_die, &tag_value, &error); - - if (tag_value == DW_TAG_subprogram || - tag_value == DW_TAG_inlined_subroutine) { - - Dwarf_Bool has_attr = 0; - if (dwarf_hasattr(current_die, DW_AT_specification, &has_attr, - &error) == DW_DLV_OK) { - if (has_attr) { - Dwarf_Attribute attr_mem; - if (dwarf_attr(current_die, DW_AT_specification, &attr_mem, - &error) == DW_DLV_OK) { - Dwarf_Off spec_offset = 0; - if (dwarf_formref(attr_mem, &spec_offset, &error) == - DW_DLV_OK) { - Dwarf_Off spec_die_offset; - if (dwarf_dieoffset(current_die, &spec_die_offset, &error) == - DW_DLV_OK) { - de.spec_section[spec_offset] = spec_die_offset; - } - } - } - dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR); - } - } - } - - int result = dwarf_siblingof(dwarf, current_die, &sibling_die, &error); - if (result == DW_DLV_ERROR) { - break; - } else if (result == DW_DLV_NO_ENTRY) { - break; - } - - if (current_die != die) { - dwarf_dealloc(dwarf, current_die, DW_DLA_DIE); - current_die = 0; - } - - current_die = sibling_die; - } - } - return de; - } - - static Dwarf_Die get_referenced_die(Dwarf_Debug dwarf, Dwarf_Die die, - Dwarf_Half attr, bool global) { - Dwarf_Error error = DW_DLE_NE; - Dwarf_Attribute attr_mem; - - Dwarf_Die found_die = NULL; - if (dwarf_attr(die, attr, &attr_mem, &error) == DW_DLV_OK) { - Dwarf_Off offset; - int result = 0; - if (global) { - result = dwarf_global_formref(attr_mem, &offset, &error); - } else { - result = dwarf_formref(attr_mem, &offset, &error); - } - - if (result == DW_DLV_OK) { - if (dwarf_offdie(dwarf, offset, &found_die, &error) != DW_DLV_OK) { - found_die = NULL; - } - } - dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR); - } - return found_die; - } - - static std::string get_referenced_die_name(Dwarf_Debug dwarf, Dwarf_Die die, - Dwarf_Half attr, bool global) { - Dwarf_Error error = DW_DLE_NE; - std::string value; - - Dwarf_Die found_die = get_referenced_die(dwarf, die, attr, global); - - if (found_die) { - char *name; - if (dwarf_diename(found_die, &name, &error) == DW_DLV_OK) { - if (name) { - value = std::string(name); - } - dwarf_dealloc(dwarf, name, DW_DLA_STRING); - } - dwarf_dealloc(dwarf, found_die, DW_DLA_DIE); - } - - return value; - } - - // Returns a spec DIE linked to the passed one. The caller should - // deallocate the DIE - static Dwarf_Die get_spec_die(dwarf_fileobject &fobj, Dwarf_Die die) { - Dwarf_Debug dwarf = fobj.dwarf_handle.get(); - Dwarf_Error error = DW_DLE_NE; - Dwarf_Off die_offset; - if (fobj.current_cu && - dwarf_die_CU_offset(die, &die_offset, &error) == DW_DLV_OK) { - die_specmap_t::iterator it = - fobj.current_cu->spec_section.find(die_offset); - - // If we have a DIE that completes the current one, check if - // that one has the pc we are looking for - if (it != fobj.current_cu->spec_section.end()) { - Dwarf_Die spec_die = 0; - if (dwarf_offdie(dwarf, it->second, &spec_die, &error) == DW_DLV_OK) { - return spec_die; - } - } - } - - // Maybe we have an abstract origin DIE with the function information? - return get_referenced_die(fobj.dwarf_handle.get(), die, - DW_AT_abstract_origin, true); - } - - static bool die_has_pc(dwarf_fileobject &fobj, Dwarf_Die die, Dwarf_Addr pc) { - Dwarf_Addr low_pc = 0, high_pc = 0; - Dwarf_Half high_pc_form = 0; - Dwarf_Form_Class return_class; - Dwarf_Error error = DW_DLE_NE; - Dwarf_Debug dwarf = fobj.dwarf_handle.get(); - bool has_lowpc = false; - bool has_highpc = false; - bool has_ranges = false; - - if (dwarf_lowpc(die, &low_pc, &error) == DW_DLV_OK) { - // If we have a low_pc check if there is a high pc. - // If we don't have a high pc this might mean we have a base - // address for the ranges list or just an address. - has_lowpc = true; - - if (dwarf_highpc_b(die, &high_pc, &high_pc_form, &return_class, &error) == - DW_DLV_OK) { - // We do have a high pc. In DWARF 4+ this is an offset from the - // low pc, but in earlier versions it's an absolute address. - - has_highpc = true; - // In DWARF 2/3 this would be a DW_FORM_CLASS_ADDRESS - if (return_class == DW_FORM_CLASS_CONSTANT) { - high_pc = low_pc + high_pc; - } - - // We have low and high pc, check if our address - // is in that range - return pc >= low_pc && pc < high_pc; - } - } else { - // Reset the low_pc, in case dwarf_lowpc failing set it to some - // undefined value. - low_pc = 0; - } - - // Check if DW_AT_ranges is present and search for the PC in the - // returned ranges list. We always add the low_pc, as it not set it will - // be 0, in case we had a DW_AT_low_pc and DW_AT_ranges pair - bool result = false; - - Dwarf_Attribute attr; - if (dwarf_attr(die, DW_AT_ranges, &attr, &error) == DW_DLV_OK) { - - Dwarf_Off offset; - if (dwarf_global_formref(attr, &offset, &error) == DW_DLV_OK) { - Dwarf_Ranges *ranges; - Dwarf_Signed ranges_count = 0; - Dwarf_Unsigned byte_count = 0; - - if (dwarf_get_ranges_a(dwarf, offset, die, &ranges, &ranges_count, - &byte_count, &error) == DW_DLV_OK) { - has_ranges = ranges_count != 0; - for (int i = 0; i < ranges_count; i++) { - if (ranges[i].dwr_addr1 != 0 && - pc >= ranges[i].dwr_addr1 + low_pc && - pc < ranges[i].dwr_addr2 + low_pc) { - result = true; - break; - } - } - dwarf_ranges_dealloc(dwarf, ranges, ranges_count); - } - } - } - - // Last attempt. We might have a single address set as low_pc. - if (!result && low_pc != 0 && pc == low_pc) { - result = true; - } - - // If we don't have lowpc, highpc and ranges maybe this DIE is a - // declaration that relies on a DW_AT_specification DIE that happens - // later. Use the specification cache we filled when we loaded this CU. - if (!result && (!has_lowpc && !has_highpc && !has_ranges)) { - Dwarf_Die spec_die = get_spec_die(fobj, die); - if (spec_die) { - result = die_has_pc(fobj, spec_die, pc); - dwarf_dealloc(dwarf, spec_die, DW_DLA_DIE); - } - } - - return result; - } - - static void get_type(Dwarf_Debug dwarf, Dwarf_Die die, std::string &type) { - Dwarf_Error error = DW_DLE_NE; - - Dwarf_Die child = 0; - if (dwarf_child(die, &child, &error) == DW_DLV_OK) { - get_type(dwarf, child, type); - } - - if (child) { - type.insert(0, "::"); - dwarf_dealloc(dwarf, child, DW_DLA_DIE); - } - - char *name; - if (dwarf_diename(die, &name, &error) == DW_DLV_OK) { - type.insert(0, std::string(name)); - dwarf_dealloc(dwarf, name, DW_DLA_STRING); - } else { - type.insert(0, ""); - } - } - - static std::string get_type_by_signature(Dwarf_Debug dwarf, Dwarf_Die die) { - Dwarf_Error error = DW_DLE_NE; - - Dwarf_Sig8 signature; - Dwarf_Bool has_attr = 0; - if (dwarf_hasattr(die, DW_AT_signature, &has_attr, &error) == DW_DLV_OK) { - if (has_attr) { - Dwarf_Attribute attr_mem; - if (dwarf_attr(die, DW_AT_signature, &attr_mem, &error) == DW_DLV_OK) { - if (dwarf_formsig8(attr_mem, &signature, &error) != DW_DLV_OK) { - return std::string(""); - } - } - dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR); - } - } - - Dwarf_Unsigned next_cu_header; - Dwarf_Sig8 tu_signature; - std::string result; - bool found = false; - - while (dwarf_next_cu_header_d(dwarf, 0, 0, 0, 0, 0, 0, 0, &tu_signature, 0, - &next_cu_header, 0, &error) == DW_DLV_OK) { - - if (strncmp(signature.signature, tu_signature.signature, 8) == 0) { - Dwarf_Die type_cu_die = 0; - if (dwarf_siblingof_b(dwarf, 0, 0, &type_cu_die, &error) == DW_DLV_OK) { - Dwarf_Die child_die = 0; - if (dwarf_child(type_cu_die, &child_die, &error) == DW_DLV_OK) { - get_type(dwarf, child_die, result); - found = !result.empty(); - dwarf_dealloc(dwarf, child_die, DW_DLA_DIE); - } - dwarf_dealloc(dwarf, type_cu_die, DW_DLA_DIE); - } - } - } - - if (found) { - while (dwarf_next_cu_header_d(dwarf, 0, 0, 0, 0, 0, 0, 0, 0, 0, - &next_cu_header, 0, &error) == DW_DLV_OK) { - // Reset the cu header state. Unfortunately, libdwarf's - // next_cu_header API keeps its own iterator per Dwarf_Debug - // that can't be reset. We need to keep fetching elements until - // the end. - } - } else { - // If we couldn't resolve the type just print out the signature - std::ostringstream string_stream; - string_stream << "<0x" << std::hex << std::setfill('0'); - for (int i = 0; i < 8; ++i) { - string_stream << std::setw(2) << std::hex - << (int)(unsigned char)(signature.signature[i]); - } - string_stream << ">"; - result = string_stream.str(); - } - return result; - } - - struct type_context_t { - bool is_const; - bool is_typedef; - bool has_type; - bool has_name; - std::string text; - - type_context_t() - : is_const(false), is_typedef(false), has_type(false), has_name(false) { - } - }; - - // Types are resolved from right to left: we get the variable name first - // and then all specifiers (like const or pointer) in a chain of DW_AT_type - // DIEs. Call this function recursively until we get a complete type - // string. - static void set_parameter_string(dwarf_fileobject &fobj, Dwarf_Die die, - type_context_t &context) { - char *name; - Dwarf_Error error = DW_DLE_NE; - - // typedefs contain also the base type, so we skip it and only - // print the typedef name - if (!context.is_typedef) { - if (dwarf_diename(die, &name, &error) == DW_DLV_OK) { - if (!context.text.empty()) { - context.text.insert(0, " "); - } - context.text.insert(0, std::string(name)); - dwarf_dealloc(fobj.dwarf_handle.get(), name, DW_DLA_STRING); - } - } else { - context.is_typedef = false; - context.has_type = true; - if (context.is_const) { - context.text.insert(0, "const "); - context.is_const = false; - } - } - - bool next_type_is_const = false; - bool is_keyword = true; - - Dwarf_Half tag = 0; - Dwarf_Bool has_attr = 0; - if (dwarf_tag(die, &tag, &error) == DW_DLV_OK) { - switch (tag) { - case DW_TAG_structure_type: - case DW_TAG_union_type: - case DW_TAG_class_type: - case DW_TAG_enumeration_type: - context.has_type = true; - if (dwarf_hasattr(die, DW_AT_signature, &has_attr, &error) == - DW_DLV_OK) { - // If we have a signature it means the type is defined - // in .debug_types, so we need to load the DIE pointed - // at by the signature and resolve it - if (has_attr) { - std::string type = - get_type_by_signature(fobj.dwarf_handle.get(), die); - if (context.is_const) - type.insert(0, "const "); - - if (!context.text.empty()) - context.text.insert(0, " "); - context.text.insert(0, type); - } - - // Treat enums like typedefs, and skip printing its - // base type - context.is_typedef = (tag == DW_TAG_enumeration_type); - } - break; - case DW_TAG_const_type: - next_type_is_const = true; - break; - case DW_TAG_pointer_type: - context.text.insert(0, "*"); - break; - case DW_TAG_reference_type: - context.text.insert(0, "&"); - break; - case DW_TAG_restrict_type: - context.text.insert(0, "restrict "); - break; - case DW_TAG_rvalue_reference_type: - context.text.insert(0, "&&"); - break; - case DW_TAG_volatile_type: - context.text.insert(0, "volatile "); - break; - case DW_TAG_typedef: - // Propagate the const-ness to the next type - // as typedefs are linked to its base type - next_type_is_const = context.is_const; - context.is_typedef = true; - context.has_type = true; - break; - case DW_TAG_base_type: - context.has_type = true; - break; - case DW_TAG_formal_parameter: - context.has_name = true; - break; - default: - is_keyword = false; - break; - } - } - - if (!is_keyword && context.is_const) { - context.text.insert(0, "const "); - } - - context.is_const = next_type_is_const; - - Dwarf_Die ref = - get_referenced_die(fobj.dwarf_handle.get(), die, DW_AT_type, true); - if (ref) { - set_parameter_string(fobj, ref, context); - dwarf_dealloc(fobj.dwarf_handle.get(), ref, DW_DLA_DIE); - } - - if (!context.has_type && context.has_name) { - context.text.insert(0, "void "); - context.has_type = true; - } - } - - // Resolve the function return type and parameters - static void set_function_parameters(std::string &function_name, - std::vector &ns, - dwarf_fileobject &fobj, Dwarf_Die die) { - Dwarf_Debug dwarf = fobj.dwarf_handle.get(); - Dwarf_Error error = DW_DLE_NE; - Dwarf_Die current_die = 0; - std::string parameters; - bool has_spec = true; - // Check if we have a spec DIE. If we do we use it as it contains - // more information, like parameter names. - Dwarf_Die spec_die = get_spec_die(fobj, die); - if (!spec_die) { - has_spec = false; - spec_die = die; - } - - std::vector::const_iterator it = ns.begin(); - std::string ns_name; - for (it = ns.begin(); it < ns.end(); ++it) { - ns_name.append(*it).append("::"); - } - - if (!ns_name.empty()) { - function_name.insert(0, ns_name); - } - - // See if we have a function return type. It can be either on the - // current die or in its spec one (usually true for inlined functions) - std::string return_type = - get_referenced_die_name(dwarf, die, DW_AT_type, true); - if (return_type.empty()) { - return_type = get_referenced_die_name(dwarf, spec_die, DW_AT_type, true); - } - if (!return_type.empty()) { - return_type.append(" "); - function_name.insert(0, return_type); - } - - if (dwarf_child(spec_die, ¤t_die, &error) == DW_DLV_OK) { - for (;;) { - Dwarf_Die sibling_die = 0; - - Dwarf_Half tag_value; - dwarf_tag(current_die, &tag_value, &error); - - if (tag_value == DW_TAG_formal_parameter) { - // Ignore artificial (ie, compiler generated) parameters - bool is_artificial = false; - Dwarf_Attribute attr_mem; - if (dwarf_attr(current_die, DW_AT_artificial, &attr_mem, &error) == - DW_DLV_OK) { - Dwarf_Bool flag = 0; - if (dwarf_formflag(attr_mem, &flag, &error) == DW_DLV_OK) { - is_artificial = flag != 0; - } - dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR); - } - - if (!is_artificial) { - type_context_t context; - set_parameter_string(fobj, current_die, context); - - if (parameters.empty()) { - parameters.append("("); - } else { - parameters.append(", "); - } - parameters.append(context.text); - } - } - - int result = dwarf_siblingof(dwarf, current_die, &sibling_die, &error); - if (result == DW_DLV_ERROR) { - break; - } else if (result == DW_DLV_NO_ENTRY) { - break; - } - - if (current_die != die) { - dwarf_dealloc(dwarf, current_die, DW_DLA_DIE); - current_die = 0; - } - - current_die = sibling_die; - } - } - if (parameters.empty()) - parameters = "("; - parameters.append(")"); - - // If we got a spec DIE we need to deallocate it - if (has_spec) - dwarf_dealloc(dwarf, spec_die, DW_DLA_DIE); - - function_name.append(parameters); - } - - // defined here because in C++98, template function cannot take locally - // defined types... grrr. - struct inliners_search_cb { - void operator()(Dwarf_Die die, std::vector &ns) { - Dwarf_Error error = DW_DLE_NE; - Dwarf_Half tag_value; - Dwarf_Attribute attr_mem; - Dwarf_Debug dwarf = fobj.dwarf_handle.get(); - - dwarf_tag(die, &tag_value, &error); - - switch (tag_value) { - char *name; - case DW_TAG_subprogram: - if (!trace.source.function.empty()) - break; - if (dwarf_diename(die, &name, &error) == DW_DLV_OK) { - trace.source.function = std::string(name); - dwarf_dealloc(dwarf, name, DW_DLA_STRING); - } else { - // We don't have a function name in this DIE. - // Check if there is a referenced non-defining - // declaration. - trace.source.function = - get_referenced_die_name(dwarf, die, DW_AT_abstract_origin, true); - if (trace.source.function.empty()) { - trace.source.function = - get_referenced_die_name(dwarf, die, DW_AT_specification, true); - } - } - - // Append the function parameters, if available - set_function_parameters(trace.source.function, ns, fobj, die); - - // If the object function name is empty, it's possible that - // there is no dynamic symbol table (maybe the executable - // was stripped or not built with -rdynamic). See if we have - // a DWARF linkage name to use instead. We try both - // linkage_name and MIPS_linkage_name because the MIPS tag - // was the unofficial one until it was adopted in DWARF4. - // Old gcc versions generate MIPS_linkage_name - if (trace.object_function.empty()) { - details::demangler demangler; - - if (dwarf_attr(die, DW_AT_linkage_name, &attr_mem, &error) != - DW_DLV_OK) { - if (dwarf_attr(die, DW_AT_MIPS_linkage_name, &attr_mem, &error) != - DW_DLV_OK) { - break; - } - } - - char *linkage; - if (dwarf_formstring(attr_mem, &linkage, &error) == DW_DLV_OK) { - trace.object_function = demangler.demangle(linkage); - dwarf_dealloc(dwarf, linkage, DW_DLA_STRING); - } - dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR); - } - break; - - case DW_TAG_inlined_subroutine: - ResolvedTrace::SourceLoc sloc; - - if (dwarf_diename(die, &name, &error) == DW_DLV_OK) { - sloc.function = std::string(name); - dwarf_dealloc(dwarf, name, DW_DLA_STRING); - } else { - // We don't have a name for this inlined DIE, it could - // be that there is an abstract origin instead. - // Get the DW_AT_abstract_origin value, which is a - // reference to the source DIE and try to get its name - sloc.function = - get_referenced_die_name(dwarf, die, DW_AT_abstract_origin, true); - } - - set_function_parameters(sloc.function, ns, fobj, die); - - std::string file = die_call_file(dwarf, die, cu_die); - if (!file.empty()) - sloc.filename = file; - - Dwarf_Unsigned number = 0; - if (dwarf_attr(die, DW_AT_call_line, &attr_mem, &error) == DW_DLV_OK) { - if (dwarf_formudata(attr_mem, &number, &error) == DW_DLV_OK) { - sloc.line = number; - } - dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR); - } - - if (dwarf_attr(die, DW_AT_call_column, &attr_mem, &error) == - DW_DLV_OK) { - if (dwarf_formudata(attr_mem, &number, &error) == DW_DLV_OK) { - sloc.col = number; - } - dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR); - } - - trace.inliners.push_back(sloc); - break; - }; - } - ResolvedTrace &trace; - dwarf_fileobject &fobj; - Dwarf_Die cu_die; - inliners_search_cb(ResolvedTrace &t, dwarf_fileobject &f, Dwarf_Die c) - : trace(t), fobj(f), cu_die(c) {} - }; - - static Dwarf_Die find_fundie_by_pc(dwarf_fileobject &fobj, - Dwarf_Die parent_die, Dwarf_Addr pc, - Dwarf_Die result) { - Dwarf_Die current_die = 0; - Dwarf_Error error = DW_DLE_NE; - Dwarf_Debug dwarf = fobj.dwarf_handle.get(); - - if (dwarf_child(parent_die, ¤t_die, &error) != DW_DLV_OK) { - return NULL; - } - - for (;;) { - Dwarf_Die sibling_die = 0; - Dwarf_Half tag_value; - dwarf_tag(current_die, &tag_value, &error); - - switch (tag_value) { - case DW_TAG_subprogram: - case DW_TAG_inlined_subroutine: - if (die_has_pc(fobj, current_die, pc)) { - return current_die; - } - }; - bool declaration = false; - Dwarf_Attribute attr_mem; - if (dwarf_attr(current_die, DW_AT_declaration, &attr_mem, &error) == - DW_DLV_OK) { - Dwarf_Bool flag = 0; - if (dwarf_formflag(attr_mem, &flag, &error) == DW_DLV_OK) { - declaration = flag != 0; - } - dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR); - } - - if (!declaration) { - // let's be curious and look deeper in the tree, functions are - // not necessarily at the first level, but might be nested - // inside a namespace, structure, a function, an inlined - // function etc. - Dwarf_Die die_mem = 0; - Dwarf_Die indie = find_fundie_by_pc(fobj, current_die, pc, die_mem); - if (indie) { - result = die_mem; - return result; - } - } - - int res = dwarf_siblingof(dwarf, current_die, &sibling_die, &error); - if (res == DW_DLV_ERROR) { - return NULL; - } else if (res == DW_DLV_NO_ENTRY) { - break; - } - - if (current_die != parent_die) { - dwarf_dealloc(dwarf, current_die, DW_DLA_DIE); - current_die = 0; - } - - current_die = sibling_die; - } - return NULL; - } - - template - static bool deep_first_search_by_pc(dwarf_fileobject &fobj, - Dwarf_Die parent_die, Dwarf_Addr pc, - std::vector &ns, CB cb) { - Dwarf_Die current_die = 0; - Dwarf_Debug dwarf = fobj.dwarf_handle.get(); - Dwarf_Error error = DW_DLE_NE; - - if (dwarf_child(parent_die, ¤t_die, &error) != DW_DLV_OK) { - return false; - } - - bool branch_has_pc = false; - bool has_namespace = false; - for (;;) { - Dwarf_Die sibling_die = 0; - - Dwarf_Half tag; - if (dwarf_tag(current_die, &tag, &error) == DW_DLV_OK) { - if (tag == DW_TAG_namespace || tag == DW_TAG_class_type) { - char *ns_name = NULL; - if (dwarf_diename(current_die, &ns_name, &error) == DW_DLV_OK) { - if (ns_name) { - ns.push_back(std::string(ns_name)); - } else { - ns.push_back(""); - } - dwarf_dealloc(dwarf, ns_name, DW_DLA_STRING); - } else { - ns.push_back(""); - } - has_namespace = true; - } - } - - bool declaration = false; - Dwarf_Attribute attr_mem; - if (tag != DW_TAG_class_type && - dwarf_attr(current_die, DW_AT_declaration, &attr_mem, &error) == - DW_DLV_OK) { - Dwarf_Bool flag = 0; - if (dwarf_formflag(attr_mem, &flag, &error) == DW_DLV_OK) { - declaration = flag != 0; - } - dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR); - } - - if (!declaration) { - // let's be curious and look deeper in the tree, function are - // not necessarily at the first level, but might be nested - // inside a namespace, structure, a function, an inlined - // function etc. - branch_has_pc = deep_first_search_by_pc(fobj, current_die, pc, ns, cb); - } - - if (!branch_has_pc) { - branch_has_pc = die_has_pc(fobj, current_die, pc); - } - - if (branch_has_pc) { - cb(current_die, ns); - } - - int result = dwarf_siblingof(dwarf, current_die, &sibling_die, &error); - if (result == DW_DLV_ERROR) { - return false; - } else if (result == DW_DLV_NO_ENTRY) { - break; - } - - if (current_die != parent_die) { - dwarf_dealloc(dwarf, current_die, DW_DLA_DIE); - current_die = 0; - } - - if (has_namespace) { - has_namespace = false; - ns.pop_back(); - } - current_die = sibling_die; - } - - if (has_namespace) { - ns.pop_back(); - } - return branch_has_pc; - } - - static std::string die_call_file(Dwarf_Debug dwarf, Dwarf_Die die, - Dwarf_Die cu_die) { - Dwarf_Attribute attr_mem; - Dwarf_Error error = DW_DLE_NE; - Dwarf_Unsigned file_index; - - std::string file; - - if (dwarf_attr(die, DW_AT_call_file, &attr_mem, &error) == DW_DLV_OK) { - if (dwarf_formudata(attr_mem, &file_index, &error) != DW_DLV_OK) { - file_index = 0; - } - dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR); - - if (file_index == 0) { - return file; - } - - char **srcfiles = 0; - Dwarf_Signed file_count = 0; - if (dwarf_srcfiles(cu_die, &srcfiles, &file_count, &error) == DW_DLV_OK) { - if (file_count > 0 && file_index <= static_cast(file_count)) { - file = std::string(srcfiles[file_index - 1]); - } - - // Deallocate all strings! - for (int i = 0; i < file_count; ++i) { - dwarf_dealloc(dwarf, srcfiles[i], DW_DLA_STRING); - } - dwarf_dealloc(dwarf, srcfiles, DW_DLA_LIST); - } - } - return file; - } - - Dwarf_Die find_die(dwarf_fileobject &fobj, Dwarf_Addr addr) { - // Let's get to work! First see if we have a debug_aranges section so - // we can speed up the search - - Dwarf_Debug dwarf = fobj.dwarf_handle.get(); - Dwarf_Error error = DW_DLE_NE; - Dwarf_Arange *aranges; - Dwarf_Signed arange_count; - - Dwarf_Die returnDie; - bool found = false; - if (dwarf_get_aranges(dwarf, &aranges, &arange_count, &error) != - DW_DLV_OK) { - aranges = NULL; - } - - if (aranges) { - // We have aranges. Get the one where our address is. - Dwarf_Arange arange; - if (dwarf_get_arange(aranges, arange_count, addr, &arange, &error) == - DW_DLV_OK) { - - // We found our address. Get the compilation-unit DIE offset - // represented by the given address range. - Dwarf_Off cu_die_offset; - if (dwarf_get_cu_die_offset(arange, &cu_die_offset, &error) == - DW_DLV_OK) { - // Get the DIE at the offset returned by the aranges search. - // We set is_info to 1 to specify that the offset is from - // the .debug_info section (and not .debug_types) - int dwarf_result = - dwarf_offdie_b(dwarf, cu_die_offset, 1, &returnDie, &error); - - found = dwarf_result == DW_DLV_OK; - } - dwarf_dealloc(dwarf, arange, DW_DLA_ARANGE); - } - } - - if (found) - return returnDie; // The caller is responsible for freeing the die - - // The search for aranges failed. Try to find our address by scanning - // all compilation units. - Dwarf_Unsigned next_cu_header; - Dwarf_Half tag = 0; - returnDie = 0; - - while (!found && - dwarf_next_cu_header_d(dwarf, 1, 0, 0, 0, 0, 0, 0, 0, 0, - &next_cu_header, 0, &error) == DW_DLV_OK) { - - if (returnDie) - dwarf_dealloc(dwarf, returnDie, DW_DLA_DIE); - - if (dwarf_siblingof(dwarf, 0, &returnDie, &error) == DW_DLV_OK) { - if ((dwarf_tag(returnDie, &tag, &error) == DW_DLV_OK) && - tag == DW_TAG_compile_unit) { - if (die_has_pc(fobj, returnDie, addr)) { - found = true; - } - } - } - } - - if (found) { - while (dwarf_next_cu_header_d(dwarf, 1, 0, 0, 0, 0, 0, 0, 0, 0, - &next_cu_header, 0, &error) == DW_DLV_OK) { - // Reset the cu header state. Libdwarf's next_cu_header API - // keeps its own iterator per Dwarf_Debug that can't be reset. - // We need to keep fetching elements until the end. - } - } - - if (found) - return returnDie; - - // We couldn't find any compilation units with ranges or a high/low pc. - // Try again by looking at all DIEs in all compilation units. - Dwarf_Die cudie; - while (dwarf_next_cu_header_d(dwarf, 1, 0, 0, 0, 0, 0, 0, 0, 0, - &next_cu_header, 0, &error) == DW_DLV_OK) { - if (dwarf_siblingof(dwarf, 0, &cudie, &error) == DW_DLV_OK) { - Dwarf_Die die_mem = 0; - Dwarf_Die resultDie = find_fundie_by_pc(fobj, cudie, addr, die_mem); - - if (resultDie) { - found = true; - break; - } - } - } - - if (found) { - while (dwarf_next_cu_header_d(dwarf, 1, 0, 0, 0, 0, 0, 0, 0, 0, - &next_cu_header, 0, &error) == DW_DLV_OK) { - // Reset the cu header state. Libdwarf's next_cu_header API - // keeps its own iterator per Dwarf_Debug that can't be reset. - // We need to keep fetching elements until the end. - } - } - - if (found) - return cudie; - - // We failed. - return NULL; - } -}; -#endif // BACKWARD_HAS_DWARF == 1 - -template <> -class TraceResolverImpl - : public TraceResolverLinuxImpl {}; - -#endif // BACKWARD_SYSTEM_LINUX - -#ifdef BACKWARD_SYSTEM_DARWIN - -template class TraceResolverDarwinImpl; - -template <> -class TraceResolverDarwinImpl - : public TraceResolverImplBase { -public: - void load_addresses(void *const*addresses, int address_count) override { - if (address_count == 0) { - return; - } - _symbols.reset(backtrace_symbols(addresses, address_count)); - } - - ResolvedTrace resolve(ResolvedTrace trace) override { - // parse: - // + - char *filename = _symbols[trace.idx]; - - // skip " " - while (*filename && *filename != ' ') - filename++; - while (*filename == ' ') - filename++; - - // find start of from end ( may contain a space) - char *p = filename + strlen(filename) - 1; - // skip to start of " + " - while (p > filename && *p != ' ') - p--; - while (p > filename && *p == ' ') - p--; - while (p > filename && *p != ' ') - p--; - while (p > filename && *p == ' ') - p--; - char *funcname_end = p + 1; - - // skip to start of "" - while (p > filename && *p != ' ') - p--; - char *funcname = p + 1; - - // skip to start of " " - while (p > filename && *p == ' ') - p--; - while (p > filename && *p != ' ') - p--; - while (p > filename && *p == ' ') - p--; - - // skip "", handling the case where it contains a - char *filename_end = p + 1; - if (p == filename) { - // something went wrong, give up - filename_end = filename + strlen(filename); - funcname = filename_end; - } - trace.object_filename.assign( - filename, filename_end); // ok even if filename_end is the ending \0 - // (then we assign entire string) - - if (*funcname) { // if it's not end of string - *funcname_end = '\0'; - - trace.object_function = this->demangle(funcname); - trace.object_function += " "; - trace.object_function += (funcname_end + 1); - trace.source.function = trace.object_function; // we cannot do better. - } - return trace; - } - -private: - details::handle _symbols; -}; - -template <> -class TraceResolverImpl - : public TraceResolverDarwinImpl {}; - -#endif // BACKWARD_SYSTEM_DARWIN - -#ifdef BACKWARD_SYSTEM_WINDOWS - -// Load all symbol info -// Based on: -// https://stackoverflow.com/questions/6205981/windows-c-stack-trace-from-a-running-app/28276227#28276227 - -struct module_data { - std::string image_name; - std::string module_name; - void *base_address; - DWORD load_size; -}; - -class get_mod_info { - HANDLE process; - static const int buffer_length = 4096; - -public: - get_mod_info(HANDLE h) : process(h) {} - - module_data operator()(HMODULE module) { - module_data ret; - char temp[buffer_length]; - MODULEINFO mi; - - GetModuleInformation(process, module, &mi, sizeof(mi)); - ret.base_address = mi.lpBaseOfDll; - ret.load_size = mi.SizeOfImage; - - GetModuleFileNameExA(process, module, temp, sizeof(temp)); - ret.image_name = temp; - GetModuleBaseNameA(process, module, temp, sizeof(temp)); - ret.module_name = temp; - std::vector img(ret.image_name.begin(), ret.image_name.end()); - std::vector mod(ret.module_name.begin(), ret.module_name.end()); - SymLoadModule64(process, 0, &img[0], &mod[0], (DWORD64)ret.base_address, - ret.load_size); - return ret; - } -}; - -template <> class TraceResolverImpl - : public TraceResolverImplBase { -public: - TraceResolverImpl() { - - HANDLE process = GetCurrentProcess(); - - std::vector modules; - DWORD cbNeeded; - std::vector module_handles(1); - SymInitialize(process, NULL, false); - DWORD symOptions = SymGetOptions(); - symOptions |= SYMOPT_LOAD_LINES | SYMOPT_UNDNAME; - SymSetOptions(symOptions); - EnumProcessModules(process, &module_handles[0], - module_handles.size() * sizeof(HMODULE), &cbNeeded); - module_handles.resize(cbNeeded / sizeof(HMODULE)); - EnumProcessModules(process, &module_handles[0], - module_handles.size() * sizeof(HMODULE), &cbNeeded); - std::transform(module_handles.begin(), module_handles.end(), - std::back_inserter(modules), get_mod_info(process)); - void *base = modules[0].base_address; - IMAGE_NT_HEADERS *h = ImageNtHeader(base); - image_type = h->FileHeader.Machine; - } - - static const int max_sym_len = 255; - struct symbol_t { - SYMBOL_INFO sym; - char buffer[max_sym_len]; - } sym; - - DWORD64 displacement; - - ResolvedTrace resolve(ResolvedTrace t) override { - HANDLE process = GetCurrentProcess(); - - char name[256]; - - memset(&sym, 0, sizeof(sym)); - sym.sym.SizeOfStruct = sizeof(SYMBOL_INFO); - sym.sym.MaxNameLen = max_sym_len; - - if (!SymFromAddr(process, (ULONG64)t.addr, &displacement, &sym.sym)) { - // TODO: error handling everywhere - char* lpMsgBuf; - DWORD dw = GetLastError(); - - FormatMessageA(FORMAT_MESSAGE_ALLOCATE_BUFFER | - FORMAT_MESSAGE_FROM_SYSTEM | - FORMAT_MESSAGE_IGNORE_INSERTS, - NULL, dw, MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), - (char*)&lpMsgBuf, 0, NULL); - - printf(lpMsgBuf); - - // abort(); - } - UnDecorateSymbolName(sym.sym.Name, (PSTR)name, 256, UNDNAME_COMPLETE); - - DWORD offset = 0; - IMAGEHLP_LINE line; - if (SymGetLineFromAddr(process, (ULONG64)t.addr, &offset, &line)) { - t.object_filename = line.FileName; - t.source.filename = line.FileName; - t.source.line = line.LineNumber; - t.source.col = offset; - } - - t.source.function = name; - t.object_filename = ""; - t.object_function = name; - - return t; - } - - DWORD machine_type() const { return image_type; } - -private: - DWORD image_type; -}; - -#endif - -class TraceResolver : public TraceResolverImpl {}; - -/*************** CODE SNIPPET ***************/ - -class SourceFile { -public: - typedef std::vector> lines_t; - - SourceFile() {} - SourceFile(const std::string &path) { - // 1. If BACKWARD_CXX_SOURCE_PREFIXES is set then assume it contains - // a colon-separated list of path prefixes. Try prepending each - // to the given path until a valid file is found. - const std::vector &prefixes = get_paths_from_env_variable(); - for (size_t i = 0; i < prefixes.size(); ++i) { - // Double slashes (//) should not be a problem. - std::string new_path = prefixes[i] + '/' + path; - _file.reset(new std::ifstream(new_path.c_str())); - if (is_open()) - break; - } - // 2. If no valid file found then fallback to opening the path as-is. - if (!_file || !is_open()) { - _file.reset(new std::ifstream(path.c_str())); - } - } - bool is_open() const { return _file->is_open(); } - - lines_t &get_lines(unsigned line_start, unsigned line_count, lines_t &lines) { - using namespace std; - // This function make uses of the dumbest algo ever: - // 1) seek(0) - // 2) read lines one by one and discard until line_start - // 3) read line one by one until line_start + line_count - // - // If you are getting snippets many time from the same file, it is - // somewhat a waste of CPU, feel free to benchmark and propose a - // better solution ;) - - _file->clear(); - _file->seekg(0); - string line; - unsigned line_idx; - - for (line_idx = 1; line_idx < line_start; ++line_idx) { - std::getline(*_file, line); - if (!*_file) { - return lines; - } - } - - // think of it like a lambda in C++98 ;) - // but look, I will reuse it two times! - // What a good boy am I. - struct isspace { - bool operator()(char c) { return std::isspace(c); } - }; - - bool started = false; - for (; line_idx < line_start + line_count; ++line_idx) { - getline(*_file, line); - if (!*_file) { - return lines; - } - if (!started) { - if (std::find_if(line.begin(), line.end(), not_isspace()) == line.end()) - continue; - started = true; - } - lines.push_back(make_pair(line_idx, line)); - } - - lines.erase( - std::find_if(lines.rbegin(), lines.rend(), not_isempty()).base(), - lines.end()); - return lines; - } - - lines_t get_lines(unsigned line_start, unsigned line_count) { - lines_t lines; - return get_lines(line_start, line_count, lines); - } - - // there is no find_if_not in C++98, lets do something crappy to - // workaround. - struct not_isspace { - bool operator()(char c) { return !std::isspace(c); } - }; - // and define this one here because C++98 is not happy with local defined - // struct passed to template functions, fuuuu. - struct not_isempty { - bool operator()(const lines_t::value_type &p) { - return !(std::find_if(p.second.begin(), p.second.end(), not_isspace()) == - p.second.end()); - } - }; - - void swap(SourceFile &b) { _file.swap(b._file); } - -#ifdef BACKWARD_ATLEAST_CXX11 - SourceFile(SourceFile &&from) : _file(nullptr) { swap(from); } - SourceFile &operator=(SourceFile &&from) { - swap(from); - return *this; - } -#else - explicit SourceFile(const SourceFile &from) { - // some sort of poor man's move semantic. - swap(const_cast(from)); - } - SourceFile &operator=(const SourceFile &from) { - // some sort of poor man's move semantic. - swap(const_cast(from)); - return *this; - } -#endif - -private: - details::handle> - _file; - - std::vector get_paths_from_env_variable_impl() { - std::vector paths; - const char *prefixes_str = std::getenv("BACKWARD_CXX_SOURCE_PREFIXES"); - if (prefixes_str && prefixes_str[0]) { - paths = details::split_source_prefixes(prefixes_str); - } - return paths; - } - - const std::vector &get_paths_from_env_variable() { - static std::vector paths = get_paths_from_env_variable_impl(); - return paths; - } - -#ifdef BACKWARD_ATLEAST_CXX11 - SourceFile(const SourceFile &) = delete; - SourceFile &operator=(const SourceFile &) = delete; -#endif -}; - -class SnippetFactory { -public: - typedef SourceFile::lines_t lines_t; - - lines_t get_snippet(const std::string &filename, unsigned line_start, - unsigned context_size) { - - SourceFile &src_file = get_src_file(filename); - unsigned start = line_start - context_size / 2; - return src_file.get_lines(start, context_size); - } - - lines_t get_combined_snippet(const std::string &filename_a, unsigned line_a, - const std::string &filename_b, unsigned line_b, - unsigned context_size) { - SourceFile &src_file_a = get_src_file(filename_a); - SourceFile &src_file_b = get_src_file(filename_b); - - lines_t lines = - src_file_a.get_lines(line_a - context_size / 4, context_size / 2); - src_file_b.get_lines(line_b - context_size / 4, context_size / 2, lines); - return lines; - } - - lines_t get_coalesced_snippet(const std::string &filename, unsigned line_a, - unsigned line_b, unsigned context_size) { - SourceFile &src_file = get_src_file(filename); - - using std::max; - using std::min; - unsigned a = min(line_a, line_b); - unsigned b = max(line_a, line_b); - - if ((b - a) < (context_size / 3)) { - return src_file.get_lines((a + b - context_size + 1) / 2, context_size); - } - - lines_t lines = src_file.get_lines(a - context_size / 4, context_size / 2); - src_file.get_lines(b - context_size / 4, context_size / 2, lines); - return lines; - } - -private: - typedef details::hashtable::type src_files_t; - src_files_t _src_files; - - SourceFile &get_src_file(const std::string &filename) { - src_files_t::iterator it = _src_files.find(filename); - if (it != _src_files.end()) { - return it->second; - } - SourceFile &new_src_file = _src_files[filename]; - new_src_file = SourceFile(filename); - return new_src_file; - } -}; - -/*************** PRINTER ***************/ - -namespace ColorMode { -enum type { automatic, never, always }; -} - -class cfile_streambuf : public std::streambuf { -public: - cfile_streambuf(FILE *_sink) : sink(_sink) {} - int_type underflow() override { return traits_type::eof(); } - int_type overflow(int_type ch) override { - if (traits_type::not_eof(ch) && fputc(ch, sink) != EOF) { - return ch; - } - return traits_type::eof(); - } - - std::streamsize xsputn(const char_type *s, std::streamsize count) override { - return static_cast( - fwrite(s, sizeof *s, static_cast(count), sink)); - } - -#ifdef BACKWARD_ATLEAST_CXX11 -public: - cfile_streambuf(const cfile_streambuf &) = delete; - cfile_streambuf &operator=(const cfile_streambuf &) = delete; -#else -private: - cfile_streambuf(const cfile_streambuf &); - cfile_streambuf &operator=(const cfile_streambuf &); -#endif - -private: - FILE *sink; - std::vector buffer; -}; - -#ifdef BACKWARD_SYSTEM_LINUX - -namespace Color { -enum type { yellow = 33, purple = 35, reset = 39 }; -} // namespace Color - -class Colorize { -public: - Colorize(std::ostream &os) : _os(os), _reset(false), _enabled(false) {} - - void activate(ColorMode::type mode) { _enabled = mode == ColorMode::always; } - - void activate(ColorMode::type mode, FILE *fp) { activate(mode, fileno(fp)); } - - void set_color(Color::type ccode) { - if (!_enabled) - return; - - // I assume that the terminal can handle basic colors. Seriously I - // don't want to deal with all the termcap shit. - _os << "\033[" << static_cast(ccode) << "m"; - _reset = (ccode != Color::reset); - } - - ~Colorize() { - if (_reset) { - set_color(Color::reset); - } - } - -private: - void activate(ColorMode::type mode, int fd) { - activate(mode == ColorMode::automatic && isatty(fd) ? ColorMode::always - : mode); - } - - std::ostream &_os; - bool _reset; - bool _enabled; -}; - -#else // ndef BACKWARD_SYSTEM_LINUX - -namespace Color { -enum type { yellow = 0, purple = 0, reset = 0 }; -} // namespace Color - -class Colorize { -public: - Colorize(std::ostream &) {} - void activate(ColorMode::type) {} - void activate(ColorMode::type, FILE *) {} - void set_color(Color::type) {} -}; - -#endif // BACKWARD_SYSTEM_LINUX - -class Printer { -public: - bool snippet; - ColorMode::type color_mode; - bool address; - bool object; - int inliner_context_size; - int trace_context_size; - - Printer() - : snippet(true), color_mode(ColorMode::automatic), address(false), - object(false), inliner_context_size(5), trace_context_size(7) {} - - template FILE *print(ST &st, FILE *fp = stderr) { - cfile_streambuf obuf(fp); - std::ostream os(&obuf); - Colorize colorize(os); - colorize.activate(color_mode, fp); - print_stacktrace(st, os, colorize); - return fp; - } - - template std::ostream &print(ST &st, std::ostream &os) { - Colorize colorize(os); - colorize.activate(color_mode); - print_stacktrace(st, os, colorize); - return os; - } - - template - FILE *print(IT begin, IT end, FILE *fp = stderr, size_t thread_id = 0) { - cfile_streambuf obuf(fp); - std::ostream os(&obuf); - Colorize colorize(os); - colorize.activate(color_mode, fp); - print_stacktrace(begin, end, os, thread_id, colorize); - return fp; - } - - template - std::ostream &print(IT begin, IT end, std::ostream &os, - size_t thread_id = 0) { - Colorize colorize(os); - colorize.activate(color_mode); - print_stacktrace(begin, end, os, thread_id, colorize); - return os; - } - - TraceResolver const &resolver() const { return _resolver; } - -private: - TraceResolver _resolver; - SnippetFactory _snippets; - - template - void print_stacktrace(ST &st, std::ostream &os, Colorize &colorize) { - print_header(os, st.thread_id()); - _resolver.load_stacktrace(st); - for (size_t trace_idx = st.size(); trace_idx > 0; --trace_idx) { - print_trace(os, _resolver.resolve(st[trace_idx - 1]), colorize); - } - } - - template - void print_stacktrace(IT begin, IT end, std::ostream &os, size_t thread_id, - Colorize &colorize) { - print_header(os, thread_id); - for (; begin != end; ++begin) { - print_trace(os, *begin, colorize); - } - } - - void print_header(std::ostream &os, size_t thread_id) { - os << "Stack trace (most recent call last)"; - if (thread_id) { - os << " in thread " << thread_id; - } - os << ":\n"; - } - - void print_trace(std::ostream &os, const ResolvedTrace &trace, - Colorize &colorize) { - os << "#" << std::left << std::setw(2) << trace.idx << std::right; - bool already_indented = true; - - if (!trace.source.filename.size() || object) { - os << " Object \"" << trace.object_filename << "\", at " << trace.addr - << ", in " << trace.object_function << "\n"; - already_indented = false; - } - - for (size_t inliner_idx = trace.inliners.size(); inliner_idx > 0; - --inliner_idx) { - if (!already_indented) { - os << " "; - } - const ResolvedTrace::SourceLoc &inliner_loc = - trace.inliners[inliner_idx - 1]; - print_source_loc(os, " | ", inliner_loc); - if (snippet) { - print_snippet(os, " | ", inliner_loc, colorize, Color::purple, - inliner_context_size); - } - already_indented = false; - } - - if (trace.source.filename.size()) { - if (!already_indented) { - os << " "; - } - print_source_loc(os, " ", trace.source, trace.addr); - if (snippet) { - print_snippet(os, " ", trace.source, colorize, Color::yellow, - trace_context_size); - } - } - } - - void print_snippet(std::ostream &os, const char *indent, - const ResolvedTrace::SourceLoc &source_loc, - Colorize &colorize, Color::type color_code, - int context_size) { - using namespace std; - typedef SnippetFactory::lines_t lines_t; - - lines_t lines = _snippets.get_snippet(source_loc.filename, source_loc.line, - static_cast(context_size)); - - for (lines_t::const_iterator it = lines.begin(); it != lines.end(); ++it) { - if (it->first == source_loc.line) { - colorize.set_color(color_code); - os << indent << ">"; - } else { - os << indent << " "; - } - os << std::setw(4) << it->first << ": " << it->second << "\n"; - if (it->first == source_loc.line) { - colorize.set_color(Color::reset); - } - } - } - - void print_source_loc(std::ostream &os, const char *indent, - const ResolvedTrace::SourceLoc &source_loc, - void *addr = nullptr) { - os << indent << "Source \"" << source_loc.filename << "\", line " - << source_loc.line << ", in " << source_loc.function; - - if (address && addr != nullptr) { - os << " [" << addr << "]"; - } - os << "\n"; - } -}; - -/*************** SIGNALS HANDLING ***************/ - -#if defined(BACKWARD_SYSTEM_LINUX) || defined(BACKWARD_SYSTEM_DARWIN) - -class SignalHandling { -public: - static std::vector make_default_signals() { - const int posix_signals[] = { - // Signals for which the default action is "Core". - SIGABRT, // Abort signal from abort(3) - SIGBUS, // Bus error (bad memory access) - SIGFPE, // Floating point exception - SIGILL, // Illegal Instruction - SIGIOT, // IOT trap. A synonym for SIGABRT - SIGQUIT, // Quit from keyboard - SIGSEGV, // Invalid memory reference - SIGSYS, // Bad argument to routine (SVr4) - SIGTRAP, // Trace/breakpoint trap - SIGXCPU, // CPU time limit exceeded (4.2BSD) - SIGXFSZ, // File size limit exceeded (4.2BSD) -#if defined(BACKWARD_SYSTEM_DARWIN) - SIGEMT, // emulation instruction executed -#endif - }; - return std::vector(posix_signals, - posix_signals + - sizeof posix_signals / sizeof posix_signals[0]); - } - - SignalHandling(const std::vector &posix_signals = make_default_signals()) - : _loaded(false) { - bool success = true; - - const size_t stack_size = 1024 * 1024 * 8; - _stack_content.reset(static_cast(malloc(stack_size))); - if (_stack_content) { - stack_t ss; - ss.ss_sp = _stack_content.get(); - ss.ss_size = stack_size; - ss.ss_flags = 0; - if (sigaltstack(&ss, nullptr) < 0) { - success = false; - } - } else { - success = false; - } - - for (size_t i = 0; i < posix_signals.size(); ++i) { - struct sigaction action; - memset(&action, 0, sizeof action); - action.sa_flags = - static_cast(SA_SIGINFO | SA_ONSTACK | SA_NODEFER | SA_RESETHAND); - sigfillset(&action.sa_mask); - sigdelset(&action.sa_mask, posix_signals[i]); -#if defined(__clang__) -#pragma clang diagnostic push -#pragma clang diagnostic ignored "-Wdisabled-macro-expansion" -#endif - action.sa_sigaction = &sig_handler; -#if defined(__clang__) -#pragma clang diagnostic pop -#endif - - int r = sigaction(posix_signals[i], &action, nullptr); - if (r < 0) - success = false; - } - - _loaded = success; - } - - bool loaded() const { return _loaded; } - - static void handleSignal(int, siginfo_t *info, void *_ctx) { - ucontext_t *uctx = static_cast(_ctx); - - StackTrace st; - void *error_addr = nullptr; -#ifdef REG_RIP // x86_64 - error_addr = reinterpret_cast(uctx->uc_mcontext.gregs[REG_RIP]); -#elif defined(REG_EIP) // x86_32 - error_addr = reinterpret_cast(uctx->uc_mcontext.gregs[REG_EIP]); -#elif defined(__arm__) - error_addr = reinterpret_cast(uctx->uc_mcontext.arm_pc); -#elif defined(__aarch64__) - #if defined(__APPLE__) - error_addr = reinterpret_cast(uctx->uc_mcontext->__ss.__pc); - #else - error_addr = reinterpret_cast(uctx->uc_mcontext.pc); - #endif -#elif defined(__mips__) - error_addr = reinterpret_cast( - reinterpret_cast(&uctx->uc_mcontext)->sc_pc); -#elif defined(__ppc__) || defined(__powerpc) || defined(__powerpc__) || \ - defined(__POWERPC__) - error_addr = reinterpret_cast(uctx->uc_mcontext.regs->nip); -#elif defined(__riscv) - error_addr = reinterpret_cast(uctx->uc_mcontext.__gregs[REG_PC]); -#elif defined(__s390x__) - error_addr = reinterpret_cast(uctx->uc_mcontext.psw.addr); -#elif defined(__APPLE__) && defined(__x86_64__) - error_addr = reinterpret_cast(uctx->uc_mcontext->__ss.__rip); -#elif defined(__APPLE__) - error_addr = reinterpret_cast(uctx->uc_mcontext->__ss.__eip); -#else -#warning ":/ sorry, ain't know no nothing none not of your architecture!" -#endif - if (error_addr) { - st.load_from(error_addr, 32, reinterpret_cast(uctx), - info->si_addr); - } else { - st.load_here(32, reinterpret_cast(uctx), info->si_addr); - } - - Printer printer; - printer.address = true; - printer.print(st, stderr); - -#if _XOPEN_SOURCE >= 700 || _POSIX_C_SOURCE >= 200809L - psiginfo(info, nullptr); -#else - (void)info; -#endif - } - -private: - details::handle _stack_content; - bool _loaded; - -#ifdef __GNUC__ - __attribute__((noreturn)) -#endif - static void - sig_handler(int signo, siginfo_t *info, void *_ctx) { - handleSignal(signo, info, _ctx); - - // try to forward the signal. - raise(info->si_signo); - - // terminate the process immediately. - puts("watf? exit"); - _exit(EXIT_FAILURE); - } -}; - -#endif // BACKWARD_SYSTEM_LINUX || BACKWARD_SYSTEM_DARWIN - -#ifdef BACKWARD_SYSTEM_WINDOWS - -class SignalHandling { -public: - SignalHandling(const std::vector & = std::vector()) - : reporter_thread_([]() { - /* We handle crashes in a utility thread: - backward structures and some Windows functions called here - need stack space, which we do not have when we encounter a - stack overflow. - To support reporting stack traces during a stack overflow, - we create a utility thread at startup, which waits until a - crash happens or the program exits normally. */ - - { - std::unique_lock lk(mtx()); - cv().wait(lk, [] { return crashed() != crash_status::running; }); - } - if (crashed() == crash_status::crashed) { - handle_stacktrace(skip_recs()); - } - { - std::unique_lock lk(mtx()); - crashed() = crash_status::ending; - } - cv().notify_one(); - }) { - SetUnhandledExceptionFilter(crash_handler); - - signal(SIGABRT, signal_handler); - _set_abort_behavior(0, _WRITE_ABORT_MSG | _CALL_REPORTFAULT); - - std::set_terminate(&terminator); -#ifndef BACKWARD_ATLEAST_CXX17 - std::set_unexpected(&terminator); -#endif - _set_purecall_handler(&terminator); - _set_invalid_parameter_handler(&invalid_parameter_handler); - } - bool loaded() const { return true; } - - ~SignalHandling() { - { - std::unique_lock lk(mtx()); - crashed() = crash_status::normal_exit; - } - - cv().notify_one(); - - reporter_thread_.join(); - } - -private: - static CONTEXT *ctx() { - static CONTEXT data; - return &data; - } - - enum class crash_status { running, crashed, normal_exit, ending }; - - static crash_status &crashed() { - static crash_status data; - return data; - } - - static std::mutex &mtx() { - static std::mutex data; - return data; - } - - static std::condition_variable &cv() { - static std::condition_variable data; - return data; - } - - static HANDLE &thread_handle() { - static HANDLE handle; - return handle; - } - - std::thread reporter_thread_; - - // TODO: how not to hardcode these? - static const constexpr int signal_skip_recs = -#ifdef __clang__ - // With clang, RtlCaptureContext also captures the stack frame of the - // current function Below that, there ar 3 internal Windows functions - 4 -#else - // With MSVC cl, RtlCaptureContext misses the stack frame of the current - // function The first entries during StackWalk are the 3 internal Windows - // functions - 3 -#endif - ; - - static int &skip_recs() { - static int data; - return data; - } - - static inline void terminator() { - crash_handler(signal_skip_recs); - abort(); - } - - static inline void signal_handler(int) { - crash_handler(signal_skip_recs); - abort(); - } - - static inline void __cdecl invalid_parameter_handler(const wchar_t *, - const wchar_t *, - const wchar_t *, - unsigned int, - uintptr_t) { - crash_handler(signal_skip_recs); - abort(); - } - - NOINLINE static LONG WINAPI crash_handler(EXCEPTION_POINTERS *info) { - // The exception info supplies a trace from exactly where the issue was, - // no need to skip records - crash_handler(0, info->ContextRecord); - return EXCEPTION_CONTINUE_SEARCH; - } - - NOINLINE static void crash_handler(int skip, CONTEXT *ct = nullptr) { - - if (ct == nullptr) { - RtlCaptureContext(ctx()); - } else { - memcpy(ctx(), ct, sizeof(CONTEXT)); - } - DuplicateHandle(GetCurrentProcess(), GetCurrentThread(), - GetCurrentProcess(), &thread_handle(), 0, FALSE, - DUPLICATE_SAME_ACCESS); - - skip_recs() = skip; - - { - std::unique_lock lk(mtx()); - crashed() = crash_status::crashed; - } - - cv().notify_one(); - - { - std::unique_lock lk(mtx()); - cv().wait(lk, [] { return crashed() != crash_status::crashed; }); - } - } - - static void handle_stacktrace(int skip_frames = 0) { - // printer creates the TraceResolver, which can supply us a machine type - // for stack walking. Without this, StackTrace can only guess using some - // macros. - // StackTrace also requires that the PDBs are already loaded, which is done - // in the constructor of TraceResolver - Printer printer; - - StackTrace st; - st.set_machine_type(printer.resolver().machine_type()); - st.set_thread_handle(thread_handle()); - st.load_here(32 + skip_frames, ctx()); - st.skip_n_firsts(skip_frames); - - printer.address = true; - printer.print(st, std::cerr); - } -}; - -#endif // BACKWARD_SYSTEM_WINDOWS - -#ifdef BACKWARD_SYSTEM_UNKNOWN - -class SignalHandling { -public: - SignalHandling(const std::vector & = std::vector()) {} - bool init() { return false; } - bool loaded() { return false; } -}; - -#endif // BACKWARD_SYSTEM_UNKNOWN - -} // namespace backward - -#endif /* H_GUARD */ diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index d1822d194..8710185a4 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -29,14 +29,6 @@ #include "cartographer_ros_msgs/srv/submap_query.hpp" #include -#include "backward.hpp" - -//namespace backward { - -//backward::SignalHandling sh; - -//} // namespace backward - namespace cartographer_rviz { diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc index 4fea32281..6ceb8bca2 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/cartographer_rviz/submaps_display.cc @@ -36,15 +36,6 @@ // Include ament_index_cpp::get_package_share_directory #include -#include "backward.hpp" - -//namespace backward { - -//backward::SignalHandling sh2; - -//} // namespace backward - - namespace cartographer_rviz { namespace { diff --git a/cartographer_rviz/package.xml b/cartographer_rviz/package.xml index fea142fda..cb0930fb1 100644 --- a/cartographer_rviz/package.xml +++ b/cartographer_rviz/package.xml @@ -55,6 +55,7 @@ rviz_common rviz_default_plugins rcutils + backward_ros ament_cmake From 6924818f7c1ec45a836b6b73e6cef3d0e867eeb2 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Thu, 16 Sep 2021 15:31:56 +0200 Subject: [PATCH 61/94] closer Signed-off-by: Guillaume Doisy --- cartographer_rviz/CMakeLists.txt | 4 ++-- .../cartographer_rviz/submaps_display.cc | 16 ++++++++-------- .../cartographer_rviz/submaps_display.h | 8 ++++---- 3 files changed, 14 insertions(+), 14 deletions(-) diff --git a/cartographer_rviz/CMakeLists.txt b/cartographer_rviz/CMakeLists.txt index dde7aa1c4..0f22280d6 100644 --- a/cartographer_rviz/CMakeLists.txt +++ b/cartographer_rviz/CMakeLists.txt @@ -181,8 +181,8 @@ target_link_libraries(${PROJECT_NAME} PUBLIC cartographer) #target_link_libraries(${PROJECT_NAME} PUBLIC cartographer_ros) pluginlib_export_plugin_description_file(rviz_common rviz_plugin_description.xml) -register_rviz_ogre_media_exports(DIRECTORIES "ogre_media/materials/scripts") -register_rviz_ogre_media_exports(DIRECTORIES "ogre_media/materials/glsl120") +#register_rviz_ogre_media_exports(DIRECTORIES "ogre_media/materials/scripts") +#register_rviz_ogre_media_exports(DIRECTORIES "ogre_media/materials/glsl120") diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc index 6ceb8bca2..13b7f1bcc 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/cartographer_rviz/submaps_display.cc @@ -63,13 +63,13 @@ SubmapsDisplay::SubmapsDisplay() : rclcpp::Node("submaps_display") { "Low Resolution", false, "Display low resolution slices.", this, SLOT(ResolutionToggled()), this); - sync_srv_client_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); - callback_group_executor_thread = std::thread([&]() { - callback_group_executor->add_callback_group(sync_srv_client_callback_group, this->get_node_base_interface()); - callback_group_executor->spin(); - }); +// sync_srv_client_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); +// callback_group_executor_thread = std::thread([&]() { +// callback_group_executor->add_callback_group(sync_srv_client_callback_group, this->get_node_base_interface()); +// callback_group_executor->spin(); +// }); - client_ = this->create_client<::cartographer_ros_msgs::srv::SubmapQuery>(kDefaultSubmapQueryServiceName,rmw_qos_profile_services_default, sync_srv_client_callback_group);//update_nh_.serviceClient<::cartographer_ros_msgs::msg::SubmapQuery>(""); + client_ = this->create_client<::cartographer_ros_msgs::srv::SubmapQuery>(kDefaultSubmapQueryServiceName,rmw_qos_profile_services_default);//update_nh_.serviceClient<::cartographer_ros_msgs::msg::SubmapQuery>(""); trajectories_category_ = new ::rviz_common::properties::Property( "Submaps", QVariant(), "List of all submaps, organized by trajectories.", this); @@ -115,13 +115,13 @@ void SubmapsDisplay::CreateClient() { } void SubmapsDisplay::onInitialize() { - RTDClass::onInitialize(); + MFDClass::onInitialize(); map_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode(); CreateClient(); } void SubmapsDisplay::reset() { - RTDClass::reset(); + MFDClass::reset(); absl::MutexLock locker(&mutex_); client_.reset(); trajectories_.clear(); diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.h b/cartographer_rviz/cartographer_rviz/submaps_display.h index 7f9ef758a..18ab779b6 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.h +++ b/cartographer_rviz/cartographer_rviz/submaps_display.h @@ -60,7 +60,7 @@ struct Trajectory : public QObject { // every submap containing pre-multiplied alpha and grayscale values, these are // then alpha blended together. class SubmapsDisplay - : public ::rviz_common::RosTopicDisplay<::cartographer_ros_msgs::msg::SubmapList>, rclcpp::Node { //::rviz_common::MessageFilterDisplay<::cartographer_ros_msgs::msg::SubmapList>, rclcpp::Node { + : public ::rviz_common::MessageFilterDisplay<::cartographer_ros_msgs::msg::SubmapList>, rclcpp::Node { //::rviz_common::RosTopicDisplay<::cartographer_ros_msgs::msg::SubmapList>, rclcpp::Node { Q_OBJECT public: @@ -104,9 +104,9 @@ class SubmapsDisplay ::rviz_common::properties::BoolProperty* pose_markers_all_enabled_; ::rviz_common::properties::FloatProperty* fade_out_start_distance_in_meters_; - rclcpp::CallbackGroup::SharedPtr sync_srv_client_callback_group; - rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor; - std::thread callback_group_executor_thread; +// rclcpp::CallbackGroup::SharedPtr sync_srv_client_callback_group; +// rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor; +// std::thread callback_group_executor_thread; }; From 6008cf38f22f775b0e6e1fbf7bb351b606e02844 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Thu, 16 Sep 2021 17:05:43 +0200 Subject: [PATCH 62/94] solved one issue, got another two Signed-off-by: Guillaume Doisy --- cartographer_rviz/CMakeLists.txt | 25 ++++++++----------------- 1 file changed, 8 insertions(+), 17 deletions(-) diff --git a/cartographer_rviz/CMakeLists.txt b/cartographer_rviz/CMakeLists.txt index 0f22280d6..df36278ea 100644 --- a/cartographer_rviz/CMakeLists.txt +++ b/cartographer_rviz/CMakeLists.txt @@ -17,7 +17,7 @@ cmake_minimum_required(VERSION 3.5) project(cartographer_rviz) set(CMAKE_POSITION_INDEPENDENT_CODE TRUE) -#add_subdirectory(../../../../../../abseil-cpp/) +add_subdirectory(../../../../../../abseil-cpp/ build) find_package(ament_cmake REQUIRED) # Default to C++17 @@ -36,7 +36,7 @@ google_initialize_cartographer_project() add_compile_options(-g) -find_package(absl REQUIRED) +#find_package(absl REQUIRED) find_package(Eigen3 REQUIRED) #find_package(cartographer_ros REQUIRED) find_package(cartographer_ros_msgs REQUIRED) @@ -75,7 +75,7 @@ register_rviz_ogre_media_exports(DIRECTORIES "ogre_media/materials/scripts") register_rviz_ogre_media_exports(DIRECTORIES "ogre_media/materials/glsl120") ament_target_dependencies(${PROJECT_NAME} PUBLIC - absl +# absl rclcpp cartographer # cartographer_ros @@ -92,26 +92,22 @@ ament_target_dependencies(${PROJECT_NAME} PUBLIC # rviz ) -target_link_libraries(${PROJECT_NAME} PUBLIC -# usb-1.0 - -lstdc++fs - -ldl -# -lexiv2 - -lboost_thread +target_link_libraries(${PROJECT_NAME} PUBLIC ${PROTOBUF_LIBRARY} absl::algorithm absl::base absl::debugging + absl::container absl::flat_hash_map -# absl::flags absl::memory absl::meta absl::numeric -# absl::random_random + absl::str_format absl::strings absl::synchronization absl::time absl::utility - ) +) + # exiv2lib # nlohmann_json::nlohmann_json # ${gphoto2_LIBRARIES} @@ -180,11 +176,6 @@ target_link_libraries(${PROJECT_NAME} PUBLIC cartographer) #target_link_libraries(${PROJECT_NAME} PUBLIC rviz_ogre_vendor) #target_link_libraries(${PROJECT_NAME} PUBLIC cartographer_ros) -pluginlib_export_plugin_description_file(rviz_common rviz_plugin_description.xml) -#register_rviz_ogre_media_exports(DIRECTORIES "ogre_media/materials/scripts") -#register_rviz_ogre_media_exports(DIRECTORIES "ogre_media/materials/glsl120") - - #install(DIRECTORY configuration_files # DESTINATION share/${PROJECT_NAME}/ From 9557e3a479b39d33e39ba140de7fc5f87e55846d Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Fri, 17 Sep 2021 13:31:10 +0200 Subject: [PATCH 63/94] dirty fix Signed-off-by: Guillaume Doisy --- cartographer_rviz/CMakeLists.txt | 37 ++++++++++++------- .../cartographer_rviz/drawable_submap.cc | 2 +- .../cartographer_rviz/drawable_submap.h | 2 +- .../cartographer_rviz/ogre_slice.cc | 16 ++++---- 4 files changed, 34 insertions(+), 23 deletions(-) diff --git a/cartographer_rviz/CMakeLists.txt b/cartographer_rviz/CMakeLists.txt index df36278ea..867821e75 100644 --- a/cartographer_rviz/CMakeLists.txt +++ b/cartographer_rviz/CMakeLists.txt @@ -17,7 +17,7 @@ cmake_minimum_required(VERSION 3.5) project(cartographer_rviz) set(CMAKE_POSITION_INDEPENDENT_CODE TRUE) -add_subdirectory(../../../../../../abseil-cpp/ build) +#add_subdirectory(../../../../../../abseil-cpp/ build) find_package(ament_cmake REQUIRED) # Default to C++17 @@ -54,6 +54,10 @@ find_package(rcutils REQUIRED) #find_package(rviz REQUIRED) find_package(backward_ros REQUIRED) +set(CMAKE_THREAD_PREFER_PTHREAD TRUE) +set(THREADS_PREFER_PTHREAD_FLAG TRUE) +find_package(Threads REQUIRED) + include_directories( include ".") @@ -93,19 +97,24 @@ ament_target_dependencies(${PROJECT_NAME} PUBLIC ) target_link_libraries(${PROJECT_NAME} PUBLIC ${PROTOBUF_LIBRARY} - absl::algorithm - absl::base - absl::debugging - absl::container - absl::flat_hash_map - absl::memory - absl::meta - absl::numeric - absl::str_format - absl::strings - absl::synchronization - absl::time - absl::utility + + -lstdc++fs + -ldl + -lboost_thread + +# absl::algorithm +# absl::base +# absl::debugging +# absl::container +# absl::flat_hash_map +# absl::memory +# absl::meta +# absl::numeric +# absl::str_format +# absl::strings +# absl::synchronization +# absl::time +# absl::utility ) # exiv2lib diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index 8710185a4..192ee0dd7 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -54,7 +54,7 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id, const bool visible, const bool pose_axes_visible, const float pose_axes_length, const float pose_axes_radius) - : rclcpp::Node("drawable_submap"), id_(id), + : id_(id), display_context_(display_context), submap_node_(map_node->createChildSceneNode()), submap_id_text_node_(submap_node_->createChildSceneNode()), diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.h b/cartographer_rviz/cartographer_rviz/drawable_submap.h index 2f6322f98..361504274 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.h +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.h @@ -43,7 +43,7 @@ namespace cartographer_rviz { // Contains all the information needed to render a submap onto the final // texture representing the whole map. -class DrawableSubmap : public QObject, rclcpp::Node{ +class DrawableSubmap : public QObject{ Q_OBJECT public: diff --git a/cartographer_rviz/cartographer_rviz/ogre_slice.cc b/cartographer_rviz/cartographer_rviz/ogre_slice.cc index 3062042d0..0c1a47962 100644 --- a/cartographer_rviz/cartographer_rviz/ogre_slice.cc +++ b/cartographer_rviz/cartographer_rviz/ogre_slice.cc @@ -24,7 +24,7 @@ #include "OgreMaterialManager.h" #include "OgreTechnique.h" #include "OgreTextureManager.h" -#include "absl/strings/str_cat.h" +//#include "absl/strings/str_cat.h" #include "cartographer/common/port.h" namespace cartographer_rviz { @@ -38,8 +38,10 @@ constexpr char kSubmapTexturePrefix[] = "SubmapTexture"; std::string GetSliceIdentifier( const ::cartographer::mapping::SubmapId& submap_id, const int slice_id) { - return absl::StrCat(submap_id.trajectory_id, "-", submap_id.submap_index, "-", - slice_id); + std::string x; + x= std::to_string(submap_id.trajectory_id) + "-" + std::to_string(submap_id.submap_index) + "-" + std::to_string(slice_id); + //return absl::StrCat(submap_id.trajectory_id, "-", submap_id.submap_index, "-",slice_id); + return x; } } // namespace @@ -60,12 +62,12 @@ OgreSlice::OgreSlice(const ::cartographer::mapping::SubmapId& id, int slice_id, scene_manager_(scene_manager), submap_node_(submap_node), slice_node_(submap_node_->createChildSceneNode()), - manual_object_(scene_manager_->createManualObject(absl::StrCat( - kManualObjectPrefix, GetSliceIdentifier(id, slice_id)))) { + manual_object_(scene_manager_->createManualObject( + kManualObjectPrefix + GetSliceIdentifier(id, slice_id))) { material_ = Ogre::MaterialManager::getSingleton().getByName( kSubmapSourceMaterialName); material_ = material_->clone( - absl::StrCat(kSubmapMaterialPrefix, GetSliceIdentifier(id_, slice_id_))); + kSubmapMaterialPrefix + GetSliceIdentifier(id_, slice_id_)); material_->setReceiveShadows(false); material_->getTechnique(0)->setLightingEnabled(false); material_->setCullingMode(Ogre::CULL_NONE); @@ -126,7 +128,7 @@ void OgreSlice::Update( texture_.setNull(); } const std::string texture_name = - absl::StrCat(kSubmapTexturePrefix, GetSliceIdentifier(id_, slice_id_)); + kSubmapTexturePrefix + GetSliceIdentifier(id_, slice_id_); texture_ = Ogre::TextureManager::getSingleton().loadRawData( texture_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, pixel_stream, submap_texture.width, submap_texture.height, From 60f52b87c080692ff2279de70ba503e27af9e7a8 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Fri, 17 Sep 2021 14:40:55 +0200 Subject: [PATCH 64/94] plugin can be loaded and used but only the submap markers are visualized Signed-off-by: Guillaume Doisy --- cartographer_rviz/cartographer_rviz/ogre_slice.cc | 12 +++++------- .../cartographer_rviz/submaps_display.cc | 2 +- 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/cartographer_rviz/cartographer_rviz/ogre_slice.cc b/cartographer_rviz/cartographer_rviz/ogre_slice.cc index 0c1a47962..8576b87b3 100644 --- a/cartographer_rviz/cartographer_rviz/ogre_slice.cc +++ b/cartographer_rviz/cartographer_rviz/ogre_slice.cc @@ -31,17 +31,15 @@ namespace cartographer_rviz { namespace { -constexpr char kManualObjectPrefix[] = "ManualObjectSubmap"; -constexpr char kSubmapSourceMaterialName[] = "cartographer_ros/Submap"; -constexpr char kSubmapMaterialPrefix[] = "SubmapMaterial"; -constexpr char kSubmapTexturePrefix[] = "SubmapTexture"; +std::string kManualObjectPrefix = "ManualObjectSubmap"; +std::string kSubmapSourceMaterialName = "cartographer_ros/Submap"; +std::string kSubmapMaterialPrefix = "SubmapMaterial"; +std::string kSubmapTexturePrefix = "SubmapTexture"; std::string GetSliceIdentifier( const ::cartographer::mapping::SubmapId& submap_id, const int slice_id) { - std::string x; - x= std::to_string(submap_id.trajectory_id) + "-" + std::to_string(submap_id.submap_index) + "-" + std::to_string(slice_id); //return absl::StrCat(submap_id.trajectory_id, "-", submap_id.submap_index, "-",slice_id); - return x; + return (std::to_string(submap_id.trajectory_id) + "-" + std::to_string(submap_id.submap_index) + "-" + std::to_string(slice_id)); } } // namespace diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc index 13b7f1bcc..0919d146d 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/cartographer_rviz/submaps_display.cc @@ -250,7 +250,7 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) { } } } catch (const tf2::TransformException& ex) { - //ROS_WARN_THROTTLE(1., "Could not compute submap fading: %s", ex.what()); + RCLCPP_WARN(this->get_logger(), "Could not compute submap fading: %s", ex.what()); } // Update the map frame to fixed frame transform. Ogre::Vector3 position; From eed779c22e70385e1f6b8cf395c572db1c519303 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Fri, 17 Sep 2021 14:48:00 +0200 Subject: [PATCH 65/94] oof Signed-off-by: Guillaume Doisy --- cartographer_rviz/cartographer_rviz/drawable_submap.cc | 2 -- 1 file changed, 2 deletions(-) diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index 192ee0dd7..a0513a202 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -45,8 +45,6 @@ constexpr int kNumberOfSlicesPerSubmap = 2; } // namespace - -// DECLARED IT AS A ROS NODE DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id, ::rviz_common::DisplayContext* const display_context, Ogre::SceneNode* const map_node, From 624d2fb7031c4c799ecfa40921e1463b2697ad88 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Tue, 7 Sep 2021 10:28:55 +0200 Subject: [PATCH 66/94] ros1 dep Signed-off-by: Guillaume Doisy --- cartographer_rviz/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/cartographer_rviz/package.xml b/cartographer_rviz/package.xml index cb0930fb1..fea142fda 100644 --- a/cartographer_rviz/package.xml +++ b/cartographer_rviz/package.xml @@ -55,7 +55,6 @@ rviz_common rviz_default_plugins rcutils - backward_ros ament_cmake From 9aadc1c02838ed6888f3bdfe1b08893f2b74dfc7 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Tue, 7 Sep 2021 10:40:21 +0200 Subject: [PATCH 67/94] more ros1 dep fix Signed-off-by: Guillaume Doisy --- cartographer_ros/package.xml | 5 +++++ cartographer_rviz/package.xml | 11 ++--------- 2 files changed, 7 insertions(+), 9 deletions(-) diff --git a/cartographer_ros/package.xml b/cartographer_ros/package.xml index 20afb2d45..6bf19c2f0 100644 --- a/cartographer_ros/package.xml +++ b/cartographer_ros/package.xml @@ -48,10 +48,15 @@ libgflags-dev libgoogle-glog-dev libpcl-all-dev + builtin_interfaces + rosidl_default_generators nav_msgs pcl_conversions robot_state_publisher + rosbag2 + rosbag2_transport rclcpp + launch sensor_msgs std_msgs tf2 diff --git a/cartographer_rviz/package.xml b/cartographer_rviz/package.xml index fea142fda..dcacca0f0 100644 --- a/cartographer_rviz/package.xml +++ b/cartographer_rviz/package.xml @@ -43,18 +43,11 @@ libqt5-core libqt5-gui libqt5-widgets + builtin_interfaces + rosidl_default_generators qtbase5-dev rclcpp - absl - - libpcl-all-dev - pcl_conversions rviz2 - rviz_assimp_vendor - rviz_rendering - rviz_common - rviz_default_plugins - rcutils ament_cmake From a4bd9ecdbdbe969bdee5aed7036779e648aa82a3 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Thu, 14 Oct 2021 14:58:03 +0200 Subject: [PATCH 68/94] fixes to offline node and pbstream to map Signed-off-by: Guillaume Doisy --- cartographer_ros/cartographer_ros/offline_node.cc | 4 +++- .../cartographer_ros/pbstream_map_publisher_main.cc | 2 +- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/cartographer_ros/cartographer_ros/offline_node.cc b/cartographer_ros/cartographer_ros/offline_node.cc index ed4af07ff..4704b7def 100644 --- a/cartographer_ros/cartographer_ros/offline_node.cc +++ b/cartographer_ros/cartographer_ros/offline_node.cc @@ -99,7 +99,9 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory, CHECK(!(FLAGS_bag_filenames.empty() && FLAGS_load_state_filename.empty())) << "-bag_filenames and -load_state_filename cannot both be unspecified."; std::vector bag_filenames; - boost::split(bag_filenames, FLAGS_bag_filenames, boost::is_any_of(",")); + if (!FLAGS_bag_filenames.empty()){ + boost::split(bag_filenames, FLAGS_bag_filenames, boost::is_any_of(",")); + } cartographer_ros::NodeOptions node_options; std::vector configuration_basenames; boost::split(configuration_basenames, FLAGS_configuration_basenames, boost::is_any_of(",")); diff --git a/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc b/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc index 8d149be65..299cb2a06 100644 --- a/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc +++ b/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc @@ -77,7 +77,7 @@ void Run(const std::string& pbstream_filename, const std::string& map_topic, << " (frame_id: " << map_frame_id << ", resolution:" << std::to_string(resolution) << ")."; pub->publish(*msg_ptr); - rclcpp::spin(cartographer_pbstream_map_publisher_node); + //rclcpp::spin(cartographer_pbstream_map_publisher_node); } } // namespace From 9d1cb2cd04cfb5cea4fb2e8c950264ff1db0aa4c Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Fri, 15 Oct 2021 14:01:31 +0200 Subject: [PATCH 69/94] revert change in pbstream map node Signed-off-by: Guillaume Doisy --- .../cartographer_ros/pbstream_map_publisher_main.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc b/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc index 299cb2a06..8d149be65 100644 --- a/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc +++ b/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc @@ -77,7 +77,7 @@ void Run(const std::string& pbstream_filename, const std::string& map_topic, << " (frame_id: " << map_frame_id << ", resolution:" << std::to_string(resolution) << ")."; pub->publish(*msg_ptr); - //rclcpp::spin(cartographer_pbstream_map_publisher_node); + rclcpp::spin(cartographer_pbstream_map_publisher_node); } } // namespace From 2d9dbbe57ae13a3672ef09cb2b992910b11b16da Mon Sep 17 00:00:00 2001 From: Guillaume dev PC Date: Wed, 27 Oct 2021 17:29:52 +0200 Subject: [PATCH 70/94] Q_EMIT / slot working but material error Signed-off-by: Guillaume Doisy --- .../cartographer_rviz/drawable_submap.cc | 12 +++---- .../cartographer_rviz/drawable_submap.h | 3 +- cartographer_rviz/cartographer_rviz/submap.cc | 9 +++--- cartographer_rviz/cartographer_rviz/submap.h | 1 + .../cartographer_rviz/submaps_display.cc | 31 ++++++++++++------- .../cartographer_rviz/submaps_display.h | 2 ++ 6 files changed, 34 insertions(+), 24 deletions(-) diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index a0513a202..7e16d3f6f 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -1,4 +1,4 @@ -/* +/* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); @@ -115,7 +115,9 @@ void DrawableSubmap::Update( .arg(metadata_version_)); } -bool DrawableSubmap::MaybeFetchTexture(rclcpp::Client::SharedPtr const client) { +bool DrawableSubmap::MaybeFetchTexture( + rclcpp::Client::SharedPtr const client, + rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor) { absl::MutexLock locker(&mutex_); // Received metadata version can also be lower if we restarted Cartographer. const bool newer_version_available = @@ -132,11 +134,9 @@ bool DrawableSubmap::MaybeFetchTexture(rclcpp::Client submap_textures = - ::cartographer_ros::FetchSubmapTextures(id_, client,std::chrono::milliseconds(10000)); + ::cartographer_ros::FetchSubmapTextures(id_, client, callback_group_executor, std::chrono::milliseconds(10000)); //,callback_group_executor,std::chrono::milliseconds(10000)); // Missing callbackgroup and timeout but this class isn't a ros node absl::MutexLock locker(&mutex_); query_in_progress_ = false; diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.h b/cartographer_rviz/cartographer_rviz/drawable_submap.h index 361504274..ecc239757 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.h +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.h @@ -63,7 +63,8 @@ class DrawableSubmap : public QObject{ // If an update is needed, it will send an RPC using 'client' to request the // new data for the submap and returns true. - bool MaybeFetchTexture(rclcpp::Client::SharedPtr client); + bool MaybeFetchTexture(rclcpp::Client::SharedPtr client, + rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor); // Returns whether an RPC is in progress. bool QueryInProgress(); diff --git a/cartographer_rviz/cartographer_rviz/submap.cc b/cartographer_rviz/cartographer_rviz/submap.cc index a526473f5..467e3ebc8 100644 --- a/cartographer_rviz/cartographer_rviz/submap.cc +++ b/cartographer_rviz/cartographer_rviz/submap.cc @@ -28,6 +28,7 @@ namespace cartographer_ros { std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures( const ::cartographer::mapping::SubmapId& submap_id, rclcpp::Client::SharedPtr client, + rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor, const std::chrono::milliseconds timeout) { auto request = std::make_shared(); @@ -35,13 +36,11 @@ std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures( request->submap_index = submap_id.submap_index; auto future_result = client->async_send_request(request); - - auto status = future_result.wait_for(timeout); - if (status != std::future_status::ready){ - + if (callback_group_executor->spin_until_future_complete(future_result, timeout) != + rclcpp::FutureReturnCode::SUCCESS) + { return nullptr; } - auto result = future_result.get(); if (result->status.code != ::cartographer_ros_msgs::msg::StatusCode::OK || diff --git a/cartographer_rviz/cartographer_rviz/submap.h b/cartographer_rviz/cartographer_rviz/submap.h index 18feef3ab..48c3a425b 100644 --- a/cartographer_rviz/cartographer_rviz/submap.h +++ b/cartographer_rviz/cartographer_rviz/submap.h @@ -35,6 +35,7 @@ namespace cartographer_ros { std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures( const ::cartographer::mapping::SubmapId& submap_id, rclcpp::Client::SharedPtr client, + rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor, const std::chrono::milliseconds timeout); } // namespace cartographer_ros diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc index 0919d146d..ed50014b3 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/cartographer_rviz/submaps_display.cc @@ -63,13 +63,16 @@ SubmapsDisplay::SubmapsDisplay() : rclcpp::Node("submaps_display") { "Low Resolution", false, "Display low resolution slices.", this, SLOT(ResolutionToggled()), this); -// sync_srv_client_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); -// callback_group_executor_thread = std::thread([&]() { -// callback_group_executor->add_callback_group(sync_srv_client_callback_group, this->get_node_base_interface()); -// callback_group_executor->spin(); -// }); - - client_ = this->create_client<::cartographer_ros_msgs::srv::SubmapQuery>(kDefaultSubmapQueryServiceName,rmw_qos_profile_services_default);//update_nh_.serviceClient<::cartographer_ros_msgs::msg::SubmapQuery>(""); + callback_group_ = this->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + callback_group_executor_ = std::make_shared(); + callback_group_executor_->add_callback_group(callback_group_, this->get_node_base_interface()); + client_ = this->create_client<::cartographer_ros_msgs::srv::SubmapQuery>( + kDefaultSubmapQueryServiceName, + rmw_qos_profile_services_default, + callback_group_ + ); trajectories_category_ = new ::rviz_common::properties::Property( "Submaps", QVariant(), "List of all submaps, organized by trajectories.", this); @@ -110,8 +113,11 @@ SubmapsDisplay::~SubmapsDisplay() { void SubmapsDisplay::Reset() { reset(); } void SubmapsDisplay::CreateClient() { - client_ = this->create_client<::cartographer_ros_msgs::srv::SubmapQuery>(//update_nh_.serviceClient<::cartographer_ros_msgs::srv::SubmapQuery>( - submap_query_service_property_->getStdString()); + client_ = this->create_client<::cartographer_ros_msgs::srv::SubmapQuery>( + submap_query_service_property_->getStdString(), + rmw_qos_profile_services_default, + callback_group_ + ); } void SubmapsDisplay::onInitialize() { @@ -229,7 +235,7 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) { it != trajectory_by_id.second->submaps.rend() && num_ongoing_requests < kMaxOnGoingRequestsPerTrajectory; ++it) { - if (it->second->MaybeFetchTexture(client_)) { + if (it->second->MaybeFetchTexture(client_, callback_group_executor_)) { ++num_ongoing_requests; } } @@ -238,10 +244,11 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) { return; } // Update the fading by z distance. + rclcpp::Time now = this->get_clock()->now(); try { const ::geometry_msgs::msg::TransformStamped transform_stamped = tf_buffer_->lookupTransform( - *map_frame_, tracking_frame_property_->getStdString(), tf2::TimePointZero); + *map_frame_, tracking_frame_property_->getStdString(), now); for (auto& trajectory_by_id : trajectories_) { for (auto& submap_entry : trajectory_by_id.second->submaps) { submap_entry.second->SetAlpha( @@ -255,7 +262,7 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) { // Update the map frame to fixed frame transform. Ogre::Vector3 position; Ogre::Quaternion orientation; - if (context_->getFrameManager()->getTransform(*map_frame_, this->now(), position, + if (context_->getFrameManager()->getTransform(*map_frame_, now, position, orientation)) { map_node_->setPosition(position); map_node_->setOrientation(orientation); diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.h b/cartographer_rviz/cartographer_rviz/submaps_display.h index 18ab779b6..6200d11bd 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.h +++ b/cartographer_rviz/cartographer_rviz/submaps_display.h @@ -90,6 +90,8 @@ class SubmapsDisplay std::unique_ptr tf_buffer_; std::shared_ptr tf_listener_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor_; rclcpp::Client::SharedPtr client_; ::rviz_common::properties::StringProperty* submap_query_service_property_; std::unique_ptr map_frame_; From 14ea58210037e57010f5b38be96bba302b5856b3 Mon Sep 17 00:00:00 2001 From: Guillaume dev PC Date: Wed, 27 Oct 2021 19:19:14 +0200 Subject: [PATCH 71/94] working submps Signed-off-by: Guillaume Doisy --- cartographer_rviz/CMakeLists.txt | 8 ++++---- cartographer_rviz/cartographer_rviz/ogre_slice.cc | 13 ++++++------- 2 files changed, 10 insertions(+), 11 deletions(-) diff --git a/cartographer_rviz/CMakeLists.txt b/cartographer_rviz/CMakeLists.txt index 867821e75..73ae157ad 100644 --- a/cartographer_rviz/CMakeLists.txt +++ b/cartographer_rviz/CMakeLists.txt @@ -75,8 +75,7 @@ target_compile_definitions(${PROJECT_NAME} PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDIN target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") pluginlib_export_plugin_description_file(rviz_common rviz_plugin_description.xml) -register_rviz_ogre_media_exports(DIRECTORIES "ogre_media/materials/scripts") -register_rviz_ogre_media_exports(DIRECTORIES "ogre_media/materials/glsl120") +register_rviz_ogre_media_exports(DIRECTORIES "ogre_media/materials/glsl120" "ogre_media/materials/scripts") ament_target_dependencies(${PROJECT_NAME} PUBLIC # absl @@ -197,8 +196,9 @@ install(FILES rviz_plugin_description.xml DESTINATION share/${PROJECT_NAME}) -install(DIRECTORY ogre_media - DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/ogre_media" + DESTINATION "share/${PROJECT_NAME}" + ) install( TARGETS ${PROJECT_NAME} diff --git a/cartographer_rviz/cartographer_rviz/ogre_slice.cc b/cartographer_rviz/cartographer_rviz/ogre_slice.cc index 8576b87b3..da95c955a 100644 --- a/cartographer_rviz/cartographer_rviz/ogre_slice.cc +++ b/cartographer_rviz/cartographer_rviz/ogre_slice.cc @@ -31,10 +31,10 @@ namespace cartographer_rviz { namespace { -std::string kManualObjectPrefix = "ManualObjectSubmap"; -std::string kSubmapSourceMaterialName = "cartographer_ros/Submap"; -std::string kSubmapMaterialPrefix = "SubmapMaterial"; -std::string kSubmapTexturePrefix = "SubmapTexture"; +constexpr char kManualObjectPrefix[] = "ManualObjectSubmap"; +constexpr char kSubmapSourceMaterialName[] = "cartographer_ros/Submap"; +constexpr char kSubmapMaterialPrefix[] = "SubmapMaterial"; +constexpr char kSubmapTexturePrefix[] = "SubmapTexture"; std::string GetSliceIdentifier( const ::cartographer::mapping::SubmapId& submap_id, const int slice_id) { @@ -62,10 +62,9 @@ OgreSlice::OgreSlice(const ::cartographer::mapping::SubmapId& id, int slice_id, slice_node_(submap_node_->createChildSceneNode()), manual_object_(scene_manager_->createManualObject( kManualObjectPrefix + GetSliceIdentifier(id, slice_id))) { - material_ = Ogre::MaterialManager::getSingleton().getByName( - kSubmapSourceMaterialName); + material_ = Ogre::MaterialManager::getSingleton().getByName(kSubmapSourceMaterialName); material_ = material_->clone( - kSubmapMaterialPrefix + GetSliceIdentifier(id_, slice_id_)); + kSubmapMaterialPrefix + GetSliceIdentifier(id_, slice_id_), true, "General"); material_->setReceiveShadows(false); material_->getTechnique(0)->setLightingEnabled(false); material_->setCullingMode(Ogre::CULL_NONE); From 991e6fbc21211a49654deb140a877d1eabcd86ea Mon Sep 17 00:00:00 2001 From: Guillaume dev PC Date: Wed, 27 Oct 2021 19:36:23 +0200 Subject: [PATCH 72/94] clean xml Signed-off-by: Guillaume Doisy --- .../rviz_plugin_description.xml | 26 ------------------- 1 file changed, 26 deletions(-) delete mode 100644 cartographer_rviz/cartographer_rviz/rviz_plugin_description.xml diff --git a/cartographer_rviz/cartographer_rviz/rviz_plugin_description.xml b/cartographer_rviz/cartographer_rviz/rviz_plugin_description.xml deleted file mode 100644 index fc7cb9e90..000000000 --- a/cartographer_rviz/cartographer_rviz/rviz_plugin_description.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - - - Displays submaps as a unified map in RViz. - https://github.com/cartographer-project/cartographer_ros - - - From a8c2f8517342205c8fbb182c46f38f0544d56318 Mon Sep 17 00:00:00 2001 From: Guillaume dev PC Date: Thu, 28 Oct 2021 11:18:11 +0200 Subject: [PATCH 73/94] carto_ros as proper lib Signed-off-by: Guillaume Doisy --- cartographer_ros/CMakeLists.txt | 59 ++- cartographer_rviz/CMakeLists.txt | 30 +- .../cartographer_rviz/drawable_submap.cc | 2 +- .../cartographer_rviz/drawable_submap.h | 2 +- .../cartographer_rviz/msg_conversion.cc | 425 ------------------ .../cartographer_rviz/msg_conversion.h | 95 ---- cartographer_rviz/cartographer_rviz/submap.cc | 65 --- cartographer_rviz/cartographer_rviz/submap.h | 43 -- .../cartographer_rviz/time_conversion.cc | 44 -- .../cartographer_rviz/time_conversion.h | 32 -- cartographer_rviz/package.xml | 1 + 11 files changed, 66 insertions(+), 732 deletions(-) delete mode 100644 cartographer_rviz/cartographer_rviz/msg_conversion.cc delete mode 100644 cartographer_rviz/cartographer_rviz/msg_conversion.h delete mode 100644 cartographer_rviz/cartographer_rviz/submap.cc delete mode 100644 cartographer_rviz/cartographer_rviz/submap.h delete mode 100644 cartographer_rviz/cartographer_rviz/time_conversion.cc delete mode 100644 cartographer_rviz/cartographer_rviz/time_conversion.h diff --git a/cartographer_ros/CMakeLists.txt b/cartographer_ros/CMakeLists.txt index 6dbe2d52a..b237524c8 100644 --- a/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/CMakeLists.txt @@ -71,6 +71,7 @@ include_directories( ) file(GLOB_RECURSE ALL_SRCS "cartographer_ros/*.cc" "cartographer_ros/*.h") +file(GLOB_RECURSE ALL_HEADERS "cartographer_ros/*.h") file(GLOB_RECURSE ALL_TESTS "cartographer_ros/*_test.cc") file(GLOB_RECURSE ALL_EXECUTABLES "cartographer_ros/*_main.cc") file(GLOB_RECURSE ALL_GRPC_FILES "cartographer_ros/cartographer_grpc/*") @@ -106,6 +107,26 @@ ament_target_dependencies(${PROJECT_NAME} PUBLIC ) add_subdirectory("cartographer_ros") target_link_libraries(${PROJECT_NAME} PUBLIC cartographer) +ament_export_targets(${PROJECT_NAME}_targets HAS_LIBRARY_TARGET) +ament_export_dependencies( + cartographer + cartographer_ros_msgs + geometry_msgs + nav_msgs + pcl_conversions + rclcpp + sensor_msgs + std_msgs + tf2 + tf2_eigen + tf2_msgs + tf2_ros + visualization_msgs + rosbag2_compression + rosbag2_cpp + rosbag2_storage + urdf + urdfdom_headers) # Lua target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${LUA_INCLUDE_DIR}) @@ -176,11 +197,43 @@ install(DIRECTORY configuration_files DESTINATION share/${PROJECT_NAME}/ ) -install(TARGETS - ${PROJECT_NAME} +install( + FILES ${ALL_HEADERS} + DESTINATION include/${PROJECT_NAME}/include +) + +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}_targets ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) + RUNTIME DESTINATION bin + INCLUDES DESTINATION include) + +ament_export_targets(${PROJECT_NAME}_targets HAS_LIBRARY_TARGET) +ament_export_dependencies( + cartographer + cartographer_ros_msgs + geometry_msgs + nav_msgs + pcl_conversions + rclcpp + sensor_msgs + std_msgs + tf2 + tf2_eigen + tf2_msgs + tf2_ros + visualization_msgs + rosbag2_compression + rosbag2_cpp + rosbag2_storage + urdf + urdfdom_headers) +ament_export_libraries(${PROJECT_NAME}) +ament_export_include_directories(include) + +# Lua # ros1 install diff --git a/cartographer_rviz/CMakeLists.txt b/cartographer_rviz/CMakeLists.txt index 73ae157ad..9092c23e1 100644 --- a/cartographer_rviz/CMakeLists.txt +++ b/cartographer_rviz/CMakeLists.txt @@ -38,7 +38,7 @@ add_compile_options(-g) #find_package(absl REQUIRED) find_package(Eigen3 REQUIRED) -#find_package(cartographer_ros REQUIRED) +find_package(cartographer_ros REQUIRED) find_package(cartographer_ros_msgs REQUIRED) find_package(pcl_conversions REQUIRED) find_package(rclcpp REQUIRED) @@ -65,8 +65,10 @@ include_directories( file(GLOB_RECURSE ALL_SRCS "cartographer_rviz/*.cc" "cartographer_rviz/*.h") set(CMAKE_AUTOMOC ON) + add_library(${PROJECT_NAME} SHARED ${ALL_SRCS}) + # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(${PROJECT_NAME} PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDING_LIBRARY") @@ -81,7 +83,7 @@ ament_target_dependencies(${PROJECT_NAME} PUBLIC # absl rclcpp cartographer -# cartographer_ros + cartographer_ros cartographer_ros_msgs rviz2 # rviz_ogre_vendor @@ -95,31 +97,13 @@ ament_target_dependencies(${PROJECT_NAME} PUBLIC # rviz ) -target_link_libraries(${PROJECT_NAME} PUBLIC ${PROTOBUF_LIBRARY} +target_link_libraries(${PROJECT_NAME} PUBLIC ${PROTOBUF_LIBRARY} + ${cartographer_ros_LIBRARIES} -lstdc++fs -ldl -lboost_thread - -# absl::algorithm -# absl::base -# absl::debugging -# absl::container -# absl::flat_hash_map -# absl::memory -# absl::meta -# absl::numeric -# absl::str_format -# absl::strings -# absl::synchronization -# absl::time -# absl::utility -) - -# exiv2lib -# nlohmann_json::nlohmann_json -# ${gphoto2_LIBRARIES} -# ${Boost_SYSTEM_LIBRARY}) + ) ament_export_include_directories(include) ament_export_dependencies(rosidl_default_runtime) diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index 7e16d3f6f..4045a795c 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -25,7 +25,7 @@ #include "Eigen/Geometry" #include "absl/memory/memory.h" #include "cartographer/common/port.h" -#include "cartographer_rviz/msg_conversion.h" +#include "cartographer_ros/include/msg_conversion.h" #include "cartographer_ros_msgs/srv/submap_query.hpp" #include diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.h b/cartographer_rviz/cartographer_rviz/drawable_submap.h index ecc239757..9471aa821 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.h +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.h @@ -28,7 +28,7 @@ #include "cartographer/io/submap_painter.h" #include "cartographer/mapping/id.h" #include "cartographer/transform/rigid_transform.h" -#include "cartographer_rviz/submap.h" +#include "cartographer_ros/include/submap.h" #include "cartographer_ros_msgs/msg/submap_entry.hpp" #include "cartographer_ros_msgs/srv/submap_query.hpp" #include "cartographer_rviz/ogre_slice.h" diff --git a/cartographer_rviz/cartographer_rviz/msg_conversion.cc b/cartographer_rviz/cartographer_rviz/msg_conversion.cc deleted file mode 100644 index 50b81227d..000000000 --- a/cartographer_rviz/cartographer_rviz/msg_conversion.cc +++ /dev/null @@ -1,425 +0,0 @@ -/* - * Copyright 2016 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. - */ - -#include "cartographer_rviz/msg_conversion.h" - -#include - -#include "cartographer/common/math.h" -#include "cartographer/common/port.h" -#include "cartographer/common/time.h" -#include "cartographer/io/submap_painter.h" -#include "cartographer/transform/proto/transform.pb.h" -#include "cartographer/transform/transform.h" -#include "cartographer_rviz/time_conversion.h" -#include "geometry_msgs/msg/pose.hpp" -#include "geometry_msgs/msg/quaternion.hpp" -#include "geometry_msgs/msg/transform.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "geometry_msgs/msg/vector3.hpp" -#include "glog/logging.h" -#include "nav_msgs/msg/occupancy_grid.hpp" - -// WHY ARE THESE NOT COMPILING? THERE ARE BEING USED IN THE CARTO_ROS PKG WITHOUT A PROB -#include "pcl/point_cloud.h" -#include "pcl/point_types.h" -//#include "rviz_rendering/objects/point_cloud.hpp" - -#include "pcl_conversions/pcl_conversions.h" -#include "builtin_interfaces/msg/time.hpp" -//#include "ros/serialization.h" -#include "sensor_msgs/msg/imu.hpp" -#include "sensor_msgs/msg/laser_scan.hpp" -#include "sensor_msgs/msg/multi_echo_laser_scan.hpp" -#include "sensor_msgs/msg/point_cloud2.hpp" - -namespace { - -// Sizes of PCL point types have to be 4n floats for alignment, as described in -// http://pointclouds.org/documentation/tutorials/adding_custom_ptype.php -struct PointXYZT { - float x; - float y; - float z; - float time; -}; - -struct PointXYZIT { - PCL_ADD_POINT4D; - float intensity; - float time; - float unused_padding[2]; -}; - -} // namespace - -POINT_CLOUD_REGISTER_POINT_STRUCT( - PointXYZT, (float, x, x)(float, y, y)(float, z, z)(float, time, time)) - -POINT_CLOUD_REGISTER_POINT_STRUCT( - PointXYZIT, - (float, x, x)(float, y, y)(float, z, z)(float, intensity, - intensity)(float, time, time)) - -namespace cartographer_ros { -namespace { - -// The ros::sensor_msgs::msg::PointCloud2 binary data contains 4 floats for each -// point. The last one must be this value or RViz is not showing the point cloud -// properly. -constexpr float kPointCloudComponentFourMagic = 1.; - -using ::cartographer::sensor::LandmarkData; -using ::cartographer::sensor::LandmarkObservation; -using ::cartographer::sensor::PointCloudWithIntensities; -using ::cartographer::transform::Rigid3d; -using ::cartographer_ros_msgs::msg::LandmarkEntry; -using ::cartographer_ros_msgs::msg::LandmarkList; - -sensor_msgs::msg::PointCloud2 PreparePointCloud2Message(const int64_t timestamp, - const std::string& frame_id, - const int num_points) { - sensor_msgs::msg::PointCloud2 msg; - msg.header.stamp = ToRos(::cartographer::common::FromUniversal(timestamp)); - msg.header.frame_id = frame_id; - msg.height = 1; - msg.width = num_points; - msg.fields.resize(3); - msg.fields[0].name = "x"; - msg.fields[0].offset = 0; - msg.fields[0].datatype = sensor_msgs::msg::PointField::FLOAT32; - msg.fields[0].count = 1; - msg.fields[1].name = "y"; - msg.fields[1].offset = 4; - msg.fields[1].datatype = sensor_msgs::msg::PointField::FLOAT32; - msg.fields[1].count = 1; - msg.fields[2].name = "z"; - msg.fields[2].offset = 8; - msg.fields[2].datatype = sensor_msgs::msg::PointField::FLOAT32; - msg.fields[2].count = 1; - msg.is_bigendian = false; - msg.point_step = 16; - msg.row_step = 16 * msg.width; - msg.is_dense = true; - msg.data.resize(16 * num_points); - return msg; -} - -// For sensor_msgs::msg::LaserScan. -bool HasEcho(float) { return true; } - -float GetFirstEcho(float range) { return range; } - -// For sensor_msgs::msg::MultiEchoLaserScan. -bool HasEcho(const sensor_msgs::msg::LaserEcho& echo) { - return !echo.echoes.empty(); -} - -float GetFirstEcho(const sensor_msgs::msg::LaserEcho& echo) { - return echo.echoes[0]; -} - -// For sensor_msgs::msg::LaserScan and sensor_msgs::msg::MultiEchoLaserScan. -template -std::tuple -LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) { - CHECK_GE(msg.range_min, 0.f); - CHECK_GE(msg.range_max, msg.range_min); - if (msg.angle_increment > 0.f) { - CHECK_GT(msg.angle_max, msg.angle_min); - } else { - CHECK_GT(msg.angle_min, msg.angle_max); - } - PointCloudWithIntensities point_cloud; - float angle = msg.angle_min; - for (size_t i = 0; i < msg.ranges.size(); ++i) { - const auto& echoes = msg.ranges[i]; - if (HasEcho(echoes)) { - const float first_echo = GetFirstEcho(echoes); - if (msg.range_min <= first_echo && first_echo <= msg.range_max) { - const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ()); - const cartographer::sensor::TimedRangefinderPoint point{ - rotation * (first_echo * Eigen::Vector3f::UnitX()), - i * msg.time_increment}; - point_cloud.points.push_back(point); - if (msg.intensities.size() > 0) { - CHECK_EQ(msg.intensities.size(), msg.ranges.size()); - const auto& echo_intensities = msg.intensities[i]; - CHECK(HasEcho(echo_intensities)); - point_cloud.intensities.push_back(GetFirstEcho(echo_intensities)); - } else { - point_cloud.intensities.push_back(0.f); - } - } - } - angle += msg.angle_increment; - } - ::cartographer::common::Time timestamp = FromRos(msg.header.stamp); - if (!point_cloud.points.empty()) { - const double duration = point_cloud.points.back().time; - timestamp += cartographer::common::FromSeconds(duration); - for (auto& point : point_cloud.points) { - point.time -= duration; - } - } - return std::make_tuple(point_cloud, timestamp); -} - -bool PointCloud2HasField(const sensor_msgs::msg::PointCloud2& pc2, - const std::string& field_name) { - for (const auto& field : pc2.fields) { - if (field.name == field_name) { - return true; - } - } - return false; -} - -} // namespace - -sensor_msgs::msg::PointCloud2 ToPointCloud2Message( - const int64_t timestamp, const std::string& frame_id, - const ::cartographer::sensor::TimedPointCloud& point_cloud) { - auto msg = PreparePointCloud2Message(timestamp, frame_id, point_cloud.size()); - size_t offset = 0; - float * const data = reinterpret_cast(&msg.data[0]); - for (const auto& point : point_cloud) { - data[offset++] = point.position.x(); - data[offset++] = point.position.y(); - data[offset++] = point.position.z(); - data[offset++] = kPointCloudComponentFourMagic; - } - return msg; -} - -std::tuple<::cartographer::sensor::PointCloudWithIntensities, - ::cartographer::common::Time> -ToPointCloudWithIntensities(const sensor_msgs::msg::LaserScan& msg) { - return LaserScanToPointCloudWithIntensities(msg); -} - -std::tuple<::cartographer::sensor::PointCloudWithIntensities, - ::cartographer::common::Time> -ToPointCloudWithIntensities(const sensor_msgs::msg::MultiEchoLaserScan& msg) { - return LaserScanToPointCloudWithIntensities(msg); -} - -std::tuple<::cartographer::sensor::PointCloudWithIntensities, - ::cartographer::common::Time> -ToPointCloudWithIntensities(const sensor_msgs::msg::PointCloud2& msg) { - PointCloudWithIntensities point_cloud; - // We check for intensity field here to avoid run-time warnings if we pass in - // a PointCloud2 without intensity. - if (PointCloud2HasField(msg, "intensity")) { - if (PointCloud2HasField(msg, "time")) { - pcl::PointCloud pcl_point_cloud; - pcl::fromROSMsg(msg, pcl_point_cloud); - point_cloud.points.reserve(pcl_point_cloud.size()); - point_cloud.intensities.reserve(pcl_point_cloud.size()); - for (const auto& point : pcl_point_cloud) { - point_cloud.points.push_back( - {Eigen::Vector3f{point.x, point.y, point.z}, point.time}); - point_cloud.intensities.push_back(point.intensity); - } - } else { - pcl::PointCloud pcl_point_cloud; - pcl::fromROSMsg(msg, pcl_point_cloud); - point_cloud.points.reserve(pcl_point_cloud.size()); - point_cloud.intensities.reserve(pcl_point_cloud.size()); - for (const auto& point : pcl_point_cloud) { - point_cloud.points.push_back( - {Eigen::Vector3f{point.x, point.y, point.z}, 0.f}); - point_cloud.intensities.push_back(point.intensity); - } - } - } else { - // If we don't have an intensity field, just copy XYZ and fill in 1.0f. - if (PointCloud2HasField(msg, "time")) { - pcl::PointCloud pcl_point_cloud; - pcl::fromROSMsg(msg, pcl_point_cloud); - point_cloud.points.reserve(pcl_point_cloud.size()); - point_cloud.intensities.reserve(pcl_point_cloud.size()); - for (const auto& point : pcl_point_cloud) { - point_cloud.points.push_back( - {Eigen::Vector3f{point.x, point.y, point.z}, point.time}); - point_cloud.intensities.push_back(1.0f); - } - } else { - pcl::PointCloud pcl_point_cloud; - pcl::fromROSMsg(msg, pcl_point_cloud); - point_cloud.points.reserve(pcl_point_cloud.size()); - point_cloud.intensities.reserve(pcl_point_cloud.size()); - for (const auto& point : pcl_point_cloud) { - point_cloud.points.push_back( - {Eigen::Vector3f{point.x, point.y, point.z}, 0.f}); - point_cloud.intensities.push_back(1.0f); - } - } - } - ::cartographer::common::Time timestamp = FromRos(msg.header.stamp); - if (!point_cloud.points.empty()) { - const double duration = point_cloud.points.back().time; - timestamp += cartographer::common::FromSeconds(duration); - for (auto& point : point_cloud.points) { - point.time -= duration; - CHECK_LE(point.time, 0.f) - << "Encountered a point with a larger stamp than " - "the last point in the cloud."; - } - } - return std::make_tuple(point_cloud, timestamp); -} - -LandmarkData ToLandmarkData(const LandmarkList& landmark_list) { - LandmarkData landmark_data; - landmark_data.time = FromRos(landmark_list.header.stamp); - for (const LandmarkEntry& entry : landmark_list.landmarks) { - landmark_data.landmark_observations.push_back( - {entry.id, ToRigid3d(entry.tracking_from_landmark_transform), - entry.translation_weight, entry.rotation_weight}); - } - return landmark_data; -} - -Rigid3d ToRigid3d(const geometry_msgs::msg::TransformStamped& transform) { - return Rigid3d(ToEigen(transform.transform.translation), - ToEigen(transform.transform.rotation)); -} - -Rigid3d ToRigid3d(const geometry_msgs::msg::Pose& pose) { - return Rigid3d({pose.position.x, pose.position.y, pose.position.z}, - ToEigen(pose.orientation)); -} - -Eigen::Vector3d ToEigen(const geometry_msgs::msg::Vector3& vector3) { - return Eigen::Vector3d(vector3.x, vector3.y, vector3.z); -} - -Eigen::Quaterniond ToEigen(const geometry_msgs::msg::Quaternion& quaternion) { - return Eigen::Quaterniond(quaternion.w, quaternion.x, quaternion.y, - quaternion.z); -} - -geometry_msgs::msg::Transform ToGeometryMsgTransform(const Rigid3d& rigid3d) { - geometry_msgs::msg::Transform transform; - transform.translation.x = rigid3d.translation().x(); - transform.translation.y = rigid3d.translation().y(); - transform.translation.z = rigid3d.translation().z(); - transform.rotation.w = rigid3d.rotation().w(); - transform.rotation.x = rigid3d.rotation().x(); - transform.rotation.y = rigid3d.rotation().y(); - transform.rotation.z = rigid3d.rotation().z(); - return transform; -} - -geometry_msgs::msg::Pose ToGeometryMsgPose(const Rigid3d& rigid3d) { - geometry_msgs::msg::Pose pose; - pose.position = ToGeometryMsgPoint(rigid3d.translation()); - pose.orientation.w = rigid3d.rotation().w(); - pose.orientation.x = rigid3d.rotation().x(); - pose.orientation.y = rigid3d.rotation().y(); - pose.orientation.z = rigid3d.rotation().z(); - return pose; -} - -geometry_msgs::msg::Point ToGeometryMsgPoint(const Eigen::Vector3d& vector3d) { - geometry_msgs::msg::Point point; - point.x = vector3d.x(); - point.y = vector3d.y(); - point.z = vector3d.z(); - return point; -} - -Eigen::Vector3d LatLongAltToEcef(const double latitude, const double longitude, - const double altitude) { - // https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_geodetic_to_ECEF_coordinates - constexpr double a = 6378137.; // semi-major axis, equator to center. - constexpr double f = 1. / 298.257223563; - constexpr double b = a * (1. - f); // semi-minor axis, pole to center. - constexpr double a_squared = a * a; - constexpr double b_squared = b * b; - constexpr double e_squared = (a_squared - b_squared) / a_squared; - const double sin_phi = std::sin(cartographer::common::DegToRad(latitude)); - const double cos_phi = std::cos(cartographer::common::DegToRad(latitude)); - const double sin_lambda = std::sin(cartographer::common::DegToRad(longitude)); - const double cos_lambda = std::cos(cartographer::common::DegToRad(longitude)); - const double N = a / std::sqrt(1 - e_squared * sin_phi * sin_phi); - const double x = (N + altitude) * cos_phi * cos_lambda; - const double y = (N + altitude) * cos_phi * sin_lambda; - const double z = (b_squared / a_squared * N + altitude) * sin_phi; - - return Eigen::Vector3d(x, y, z); -} - -cartographer::transform::Rigid3d ComputeLocalFrameFromLatLong( - const double latitude, const double longitude) { - const Eigen::Vector3d translation = LatLongAltToEcef(latitude, longitude, 0.); - const Eigen::Quaterniond rotation = - Eigen::AngleAxisd(cartographer::common::DegToRad(latitude - 90.), - Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(cartographer::common::DegToRad(-longitude), - Eigen::Vector3d::UnitZ()); - return cartographer::transform::Rigid3d(rotation * -translation, rotation); -} - -std::unique_ptr CreateOccupancyGridMsg( - const cartographer::io::PaintSubmapSlicesResult& painted_slices, - const double resolution, const std::string& frame_id, - const rclcpp::Time& time) { - auto occupancy_grid = absl::make_unique(); - - const int width = cairo_image_surface_get_width(painted_slices.surface.get()); - const int height = - cairo_image_surface_get_height(painted_slices.surface.get()); - - occupancy_grid->header.stamp = time; - occupancy_grid->header.frame_id = frame_id; - occupancy_grid->info.map_load_time = time; - occupancy_grid->info.resolution = resolution; - occupancy_grid->info.width = width; - occupancy_grid->info.height = height; - occupancy_grid->info.origin.position.x = - -painted_slices.origin.x() * resolution; - occupancy_grid->info.origin.position.y = - (-height + painted_slices.origin.y()) * resolution; - occupancy_grid->info.origin.position.z = 0.; - occupancy_grid->info.origin.orientation.w = 1.; - occupancy_grid->info.origin.orientation.x = 0.; - occupancy_grid->info.origin.orientation.y = 0.; - occupancy_grid->info.origin.orientation.z = 0.; - - const uint32_t* pixel_data = reinterpret_cast( - cairo_image_surface_get_data(painted_slices.surface.get())); - occupancy_grid->data.reserve(width * height); - for (int y = height - 1; y >= 0; --y) { - for (int x = 0; x < width; ++x) { - const uint32_t packed = pixel_data[y * width + x]; - const unsigned char color = packed >> 16; - const unsigned char observed = packed >> 8; - const int value = - observed == 0 - ? -1 - : ::cartographer::common::RoundToInt((1. - color / 255.) * 100.); - CHECK_LE(-1, value); - CHECK_GE(100, value); - occupancy_grid->data.push_back(value); - } - } - - return occupancy_grid; -} - -} // namespace cartographer_ros diff --git a/cartographer_rviz/cartographer_rviz/msg_conversion.h b/cartographer_rviz/cartographer_rviz/msg_conversion.h deleted file mode 100644 index e895144b2..000000000 --- a/cartographer_rviz/cartographer_rviz/msg_conversion.h +++ /dev/null @@ -1,95 +0,0 @@ -/* - * Copyright 2016 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. - */ - -#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MSG_CONVERSION_H -#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MSG_CONVERSION_H - -#include "cartographer/common/time.h" -#include "cartographer/io/submap_painter.h" -#include "cartographer/sensor/landmark_data.h" -#include "cartographer/sensor/point_cloud.h" -#include "cartographer/transform/rigid_transform.h" -#include "cartographer_ros_msgs/msg/landmark_list.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "geometry_msgs/msg/point.hpp" -#include "geometry_msgs/msg/transform.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "nav_msgs/msg/occupancy_grid.hpp" -#include "sensor_msgs/msg/laser_scan.hpp" -#include "sensor_msgs/msg/multi_echo_laser_scan.hpp" -#include "sensor_msgs/msg/point_cloud2.hpp" -#include - -namespace cartographer_ros { - -sensor_msgs::msg::PointCloud2 ToPointCloud2Message( - int64_t timestamp, const std::string& frame_id, - const ::cartographer::sensor::TimedPointCloud& point_cloud); - -geometry_msgs::msg::Transform ToGeometryMsgTransform( - const ::cartographer::transform::Rigid3d& rigid3d); - -geometry_msgs::msg::Pose ToGeometryMsgPose( - const ::cartographer::transform::Rigid3d& rigid3d); - -geometry_msgs::msg::Point ToGeometryMsgPoint(const Eigen::Vector3d& vector3d); - -// Converts ROS message to point cloud. Returns the time when the last point -// was acquired (different from the ROS timestamp). Timing of points is given in -// the fourth component of each point relative to `Time`. -std::tuple<::cartographer::sensor::PointCloudWithIntensities, - ::cartographer::common::Time> -ToPointCloudWithIntensities(const sensor_msgs::msg::LaserScan& msg); - -std::tuple<::cartographer::sensor::PointCloudWithIntensities, - ::cartographer::common::Time> -ToPointCloudWithIntensities(const sensor_msgs::msg::MultiEchoLaserScan& msg); - -std::tuple<::cartographer::sensor::PointCloudWithIntensities, - ::cartographer::common::Time> -ToPointCloudWithIntensities(const sensor_msgs::msg::PointCloud2& msg); - -::cartographer::sensor::LandmarkData ToLandmarkData( - const cartographer_ros_msgs::msg::LandmarkList& landmark_list); - -::cartographer::transform::Rigid3d ToRigid3d( - const geometry_msgs::msg::TransformStamped& transform); - -::cartographer::transform::Rigid3d ToRigid3d(const geometry_msgs::msg::Pose& pose); - -Eigen::Vector3d ToEigen(const geometry_msgs::msg::Vector3& vector3); - -Eigen::Quaterniond ToEigen(const geometry_msgs::msg::Quaternion& quaternion); - -// Converts from WGS84 (latitude, longitude, altitude) to ECEF. -Eigen::Vector3d LatLongAltToEcef(double latitude, double longitude, - double altitude); - -// Returns a transform that takes ECEF coordinates from nearby points to a local -// frame that has z pointing upwards. -cartographer::transform::Rigid3d ComputeLocalFrameFromLatLong(double latitude, - double longitude); - -// Points to an occupancy grid message at a specific resolution from painted -// submap slices obtained via ::cartographer::io::PaintSubmapSlices(...). -std::unique_ptr CreateOccupancyGridMsg( - const cartographer::io::PaintSubmapSlicesResult& painted_slices, - const double resolution, const std::string& frame_id, - const rclcpp::Time& time); - -} // namespace cartographer_ros - -#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MSG_CONVERSION_H diff --git a/cartographer_rviz/cartographer_rviz/submap.cc b/cartographer_rviz/cartographer_rviz/submap.cc deleted file mode 100644 index 467e3ebc8..000000000 --- a/cartographer_rviz/cartographer_rviz/submap.cc +++ /dev/null @@ -1,65 +0,0 @@ -/* - * Copyright 2016 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. - */ - -#include "cartographer_rviz/submap.h" - -#include "absl/memory/memory.h" -#include "cartographer/common/port.h" -#include "cartographer/transform/transform.h" -#include "cartographer_rviz/msg_conversion.h" -#include "cartographer_ros_msgs/msg/status_code.hpp" -#include "cartographer_ros_msgs/srv/submap_query.hpp" - -namespace cartographer_ros { - -std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures( - const ::cartographer::mapping::SubmapId& submap_id, - rclcpp::Client::SharedPtr client, - rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor, - const std::chrono::milliseconds timeout) -{ - auto request = std::make_shared(); - request->trajectory_id = submap_id.trajectory_id; - request->submap_index = submap_id.submap_index; - auto future_result = client->async_send_request(request); - - if (callback_group_executor->spin_until_future_complete(future_result, timeout) != - rclcpp::FutureReturnCode::SUCCESS) - { - return nullptr; - } - auto result = future_result.get(); - - if (result->status.code != ::cartographer_ros_msgs::msg::StatusCode::OK || - result->textures.empty()) { - return nullptr; - } - - auto response = absl::make_unique<::cartographer::io::SubmapTextures>(); - response->version = result->submap_version; - for (const auto& texture : result->textures) { - const std::string compressed_cells(texture.cells.begin(), - texture.cells.end()); - response->textures.emplace_back(::cartographer::io::SubmapTexture{ - ::cartographer::io::UnpackTextureData(compressed_cells, texture.width, - texture.height), - texture.width, texture.height, texture.resolution, - ToRigid3d(texture.slice_pose)}); - } - return response; -} - -} // namespace cartographer_ros diff --git a/cartographer_rviz/cartographer_rviz/submap.h b/cartographer_rviz/cartographer_rviz/submap.h deleted file mode 100644 index 48c3a425b..000000000 --- a/cartographer_rviz/cartographer_rviz/submap.h +++ /dev/null @@ -1,43 +0,0 @@ -/* - * Copyright 2016 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. - */ - -#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SUBMAP_H -#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SUBMAP_H - -#include -#include -#include - -#include "cartographer/io/image.h" -#include "cartographer/io/submap_painter.h" -#include "cartographer/mapping/id.h" -#include "cartographer/transform/rigid_transform.h" -#include "cartographer_ros_msgs/srv/submap_query.hpp" -#include - -namespace cartographer_ros { - -// Fetch 'submap_id' using the 'client' and returning the response or 'nullptr' -// on error. -std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures( - const ::cartographer::mapping::SubmapId& submap_id, - rclcpp::Client::SharedPtr client, - rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor, - const std::chrono::milliseconds timeout); - -} // namespace cartographer_ros - -#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SUBMAP_H diff --git a/cartographer_rviz/cartographer_rviz/time_conversion.cc b/cartographer_rviz/cartographer_rviz/time_conversion.cc deleted file mode 100644 index 37afff196..000000000 --- a/cartographer_rviz/cartographer_rviz/time_conversion.cc +++ /dev/null @@ -1,44 +0,0 @@ -/* - * Copyright 2016 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. - */ - -#include "cartographer_rviz/time_conversion.h" - -#include "cartographer/common/time.h" -#include - -namespace cartographer_ros { - -rclcpp::Time ToRos(::cartographer::common::Time time) { - int64_t uts_timestamp = ::cartographer::common::ToUniversal(time); - int64_t ns_since_unix_epoch = - (uts_timestamp - - ::cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds * - 10000000ll) * - 100ll; - rclcpp::Time ros_time(ns_since_unix_epoch, rcl_clock_type_t::RCL_ROS_TIME); - return ros_time; -} - -// TODO(pedrofernandez): Write test. -::cartographer::common::Time FromRos(const rclcpp::Time& time) { - // The epoch of the ICU Universal Time Scale is "0001-01-01 00:00:00.0 +0000", - // exactly 719162 days before the Unix epoch. - return ::cartographer::common::FromUniversal( - cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds * 10000000ll + - (time.nanoseconds() + 50) / 100); // + 50 to get the rounding correct. -} - -} // namespace cartographer_ros diff --git a/cartographer_rviz/cartographer_rviz/time_conversion.h b/cartographer_rviz/cartographer_rviz/time_conversion.h deleted file mode 100644 index 5555f4d49..000000000 --- a/cartographer_rviz/cartographer_rviz/time_conversion.h +++ /dev/null @@ -1,32 +0,0 @@ -/* - * Copyright 2016 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. - */ - -#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TIME_CONVERSION_H -#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TIME_CONVERSION_H - -#include "cartographer/common/time.h" -#include -#include - -namespace cartographer_ros { - -rclcpp::Time ToRos(::cartographer::common::Time time); - -::cartographer::common::Time FromRos(const rclcpp::Time& time); - -} // namespace cartographer_ros - -#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TIME_CONVERSION_H diff --git a/cartographer_rviz/package.xml b/cartographer_rviz/package.xml index dcacca0f0..622f3d5ae 100644 --- a/cartographer_rviz/package.xml +++ b/cartographer_rviz/package.xml @@ -39,6 +39,7 @@ git cartographer + cartographer_ros cartographer_ros_msgs libqt5-core libqt5-gui From 3d6bcbcb6aacfb2a21ca41f619f9aadb53be2d21 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Thu, 28 Oct 2021 12:06:13 +0200 Subject: [PATCH 74/94] clean comments not useful Signed-off-by: Guillaume Doisy --- cartographer_ros/CMakeLists.txt | 61 ------------------- cartographer_ros/package.xml | 1 - cartographer_rviz/CMakeLists.txt | 44 ++----------- .../cartographer_rviz/drawable_submap.cc | 1 - .../cartographer_rviz/ogre_slice.cc | 2 - .../cartographer_rviz/submaps_display.cc | 3 - .../cartographer_rviz/submaps_display.h | 6 +- 7 files changed, 5 insertions(+), 113 deletions(-) diff --git a/cartographer_ros/CMakeLists.txt b/cartographer_ros/CMakeLists.txt index b237524c8..1df1fda2c 100644 --- a/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/CMakeLists.txt @@ -25,8 +25,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra) endif() -#add_compile_options(-g) - find_package(cartographer REQUIRED) include("${CARTOGRAPHER_CMAKE_DIR}/functions.cmake") set(BUILD_SHARED_LIBS OFF) @@ -49,7 +47,6 @@ find_package(visualization_msgs REQUIRED) find_package(rosbag2_compression REQUIRED) find_package(rosbag2_cpp REQUIRED) find_package(rosbag2_storage REQUIRED) -#find_package(backward_ros REQUIRED) find_package(absl REQUIRED) find_package(LuaGoogle REQUIRED) @@ -85,7 +82,6 @@ endif() add_library(${PROJECT_NAME} STATIC ${ALL_SRCS}) ament_target_dependencies(${PROJECT_NAME} PUBLIC - #backward_ros cartographer cartographer_ros_msgs geometry_msgs @@ -147,10 +143,6 @@ endforeach() target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC "${EIGEN3_INCLUDE_DIR}") -## YAML in ros2 1.0 -#target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${YAMLCPP_INCLUDE_DIRS}) -#target_link_libraries(${PROJECT_NAME} PUBLIC ${YAMLCPP_LIBRARIES}) - # Boost not in ros2 target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC "${Boost_INCLUDE_DIRS}") @@ -171,28 +163,6 @@ set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${GOOG_CXX_FLAGS}") set_target_properties(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS ${TARGET_COMPILE_FLAGS}) -#if (CATKIN_ENABLE_TESTING) -# foreach(TEST_SOURCE_FILENAME ${ALL_TESTS}) -# get_filename_component(TEST_NAME ${TEST_SOURCE_FILENAME} NAME_WE) -# catkin_add_gtest(${TEST_NAME} ${TEST_SOURCE_FILENAME}) -# # catkin_add_gtest uses a plain (i.e. no PUBLIC/PRIVATE/INTERFACE) call to -# # target_link_libraries. That forces us to do the same. -# target_link_libraries(${TEST_NAME} ${GMOCK_LIBRARIES} ${GTEST_MAIN_LIBRARIES}) -# target_include_directories(${TEST_NAME} SYSTEM PUBLIC ${LUA_INCLUDE_DIR}) -# target_link_libraries(${TEST_NAME} ${LUA_LIBRARIES}) -# target_include_directories(${TEST_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS}) -# target_link_libraries(${TEST_NAME} ${catkin_LIBRARIES}) -# add_dependencies(${TEST_NAME} ${catkin_EXPORTED_TARGETS}) -# target_link_libraries(${TEST_NAME} cartographer) -# target_link_libraries(${TEST_NAME} ${PROJECT_NAME}) -# set_target_properties(${TEST_NAME} PROPERTIES COMPILE_FLAGS ${TARGET_COMPILE_FLAGS}) -# # Needed for dynamically linked GTest on Windows. -# if (WIN32) -# target_compile_definitions(${TEST_NAME} PUBLIC -DGTEST_LINKED_AS_SHARED_LIBRARY) -# endif() -# endforeach() -#endif() - install(DIRECTORY configuration_files DESTINATION share/${PROJECT_NAME}/ ) @@ -233,35 +203,4 @@ ament_export_dependencies( ament_export_libraries(${PROJECT_NAME}) ament_export_include_directories(include) -# Lua - - -# ros1 install -#install(DIRECTORY launch urdf configuration_files -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -#) - -#install(PROGRAMS scripts/tf_remove_frames.py -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -#) - -#install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -#) - -## Install source headers. -#file(GLOB_RECURSE HDRS "cartographer_ros/*.h") -#foreach(HDR ${HDRS}) -# file(RELATIVE_PATH REL_FIL ${PROJECT_SOURCE_DIR} ${HDR}) -# get_filename_component(INSTALL_DIR ${REL_FIL} DIRECTORY) -# install( -# FILES -# ${HDR} -# DESTINATION -# include/${INSTALL_DIR} -# ) -#endforeach() - ament_package() diff --git a/cartographer_ros/package.xml b/cartographer_ros/package.xml index 6bf19c2f0..1ce505e0c 100644 --- a/cartographer_ros/package.xml +++ b/cartographer_ros/package.xml @@ -41,7 +41,6 @@ protobuf-dev python3-sphinx - cartographer cartographer_ros_msgs geometry_msgs diff --git a/cartographer_rviz/CMakeLists.txt b/cartographer_rviz/CMakeLists.txt index 9092c23e1..d2676aba0 100644 --- a/cartographer_rviz/CMakeLists.txt +++ b/cartographer_rviz/CMakeLists.txt @@ -17,7 +17,6 @@ cmake_minimum_required(VERSION 3.5) project(cartographer_rviz) set(CMAKE_POSITION_INDEPENDENT_CODE TRUE) -#add_subdirectory(../../../../../../abseil-cpp/ build) find_package(ament_cmake REQUIRED) # Default to C++17 @@ -34,16 +33,12 @@ set(BUILD_SHARED_LIBS OFF) option(BUILD_GRPC "build features that require Cartographer gRPC support" false) google_initialize_cartographer_project() -add_compile_options(-g) - -#find_package(absl REQUIRED) find_package(Eigen3 REQUIRED) find_package(cartographer_ros REQUIRED) find_package(cartographer_ros_msgs REQUIRED) find_package(pcl_conversions REQUIRED) find_package(rclcpp REQUIRED) find_package(rviz2 REQUIRED) -#find_package(rviz_ogre_vendor REQUIRED) find_package(PCL REQUIRED COMPONENTS common) find_package(rviz_assimp_vendor REQUIRED) find_package(rviz_rendering REQUIRED) @@ -51,8 +46,6 @@ find_package(rviz_common REQUIRED) find_package(rviz_default_plugins REQUIRED) find_package(pluginlib REQUIRED) find_package(rcutils REQUIRED) -#find_package(rviz REQUIRED) -find_package(backward_ros REQUIRED) set(CMAKE_THREAD_PREFER_PTHREAD TRUE) set(THREADS_PREFER_PTHREAD_FLAG TRUE) @@ -80,21 +73,17 @@ pluginlib_export_plugin_description_file(rviz_common rviz_plugin_description.xml register_rviz_ogre_media_exports(DIRECTORIES "ogre_media/materials/glsl120" "ogre_media/materials/scripts") ament_target_dependencies(${PROJECT_NAME} PUBLIC -# absl rclcpp cartographer cartographer_ros cartographer_ros_msgs rviz2 -# rviz_ogre_vendor rviz_assimp_vendor rviz_rendering rviz_common rviz_default_plugins pcl_conversions -# libdw rcutils -# rviz ) @@ -120,20 +109,13 @@ foreach(DEFINITION ${PCL_DEFINITIONS}) endforeach() -#if(rviz2_QT_VERSION VERSION_LESS "5") -# message(STATUS "Using Qt4 based on the rviz2_QT_VERSION: ${rviz2_QT_VERSION}") - #find_package(Qt4 ${rviz2_QT_VERSION} EXACT REQUIRED QtCore QtGui) +message(STATUS "Using Qt5 based on the rviz2_QT_VERSION: ${rviz2_QT_VERSION}") +find_package(Qt5 ${rviz2_QT_VERSION} EXACT REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Core Qt5::Widgets) +include_directories(${Qt5Widgets_INCLUDE_DIRS}) -# include(${QT_USE_FILE}) -#else() - message(STATUS "Using Qt5 based on the rviz2_QT_VERSION: ${rviz2_QT_VERSION}") - find_package(Qt5 ${rviz2_QT_VERSION} EXACT REQUIRED Core Widgets) - set(QT_LIBRARIES Qt5::Core Qt5::Widgets) - include_directories(${Qt5Widgets_INCLUDE_DIRS}) -#endif() add_definitions(-DQT_NO_KEYWORDS) -#add_library(${PROJECT_NAME} ${ALL_SRCS}) target_link_libraries(${PROJECT_NAME} PUBLIC ${QT_LIBRARIES}) # Add the binary directory first, so that port.h is included after it has @@ -148,33 +130,15 @@ target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC "${EIGEN3_INCLUDE_DIR}") target_link_libraries(${PROJECT_NAME} PUBLIC ${EIGEN3_LIBRARIES}) -#target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC -# "${CARTOGRAPHER_ROS_INCLUDE_DIR}") -#target_link_libraries(${PROJECT_NAME} PUBLIC ${CARTOGRAPHER_ROS_LIBRARIES}) - target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC "${Boost_INCLUDE_DIRS}") target_link_libraries(${PROJECT_NAME} PUBLIC ${Boost_LIBRARIES}) -#target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC -# "${RVIZ_OGRE_VENDOR_INCLUDE_DIRS}") -#target_link_libraries(${PROJECT_NAME} PUBLIC ${RVIZ_OGRE_VENDOR_LIBRARIES}) - set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${GOOG_CXX_FLAGS}") set_target_properties(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS ${TARGET_COMPILE_FLAGS}) target_link_libraries(${PROJECT_NAME} PUBLIC cartographer) -#target_link_libraries(${PROJECT_NAME} PUBLIC rviz_ogre_vendor) -#target_link_libraries(${PROJECT_NAME} PUBLIC cartographer_ros) - - -#install(DIRECTORY configuration_files -# DESTINATION share/${PROJECT_NAME}/ -#) - - - install(FILES rviz_plugin_description.xml diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index 4045a795c..82d237172 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -137,7 +137,6 @@ bool DrawableSubmap::MaybeFetchTexture( rpc_request_future_ = std::async(std::launch::async, [this, client, callback_group_executor]() { std::unique_ptr<::cartographer::io::SubmapTextures> submap_textures = ::cartographer_ros::FetchSubmapTextures(id_, client, callback_group_executor, std::chrono::milliseconds(10000)); - //,callback_group_executor,std::chrono::milliseconds(10000)); // Missing callbackgroup and timeout but this class isn't a ros node absl::MutexLock locker(&mutex_); query_in_progress_ = false; if (submap_textures != nullptr) { diff --git a/cartographer_rviz/cartographer_rviz/ogre_slice.cc b/cartographer_rviz/cartographer_rviz/ogre_slice.cc index da95c955a..23bc4bad4 100644 --- a/cartographer_rviz/cartographer_rviz/ogre_slice.cc +++ b/cartographer_rviz/cartographer_rviz/ogre_slice.cc @@ -24,7 +24,6 @@ #include "OgreMaterialManager.h" #include "OgreTechnique.h" #include "OgreTextureManager.h" -//#include "absl/strings/str_cat.h" #include "cartographer/common/port.h" namespace cartographer_rviz { @@ -38,7 +37,6 @@ constexpr char kSubmapTexturePrefix[] = "SubmapTexture"; std::string GetSliceIdentifier( const ::cartographer::mapping::SubmapId& submap_id, const int slice_id) { - //return absl::StrCat(submap_id.trajectory_id, "-", submap_id.submap_index, "-",slice_id); return (std::to_string(submap_id.trajectory_id) + "-" + std::to_string(submap_id.submap_index) + "-" + std::to_string(slice_id)); } diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc index ed50014b3..07a3d2fff 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/cartographer_rviz/submaps_display.cc @@ -25,15 +25,12 @@ #include "cartographer_ros_msgs/srv/submap_query.hpp" #include #include -//#include "ros/package.h" #include #include #include #include #include #include - -// Include ament_index_cpp::get_package_share_directory #include namespace cartographer_rviz { diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.h b/cartographer_rviz/cartographer_rviz/submaps_display.h index 6200d11bd..2c85229a7 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.h +++ b/cartographer_rviz/cartographer_rviz/submaps_display.h @@ -60,7 +60,7 @@ struct Trajectory : public QObject { // every submap containing pre-multiplied alpha and grayscale values, these are // then alpha blended together. class SubmapsDisplay - : public ::rviz_common::MessageFilterDisplay<::cartographer_ros_msgs::msg::SubmapList>, rclcpp::Node { //::rviz_common::RosTopicDisplay<::cartographer_ros_msgs::msg::SubmapList>, rclcpp::Node { + : public ::rviz_common::MessageFilterDisplay<::cartographer_ros_msgs::msg::SubmapList>, rclcpp::Node { Q_OBJECT public: @@ -106,10 +106,6 @@ class SubmapsDisplay ::rviz_common::properties::BoolProperty* pose_markers_all_enabled_; ::rviz_common::properties::FloatProperty* fade_out_start_distance_in_meters_; -// rclcpp::CallbackGroup::SharedPtr sync_srv_client_callback_group; -// rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor; -// std::thread callback_group_executor_thread; - }; } // namespace cartographer_rviz From 36e7d16770934cd15c88471ab505c70303608ea9 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Thu, 28 Oct 2021 14:09:20 +0200 Subject: [PATCH 75/94] cleaned cmake rviz Signed-off-by: Guillaume Doisy --- cartographer_rviz/CMakeLists.txt | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/cartographer_rviz/CMakeLists.txt b/cartographer_rviz/CMakeLists.txt index d2676aba0..2eba48526 100644 --- a/cartographer_rviz/CMakeLists.txt +++ b/cartographer_rviz/CMakeLists.txt @@ -38,12 +38,8 @@ find_package(cartographer_ros REQUIRED) find_package(cartographer_ros_msgs REQUIRED) find_package(pcl_conversions REQUIRED) find_package(rclcpp REQUIRED) -find_package(rviz2 REQUIRED) find_package(PCL REQUIRED COMPONENTS common) -find_package(rviz_assimp_vendor REQUIRED) -find_package(rviz_rendering REQUIRED) find_package(rviz_common REQUIRED) -find_package(rviz_default_plugins REQUIRED) find_package(pluginlib REQUIRED) find_package(rcutils REQUIRED) @@ -58,10 +54,8 @@ include_directories( file(GLOB_RECURSE ALL_SRCS "cartographer_rviz/*.cc" "cartographer_rviz/*.h") set(CMAKE_AUTOMOC ON) - add_library(${PROJECT_NAME} SHARED ${ALL_SRCS}) - # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(${PROJECT_NAME} PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDING_LIBRARY") @@ -77,16 +71,11 @@ ament_target_dependencies(${PROJECT_NAME} PUBLIC cartographer cartographer_ros cartographer_ros_msgs - rviz2 - rviz_assimp_vendor - rviz_rendering rviz_common - rviz_default_plugins pcl_conversions rcutils ) - target_link_libraries(${PROJECT_NAME} PUBLIC ${PROTOBUF_LIBRARY} ${cartographer_ros_LIBRARIES} -lstdc++fs @@ -108,13 +97,11 @@ foreach(DEFINITION ${PCL_DEFINITIONS}) set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${DEFINITION}") endforeach() - message(STATUS "Using Qt5 based on the rviz2_QT_VERSION: ${rviz2_QT_VERSION}") find_package(Qt5 ${rviz2_QT_VERSION} EXACT REQUIRED Core Widgets) set(QT_LIBRARIES Qt5::Core Qt5::Widgets) include_directories(${Qt5Widgets_INCLUDE_DIRS}) - add_definitions(-DQT_NO_KEYWORDS) target_link_libraries(${PROJECT_NAME} PUBLIC ${QT_LIBRARIES}) From 4865f84e7b6eb676f913fff84b3be8d28e476bae Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Thu, 18 Nov 2021 12:29:21 +0100 Subject: [PATCH 76/94] sync Signed-off-by: Guillaume Doisy --- cartographer_rviz/CMakeLists.txt | 2 ++ cartographer_rviz/package.xml | 5 +++++ 2 files changed, 7 insertions(+) diff --git a/cartographer_rviz/CMakeLists.txt b/cartographer_rviz/CMakeLists.txt index 2eba48526..60f315d65 100644 --- a/cartographer_rviz/CMakeLists.txt +++ b/cartographer_rviz/CMakeLists.txt @@ -38,6 +38,7 @@ find_package(cartographer_ros REQUIRED) find_package(cartographer_ros_msgs REQUIRED) find_package(pcl_conversions REQUIRED) find_package(rclcpp REQUIRED) +find_package(rviz2 REQUIRED) find_package(PCL REQUIRED COMPONENTS common) find_package(rviz_common REQUIRED) find_package(pluginlib REQUIRED) @@ -71,6 +72,7 @@ ament_target_dependencies(${PROJECT_NAME} PUBLIC cartographer cartographer_ros cartographer_ros_msgs + rviz2 rviz_common pcl_conversions rcutils diff --git a/cartographer_rviz/package.xml b/cartographer_rviz/package.xml index 622f3d5ae..5527fbc05 100644 --- a/cartographer_rviz/package.xml +++ b/cartographer_rviz/package.xml @@ -48,7 +48,12 @@ rosidl_default_generators qtbase5-dev rclcpp + absl + libpcl-all-dev + pcl_conversions + rcutils rviz2 + rviz_common ament_cmake From 71b1b696e1c4c4937aae11ac71337e5fe7cfd0b8 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Thu, 18 Nov 2021 16:57:35 +0100 Subject: [PATCH 77/94] RCL_ROS_TIME Signed-off-by: Guillaume Doisy --- cartographer_ros/cartographer_ros/assets_writer.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cartographer_ros/cartographer_ros/assets_writer.cc b/cartographer_ros/cartographer_ros/assets_writer.cc index 55e9a66fa..620b8de9a 100644 --- a/cartographer_ros/cartographer_ros/assets_writer.cc +++ b/cartographer_ros/cartographer_ros/assets_writer.cc @@ -178,7 +178,7 @@ void AssetsWriter::Run(const std::string& configuration_directory, const std::string tracking_frame = lua_parameter_dictionary->GetString("tracking_frame"); - rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_ROS_TIME); do { for (size_t trajectory_id = 0; trajectory_id < bag_filenames_.size(); ++trajectory_id) { From 7eab5909c8b7943962c32f6ecdd8213ad5114e00 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Mon, 22 Nov 2021 13:32:39 +0100 Subject: [PATCH 78/94] de-boostify Signed-off-by: Guillaume Doisy --- cartographer_ros/CMakeLists.txt | 7 ----- .../cartographer_ros/assets_writer_main.cc | 11 ++++++-- .../cartographer_ros/offline_node.cc | 28 ++++++++++++++----- .../cartographer_ros/urdf_reader.cc | 4 --- 4 files changed, 29 insertions(+), 21 deletions(-) diff --git a/cartographer_ros/CMakeLists.txt b/cartographer_ros/CMakeLists.txt index 1df1fda2c..eb12ac042 100644 --- a/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/CMakeLists.txt @@ -55,13 +55,6 @@ find_package(Eigen3 REQUIRED) find_package(urdf REQUIRED) find_package(urdfdom_headers REQUIRED) -if(DEFINED urdfdom_headers_VERSION) - if(${urdfdom_headers_VERSION} GREATER 0.4.1) - add_definitions(-DURDFDOM_HEADERS_HAS_SHARED_PTR_DEFS) - endif() -endif() - - include_directories( include "." diff --git a/cartographer_ros/cartographer_ros/assets_writer_main.cc b/cartographer_ros/cartographer_ros/assets_writer_main.cc index f9930555a..eaa8422f5 100644 --- a/cartographer_ros/cartographer_ros/assets_writer_main.cc +++ b/cartographer_ros/cartographer_ros/assets_writer_main.cc @@ -17,7 +17,8 @@ #include "cartographer_ros/assets_writer.h" #include "gflags/gflags.h" #include "glog/logging.h" -#include +#include +#include DEFINE_string(configuration_directory, "", "First directory in which configuration files are searched, " @@ -58,8 +59,12 @@ int main(int argc, char** argv) { CHECK(!FLAGS_pose_graph_filename.empty()) << "-pose_graph_filename is missing."; - std::vector bag_filenames; - boost::split(bag_filenames, FLAGS_bag_filenames, boost::is_any_of(",")); + std::regex regex(","); + std::vector bag_filenames( + std::sregex_token_iterator( + FLAGS_bag_filenames.begin(), FLAGS_bag_filenames.end(), regex, -1), + std::sregex_token_iterator() + ); ::cartographer_ros::AssetsWriter asset_writer( FLAGS_pose_graph_filename, diff --git a/cartographer_ros/cartographer_ros/offline_node.cc b/cartographer_ros/cartographer_ros/offline_node.cc index 4704b7def..ad15336d2 100644 --- a/cartographer_ros/cartographer_ros/offline_node.cc +++ b/cartographer_ros/cartographer_ros/offline_node.cc @@ -33,8 +33,8 @@ #include "tf2_ros/static_transform_broadcaster.h" #include "urdf/model.h" #include "rclcpp/exceptions.hpp" -//#include -#include +#include +#include DEFINE_bool(collect_metrics, false, "Activates the collection of runtime metrics. If activated, the " @@ -98,13 +98,24 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory, LOG(WARNING) << "FLAGS_configuration_basenames " << FLAGS_configuration_basenames; CHECK(!(FLAGS_bag_filenames.empty() && FLAGS_load_state_filename.empty())) << "-bag_filenames and -load_state_filename cannot both be unspecified."; + std::regex regex(","); std::vector bag_filenames; if (!FLAGS_bag_filenames.empty()){ - boost::split(bag_filenames, FLAGS_bag_filenames, boost::is_any_of(",")); + std::regex regex(","); + std::vector if_bag_filenames( + std::sregex_token_iterator( + FLAGS_bag_filenames.begin(), FLAGS_bag_filenames.end(), regex, -1), + std::sregex_token_iterator() + ); + bag_filenames = if_bag_filenames; } cartographer_ros::NodeOptions node_options; - std::vector configuration_basenames; - boost::split(configuration_basenames, FLAGS_configuration_basenames, boost::is_any_of(",")); + std::vector configuration_basenames( + std::sregex_token_iterator( + FLAGS_configuration_basenames.begin(), FLAGS_configuration_basenames.end(), regex, -1), + std::sregex_token_iterator() + ); + std::vector bag_trajectory_options(1); std::tie(node_options, bag_trajectory_options.at(0)) = LoadOptions(FLAGS_configuration_directory, configuration_basenames.at(0)); @@ -142,8 +153,11 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory, std::vector urdf_transforms; if (!FLAGS_urdf_filenames.empty()) { - std::vector urdf_filenames; - boost::split(urdf_filenames, FLAGS_urdf_filenames, boost::is_any_of(",")); + std::vector urdf_filenames( + std::sregex_token_iterator( + FLAGS_urdf_filenames.begin(), FLAGS_urdf_filenames.end(), regex, -1), + std::sregex_token_iterator() + ); for (const auto& urdf_filename : urdf_filenames) { const auto current_urdf_transforms = ReadStaticTransformsFromUrdf(urdf_filename, tf_buffer); diff --git a/cartographer_ros/cartographer_ros/urdf_reader.cc b/cartographer_ros/cartographer_ros/urdf_reader.cc index 241cbbb0c..d2380cd40 100644 --- a/cartographer_ros/cartographer_ros/urdf_reader.cc +++ b/cartographer_ros/cartographer_ros/urdf_reader.cc @@ -28,11 +28,7 @@ std::vector ReadStaticTransformsFromUrdf( const std::string& urdf_filename, std::shared_ptr tf_buffer) { urdf::Model model; CHECK(model.initFile(urdf_filename)); -#if URDFDOM_HEADERS_HAS_SHARED_PTR_DEFS std::vector links; -#else - std::vector > links; -#endif model.getLinks(links); std::vector transforms; for (const auto& link : links) { From c7d0a7e92097c4b47b11884b78bec4322e0de18d Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 23 Nov 2021 10:06:40 +0100 Subject: [PATCH 79/94] got rid of most stderr warnings Signed-off-by: Guillaume Doisy --- cartographer_ros/cartographer_ros/node.cc | 6 ++++++ cartographer_ros/cartographer_ros/ros_log_sink.cc | 2 ++ cartographer_ros/cartographer_ros/sensor_bridge.cc | 6 +++++- cartographer_rviz/CMakeLists.txt | 2 +- cartographer_rviz/cartographer_rviz/drawable_submap.cc | 1 + cartographer_rviz/cartographer_rviz/ogre_slice.cc | 10 +++++----- cartographer_rviz/cartographer_rviz/submaps_display.cc | 4 ++++ 7 files changed, 24 insertions(+), 7 deletions(-) diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 2336134b2..e0dafa39f 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -676,6 +676,9 @@ int Node::AddOfflineTrajectory( bool Node::handleGetTrajectoryStates( const cartographer_ros_msgs::srv::GetTrajectoryStates::Request::SharedPtr request, cartographer_ros_msgs::srv::GetTrajectoryStates::Response::SharedPtr response) { + + (void) request; + using TrajectoryState = ::cartographer::mapping::PoseGraphInterface::TrajectoryState; absl::MutexLock lock(&mutex_); @@ -733,6 +736,9 @@ bool Node::handleWriteState( bool Node::handleReadMetrics( const cartographer_ros_msgs::srv::ReadMetrics::Request::SharedPtr request, cartographer_ros_msgs::srv::ReadMetrics::Response::SharedPtr response) { + + (void) request; + absl::MutexLock lock(&mutex_); response->timestamp = node_->now(); if (!metrics_registry_) { diff --git a/cartographer_ros/cartographer_ros/ros_log_sink.cc b/cartographer_ros/cartographer_ros/ros_log_sink.cc index 95ce0c0d3..1cd18e2a3 100644 --- a/cartographer_ros/cartographer_ros/ros_log_sink.cc +++ b/cartographer_ros/cartographer_ros/ros_log_sink.cc @@ -43,6 +43,8 @@ void ScopedRosLogSink::send(const ::google::LogSeverity severity, const struct std::tm* const tm_time, const char* const message, const size_t message_len) { + (void) base_filename; + const std::string message_string = ::google::LogSink::ToString( severity, GetBasename(filename), line, tm_time, message, message_len); switch (severity) { diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.cc b/cartographer_ros/cartographer_ros/sensor_bridge.cc index f7cbb9ec4..d47978e89 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.cc +++ b/cartographer_ros/cartographer_ros/sensor_bridge.cc @@ -229,6 +229,10 @@ void SensorBridge::HandleRangefinder( if (!ranges.empty()) { CHECK_LE(ranges.back().time, 0.f); } + + // This was added to get rid of the TimedPointCloudData warning for a missing argument + std::vector intensities_; + const auto sensor_to_tracking = tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id)); if (sensor_to_tracking != nullptr) { @@ -236,7 +240,7 @@ void SensorBridge::HandleRangefinder( sensor_id, carto::sensor::TimedPointCloudData{ time, sensor_to_tracking->translation().cast(), carto::sensor::TransformTimedPointCloud( - ranges, sensor_to_tracking->cast())}); + ranges, sensor_to_tracking->cast()),intensities_}); } } diff --git a/cartographer_rviz/CMakeLists.txt b/cartographer_rviz/CMakeLists.txt index 60f315d65..c44454afb 100644 --- a/cartographer_rviz/CMakeLists.txt +++ b/cartographer_rviz/CMakeLists.txt @@ -100,7 +100,7 @@ foreach(DEFINITION ${PCL_DEFINITIONS}) endforeach() message(STATUS "Using Qt5 based on the rviz2_QT_VERSION: ${rviz2_QT_VERSION}") -find_package(Qt5 ${rviz2_QT_VERSION} EXACT REQUIRED Core Widgets) +find_package(Qt5 ${rviz2_QT_VERSION} REQUIRED Core Widgets) set(QT_LIBRARIES Qt5::Core Qt5::Widgets) include_directories(${Qt5Widgets_INCLUDE_DIRS}) diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index 82d237172..d974461f0 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -99,6 +99,7 @@ DrawableSubmap::~DrawableSubmap() { void DrawableSubmap::Update( const ::std_msgs::msg::Header& header, const ::cartographer_ros_msgs::msg::SubmapEntry& metadata) { + (void) header; absl::MutexLock locker(&mutex_); metadata_version_ = metadata.submap_version; pose_ = ::cartographer_ros::ToRigid3d(metadata.pose); diff --git a/cartographer_rviz/cartographer_rviz/ogre_slice.cc b/cartographer_rviz/cartographer_rviz/ogre_slice.cc index 23bc4bad4..3fd7b9512 100644 --- a/cartographer_rviz/cartographer_rviz/ogre_slice.cc +++ b/cartographer_rviz/cartographer_rviz/ogre_slice.cc @@ -73,9 +73,9 @@ OgreSlice::OgreSlice(const ::cartographer::mapping::SubmapId& id, int slice_id, OgreSlice::~OgreSlice() { Ogre::MaterialManager::getSingleton().remove(material_->getHandle()); - if (!texture_.isNull()) { + if (!texture_.operator bool()) { Ogre::TextureManager::getSingleton().remove(texture_->getHandle()); - texture_.setNull(); + texture_.reset(); } scene_manager_->destroySceneNode(slice_node_); scene_manager_->destroyManualObject(manual_object_); @@ -116,11 +116,11 @@ void OgreSlice::Update( manual_object_->end(); Ogre::DataStreamPtr pixel_stream; - pixel_stream.bind(new Ogre::MemoryDataStream(rgb.data(), rgb.size())); + pixel_stream.reset(new Ogre::MemoryDataStream(rgb.data(), rgb.size())); - if (!texture_.isNull()) { + if (!texture_.operator bool()) { Ogre::TextureManager::getSingleton().remove(texture_->getHandle()); - texture_.setNull(); + texture_.reset(); } const std::string texture_name = kSubmapTexturePrefix + GetSliceIdentifier(id_, slice_id_); diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc index 07a3d2fff..318d52bc2 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/cartographer_rviz/submaps_display.cc @@ -219,6 +219,10 @@ void SubmapsDisplay::processMessage( const ::cartographer_ros_msgs::msg::SubmapL } void SubmapsDisplay::update(const float wall_dt, const float ros_dt) { + + (void) wall_dt; + (void) ros_dt; + absl::MutexLock locker(&mutex_); // Schedule fetching of new submap textures. for (const auto& trajectory_by_id : trajectories_) { From 923c8fccd76d299f1a4e5fc1187bb37181c9a62d Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 23 Nov 2021 11:40:05 +0100 Subject: [PATCH 80/94] fix sharedptr operator bool misuse Signed-off-by: Guillaume Doisy --- cartographer_rviz/cartographer_rviz/ogre_slice.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cartographer_rviz/cartographer_rviz/ogre_slice.cc b/cartographer_rviz/cartographer_rviz/ogre_slice.cc index 3fd7b9512..4be255419 100644 --- a/cartographer_rviz/cartographer_rviz/ogre_slice.cc +++ b/cartographer_rviz/cartographer_rviz/ogre_slice.cc @@ -73,7 +73,7 @@ OgreSlice::OgreSlice(const ::cartographer::mapping::SubmapId& id, int slice_id, OgreSlice::~OgreSlice() { Ogre::MaterialManager::getSingleton().remove(material_->getHandle()); - if (!texture_.operator bool()) { + if (texture_) { Ogre::TextureManager::getSingleton().remove(texture_->getHandle()); texture_.reset(); } @@ -118,7 +118,7 @@ void OgreSlice::Update( Ogre::DataStreamPtr pixel_stream; pixel_stream.reset(new Ogre::MemoryDataStream(rgb.data(), rgb.size())); - if (!texture_.operator bool()) { + if (texture_) { Ogre::TextureManager::getSingleton().remove(texture_->getHandle()); texture_.reset(); } From dbcac0283a76bdcbd013fa491f1bb5baa2bc7157 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 23 Nov 2021 12:57:02 +0100 Subject: [PATCH 81/94] using latest tf available to avoid warning Signed-off-by: Guillaume Doisy --- cartographer_rviz/cartographer_rviz/submaps_display.cc | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc index 318d52bc2..055418068 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/cartographer_rviz/submaps_display.cc @@ -244,12 +244,13 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) { if (map_frame_ == nullptr) { return; } + // Update the fading by z distance. - rclcpp::Time now = this->get_clock()->now(); + const auto klatest = this->get_clock()->now(); try { const ::geometry_msgs::msg::TransformStamped transform_stamped = tf_buffer_->lookupTransform( - *map_frame_, tracking_frame_property_->getStdString(), now); + *map_frame_, tracking_frame_property_->getStdString(), tf2::TimePointZero); for (auto& trajectory_by_id : trajectories_) { for (auto& submap_entry : trajectory_by_id.second->submaps) { submap_entry.second->SetAlpha( @@ -263,7 +264,7 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) { // Update the map frame to fixed frame transform. Ogre::Vector3 position; Ogre::Quaternion orientation; - if (context_->getFrameManager()->getTransform(*map_frame_, now, position, + if (context_->getFrameManager()->getTransform(*map_frame_, klatest, position, orientation)) { map_node_->setPosition(position); map_node_->setOrientation(orientation); From d1fbd068a1b2c2c42c64e2dbabddb865b541e226 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Tue, 23 Nov 2021 19:54:07 +0100 Subject: [PATCH 82/94] fix deserialization Signed-off-by: Guillaume Doisy --- cartographer_ros/cartographer_ros/offline_node.cc | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/cartographer_ros/cartographer_ros/offline_node.cc b/cartographer_ros/cartographer_ros/offline_node.cc index ad15336d2..0a7a37928 100644 --- a/cartographer_ros/cartographer_ros/offline_node.cc +++ b/cartographer_ros/cartographer_ros/offline_node.cc @@ -364,42 +364,42 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory, rclcpp::SerializedMessage serialized_msg(*msg.serialized_data); sensor_msgs::msg::MultiEchoLaserScan::SharedPtr multi_echo_laser_scan_msg = std::make_shared(); - multi_echo_laser_scan_serializer.deserialize_message(&serialized_msg, &multi_echo_laser_scan_msg); + multi_echo_laser_scan_serializer.deserialize_message(&serialized_msg, multi_echo_laser_scan_msg.get()); node.HandleMultiEchoLaserScanMessage(trajectory_id, sensor_id, multi_echo_laser_scan_msg); } else if (topic_type == "sensor_msgs/msg/PointCloud2") { rclcpp::SerializedMessage serialized_msg(*msg.serialized_data); sensor_msgs::msg::PointCloud2::SharedPtr pcl2_scan_msg = std::make_shared(); - pcl2_serializer.deserialize_message(&serialized_msg, &pcl2_scan_msg); + pcl2_serializer.deserialize_message(&serialized_msg, pcl2_scan_msg.get()); node.HandlePointCloud2Message(trajectory_id, sensor_id, pcl2_scan_msg); } else if (topic_type == "sensor_msgs/msg/Imu") { rclcpp::SerializedMessage serialized_msg(*msg.serialized_data); sensor_msgs::msg::Imu::SharedPtr imu_scan_msg = std::make_shared(); - imu_serializer.deserialize_message(&serialized_msg, &imu_scan_msg); + imu_serializer.deserialize_message(&serialized_msg, imu_scan_msg.get()); node.HandleImuMessage(trajectory_id, sensor_id, imu_scan_msg); } else if (topic_type == "nav_msgs/msg/Odometry") { rclcpp::SerializedMessage serialized_msg(*msg.serialized_data); nav_msgs::msg::Odometry::SharedPtr odom_scan_msg = std::make_shared(); - odom_serializer.deserialize_message(&serialized_msg, &odom_scan_msg); + odom_serializer.deserialize_message(&serialized_msg, odom_scan_msg.get()); node.HandleOdometryMessage(trajectory_id, sensor_id, odom_scan_msg); } else if (topic_type == "sensor_msgs/msg/NavSatFix") { rclcpp::SerializedMessage serialized_msg(*msg.serialized_data); sensor_msgs::msg::NavSatFix::SharedPtr nav_sat_fix_msg = std::make_shared(); - nav_sat_fix_serializer.deserialize_message(&serialized_msg, &nav_sat_fix_msg); + nav_sat_fix_serializer.deserialize_message(&serialized_msg, nav_sat_fix_msg.get()); node.HandleNavSatFixMessage(trajectory_id, sensor_id, nav_sat_fix_msg); } else if (topic_type == "cartographer_ros_msgs/msg/LandmarkList") { rclcpp::SerializedMessage serialized_msg(*msg.serialized_data); cartographer_ros_msgs::msg::LandmarkList::SharedPtr landmark_list_msg = std::make_shared(); - landmark_list_serializer.deserialize_message(&serialized_msg, &landmark_list_msg); + landmark_list_serializer.deserialize_message(&serialized_msg, landmark_list_msg.get()); node.HandleLandmarkMessage(trajectory_id, sensor_id, landmark_list_msg); } From eb93b2da3a241015c601babcafac496fa0e51f52 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Wed, 24 Nov 2021 09:58:13 +0100 Subject: [PATCH 83/94] removed unused function arg + comment Signed-off-by: Guillaume Doisy --- cartographer_ros/cartographer_ros/node.cc | 8 ++------ cartographer_ros/cartographer_ros/node.h | 5 ++--- cartographer_ros/cartographer_ros/ros_log_sink.cc | 2 +- cartographer_ros/cartographer_ros/sensor_bridge.cc | 2 +- cartographer_rviz/cartographer_rviz/drawable_submap.cc | 2 +- cartographer_rviz/cartographer_rviz/submaps_display.cc | 6 +----- cartographer_rviz/cartographer_rviz/submaps_display.h | 2 +- 7 files changed, 9 insertions(+), 18 deletions(-) diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index e0dafa39f..2683501ba 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -674,11 +674,9 @@ int Node::AddOfflineTrajectory( } bool Node::handleGetTrajectoryStates( - const cartographer_ros_msgs::srv::GetTrajectoryStates::Request::SharedPtr request, + const cartographer_ros_msgs::srv::GetTrajectoryStates::Request::SharedPtr , cartographer_ros_msgs::srv::GetTrajectoryStates::Response::SharedPtr response) { - (void) request; - using TrajectoryState = ::cartographer::mapping::PoseGraphInterface::TrajectoryState; absl::MutexLock lock(&mutex_); @@ -734,11 +732,9 @@ bool Node::handleWriteState( } bool Node::handleReadMetrics( - const cartographer_ros_msgs::srv::ReadMetrics::Request::SharedPtr request, + const cartographer_ros_msgs::srv::ReadMetrics::Request::SharedPtr, cartographer_ros_msgs::srv::ReadMetrics::Response::SharedPtr response) { - (void) request; - absl::MutexLock lock(&mutex_); response->timestamp = node_->now(); if (!metrics_registry_) { diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index 4cea9e862..f3527cae1 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -147,10 +147,9 @@ class Node { const cartographer_ros_msgs::srv::WriteState::Request::SharedPtr request, cartographer_ros_msgs::srv::WriteState::Response::SharedPtr response); bool handleGetTrajectoryStates( - const cartographer_ros_msgs::srv::GetTrajectoryStates::Request::SharedPtr request, + const cartographer_ros_msgs::srv::GetTrajectoryStates::Request::SharedPtr, cartographer_ros_msgs::srv::GetTrajectoryStates::Response::SharedPtr response); - bool handleReadMetrics( - const cartographer_ros_msgs::srv::ReadMetrics::Request::SharedPtr request, + bool handleReadMetrics(const cartographer_ros_msgs::srv::ReadMetrics::Request::SharedPtr, cartographer_ros_msgs::srv::ReadMetrics::Response::SharedPtr response); // Returns the set of SensorIds expected for a trajectory. diff --git a/cartographer_ros/cartographer_ros/ros_log_sink.cc b/cartographer_ros/cartographer_ros/ros_log_sink.cc index 1cd18e2a3..d9b8ee2c1 100644 --- a/cartographer_ros/cartographer_ros/ros_log_sink.cc +++ b/cartographer_ros/cartographer_ros/ros_log_sink.cc @@ -43,7 +43,7 @@ void ScopedRosLogSink::send(const ::google::LogSeverity severity, const struct std::tm* const tm_time, const char* const message, const size_t message_len) { - (void) base_filename; + (void) base_filename; // TODO: remove unused arg ? const std::string message_string = ::google::LogSink::ToString( severity, GetBasename(filename), line, tm_time, message, message_len); diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.cc b/cartographer_ros/cartographer_ros/sensor_bridge.cc index d47978e89..d1e4b18c9 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.cc +++ b/cartographer_ros/cartographer_ros/sensor_bridge.cc @@ -240,7 +240,7 @@ void SensorBridge::HandleRangefinder( sensor_id, carto::sensor::TimedPointCloudData{ time, sensor_to_tracking->translation().cast(), carto::sensor::TransformTimedPointCloud( - ranges, sensor_to_tracking->cast()),intensities_}); + ranges, sensor_to_tracking->cast()), intensities_}); } } diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index d974461f0..db0d8a716 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -99,7 +99,7 @@ DrawableSubmap::~DrawableSubmap() { void DrawableSubmap::Update( const ::std_msgs::msg::Header& header, const ::cartographer_ros_msgs::msg::SubmapEntry& metadata) { - (void) header; + (void) header; // TODO: remove unused arg ? absl::MutexLock locker(&mutex_); metadata_version_ = metadata.submap_version; pose_ = ::cartographer_ros::ToRigid3d(metadata.pose); diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc index 055418068..0e89d5ee0 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/cartographer_rviz/submaps_display.cc @@ -218,11 +218,7 @@ void SubmapsDisplay::processMessage( const ::cartographer_ros_msgs::msg::SubmapL } } -void SubmapsDisplay::update(const float wall_dt, const float ros_dt) { - - (void) wall_dt; - (void) ros_dt; - +void SubmapsDisplay::update(const float , const float) { absl::MutexLock locker(&mutex_); // Schedule fetching of new submap textures. for (const auto& trajectory_by_id : trajectories_) { diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.h b/cartographer_rviz/cartographer_rviz/submaps_display.h index 2c85229a7..4b128aabd 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.h +++ b/cartographer_rviz/cartographer_rviz/submaps_display.h @@ -86,7 +86,7 @@ class SubmapsDisplay void reset() override; void processMessage( const ::cartographer_ros_msgs::msg::SubmapList::ConstSharedPtr msg) override; - void update(float wall_dt, float ros_dt) override; + void update(float , float) override; std::unique_ptr tf_buffer_; std::shared_ptr tf_listener_; From 22b3bbc54f8e9eae0a091669d2c2f281446bd66e Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Wed, 24 Nov 2021 12:03:48 +0100 Subject: [PATCH 84/94] added 100ms timeout lookuptf rviz Signed-off-by: Guillaume Doisy --- cartographer_rviz/cartographer_rviz/submaps_display.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc index 0e89d5ee0..edc8cd828 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/cartographer_rviz/submaps_display.cc @@ -246,7 +246,7 @@ void SubmapsDisplay::update(const float , const float) { try { const ::geometry_msgs::msg::TransformStamped transform_stamped = tf_buffer_->lookupTransform( - *map_frame_, tracking_frame_property_->getStdString(), tf2::TimePointZero); + *map_frame_, tracking_frame_property_->getStdString(), tf2::TimePointZero,std::chrono::milliseconds(100)); for (auto& trajectory_by_id : trajectories_) { for (auto& submap_entry : trajectory_by_id.second->submaps) { submap_entry.second->SetAlpha( From 4f3819496d75231b8879119f7e44c2455fd58b19 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Thu, 9 Dec 2021 11:55:49 +0100 Subject: [PATCH 85/94] fix offline node remappings Signed-off-by: Guillaume Doisy --- cartographer_ros/cartographer_ros/offline_node.cc | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/cartographer_ros/cartographer_ros/offline_node.cc b/cartographer_ros/cartographer_ros/offline_node.cc index 0a7a37928..3cfa9dcd5 100644 --- a/cartographer_ros/cartographer_ros/offline_node.cc +++ b/cartographer_ros/cartographer_ros/offline_node.cc @@ -217,7 +217,6 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory, } for (const auto& expected_sensor_id : bag_expected_sensor_ids.at(current_bag_index)) { - // TODO: check resolved topic LOG(INFO) << "expected_sensor_id.id " << expected_sensor_id.id; const auto bag_resolved_topic = std::make_pair( static_cast(current_bag_index), @@ -277,9 +276,10 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory, std::set bag_topics; std::stringstream bag_topics_string; for (const auto& topic : playable_bag_multiplexer.topics()) { - // TODO: check resolved topic - bag_topics.insert(topic); - bag_topics_string << topic << ","; + std::string resolved_topic = cartographer_offline_node->get_node_base_interface()-> + resolve_topic_or_service_name(topic, false); + bag_topics.insert(resolved_topic); + bag_topics_string << resolved_topic << ","; } bool print_topics = false; for (const auto& entry : bag_topic_to_sensor_id) { @@ -346,8 +346,8 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory, const auto bag_topic = std::make_pair( bag_index, - // TODO: check resolved topic - msg.topic_name); + cartographer_offline_node->get_node_base_interface()-> + resolve_topic_or_service_name(msg.topic_name, false)); auto it = bag_topic_to_sensor_id.find(bag_topic); if (it != bag_topic_to_sensor_id.end()) { From 0c480927cfc0d74d6e92d709e7b9c657abefeebf Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Mon, 10 Jan 2022 23:12:09 +0100 Subject: [PATCH 86/94] proper cmakelists install Signed-off-by: Guillaume Doisy --- cartographer_ros/CMakeLists.txt | 2 +- cartographer_rviz/cartographer_rviz/drawable_submap.cc | 2 +- cartographer_rviz/cartographer_rviz/drawable_submap.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/cartographer_ros/CMakeLists.txt b/cartographer_ros/CMakeLists.txt index eb12ac042..1413d97d0 100644 --- a/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/CMakeLists.txt @@ -162,7 +162,7 @@ install(DIRECTORY configuration_files install( FILES ${ALL_HEADERS} - DESTINATION include/${PROJECT_NAME}/include + DESTINATION include/${PROJECT_NAME} ) install( diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index db0d8a716..c2b26db9e 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -25,7 +25,7 @@ #include "Eigen/Geometry" #include "absl/memory/memory.h" #include "cartographer/common/port.h" -#include "cartographer_ros/include/msg_conversion.h" +#include "cartographer_ros/msg_conversion.h" #include "cartographer_ros_msgs/srv/submap_query.hpp" #include diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.h b/cartographer_rviz/cartographer_rviz/drawable_submap.h index 9471aa821..fe9d5ba04 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.h +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.h @@ -28,7 +28,7 @@ #include "cartographer/io/submap_painter.h" #include "cartographer/mapping/id.h" #include "cartographer/transform/rigid_transform.h" -#include "cartographer_ros/include/submap.h" +#include "cartographer_ros/submap.h" #include "cartographer_ros_msgs/msg/submap_entry.hpp" #include "cartographer_ros_msgs/srv/submap_query.hpp" #include "cartographer_rviz/ogre_slice.h" From 4d650698235b5effacb5f8697a01364b7a97afbd Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Tue, 11 Jan 2022 00:11:35 +0100 Subject: [PATCH 87/94] offline_backpack_2d.launch conversion Signed-off-by: Guillaume Doisy --- cartographer_ros/CMakeLists.txt | 2 +- .../configuration_files/demo_2d.rviz | 451 ++++++++++++------ .../launch/offline_backpack_2d.launch | 36 -- .../launch/offline_backpack_2d.launch.py | 60 +++ cartographer_ros/launch/offline_node.launch | 50 -- .../launch/offline_node.launch.py | 81 ++++ 6 files changed, 458 insertions(+), 222 deletions(-) delete mode 100644 cartographer_ros/launch/offline_backpack_2d.launch create mode 100644 cartographer_ros/launch/offline_backpack_2d.launch.py delete mode 100644 cartographer_ros/launch/offline_node.launch create mode 100644 cartographer_ros/launch/offline_node.launch.py diff --git a/cartographer_ros/CMakeLists.txt b/cartographer_ros/CMakeLists.txt index 1413d97d0..24b165dd3 100644 --- a/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/CMakeLists.txt @@ -156,7 +156,7 @@ set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${GOOG_CXX_FLAGS}") set_target_properties(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS ${TARGET_COMPILE_FLAGS}) -install(DIRECTORY configuration_files +install(DIRECTORY configuration_files urdf launch DESTINATION share/${PROJECT_NAME}/ ) diff --git a/cartographer_ros/configuration_files/demo_2d.rviz b/cartographer_ros/configuration_files/demo_2d.rviz index 85125cd66..57d084ead 100644 --- a/cartographer_ros/configuration_files/demo_2d.rviz +++ b/cartographer_ros/configuration_files/demo_2d.rviz @@ -1,42 +1,35 @@ -# Copyright 2016 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. - Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - - /Submaps1 - - /PointCloud21 - Splitter Ratio: 0.600671 - Tree Height: 821 - - Class: rviz/Selection + - /Global Options1 + - /RobotModel1/Description Topic1 + - /map1/Topic1 + - /SubmapsDisplay1/Submaps1 + - /Trajectories1/Namespaces1 + Splitter Ratio: 0.47058823704719543 + Tree Height: 1147 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 + - /2D Goal Pose1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679 - - Class: rviz/Views + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: teleop_panel/Teleop + Name: Teleop + Topic: /rt_cmd_vel + - Class: wyca_rviz_plugin/Wyca + Name: Wyca + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 @@ -44,31 +37,36 @@ Panels: Visualization Manager: Class: "" Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description Enabled: true - Line Style: - Line Width: 0.03 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 100 - Reference Frame: + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: RobotModel + TF Prefix: "" + Update Interval: 0 Value: true - - Class: rviz/TF + Visual Enabled: true + - Class: rviz_default_plugins/TF Enabled: true Frame Timeout: 15 Frames: - All Enabled: true + All Enabled: false base_link: - Value: true + Value: false horizontal_laser_link: Value: true imu_link: @@ -79,7 +77,7 @@ Visualization Manager: Value: true vertical_laser_link: Value: true - Marker Scale: 1 + Marker Scale: 0.30000001192092896 Name: TF Show Arrows: true Show Axes: true @@ -96,48 +94,26 @@ Visualization Manager: {} Update Interval: 0 Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - horizontal_laser_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - imu_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - vertical_laser_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: Submaps + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false Enabled: true - Name: Submaps - Submap query service: /submap_query - Topic: /submap_list - Tracking frame: base_link - Unreliable: false + Name: map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false Value: true - Alpha: 1 Autocompute Intensity Bounds: true @@ -147,8 +123,8 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 - Color: 0; 255; 0 + Class: rviz_default_plugins/PointCloud2 + Color: 239; 41; 41 Color Transformer: FlatColor Decay Time: 0 Enabled: true @@ -159,93 +135,298 @@ Visualization Manager: Min Intensity: 0 Name: PointCloud2 Position Transformer: XYZ - Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.05 + Size (m): 0.10000000149011612 Style: Flat Squares - Topic: /scan_matched_points2 - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /scan_matched_points2 Use Fixed Frame: true - Use rainbow: true + Use rainbow: false Value: true - - Class: rviz/MarkerArray + - Class: cartographer_rviz/SubmapsDisplay Enabled: true - Marker Topic: /trajectory_node_list + Fade-out distance: 1 + High Resolution: true + Low Resolution: false + Name: SubmapsDisplay + Submap query service: /submap_query + Submaps: + All: true + All Submap Pose Markers: true + Trajectory 0: + 0.180: true + 1.180: true + 10.180: true + 100.180: true + 101.180: true + 102.180: true + 103.180: true + 104.180: true + 105.180: true + 106.180: true + 107.180: true + 108.180: true + 109.180: true + 11.180: true + 110.180: true + 111.180: true + 112.180: true + 113.180: true + 114.180: true + 115.180: true + 116.180: true + 117.180: true + 118.180: true + 119.180: true + 12.180: true + 120.180: true + 121.180: true + 122.180: true + 123.180: true + 124.180: true + 125.180: true + 126.180: true + 127.154: true + 128.64: true + 13.180: true + 14.180: true + 15.180: true + 16.180: true + 17.180: true + 18.180: true + 19.180: true + 2.180: true + 20.180: true + 21.180: true + 22.180: true + 23.180: true + 24.180: true + 25.180: true + 26.180: true + 27.180: true + 28.180: true + 29.180: true + 3.180: true + 30.180: true + 31.180: true + 32.180: true + 33.180: true + 34.180: true + 35.180: true + 36.180: true + 37.180: true + 38.180: true + 39.180: true + 4.180: true + 40.180: true + 41.180: true + 42.180: true + 43.180: true + 44.180: true + 45.180: true + 46.180: true + 47.180: true + 48.180: true + 49.180: true + 5.180: true + 50.180: true + 51.180: true + 52.180: true + 53.180: true + 54.180: true + 55.180: true + 56.180: true + 57.180: true + 58.180: true + 59.180: true + 6.180: true + 60.180: true + 61.180: true + 62.180: true + 63.180: true + 64.180: true + 65.180: true + 66.180: true + 67.180: true + 68.180: true + 69.180: true + 7.180: true + 70.180: true + 71.180: true + 72.180: true + 73.180: true + 74.180: true + 75.180: true + 76.180: true + 77.180: true + 78.180: true + 79.180: true + 8.180: true + 80.180: true + 81.180: true + 82.180: true + 83.180: true + 84.180: true + 85.180: true + 86.180: true + 87.180: true + 88.180: true + 89.180: true + 9.180: true + 90.180: true + 91.180: true + 92.180: true + 93.180: true + 94.180: true + 95.180: true + 96.180: true + 97.180: true + 98.180: true + 99.180: true + Submap Pose Markers: true + Value: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /submap_list + Tracking frame: base_link + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false Name: Trajectories Namespaces: - "": true - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /trajectory_node_list + Value: false + - Class: rviz_default_plugins/MarkerArray Enabled: true - Marker Topic: /landmark_poses_list - Name: Landmark Poses + Name: Constraints Namespaces: - "": true - Queue Size: 100 + Inter constraints, different trajectories: true + Inter constraints, same trajectory: true + Inter residuals, different trajectories: true + Inter residuals, same trajectory: true + Intra constraints: true + Intra residuals: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /constraint_list Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true - Marker Topic: /constraint_list - Name: Constraints + Name: Landmarks Namespaces: - "": true - Queue Size: 100 + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /landmark_poses_list Value: true Enabled: true Global Options: - Background Color: 100; 100; 100 + Background Color: 48; 48; 48 Fixed Frame: map Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + - Class: nav2_rviz_plugins/GoalTool + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Angle: 0 - Class: rviz/TopDownOrtho + Class: rviz_default_plugins/ThirdPersonFollower + Distance: 90.55636596679688 Enable Stereo Rendering: - Stereo Eye Separation: 0.06 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false + Focal Point: + X: -25.26685333251953 + Y: 3.4120447635650635 + Z: -8.647453068988398e-05 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false Name: Current View - Near Clip Distance: 0.01 - Scale: 10 - Target Frame: - Value: TopDownOrtho (rviz) - X: 0 - Y: 0 + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5697963237762451 + Target Frame: cartomap + Value: ThirdPersonFollower (rviz_default_plugins) + Yaw: 6.123587131500244 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1123 + Height: 1376 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001c5000003c0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000041000003c0000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003c0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000041000003c0000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077e0000003efc0100000002fb0000000800540069006d006501000000000000077e000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000049e000003c000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001ac00000506fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000506000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e002000320000000324000000b70000000000000000fb0000000c00540065006c0065006f007000000002af00000164000000bb00fffffffb00000008005700790063006100000003a5000001000000006000fffffffb0000000800540069006d00650000000484000000bf0000003900ffffff000000010000010f00000506fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000506000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000001fb0000000800540069006d00650100000000000004500000000000000000000006e10000050600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false + Teleop: + collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: false - Width: 1918 - X: 0 - Y: 24 + Width: 2472 + Wyca: + collapsed: false + X: 2648 + Y: 27 diff --git a/cartographer_ros/launch/offline_backpack_2d.launch b/cartographer_ros/launch/offline_backpack_2d.launch deleted file mode 100644 index bb8ae0a70..000000000 --- a/cartographer_ros/launch/offline_backpack_2d.launch +++ /dev/null @@ -1,36 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/cartographer_ros/launch/offline_backpack_2d.launch.py b/cartographer_ros/launch/offline_backpack_2d.launch.py new file mode 100644 index 000000000..0356ae160 --- /dev/null +++ b/cartographer_ros/launch/offline_backpack_2d.launch.py @@ -0,0 +1,60 @@ +""" + Copyright 2018 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource + +def generate_launch_description(): + + ## ***** Launch arguments ***** + bag_filenames_arg = DeclareLaunchArgument('bag_filenames') + no_rviz_arg = DeclareLaunchArgument('no_rviz', default_value='false') + rviz_config_arg = DeclareLaunchArgument('rviz_config', default_value = FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files/demo_2d.rviz') + configuration_directory_arg = DeclareLaunchArgument('configuration_directory', default_value = FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files') + configuration_basenames_arg = DeclareLaunchArgument('configuration_basenames', default_value = 'backpack_2d.lua') + urdf_filenames_arg = DeclareLaunchArgument('urdf_filenames', default_value = FindPackageShare('cartographer_ros').find('cartographer_ros') + '/urdf/backpack_2d.urdf') + + ## ***** Nodes ***** + offline_node_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(FindPackageShare('cartographer_ros').find('cartographer_ros') + '/launch/offline_node.launch.py'), + launch_arguments = { + 'bag_filenames': LaunchConfiguration('bag_filenames'), + 'no_rviz': LaunchConfiguration('no_rviz'), + 'rviz_config': LaunchConfiguration('rviz_config'), + 'configuration_directory': LaunchConfiguration('configuration_directory'), + 'configuration_basenames': LaunchConfiguration('configuration_basenames'), + 'urdf_filenames': LaunchConfiguration('urdf_filenames')}.items(), + ) + SetRemap('horizontal_laser_2d', 'echoes') + + return LaunchDescription([ + # Launch arguments + bag_filenames_arg, + no_rviz_arg, + rviz_config_arg, + configuration_directory_arg, + configuration_basenames_arg, + urdf_filenames_arg, + + # Nodes + offline_node_launch, + ]) diff --git a/cartographer_ros/launch/offline_node.launch b/cartographer_ros/launch/offline_node.launch deleted file mode 100644 index cf404a007..000000000 --- a/cartographer_ros/launch/offline_node.launch +++ /dev/null @@ -1,50 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/cartographer_ros/launch/offline_node.launch.py b/cartographer_ros/launch/offline_node.launch.py new file mode 100644 index 000000000..bfa64b7b2 --- /dev/null +++ b/cartographer_ros/launch/offline_node.launch.py @@ -0,0 +1,81 @@ +""" + Copyright 2018 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from os import getenv + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +def generate_launch_description(): + + ## ***** Launch arguments ***** + bag_filenames_arg = DeclareLaunchArgument('bag_filenames') + no_rviz_arg = DeclareLaunchArgument('no_rviz') + rviz_config_arg = DeclareLaunchArgument('rviz_config') + configuration_directory_arg = DeclareLaunchArgument('configuration_directory') + configuration_basenames_arg = DeclareLaunchArgument('configuration_basenames') + urdf_filenames_arg = DeclareLaunchArgument('urdf_filenames') + + ## ***** Nodes ***** + rviz_node = Node( + package = 'rviz2', + executable = 'rviz2', + arguments = ['-d', LaunchConfiguration('rviz_config')], + parameters = [{'use_sim_time': True}], + condition = UnlessCondition(LaunchConfiguration('no_rviz')) + ) + + cartographer_occupancy_grid_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_occupancy_grid_node', + parameters = [ + {'use_sim_time': True}, + {'resolution': 0.05}], + ) + + cartographer_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_offline_node', + parameters = [{'use_sim_time': True}], + arguments = [ + '-configuration_directory', LaunchConfiguration('configuration_directory'), + '-configuration_basenames', LaunchConfiguration('configuration_basenames'), + '-urdf_filenames', LaunchConfiguration('urdf_filenames'), + '-bag_filenames', LaunchConfiguration('bag_filenames')], + output = 'screen' + ) + + + return LaunchDescription([ + # Launch arguments + bag_filenames_arg, + no_rviz_arg, + rviz_config_arg, + configuration_directory_arg, + configuration_basenames_arg, + urdf_filenames_arg, + + # Nodes + rviz_node, + cartographer_occupancy_grid_node, + cartographer_node, + ]) From c2760d6121fd24a8452344cc1dc534741f094290 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Tue, 11 Jan 2022 00:20:03 +0100 Subject: [PATCH 88/94] fix demo_2d.rviz and set_remap for offline demo Signed-off-by: Guillaume Doisy --- .../configuration_files/demo_2d.rviz | 104 +----------------- .../launch/offline_backpack_2d.launch.py | 7 +- 2 files changed, 10 insertions(+), 101 deletions(-) diff --git a/cartographer_ros/configuration_files/demo_2d.rviz b/cartographer_ros/configuration_files/demo_2d.rviz index 57d084ead..58ef9a397 100644 --- a/cartographer_ros/configuration_files/demo_2d.rviz +++ b/cartographer_ros/configuration_files/demo_2d.rviz @@ -163,37 +163,8 @@ Visualization Manager: 0.180: true 1.180: true 10.180: true - 100.180: true - 101.180: true - 102.180: true - 103.180: true - 104.180: true - 105.180: true - 106.180: true - 107.180: true - 108.180: true - 109.180: true 11.180: true - 110.180: true - 111.180: true - 112.180: true - 113.180: true - 114.180: true - 115.180: true - 116.180: true - 117.180: true - 118.180: true - 119.180: true 12.180: true - 120.180: true - 121.180: true - 122.180: true - 123.180: true - 124.180: true - 125.180: true - 126.180: true - 127.154: true - 128.64: true 13.180: true 14.180: true 15.180: true @@ -214,81 +185,14 @@ Visualization Manager: 29.180: true 3.180: true 30.180: true - 31.180: true - 32.180: true - 33.180: true - 34.180: true - 35.180: true - 36.180: true - 37.180: true - 38.180: true - 39.180: true + 31.155: true + 32.65: true 4.180: true - 40.180: true - 41.180: true - 42.180: true - 43.180: true - 44.180: true - 45.180: true - 46.180: true - 47.180: true - 48.180: true - 49.180: true 5.180: true - 50.180: true - 51.180: true - 52.180: true - 53.180: true - 54.180: true - 55.180: true - 56.180: true - 57.180: true - 58.180: true - 59.180: true 6.180: true - 60.180: true - 61.180: true - 62.180: true - 63.180: true - 64.180: true - 65.180: true - 66.180: true - 67.180: true - 68.180: true - 69.180: true 7.180: true - 70.180: true - 71.180: true - 72.180: true - 73.180: true - 74.180: true - 75.180: true - 76.180: true - 77.180: true - 78.180: true - 79.180: true 8.180: true - 80.180: true - 81.180: true - 82.180: true - 83.180: true - 84.180: true - 85.180: true - 86.180: true - 87.180: true - 88.180: true - 89.180: true 9.180: true - 90.180: true - 91.180: true - 92.180: true - 93.180: true - 94.180: true - 95.180: true - 96.180: true - 97.180: true - 98.180: true - 99.180: true Submap Pose Markers: true Value: true Topic: @@ -388,7 +292,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/ThirdPersonFollower - Distance: 90.55636596679688 + Distance: 251.1199188232422 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -404,7 +308,7 @@ Visualization Manager: Name: Current View Near Clip Distance: 0.009999999776482582 Pitch: 1.5697963237762451 - Target Frame: cartomap + Target Frame: map Value: ThirdPersonFollower (rviz_default_plugins) Yaw: 6.123587131500244 Saved: ~ diff --git a/cartographer_ros/launch/offline_backpack_2d.launch.py b/cartographer_ros/launch/offline_backpack_2d.launch.py index 0356ae160..e855e28e2 100644 --- a/cartographer_ros/launch/offline_backpack_2d.launch.py +++ b/cartographer_ros/launch/offline_backpack_2d.launch.py @@ -43,8 +43,12 @@ def generate_launch_description(): 'configuration_directory': LaunchConfiguration('configuration_directory'), 'configuration_basenames': LaunchConfiguration('configuration_basenames'), 'urdf_filenames': LaunchConfiguration('urdf_filenames')}.items(), + ) - SetRemap('horizontal_laser_2d', 'echoes') + set_remap = SetRemap('horizontal_laser_2d', 'echoes') + +# remappings = [ +# ('horizontal_laser_2d', 'echoes')], return LaunchDescription([ # Launch arguments @@ -56,5 +60,6 @@ def generate_launch_description(): urdf_filenames_arg, # Nodes + set_remap, offline_node_launch, ]) From cb2e96b9e86b992a6077ec870550115d6c50a19a Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Thu, 13 Jan 2022 16:13:38 +0100 Subject: [PATCH 89/94] all bags launch files Signed-off-by: Guillaume Doisy --- .../configuration_files/demo_2d.rviz | 108 ++--- .../configuration_files/demo_3d.rviz | 437 +++++++++++------- .../configuration_files/mir-100-mapping.lua | 77 +++ cartographer_ros/launch/backpack_2d.launch | 34 -- cartographer_ros/launch/backpack_2d.launch.py | 75 +++ cartographer_ros/launch/backpack_3d.launch | 35 -- cartographer_ros/launch/backpack_3d.launch.py | 76 +++ .../launch/demo_backpack_2d.launch | 26 -- .../launch/demo_backpack_2d.launch.py | 57 +++ .../demo_backpack_2d_localization.launch | 42 -- .../demo_backpack_2d_localization.launch.py | 94 ++++ .../launch/demo_backpack_3d.launch | 26 -- .../launch/demo_backpack_3d.launch.py | 57 +++ .../demo_backpack_3d_localization.launch | 43 -- .../demo_backpack_3d_localization.launch.py | 95 ++++ cartographer_ros/launch/demo_revo_lds.launch | 35 -- .../launch/demo_revo_lds.launch.py | 91 ++++ .../launch/demo_taurob_tracker.launch | 26 -- .../launch/demo_taurob_tracker.launch.py | 57 +++ .../launch/offline_backpack_2d.launch.py | 3 - .../launch/offline_backpack_3d.launch | 37 -- .../launch/offline_backpack_3d.launch.py | 64 +++ .../launch/offline_mir_100_rviz.launch.py | 76 +++ .../launch/offline_node.launch.py | 6 +- cartographer_ros/launch/taurob_tracker.launch | 29 -- .../launch/taurob_tracker.launch.py | 58 +++ cartographer_ros/urdf/mir-100.urdf | 107 +++++ 27 files changed, 1302 insertions(+), 569 deletions(-) create mode 100644 cartographer_ros/configuration_files/mir-100-mapping.lua delete mode 100644 cartographer_ros/launch/backpack_2d.launch create mode 100644 cartographer_ros/launch/backpack_2d.launch.py delete mode 100644 cartographer_ros/launch/backpack_3d.launch create mode 100644 cartographer_ros/launch/backpack_3d.launch.py delete mode 100644 cartographer_ros/launch/demo_backpack_2d.launch create mode 100644 cartographer_ros/launch/demo_backpack_2d.launch.py delete mode 100644 cartographer_ros/launch/demo_backpack_2d_localization.launch create mode 100644 cartographer_ros/launch/demo_backpack_2d_localization.launch.py delete mode 100644 cartographer_ros/launch/demo_backpack_3d.launch create mode 100644 cartographer_ros/launch/demo_backpack_3d.launch.py delete mode 100644 cartographer_ros/launch/demo_backpack_3d_localization.launch create mode 100644 cartographer_ros/launch/demo_backpack_3d_localization.launch.py delete mode 100644 cartographer_ros/launch/demo_revo_lds.launch create mode 100644 cartographer_ros/launch/demo_revo_lds.launch.py delete mode 100644 cartographer_ros/launch/demo_taurob_tracker.launch create mode 100644 cartographer_ros/launch/demo_taurob_tracker.launch.py delete mode 100644 cartographer_ros/launch/offline_backpack_3d.launch create mode 100644 cartographer_ros/launch/offline_backpack_3d.launch.py create mode 100644 cartographer_ros/launch/offline_mir_100_rviz.launch.py delete mode 100644 cartographer_ros/launch/taurob_tracker.launch create mode 100644 cartographer_ros/launch/taurob_tracker.launch.py create mode 100644 cartographer_ros/urdf/mir-100.urdf diff --git a/cartographer_ros/configuration_files/demo_2d.rviz b/cartographer_ros/configuration_files/demo_2d.rviz index 58ef9a397..6db01920c 100644 --- a/cartographer_ros/configuration_files/demo_2d.rviz +++ b/cartographer_ros/configuration_files/demo_2d.rviz @@ -5,12 +5,16 @@ Panels: Property Tree Widget: Expanded: - /Global Options1 + - /RobotModel1/Status1 - /RobotModel1/Description Topic1 + - /TF1/Tree1 + - /TF1/Tree1/map1 + - /TF1/Tree1/map1/odom1 - /map1/Topic1 - /SubmapsDisplay1/Submaps1 - /Trajectories1/Namespaces1 - Splitter Ratio: 0.47058823704719543 - Tree Height: 1147 + Splitter Ratio: 0.42203986644744873 + Tree Height: 1174 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -55,6 +59,25 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + horizontal_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + vertical_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true Name: RobotModel TF Prefix: "" Update Interval: 0 @@ -85,13 +108,8 @@ Visualization Manager: Tree: map: odom: - base_link: - horizontal_laser_link: - {} - imu_link: - {} - vertical_laser_link: - {} + horizontal_laser_link: + {} Update Interval: 0 Value: true - Alpha: 0.699999988079071 @@ -160,39 +178,23 @@ Visualization Manager: All: true All Submap Pose Markers: true Trajectory 0: - 0.180: true - 1.180: true - 10.180: true - 11.180: true - 12.180: true - 13.180: true - 14.180: true - 15.180: true - 16.180: true - 17.180: true - 18.180: true - 19.180: true - 2.180: true - 20.180: true - 21.180: true - 22.180: true - 23.180: true - 24.180: true - 25.180: true - 26.180: true - 27.180: true - 28.180: true - 29.180: true - 3.180: true - 30.180: true - 31.155: true - 32.65: true - 4.180: true - 5.180: true - 6.180: true - 7.180: true - 8.180: true - 9.180: true + 0.70: true + 1.70: true + 10.70: true + 11.70: true + 12.70: true + 13.70: true + 14.70: true + 15.68: true + 16.33: true + 2.70: true + 3.70: true + 4.70: true + 5.70: true + 6.70: true + 7.70: true + 8.70: true + 9.70: true Submap Pose Markers: true Value: true Topic: @@ -202,7 +204,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /submap_list - Tracking frame: base_link + Tracking frame: odom Value: true - Class: rviz_default_plugins/MarkerArray Enabled: false @@ -292,16 +294,16 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/ThirdPersonFollower - Distance: 251.1199188232422 + Distance: 29.796886444091797 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -25.26685333251953 - Y: 3.4120447635650635 - Z: -8.647453068988398e-05 + X: -11.516843795776367 + Y: 13.40983772277832 + Z: 7.403941708616912e-05 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false @@ -310,15 +312,15 @@ Visualization Manager: Pitch: 1.5697963237762451 Target Frame: map Value: ThirdPersonFollower (rviz_default_plugins) - Yaw: 6.123587131500244 + Yaw: 1.6953976154327393 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1376 + Height: 1403 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001ac00000506fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000506000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e002000320000000324000000b70000000000000000fb0000000c00540065006c0065006f007000000002af00000164000000bb00fffffffb00000008005700790063006100000003a5000001000000006000fffffffb0000000800540069006d00650000000484000000bf0000003900ffffff000000010000010f00000506fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000506000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000001fb0000000800540069006d00650100000000000004500000000000000000000006e10000050600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000028100000521fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000521000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e002000320000000324000000b70000000000000000fb0000000c00540065006c0065006f007000000002af00000164000000bb00fffffffb00000008005700790063006100000003a5000001000000006000fffffffb0000000800540069006d00650000000484000000bf0000003900ffffff000000010000010f00000521fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000521000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000001fb0000000800540069006d00650100000000000004500000000000000000000006640000052100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Teleop: @@ -329,8 +331,8 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 2472 + Width: 2560 Wyca: collapsed: false - X: 2648 - Y: 27 + X: 3840 + Y: 0 diff --git a/cartographer_ros/configuration_files/demo_3d.rviz b/cartographer_ros/configuration_files/demo_3d.rviz index e061b6a91..af4523b0e 100644 --- a/cartographer_ros/configuration_files/demo_3d.rviz +++ b/cartographer_ros/configuration_files/demo_3d.rviz @@ -1,76 +1,92 @@ -# Copyright 2016 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. - Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - - /Submaps1 - - /PointCloud23 - - /PointCloud23/Autocompute Value Bounds1 - Splitter Ratio: 0.600671 - Tree Height: 817 - - Class: rviz/Selection + - /RobotModel1/Description Topic1 + - /map1/Status1 + - /map1/Topic1 + - /SubmapsDisplay1/Submaps1 + - /Trajectories1/Namespaces1 + Splitter Ratio: 0.47058823704719543 + Tree Height: 1174 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 + - /2D Goal Pose1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679 - - Class: rviz/Views + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: teleop_panel/Teleop + Name: Teleop + Topic: /rt_cmd_vel + - Class: wyca_rviz_plugin/Wyca + Name: Wyca + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 - SyncSource: PointCloud2 + SyncSource: matched_points Visualization Manager: Class: "" Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description Enabled: true - Line Style: - Line Width: 0.03 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 100 - Reference Frame: + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + horizontal_vlp16_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + vertical_vlp16_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + TF Prefix: "" + Update Interval: 0 Value: true - - Class: rviz/TF + Visual Enabled: true + - Class: rviz_default_plugins/TF Enabled: true Frame Timeout: 15 Frames: - All Enabled: true + All Enabled: false base_link: - Value: true + Value: false horizontal_vlp16_link: Value: true imu_link: @@ -81,7 +97,7 @@ Visualization Manager: Value: true vertical_vlp16_link: Value: true - Marker Scale: 1 + Marker Scale: 0.30000001192092896 Name: TF Show Arrows: true Show Axes: true @@ -98,216 +114,281 @@ Visualization Manager: {} Update Interval: 0 Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - horizontal_vlp16_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - imu_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - vertical_vlp16_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 + Name: map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false Value: true - Visual Enabled: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 7.6265 - Min Value: 5.66667 + Max Value: 10 + Min Value: -10 Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 - Color: 20; 220; 20 + Class: rviz_default_plugins/PointCloud2 + Color: 239; 41; 41 Color Transformer: FlatColor - Decay Time: 0.1 - Enabled: false + Decay Time: 0 + Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 - Name: PointCloud2 + Name: matched_points Position Transformer: XYZ - Queue Size: 200 Selectable: true Size (Pixels): 3 - Size (m): 0.03 + Size (m): 0.10000000149011612 Style: Flat Squares - Topic: /horizontal_laser_3d - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /scan_matched_points2 Use Fixed Frame: true - Use rainbow: true + Use rainbow: false + Value: true + - Class: cartographer_rviz/SubmapsDisplay + Enabled: true + Fade-out distance: 1 + High Resolution: true + Low Resolution: false + Name: SubmapsDisplay + Submap query service: /submap_query + Submaps: + All: true + All Submap Pose Markers: true + Trajectory 0: + 0.320: true + 1.204: true + 2.44: true + Submap Pose Markers: true + Value: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /submap_list + Tracking frame: odom + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Trajectories + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /trajectory_node_list Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Constraints + Namespaces: + Inter constraints, different trajectories: true + Inter constraints, same trajectory: true + Inter residuals, different trajectories: true + Inter residuals, same trajectory: true + Intra constraints: true + Intra residuals: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /constraint_list + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Landmarks + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /landmark_poses_list + Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 5.6087 - Min Value: 3.77875 + Max Value: 10 + Min Value: -10 Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 - Color: 240; 220; 20 - Color Transformer: FlatColor - Decay Time: 0.1 + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 Enabled: false - Invert Rainbow: true + Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 + Max Intensity: 57 Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 + Min Intensity: 1 + Name: horizontal_laser Position Transformer: XYZ - Queue Size: 200 Selectable: true Size (Pixels): 3 - Size (m): 0.03 + Size (m): 0.009999999776482582 Style: Flat Squares - Topic: /vertical_laser_3d - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /horizontal_laser_3d Use Fixed Frame: true Use rainbow: true Value: false - - Class: Submaps - Enabled: true - Name: Submaps - Submap query service: /submap_query - Topic: /submap_list - Tracking frame: base_link - Unreliable: false - Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 - Value: false + Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 - Color: 0; 255; 0 - Color Transformer: AxisColor + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 + Max Intensity: 90 Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 + Min Intensity: 1 + Name: vertical_laser Position Transformer: XYZ - Queue Size: 20 Selectable: true Size (Pixels): 3 - Size (m): 0.05 + Size (m): 0.009999999776482582 Style: Flat Squares - Topic: /scan_matched_points2 - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vertical_laser_3d Use Fixed Frame: true Use rainbow: true - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /trajectory_node_list - Name: Trajectories - Namespaces: - "": true - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /landmark_poses_list - Name: Landmark Poses - Namespaces: - "": true - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /constraint_list - Name: Constraints - Namespaces: - "": true - Queue Size: 100 - Value: true + Value: false Enabled: true Global Options: - Background Color: 100; 100; 100 + Background Color: 48; 48; 48 Fixed Frame: map Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + - Class: nav2_rviz_plugins/GoalTool + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Angle: 0 - Class: rviz/TopDownOrtho + Class: rviz_default_plugins/ThirdPersonFollower + Distance: 59.301063537597656 Enable Stereo Rendering: - Stereo Eye Separation: 0.06 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false + Focal Point: + X: -0.8213143348693848 + Y: -0.22988438606262207 + Z: -0.00012816624075639993 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false Name: Current View - Near Clip Distance: 0.01 - Scale: 10 - Target Frame: - Value: TopDownOrtho (rviz) - X: 0 - Y: 0 + Near Clip Distance: 0.009999999776482582 + Pitch: 1.1647964715957642 + Target Frame: map + Value: ThirdPersonFollower (rviz_default_plugins) + Yaw: 0.3154185116291046 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1123 + Height: 1403 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001c5000003c0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000041000003c0000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003c0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000041000003c0000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077e0000003efc0100000002fb0000000800540069006d006501000000000000077e000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000049e000003c000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001ac00000521fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000521000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e002000320000000324000000b70000000000000000fb0000000c00540065006c0065006f007000000002af00000164000000bb00fffffffb00000008005700790063006100000003a5000001000000006000fffffffb0000000800540069006d00650000000484000000bf0000003900ffffff000000010000010f00000521fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000521000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000001fb0000000800540069006d00650100000000000004500000000000000000000007390000052100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false + Teleop: + collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: false - Width: 1918 - X: 0 - Y: 24 + Width: 2560 + Wyca: + collapsed: false + X: 3840 + Y: 0 diff --git a/cartographer_ros/configuration_files/mir-100-mapping.lua b/cartographer_ros/configuration_files/mir-100-mapping.lua new file mode 100644 index 000000000..1ed5087fe --- /dev/null +++ b/cartographer_ros/configuration_files/mir-100-mapping.lua @@ -0,0 +1,77 @@ +-- Copyright 2018 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. + +include "map_builder.lua" +include "trajectory_builder.lua" + +options = { + map_builder = MAP_BUILDER, + trajectory_builder = TRAJECTORY_BUILDER, + map_frame = "map", + tracking_frame = "imu_frame", + published_frame = "base_link", + odom_frame = "odom", + provide_odom_frame = true, + publish_frame_projected_to_2d = false, + use_odometry = true, + use_pose_extrapolator = true, + use_nav_sat = false, + use_landmarks = true, + num_laser_scans = 2, + num_multi_echo_laser_scans = 0, + num_subdivisions_per_laser_scan = 1, + num_point_clouds = 0, + lookup_transform_timeout_sec = 0.2, + submap_publish_period_sec = 0.3, + pose_publish_period_sec = 5e-3, + trajectory_publish_period_sec = 30e-3, + rangefinder_sampling_ratio = 1., + odometry_sampling_ratio = 1., + fixed_frame_pose_sampling_ratio = 1., + imu_sampling_ratio = 1., + landmarks_sampling_ratio = 1., +} + +MAP_BUILDER.use_trajectory_builder_2d = true +TRAJECTORY_BUILDER.collate_landmarks = false +TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 2 +TRAJECTORY_BUILDER_2D.use_imu_data = true +TRAJECTORY_BUILDER_2D.submaps.num_range_data = 45 + +-- more points +TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_length = 0.2 +TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.min_num_points = 400 +-- slightly slower insertion +TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.hit_probability = 0.53 +TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.miss_probability = 0.493 +-- slightly shorter rays +TRAJECTORY_BUILDER_2D.max_range = 15. +-- wheel odometry is fine +TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 20 +-- IMU is ok +TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 20 + +-- less outliers +POSE_GRAPH.constraint_builder.max_constraint_distance = 5. +POSE_GRAPH.constraint_builder.min_score = 0.5 +-- tune down IMU in optimization +POSE_GRAPH.optimization_problem.acceleration_weight = 0.1 * 1e3 +POSE_GRAPH.optimization_problem.rotation_weight = 0.1 * 3e5 +-- ignore wheels in optimization +POSE_GRAPH.optimization_problem.odometry_translation_weight = 0. +POSE_GRAPH.optimization_problem.odometry_rotation_weight = 0. +POSE_GRAPH.optimization_problem.log_solver_summary = true + +return options + diff --git a/cartographer_ros/launch/backpack_2d.launch b/cartographer_ros/launch/backpack_2d.launch deleted file mode 100644 index e016df4e4..000000000 --- a/cartographer_ros/launch/backpack_2d.launch +++ /dev/null @@ -1,34 +0,0 @@ - - - - - - - - - - - - - diff --git a/cartographer_ros/launch/backpack_2d.launch.py b/cartographer_ros/launch/backpack_2d.launch.py new file mode 100644 index 000000000..7f6f46e8d --- /dev/null +++ b/cartographer_ros/launch/backpack_2d.launch.py @@ -0,0 +1,75 @@ +""" + Copyright 2018 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +import os + +def generate_launch_description(): + + ## ***** Launch arguments ***** + use_sim_time_arg = DeclareLaunchArgument('use_sim_time', default_value = 'False') + + ## ***** File paths ****** + pkg_share = FindPackageShare('cartographer_ros').find('cartographer_ros') + urdf_dir = os.path.join(pkg_share, 'urdf') + urdf_file = os.path.join(urdf_dir, 'backpack_2d.urdf') + with open(urdf_file, 'r') as infp: + robot_desc = infp.read() + + ## ***** Nodes ***** + robot_state_publisher_node = Node( + package = 'robot_state_publisher', + executable = 'robot_state_publisher', + parameters=[ + {'robot_description': robot_desc}, + {'use_sim_time': LaunchConfiguration('use_sim_time')}], + output = 'screen' + ) + + cartographer_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_node', + parameters = [{'use_sim_time': LaunchConfiguration('use_sim_time')}], + arguments = [ + '-configuration_directory', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files', + '-configuration_basename', 'backpack_2d.lua'], + remappings = [ + ('echoes', 'horizontal_laser_2d')], + output = 'screen' + ) + + cartographer_occupancy_grid_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_occupancy_grid_node', + parameters = [ + {'use_sim_time': True}, + {'resolution': 0.05}], + ) + + return LaunchDescription([ + use_sim_time_arg, + # Nodes + robot_state_publisher_node, + cartographer_node, + cartographer_occupancy_grid_node, + ]) diff --git a/cartographer_ros/launch/backpack_3d.launch b/cartographer_ros/launch/backpack_3d.launch deleted file mode 100644 index 52dea599c..000000000 --- a/cartographer_ros/launch/backpack_3d.launch +++ /dev/null @@ -1,35 +0,0 @@ - - - - - - - - - - - - - - diff --git a/cartographer_ros/launch/backpack_3d.launch.py b/cartographer_ros/launch/backpack_3d.launch.py new file mode 100644 index 000000000..7adc48207 --- /dev/null +++ b/cartographer_ros/launch/backpack_3d.launch.py @@ -0,0 +1,76 @@ +""" + Copyright 2018 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +import os + +def generate_launch_description(): + + ## ***** Launch arguments ***** + use_sim_time_arg = DeclareLaunchArgument('use_sim_time', default_value = 'False') + + ## ***** File paths ****** + pkg_share = FindPackageShare('cartographer_ros').find('cartographer_ros') + urdf_dir = os.path.join(pkg_share, 'urdf') + urdf_file = os.path.join(urdf_dir, 'backpack_3d.urdf') + with open(urdf_file, 'r') as infp: + robot_desc = infp.read() + + ## ***** Nodes ***** + robot_state_publisher_node = Node( + package = 'robot_state_publisher', + executable = 'robot_state_publisher', + parameters=[ + {'robot_description': robot_desc}, + {'use_sim_time': LaunchConfiguration('use_sim_time')}], + output = 'screen' + ) + + cartographer_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_node', + parameters = [{'use_sim_time': LaunchConfiguration('use_sim_time')}], + arguments = [ + '-configuration_directory', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files', + '-configuration_basename', 'backpack_3d.lua'], + remappings = [ + ('points2_1', 'horizontal_laser_3d'), + ('points2_2', 'vertical_laser_3d')], + output = 'screen' + ) + + cartographer_occupancy_grid_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_occupancy_grid_node', + parameters = [ + {'use_sim_time': True}, + {'resolution': 0.05}], + ) + + return LaunchDescription([ + use_sim_time_arg, + # Nodes + robot_state_publisher_node, + cartographer_node, + cartographer_occupancy_grid_node, + ]) diff --git a/cartographer_ros/launch/demo_backpack_2d.launch b/cartographer_ros/launch/demo_backpack_2d.launch deleted file mode 100644 index 248e14b05..000000000 --- a/cartographer_ros/launch/demo_backpack_2d.launch +++ /dev/null @@ -1,26 +0,0 @@ - - - - - - - - - - diff --git a/cartographer_ros/launch/demo_backpack_2d.launch.py b/cartographer_ros/launch/demo_backpack_2d.launch.py new file mode 100644 index 000000000..d1d5cd080 --- /dev/null +++ b/cartographer_ros/launch/demo_backpack_2d.launch.py @@ -0,0 +1,57 @@ +""" + Copyright 2018 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, ExecuteProcess +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import Shutdown + +def generate_launch_description(): + ## ***** Launch arguments ***** + bag_filename_arg = DeclareLaunchArgument('bag_filename') + + ## ***** Nodes ***** + backpack_2d_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(FindPackageShare('cartographer_ros').find('cartographer_ros') + '/launch/backpack_2d.launch.py'), + launch_arguments = {'use_sim_time': 'True'}.items() + ) + + rviz_node = Node( + package = 'rviz2', + executable = 'rviz2', + on_exit = Shutdown(), + arguments = ['-d', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files/demo_2d.rviz'], + parameters = [{'use_sim_time': True}], + ) + + ros2_bag_play_cmd = ExecuteProcess( + cmd = ['ros2', 'bag', 'play', LaunchConfiguration('bag_filename'), '--clock'], + name = 'rosbag_play', + ) + + return LaunchDescription([ + # Launch arguments + bag_filename_arg, + # Nodes + backpack_2d_launch, + rviz_node, + ros2_bag_play_cmd + ]) diff --git a/cartographer_ros/launch/demo_backpack_2d_localization.launch b/cartographer_ros/launch/demo_backpack_2d_localization.launch deleted file mode 100644 index e91e99f5a..000000000 --- a/cartographer_ros/launch/demo_backpack_2d_localization.launch +++ /dev/null @@ -1,42 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/cartographer_ros/launch/demo_backpack_2d_localization.launch.py b/cartographer_ros/launch/demo_backpack_2d_localization.launch.py new file mode 100644 index 000000000..be6edd8d2 --- /dev/null +++ b/cartographer_ros/launch/demo_backpack_2d_localization.launch.py @@ -0,0 +1,94 @@ +""" + Copyright 2018 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, ExecuteProcess +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import Shutdown +import os + +def generate_launch_description(): + ## ***** Launch arguments ***** + bag_filename_arg = DeclareLaunchArgument('bag_filename') + load_state_filename_arg = DeclareLaunchArgument('load_state_filename') + + ## ***** File paths ****** + pkg_share = FindPackageShare('cartographer_ros').find('cartographer_ros') + urdf_dir = os.path.join(pkg_share, 'urdf') + urdf_file = os.path.join(urdf_dir, 'backpack_2d.urdf') + with open(urdf_file, 'r') as infp: + robot_desc = infp.read() + + ## ***** Nodes ***** + robot_state_publisher_node = Node( + package = 'robot_state_publisher', + executable = 'robot_state_publisher', + parameters=[ + {'robot_description': robot_desc}, + {'use_sim_time': True}], + output = 'screen' + ) + + cartographer_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_node', + parameters = [{'use_sim_time': True}], + arguments = [ + '-configuration_directory', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files', + '-configuration_basename', 'backpack_2d_localization.lua', + '-load_state_filename', LaunchConfiguration('load_state_filename')], + remappings = [ + ('echoes', 'horizontal_laser_2d')], + output = 'screen' + ) + + cartographer_occupancy_grid_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_occupancy_grid_node', + parameters = [ + {'use_sim_time': True}, + {'resolution': 0.05}], + ) + + rviz_node = Node( + package = 'rviz2', + executable = 'rviz2', + on_exit = Shutdown(), + arguments = ['-d', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files/demo_2d.rviz'], + parameters = [{'use_sim_time': True}], + ) + + ros2_bag_play_cmd = ExecuteProcess( + cmd = ['ros2', 'bag', 'play', LaunchConfiguration('bag_filename'), '--clock'], + name = 'rosbag_play', + ) + + return LaunchDescription([ + # Launch arguments + bag_filename_arg, + load_state_filename_arg, + # Nodes + robot_state_publisher_node, + cartographer_node, + cartographer_occupancy_grid_node, + rviz_node, + ros2_bag_play_cmd + ]) diff --git a/cartographer_ros/launch/demo_backpack_3d.launch b/cartographer_ros/launch/demo_backpack_3d.launch deleted file mode 100644 index f5bfc3a91..000000000 --- a/cartographer_ros/launch/demo_backpack_3d.launch +++ /dev/null @@ -1,26 +0,0 @@ - - - - - - - - - - diff --git a/cartographer_ros/launch/demo_backpack_3d.launch.py b/cartographer_ros/launch/demo_backpack_3d.launch.py new file mode 100644 index 000000000..14594c272 --- /dev/null +++ b/cartographer_ros/launch/demo_backpack_3d.launch.py @@ -0,0 +1,57 @@ +""" + Copyright 2018 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, ExecuteProcess +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import Shutdown + +def generate_launch_description(): + ## ***** Launch arguments ***** + bag_filename_arg = DeclareLaunchArgument('bag_filename') + + ## ***** Nodes ***** + backpack_3d_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(FindPackageShare('cartographer_ros').find('cartographer_ros') + '/launch/backpack_3d.launch.py'), + launch_arguments = {'use_sim_time': 'True'}.items() + ) + + rviz_node = Node( + package = 'rviz2', + executable = 'rviz2', + on_exit = Shutdown(), + arguments = ['-d', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files/demo_3d.rviz'], + parameters = [{'use_sim_time': True}], + ) + + ros2_bag_play_cmd = ExecuteProcess( + cmd = ['ros2', 'bag', 'play', LaunchConfiguration('bag_filename'), '--clock'], + name = 'rosbag_play', + ) + + return LaunchDescription([ + # Launch arguments + bag_filename_arg, + # Nodes + backpack_3d_launch, + rviz_node, + ros2_bag_play_cmd + ]) diff --git a/cartographer_ros/launch/demo_backpack_3d_localization.launch b/cartographer_ros/launch/demo_backpack_3d_localization.launch deleted file mode 100644 index 94304a1bc..000000000 --- a/cartographer_ros/launch/demo_backpack_3d_localization.launch +++ /dev/null @@ -1,43 +0,0 @@ - - - - - - - - - - - - - - - - - - - diff --git a/cartographer_ros/launch/demo_backpack_3d_localization.launch.py b/cartographer_ros/launch/demo_backpack_3d_localization.launch.py new file mode 100644 index 000000000..c23ac5e30 --- /dev/null +++ b/cartographer_ros/launch/demo_backpack_3d_localization.launch.py @@ -0,0 +1,95 @@ +""" + Copyright 2018 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, ExecuteProcess +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import Shutdown +import os + +def generate_launch_description(): + ## ***** Launch arguments ***** + bag_filename_arg = DeclareLaunchArgument('bag_filename') + load_state_filename_arg = DeclareLaunchArgument('load_state_filename') + + ## ***** File paths ****** + pkg_share = FindPackageShare('cartographer_ros').find('cartographer_ros') + urdf_dir = os.path.join(pkg_share, 'urdf') + urdf_file = os.path.join(urdf_dir, 'backpack_3d.urdf') + with open(urdf_file, 'r') as infp: + robot_desc = infp.read() + + ## ***** Nodes ***** + robot_state_publisher_node = Node( + package = 'robot_state_publisher', + executable = 'robot_state_publisher', + parameters=[ + {'robot_description': robot_desc}, + {'use_sim_time': True}], + output = 'screen' + ) + + cartographer_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_node', + parameters = [{'use_sim_time': True}], + arguments = [ + '-configuration_directory', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files', + '-configuration_basename', 'backpack_3d_localization.lua', + '-load_state_filename', LaunchConfiguration('load_state_filename'),], + remappings = [ + ('points2_1', 'horizontal_laser_3d'), + ('points2_2', 'vertical_laser_3d')], + output = 'screen' + ) + + cartographer_occupancy_grid_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_occupancy_grid_node', + parameters = [ + {'use_sim_time': True}, + {'resolution': 0.05}], + ) + + rviz_node = Node( + package = 'rviz2', + executable = 'rviz2', + on_exit = Shutdown(), + arguments = ['-d', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files/demo_3d.rviz'], + parameters = [{'use_sim_time': True}], + ) + + ros2_bag_play_cmd = ExecuteProcess( + cmd = ['ros2', 'bag', 'play', LaunchConfiguration('bag_filename'), '--clock'], + name = 'rosbag_play', + ) + + return LaunchDescription([ + # Launch arguments + bag_filename_arg, + load_state_filename_arg, + # Nodes + robot_state_publisher_node, + cartographer_node, + cartographer_occupancy_grid_node, + rviz_node, + ros2_bag_play_cmd + ]) diff --git a/cartographer_ros/launch/demo_revo_lds.launch b/cartographer_ros/launch/demo_revo_lds.launch deleted file mode 100644 index 387664491..000000000 --- a/cartographer_ros/launch/demo_revo_lds.launch +++ /dev/null @@ -1,35 +0,0 @@ - - - - - - - - - - - - - - diff --git a/cartographer_ros/launch/demo_revo_lds.launch.py b/cartographer_ros/launch/demo_revo_lds.launch.py new file mode 100644 index 000000000..4f88963f1 --- /dev/null +++ b/cartographer_ros/launch/demo_revo_lds.launch.py @@ -0,0 +1,91 @@ +""" + Copyright 2018 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, ExecuteProcess +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import Shutdown +import os + +def generate_launch_description(): + ## ***** Launch arguments ***** + bag_filename_arg = DeclareLaunchArgument('bag_filename') + + ## ***** File paths ****** + pkg_share = FindPackageShare('cartographer_ros').find('cartographer_ros') + urdf_dir = os.path.join(pkg_share, 'urdf') + urdf_file = os.path.join(urdf_dir, 'backpack_2d.urdf') + with open(urdf_file, 'r') as infp: + robot_desc = infp.read() + + ## ***** Nodes ***** + robot_state_publisher_node = Node( + package = 'robot_state_publisher', + executable = 'robot_state_publisher', + parameters=[ + {'robot_description': robot_desc}, + {'use_sim_time': True}], + output = 'screen' + ) + + cartographer_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_node', + parameters = [{'use_sim_time': True}], + arguments = [ + '-configuration_directory', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files', + '-configuration_basename', 'revo_lds.lua'], + remappings = [ + ('scan', 'horizontal_laser_2d')], + output = 'screen' + ) + + cartographer_occupancy_grid_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_occupancy_grid_node', + parameters = [ + {'use_sim_time': True}, + {'resolution': 0.05}], + ) + + rviz_node = Node( + package = 'rviz2', + executable = 'rviz2', + on_exit = Shutdown(), + arguments = ['-d', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files/demo_2d.rviz'], + parameters = [{'use_sim_time': True}], + ) + + ros2_bag_play_cmd = ExecuteProcess( + cmd = ['ros2', 'bag', 'play', LaunchConfiguration('bag_filename'), '--clock'], + name = 'rosbag_play', + ) + + return LaunchDescription([ + # Launch arguments + bag_filename_arg, + # Nodes + robot_state_publisher_node, + cartographer_node, + cartographer_occupancy_grid_node, + rviz_node, + ros2_bag_play_cmd + ]) diff --git a/cartographer_ros/launch/demo_taurob_tracker.launch b/cartographer_ros/launch/demo_taurob_tracker.launch deleted file mode 100644 index 1af7f6583..000000000 --- a/cartographer_ros/launch/demo_taurob_tracker.launch +++ /dev/null @@ -1,26 +0,0 @@ - - - - - - - - - - diff --git a/cartographer_ros/launch/demo_taurob_tracker.launch.py b/cartographer_ros/launch/demo_taurob_tracker.launch.py new file mode 100644 index 000000000..bfd16f5da --- /dev/null +++ b/cartographer_ros/launch/demo_taurob_tracker.launch.py @@ -0,0 +1,57 @@ +""" + Copyright 2018 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, ExecuteProcess +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import Shutdown + +def generate_launch_description(): + ## ***** Launch arguments ***** + bag_filename_arg = DeclareLaunchArgument('bag_filename') + + ## ***** Nodes ***** + taurob_tracker_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(FindPackageShare('cartographer_ros').find('cartographer_ros') + '/launch/taurob_tracker.launch.py'), + launch_arguments = {'use_sim_time': 'True'}.items() + ) + + rviz_node = Node( + package = 'rviz2', + executable = 'rviz2', + on_exit = Shutdown(), + arguments = ['-d', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files/demo_3d.rviz'], + parameters = [{'use_sim_time': True}], + ) + + ros2_bag_play_cmd = ExecuteProcess( + cmd = ['ros2', 'bag', 'play', LaunchConfiguration('bag_filename'), '--clock'], + name = 'rosbag_play', + ) + + return LaunchDescription([ + # Launch arguments + bag_filename_arg, + # Nodes + taurob_tracker_launch, + rviz_node, + ros2_bag_play_cmd + ]) diff --git a/cartographer_ros/launch/offline_backpack_2d.launch.py b/cartographer_ros/launch/offline_backpack_2d.launch.py index e855e28e2..675df16ea 100644 --- a/cartographer_ros/launch/offline_backpack_2d.launch.py +++ b/cartographer_ros/launch/offline_backpack_2d.launch.py @@ -47,9 +47,6 @@ def generate_launch_description(): ) set_remap = SetRemap('horizontal_laser_2d', 'echoes') -# remappings = [ -# ('horizontal_laser_2d', 'echoes')], - return LaunchDescription([ # Launch arguments bag_filenames_arg, diff --git a/cartographer_ros/launch/offline_backpack_3d.launch b/cartographer_ros/launch/offline_backpack_3d.launch deleted file mode 100644 index 8dfa0f836..000000000 --- a/cartographer_ros/launch/offline_backpack_3d.launch +++ /dev/null @@ -1,37 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - diff --git a/cartographer_ros/launch/offline_backpack_3d.launch.py b/cartographer_ros/launch/offline_backpack_3d.launch.py new file mode 100644 index 000000000..4e5d2caae --- /dev/null +++ b/cartographer_ros/launch/offline_backpack_3d.launch.py @@ -0,0 +1,64 @@ +""" + Copyright 2018 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource + +def generate_launch_description(): + + ## ***** Launch arguments ***** + bag_filenames_arg = DeclareLaunchArgument('bag_filenames') + no_rviz_arg = DeclareLaunchArgument('no_rviz', default_value='false') + rviz_config_arg = DeclareLaunchArgument('rviz_config', default_value = FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files/demo_3d.rviz') + configuration_directory_arg = DeclareLaunchArgument('configuration_directory', default_value = FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files') + configuration_basenames_arg = DeclareLaunchArgument('configuration_basenames', default_value = 'backpack_3d.lua') + urdf_filenames_arg = DeclareLaunchArgument('urdf_filenames', default_value = FindPackageShare('cartographer_ros').find('cartographer_ros') + '/urdf/backpack_3d.urdf') + + ## ***** Nodes ***** + offline_node_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(FindPackageShare('cartographer_ros').find('cartographer_ros') + '/launch/offline_node.launch.py'), + launch_arguments = { + 'bag_filenames': LaunchConfiguration('bag_filenames'), + 'no_rviz': LaunchConfiguration('no_rviz'), + 'rviz_config': LaunchConfiguration('rviz_config'), + 'configuration_directory': LaunchConfiguration('configuration_directory'), + 'configuration_basenames': LaunchConfiguration('configuration_basenames'), + 'urdf_filenames': LaunchConfiguration('urdf_filenames')}.items(), + + ) + set_remap1 = SetRemap('horizontal_laser_3d', 'points2_1') + set_remap2 = SetRemap('vertical_laser_3d', 'points2_2') + + return LaunchDescription([ + # Launch arguments + bag_filenames_arg, + no_rviz_arg, + rviz_config_arg, + configuration_directory_arg, + configuration_basenames_arg, + urdf_filenames_arg, + + # Nodes + set_remap1, + set_remap2, + offline_node_launch, + ]) diff --git a/cartographer_ros/launch/offline_mir_100_rviz.launch.py b/cartographer_ros/launch/offline_mir_100_rviz.launch.py new file mode 100644 index 000000000..b8e796c37 --- /dev/null +++ b/cartographer_ros/launch/offline_mir_100_rviz.launch.py @@ -0,0 +1,76 @@ +""" + Copyright 2018 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from os import getenv + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch.actions import Shutdown + +def generate_launch_description(): + + ## ***** Launch arguments ***** + bag_filenames_arg = DeclareLaunchArgument('bag_filename') + no_rviz_arg = DeclareLaunchArgument('no_rviz', default_value = 'False') + keep_running_arg = DeclareLaunchArgument('keep_running', default_value = 'False') + + ## ***** Nodes ***** + rviz_node = Node( + package = 'rviz2', + executable = 'rviz2', + on_exit = Shutdown(), + arguments = ['-d', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files/demo_2d.rviz'], + parameters = [{'use_sim_time': True}], + condition = UnlessCondition(LaunchConfiguration('no_rviz')) + ) + + cartographer_offline_node_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_offline_node', + parameters = [{'use_sim_time': True}], + arguments = [ + '-configuration_directory', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files', + '-configuration_basenames', 'mir-100-mapping.lua', + '-urdf_filenames', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/urdf/mir-100.urdf', + '-use_bag_transforms', 'false', + '-keep_running', LaunchConfiguration('keep_running'), + '-bag_filenames', LaunchConfiguration('bag_filename')], + remappings = [ + ('f_scan', 'scan_1'), + ( 'b_scan', 'scan_2'), + ( 'imu_data','imu'), + ('odom','ignore_odom'), + ('odom_enc','odom')], + output = 'screen' + ) + + + return LaunchDescription([ + # Launch arguments + bag_filenames_arg, + no_rviz_arg, + keep_running_arg, + + # Nodes + rviz_node, + cartographer_offline_node_node, + ]) diff --git a/cartographer_ros/launch/offline_node.launch.py b/cartographer_ros/launch/offline_node.launch.py index bfa64b7b2..106c8ed6e 100644 --- a/cartographer_ros/launch/offline_node.launch.py +++ b/cartographer_ros/launch/offline_node.launch.py @@ -24,6 +24,7 @@ from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare +from launch.actions import Shutdown def generate_launch_description(): @@ -39,6 +40,7 @@ def generate_launch_description(): rviz_node = Node( package = 'rviz2', executable = 'rviz2', + on_exit = Shutdown(), arguments = ['-d', LaunchConfiguration('rviz_config')], parameters = [{'use_sim_time': True}], condition = UnlessCondition(LaunchConfiguration('no_rviz')) @@ -52,7 +54,7 @@ def generate_launch_description(): {'resolution': 0.05}], ) - cartographer_node = Node( + cartographer_offline_node_node = Node( package = 'cartographer_ros', executable = 'cartographer_offline_node', parameters = [{'use_sim_time': True}], @@ -77,5 +79,5 @@ def generate_launch_description(): # Nodes rviz_node, cartographer_occupancy_grid_node, - cartographer_node, + cartographer_offline_node_node, ]) diff --git a/cartographer_ros/launch/taurob_tracker.launch b/cartographer_ros/launch/taurob_tracker.launch deleted file mode 100644 index 4111fa094..000000000 --- a/cartographer_ros/launch/taurob_tracker.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - diff --git a/cartographer_ros/launch/taurob_tracker.launch.py b/cartographer_ros/launch/taurob_tracker.launch.py new file mode 100644 index 000000000..eda55c886 --- /dev/null +++ b/cartographer_ros/launch/taurob_tracker.launch.py @@ -0,0 +1,58 @@ +""" + Copyright 2018 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +import os + +def generate_launch_description(): + + ## ***** Launch arguments ***** + use_sim_time_arg = DeclareLaunchArgument('use_sim_time', default_value = 'False') + + cartographer_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_node', + parameters = [{'use_sim_time': LaunchConfiguration('use_sim_time')}], + arguments = [ + '-configuration_directory', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files', + '-configuration_basename', 'taurob_tracker.lua'], + remappings = [ + ('scan', '/spin_laser/scan'), + ('imu', '/imu/data')], + output = 'screen' + ) + + cartographer_occupancy_grid_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_occupancy_grid_node', + parameters = [ + {'use_sim_time': LaunchConfiguration('use_sim_time')}, + {'resolution': 0.05}], + ) + + return LaunchDescription([ + use_sim_time_arg, + # Nodes + cartographer_node, + cartographer_occupancy_grid_node, + ]) diff --git a/cartographer_ros/urdf/mir-100.urdf b/cartographer_ros/urdf/mir-100.urdf new file mode 100644 index 000000000..1b2e87c83 --- /dev/null +++ b/cartographer_ros/urdf/mir-100.urdf @@ -0,0 +1,107 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 5e0430675ff326336edee7b35a58e6d1716a17e7 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Thu, 27 Jan 2022 23:52:44 +0100 Subject: [PATCH 90/94] fix assets_writer for MultiEchoLaserScan and PointCloud2 Signed-off-by: Guillaume Doisy --- cartographer_ros/cartographer_ros/assets_writer.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cartographer_ros/cartographer_ros/assets_writer.cc b/cartographer_ros/cartographer_ros/assets_writer.cc index 620b8de9a..5f75b4135 100644 --- a/cartographer_ros/cartographer_ros/assets_writer.cc +++ b/cartographer_ros/cartographer_ros/assets_writer.cc @@ -252,7 +252,7 @@ void AssetsWriter::Run(const std::string& configuration_directory, } else if (topic_info.topic_metadata.type == "sensor_msgs/msg/MultiEchoLaserScan") { sensor_msgs::msg::MultiEchoLaserScan::SharedPtr multi_echo_laser_scan_msg = std::make_shared(); - multi_echo_laser_scan_serializer.deserialize_message(&serialized_msg, &multi_echo_laser_scan_msg); + multi_echo_laser_scan_serializer.deserialize_message(&serialized_msg, multi_echo_laser_scan_msg.get()); points_batch = HandleMessage( multi_echo_laser_scan_msg, tracking_frame, tf_buffer, transform_interpolation_buffer); @@ -260,7 +260,7 @@ void AssetsWriter::Run(const std::string& configuration_directory, else if (topic_info.topic_metadata.type == "sensor_msgs/msg/PointCloud2") { sensor_msgs::msg::PointCloud2::SharedPtr pcl2_scan_msg = std::make_shared(); - pcl2_serializer.deserialize_message(&serialized_msg, &pcl2_scan_msg); + pcl2_serializer.deserialize_message(&serialized_msg, pcl2_scan_msg.get()); points_batch = HandleMessage( pcl2_scan_msg, tracking_frame, tf_buffer, transform_interpolation_buffer); From d9daae88d2b540583ff9d9d62867b97e2c2da237 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Fri, 28 Jan 2022 00:53:52 +0100 Subject: [PATCH 91/94] assets writer launch conversion Signed-off-by: Guillaume Doisy --- .../launch/assets_writer_backpack_2d.launch | 28 --------- .../assets_writer_backpack_2d.launch.py | 58 +++++++++++++++++++ .../launch/assets_writer_backpack_3d.launch | 27 --------- .../assets_writer_backpack_3d.launch.py | 54 +++++++++++++++++ .../launch/assets_writer_ros_map.launch | 33 ----------- .../launch/assets_writer_ros_map.launch.py | 54 +++++++++++++++++ .../launch/offline_node.launch.py | 2 - 7 files changed, 166 insertions(+), 90 deletions(-) delete mode 100644 cartographer_ros/launch/assets_writer_backpack_2d.launch create mode 100644 cartographer_ros/launch/assets_writer_backpack_2d.launch.py delete mode 100644 cartographer_ros/launch/assets_writer_backpack_3d.launch create mode 100644 cartographer_ros/launch/assets_writer_backpack_3d.launch.py delete mode 100644 cartographer_ros/launch/assets_writer_ros_map.launch create mode 100644 cartographer_ros/launch/assets_writer_ros_map.launch.py diff --git a/cartographer_ros/launch/assets_writer_backpack_2d.launch b/cartographer_ros/launch/assets_writer_backpack_2d.launch deleted file mode 100644 index 939b6d730..000000000 --- a/cartographer_ros/launch/assets_writer_backpack_2d.launch +++ /dev/null @@ -1,28 +0,0 @@ - - - - - - - diff --git a/cartographer_ros/launch/assets_writer_backpack_2d.launch.py b/cartographer_ros/launch/assets_writer_backpack_2d.launch.py new file mode 100644 index 000000000..ae9eecdab --- /dev/null +++ b/cartographer_ros/launch/assets_writer_backpack_2d.launch.py @@ -0,0 +1,58 @@ +""" + Copyright 2016 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +import os + +def generate_launch_description(): + + ## ***** Launch arguments ***** + configuration_directory_arg = DeclareLaunchArgument('configuration_directory', default_value = FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files') + config_file_arg = DeclareLaunchArgument('config_file', default_value = 'assets_writer_backpack_2d.lua') + urdf_filename_arg = DeclareLaunchArgument('urdf_filename', default_value = FindPackageShare('cartographer_ros').find('cartographer_ros') + '/urdf/backpack_2d.urdf') + bag_filenames_arg = DeclareLaunchArgument('bag_filenames') + pose_graph_filename_arg = DeclareLaunchArgument('pose_graph_filename') + + ## ***** Nodes ***** + cartographer_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_assets_writer', + parameters = [{'use_sim_time': False}], + arguments = [ + '-configuration_directory', LaunchConfiguration('configuration_directory'), + '-configuration_basename', LaunchConfiguration('config_file'), + '-urdf_filename', LaunchConfiguration('urdf_filename'), + '-bag_filenames', LaunchConfiguration('bag_filenames'), + '-pose_graph_filename', LaunchConfiguration('pose_graph_filename')], + output = 'screen' + ) + + return LaunchDescription([ + configuration_directory_arg, + config_file_arg, + urdf_filename_arg, + bag_filenames_arg, + pose_graph_filename_arg, + # Nodes + cartographer_node, + ]) diff --git a/cartographer_ros/launch/assets_writer_backpack_3d.launch b/cartographer_ros/launch/assets_writer_backpack_3d.launch deleted file mode 100644 index 2b3d074ca..000000000 --- a/cartographer_ros/launch/assets_writer_backpack_3d.launch +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - diff --git a/cartographer_ros/launch/assets_writer_backpack_3d.launch.py b/cartographer_ros/launch/assets_writer_backpack_3d.launch.py new file mode 100644 index 000000000..6da91dde8 --- /dev/null +++ b/cartographer_ros/launch/assets_writer_backpack_3d.launch.py @@ -0,0 +1,54 @@ +""" + Copyright 2016 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +import os + +def generate_launch_description(): + + ## ***** Launch arguments ***** + config_file_arg = DeclareLaunchArgument('config_file', default_value = 'assets_writer_backpack_3d.lua') + bag_filenames_arg = DeclareLaunchArgument('bag_filenames') + pose_graph_filename_arg = DeclareLaunchArgument('pose_graph_filename') + + ## ***** Nodes ***** + cartographer_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_assets_writer', + parameters = [{'use_sim_time': False}], + arguments = [ + '-configuration_directory', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files', + '-configuration_basename', LaunchConfiguration('config_file'), + '-urdf_filename', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/urdf/backpack_3d.urdf', + '-bag_filenames', LaunchConfiguration('bag_filenames'), + '-pose_graph_filename', LaunchConfiguration('pose_graph_filename')], + output = 'screen' + ) + + return LaunchDescription([ + config_file_arg, + bag_filenames_arg, + pose_graph_filename_arg, + # Nodes + cartographer_node, + ]) diff --git a/cartographer_ros/launch/assets_writer_ros_map.launch b/cartographer_ros/launch/assets_writer_ros_map.launch deleted file mode 100644 index a5c4dca92..000000000 --- a/cartographer_ros/launch/assets_writer_ros_map.launch +++ /dev/null @@ -1,33 +0,0 @@ - - - - - - - - - - diff --git a/cartographer_ros/launch/assets_writer_ros_map.launch.py b/cartographer_ros/launch/assets_writer_ros_map.launch.py new file mode 100644 index 000000000..a2c7e2b5c --- /dev/null +++ b/cartographer_ros/launch/assets_writer_ros_map.launch.py @@ -0,0 +1,54 @@ +""" + Copyright 2018 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +import os + +def generate_launch_description(): + + ## ***** Launch arguments ***** + urdf_filename_arg = DeclareLaunchArgument('urdf_filename', default_value = FindPackageShare('cartographer_ros').find('cartographer_ros') + '/urdf/backpack_2d.urdf') + bag_filenames_arg = DeclareLaunchArgument('bag_filenames') + pose_graph_filename_arg = DeclareLaunchArgument('pose_graph_filename') + + ## ***** Nodes ***** + cartographer_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_assets_writer', + parameters = [{'use_sim_time': False}], + arguments = [ + '-configuration_directory', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files', + '-configuration_basename', 'assets_writer_ros_map.lua', + '-urdf_filename', LaunchConfiguration('urdf_filename'), + '-bag_filenames', LaunchConfiguration('bag_filenames'), + '-pose_graph_filename', LaunchConfiguration('pose_graph_filename')], + output = 'screen' + ) + + return LaunchDescription([ + urdf_filename_arg, + bag_filenames_arg, + pose_graph_filename_arg, + # Nodes + cartographer_node, + ]) diff --git a/cartographer_ros/launch/offline_node.launch.py b/cartographer_ros/launch/offline_node.launch.py index 106c8ed6e..f38649eee 100644 --- a/cartographer_ros/launch/offline_node.launch.py +++ b/cartographer_ros/launch/offline_node.launch.py @@ -15,8 +15,6 @@ limitations under the License. """ -from os import getenv - from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition, UnlessCondition From 446f5a375fa51d9eb74da7068615a849d10c0d53 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Fri, 28 Jan 2022 01:08:08 +0100 Subject: [PATCH 92/94] args for asserts writer backpack 3d Signed-off-by: Guillaume Doisy --- .../launch/assets_writer_backpack_3d.launch.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/cartographer_ros/launch/assets_writer_backpack_3d.launch.py b/cartographer_ros/launch/assets_writer_backpack_3d.launch.py index 6da91dde8..9ed464683 100644 --- a/cartographer_ros/launch/assets_writer_backpack_3d.launch.py +++ b/cartographer_ros/launch/assets_writer_backpack_3d.launch.py @@ -27,7 +27,9 @@ def generate_launch_description(): ## ***** Launch arguments ***** + configuration_directory_arg = DeclareLaunchArgument('configuration_directory', default_value = FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files') config_file_arg = DeclareLaunchArgument('config_file', default_value = 'assets_writer_backpack_3d.lua') + urdf_filename_arg = DeclareLaunchArgument('urdf_filename', default_value = FindPackageShare('cartographer_ros').find('cartographer_ros') + '/urdf/backpack_3d.urdf') bag_filenames_arg = DeclareLaunchArgument('bag_filenames') pose_graph_filename_arg = DeclareLaunchArgument('pose_graph_filename') @@ -37,16 +39,18 @@ def generate_launch_description(): executable = 'cartographer_assets_writer', parameters = [{'use_sim_time': False}], arguments = [ - '-configuration_directory', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files', + '-configuration_directory', LaunchConfiguration('configuration_directory'), '-configuration_basename', LaunchConfiguration('config_file'), - '-urdf_filename', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/urdf/backpack_3d.urdf', + '-urdf_filename', LaunchConfiguration('urdf_filename'), '-bag_filenames', LaunchConfiguration('bag_filenames'), '-pose_graph_filename', LaunchConfiguration('pose_graph_filename')], output = 'screen' ) return LaunchDescription([ + configuration_directory_arg, config_file_arg, + urdf_filename_arg, bag_filenames_arg, pose_graph_filename_arg, # Nodes From c91cac5f535cc7f680290b76aefb3a07b680ce93 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Tue, 22 Feb 2022 12:18:23 +0100 Subject: [PATCH 93/94] change folder structure to standard ros2 Signed-off-by: Guillaume Doisy --- cartographer_ros/CMakeLists.txt | 248 +++++++++--------- .../cartographer_ros/CMakeLists.txt | 97 ------- .../cartographer_ros/assets_writer.h | 0 .../cartographer_ros/map_builder_bridge.h | 0 .../cartographer_ros/metrics/family_factory.h | 0 .../metrics/internal/counter.h | 0 .../metrics/internal/family.h | 0 .../cartographer_ros/metrics/internal/gauge.h | 0 .../metrics/internal/histogram.h | 0 .../cartographer_ros/msg_conversion.h | 0 .../{ => include}/cartographer_ros/node.h | 0 .../cartographer_ros/node_constants.h | 0 .../cartographer_ros/node_options.h | 0 .../cartographer_ros/offline_node.h | 0 .../cartographer_ros/playable_bag.h | 0 .../cartographer_ros/ros_log_sink.h | 0 .../{ => include}/cartographer_ros/ros_map.h | 0 .../ros_map_writing_points_processor.h | 0 .../cartographer_ros/sensor_bridge.h | 0 .../{ => include}/cartographer_ros/submap.h | 0 .../cartographer_ros/tf_bridge.h | 0 .../cartographer_ros/time_conversion.h | 0 .../cartographer_ros/trajectory_options.h | 0 .../cartographer_ros/urdf_reader.h | 0 .../{cartographer_ros => src}/.clang-format | 0 .../assets_writer.cpp} | 0 .../assets_writer_main.cpp} | 0 .../cartographer_grpc/node_grpc_main.cpp} | 0 .../offline_node_grpc_main.cpp} | 0 .../configuration_files_test.cpp} | 0 .../pbstream_trajectories_to_rosbag_main.cpp} | 0 .../dev/rosbag_publisher_main.cpp} | 0 .../dev/trajectory_comparison_main.cpp} | 0 .../map_builder_bridge.cpp} | 0 .../metrics/family_factory.cpp} | 0 .../metrics/internal/family.cpp} | 0 .../metrics/internal/histogram.cpp} | 0 .../metrics/internal/metrics_test.cpp} | 0 .../msg_conversion.cpp} | 0 .../msg_conversion_test.cpp} | 0 .../node.cc => src/node.cpp} | 0 .../node_constants.cpp} | 0 .../node_main.cc => src/node_main.cpp} | 0 .../node_options.cc => src/node_options.cpp} | 0 .../occupancy_grid_node_main.cpp} | 0 .../offline_node.cc => src/offline_node.cpp} | 0 .../offline_node_main.cpp} | 0 .../pbstream_map_publisher_main.cpp} | 0 .../pbstream_to_ros_map_main.cpp} | 0 .../playable_bag.cc => src/playable_bag.cpp} | 0 .../ros_log_sink.cc => src/ros_log_sink.cpp} | 0 .../ros_map.cc => src/ros_map.cpp} | 0 .../ros_map_writing_points_processor.cpp} | 0 .../rosbag_validate_main.cpp} | 0 .../sensor_bridge.cpp} | 0 .../submap.cc => src/submap.cpp} | 0 .../tf_bridge.cc => src/tf_bridge.cpp} | 0 .../time_conversion.cpp} | 0 .../time_conversion_test.cpp} | 0 .../trajectory_options.cpp} | 0 .../urdf_reader.cc => src/urdf_reader.cpp} | 0 cartographer_rviz/CMakeLists.txt | 128 ++++----- .../cartographer_rviz/drawable_submap.h | 0 .../cartographer_rviz/ogre_slice.h | 0 .../cartographer_rviz/submaps_display.h | 0 .../{cartographer_rviz => src}/.clang-format | 0 .../drawable_submap.cpp} | 0 .../ogre_slice.cc => src/ogre_slice.cpp} | 0 .../submaps_display.cpp} | 0 69 files changed, 184 insertions(+), 289 deletions(-) delete mode 100644 cartographer_ros/cartographer_ros/CMakeLists.txt rename cartographer_ros/{ => include}/cartographer_ros/assets_writer.h (100%) rename cartographer_ros/{ => include}/cartographer_ros/map_builder_bridge.h (100%) rename cartographer_ros/{ => include}/cartographer_ros/metrics/family_factory.h (100%) rename cartographer_ros/{ => include}/cartographer_ros/metrics/internal/counter.h (100%) rename cartographer_ros/{ => include}/cartographer_ros/metrics/internal/family.h (100%) rename cartographer_ros/{ => include}/cartographer_ros/metrics/internal/gauge.h (100%) rename cartographer_ros/{ => include}/cartographer_ros/metrics/internal/histogram.h (100%) rename cartographer_ros/{ => include}/cartographer_ros/msg_conversion.h (100%) rename cartographer_ros/{ => include}/cartographer_ros/node.h (100%) rename cartographer_ros/{ => include}/cartographer_ros/node_constants.h (100%) rename cartographer_ros/{ => include}/cartographer_ros/node_options.h (100%) rename cartographer_ros/{ => include}/cartographer_ros/offline_node.h (100%) rename cartographer_ros/{ => include}/cartographer_ros/playable_bag.h (100%) rename cartographer_ros/{ => include}/cartographer_ros/ros_log_sink.h (100%) rename cartographer_ros/{ => include}/cartographer_ros/ros_map.h (100%) rename cartographer_ros/{ => include}/cartographer_ros/ros_map_writing_points_processor.h (100%) rename cartographer_ros/{ => include}/cartographer_ros/sensor_bridge.h (100%) rename cartographer_ros/{ => include}/cartographer_ros/submap.h (100%) rename cartographer_ros/{ => include}/cartographer_ros/tf_bridge.h (100%) rename cartographer_ros/{ => include}/cartographer_ros/time_conversion.h (100%) rename cartographer_ros/{ => include}/cartographer_ros/trajectory_options.h (100%) rename cartographer_ros/{ => include}/cartographer_ros/urdf_reader.h (100%) rename cartographer_ros/{cartographer_ros => src}/.clang-format (100%) rename cartographer_ros/{cartographer_ros/assets_writer.cc => src/assets_writer.cpp} (100%) rename cartographer_ros/{cartographer_ros/assets_writer_main.cc => src/assets_writer_main.cpp} (100%) rename cartographer_ros/{cartographer_ros/cartographer_grpc/node_grpc_main.cc => src/cartographer_grpc/node_grpc_main.cpp} (100%) rename cartographer_ros/{cartographer_ros/cartographer_grpc/offline_node_grpc_main.cc => src/cartographer_grpc/offline_node_grpc_main.cpp} (100%) rename cartographer_ros/{cartographer_ros/configuration_files_test.cc => src/configuration_files_test.cpp} (100%) rename cartographer_ros/{cartographer_ros/dev/pbstream_trajectories_to_rosbag_main.cc => src/dev/pbstream_trajectories_to_rosbag_main.cpp} (100%) rename cartographer_ros/{cartographer_ros/dev/rosbag_publisher_main.cc => src/dev/rosbag_publisher_main.cpp} (100%) rename cartographer_ros/{cartographer_ros/dev/trajectory_comparison_main.cc => src/dev/trajectory_comparison_main.cpp} (100%) rename cartographer_ros/{cartographer_ros/map_builder_bridge.cc => src/map_builder_bridge.cpp} (100%) rename cartographer_ros/{cartographer_ros/metrics/family_factory.cc => src/metrics/family_factory.cpp} (100%) rename cartographer_ros/{cartographer_ros/metrics/internal/family.cc => src/metrics/internal/family.cpp} (100%) rename cartographer_ros/{cartographer_ros/metrics/internal/histogram.cc => src/metrics/internal/histogram.cpp} (100%) rename cartographer_ros/{cartographer_ros/metrics/internal/metrics_test.cc => src/metrics/internal/metrics_test.cpp} (100%) rename cartographer_ros/{cartographer_ros/msg_conversion.cc => src/msg_conversion.cpp} (100%) rename cartographer_ros/{cartographer_ros/msg_conversion_test.cc => src/msg_conversion_test.cpp} (100%) rename cartographer_ros/{cartographer_ros/node.cc => src/node.cpp} (100%) rename cartographer_ros/{cartographer_ros/node_constants.cc => src/node_constants.cpp} (100%) rename cartographer_ros/{cartographer_ros/node_main.cc => src/node_main.cpp} (100%) rename cartographer_ros/{cartographer_ros/node_options.cc => src/node_options.cpp} (100%) rename cartographer_ros/{cartographer_ros/occupancy_grid_node_main.cc => src/occupancy_grid_node_main.cpp} (100%) rename cartographer_ros/{cartographer_ros/offline_node.cc => src/offline_node.cpp} (100%) rename cartographer_ros/{cartographer_ros/offline_node_main.cc => src/offline_node_main.cpp} (100%) rename cartographer_ros/{cartographer_ros/pbstream_map_publisher_main.cc => src/pbstream_map_publisher_main.cpp} (100%) rename cartographer_ros/{cartographer_ros/pbstream_to_ros_map_main.cc => src/pbstream_to_ros_map_main.cpp} (100%) rename cartographer_ros/{cartographer_ros/playable_bag.cc => src/playable_bag.cpp} (100%) rename cartographer_ros/{cartographer_ros/ros_log_sink.cc => src/ros_log_sink.cpp} (100%) rename cartographer_ros/{cartographer_ros/ros_map.cc => src/ros_map.cpp} (100%) rename cartographer_ros/{cartographer_ros/ros_map_writing_points_processor.cc => src/ros_map_writing_points_processor.cpp} (100%) rename cartographer_ros/{cartographer_ros/rosbag_validate_main.cc => src/rosbag_validate_main.cpp} (100%) rename cartographer_ros/{cartographer_ros/sensor_bridge.cc => src/sensor_bridge.cpp} (100%) rename cartographer_ros/{cartographer_ros/submap.cc => src/submap.cpp} (100%) rename cartographer_ros/{cartographer_ros/tf_bridge.cc => src/tf_bridge.cpp} (100%) rename cartographer_ros/{cartographer_ros/time_conversion.cc => src/time_conversion.cpp} (100%) rename cartographer_ros/{cartographer_ros/time_conversion_test.cc => src/time_conversion_test.cpp} (100%) rename cartographer_ros/{cartographer_ros/trajectory_options.cc => src/trajectory_options.cpp} (100%) rename cartographer_ros/{cartographer_ros/urdf_reader.cc => src/urdf_reader.cpp} (100%) rename cartographer_rviz/{ => include}/cartographer_rviz/drawable_submap.h (100%) rename cartographer_rviz/{ => include}/cartographer_rviz/ogre_slice.h (100%) rename cartographer_rviz/{ => include}/cartographer_rviz/submaps_display.h (100%) rename cartographer_rviz/{cartographer_rviz => src}/.clang-format (100%) rename cartographer_rviz/{cartographer_rviz/drawable_submap.cc => src/drawable_submap.cpp} (100%) rename cartographer_rviz/{cartographer_rviz/ogre_slice.cc => src/ogre_slice.cpp} (100%) rename cartographer_rviz/{cartographer_rviz/submaps_display.cc => src/submaps_display.cpp} (100%) diff --git a/cartographer_ros/CMakeLists.txt b/cartographer_ros/CMakeLists.txt index 24b165dd3..306a660d7 100644 --- a/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/CMakeLists.txt @@ -1,4 +1,5 @@ # Copyright 2016 The Cartographer Authors +# Copyright 2022 Wyca Robotics (for the ROS2 conversion) # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -25,13 +26,9 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra) endif() -find_package(cartographer REQUIRED) -include("${CARTOGRAPHER_CMAKE_DIR}/functions.cmake") -set(BUILD_SHARED_LIBS OFF) -option(BUILD_GRPC "build features that require Cartographer gRPC support" false) - -google_initialize_cartographer_project() +set(CMAKE_POSITION_INDEPENDENT_CODE ON) +find_package(cartographer REQUIRED) find_package(cartographer_ros_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) @@ -47,7 +44,6 @@ find_package(visualization_msgs REQUIRED) find_package(rosbag2_compression REQUIRED) find_package(rosbag2_cpp REQUIRED) find_package(rosbag2_storage REQUIRED) - find_package(absl REQUIRED) find_package(LuaGoogle REQUIRED) find_package(PCL REQUIRED COMPONENTS common) @@ -57,47 +53,43 @@ find_package(urdfdom_headers REQUIRED) include_directories( include - "." + ${LUA_INCLUDE_DIR} + ${PCL_INCLUDE_DIRS} + ${urdfdom_headers_INCLUDE_DIRS} ) -file(GLOB_RECURSE ALL_SRCS "cartographer_ros/*.cc" "cartographer_ros/*.h") -file(GLOB_RECURSE ALL_HEADERS "cartographer_ros/*.h") -file(GLOB_RECURSE ALL_TESTS "cartographer_ros/*_test.cc") -file(GLOB_RECURSE ALL_EXECUTABLES "cartographer_ros/*_main.cc") -file(GLOB_RECURSE ALL_GRPC_FILES "cartographer_ros/cartographer_grpc/*") -list(REMOVE_ITEM ALL_SRCS ${ALL_TESTS}) -list(REMOVE_ITEM ALL_SRCS ${ALL_EXECUTABLES}) -if (NOT ${BUILD_GRPC}) - list(REMOVE_ITEM ALL_SRCS ${ALL_GRPC_FILES}) - list(REMOVE_ITEM ALL_TESTS ${ALL_GRPC_FILES}) - list(REMOVE_ITEM ALL_EXECUTABLES ${ALL_GRPC_FILES}) -endif() - -add_library(${PROJECT_NAME} STATIC ${ALL_SRCS}) -ament_target_dependencies(${PROJECT_NAME} PUBLIC - cartographer - cartographer_ros_msgs - geometry_msgs - nav_msgs - pcl_conversions - rclcpp - sensor_msgs - std_msgs - tf2 - tf2_eigen - tf2_msgs - tf2_ros - visualization_msgs - rosbag2_compression - rosbag2_cpp - rosbag2_storage - urdf - urdfdom_headers -) -add_subdirectory("cartographer_ros") -target_link_libraries(${PROJECT_NAME} PUBLIC cartographer) -ament_export_targets(${PROJECT_NAME}_targets HAS_LIBRARY_TARGET) -ament_export_dependencies( +# Library +add_library(${PROJECT_NAME} + src/assets_writer.cpp + src/assets_writer_main.cpp + src/map_builder_bridge.cpp + src/msg_conversion.cpp + src/node_constants.cpp + src/node.cpp + src/node_main.cpp + src/node_options.cpp + src/occupancy_grid_node_main.cpp + src/offline_node.cpp + src/offline_node_main.cpp + src/pbstream_map_publisher_main.cpp + src/pbstream_to_ros_map_main.cpp + src/playable_bag.cpp + src/rosbag_validate_main.cpp + src/ros_log_sink.cpp + src/ros_map.cpp + src/ros_map_writing_points_processor.cpp + src/sensor_bridge.cpp + src/submap.cpp + src/tf_bridge.cpp + src/time_conversion.cpp + src/trajectory_options.cpp + src/urdf_reader.cpp + src/metrics/family_factory.cpp + src/metrics/internal/family.cpp + src/metrics/internal/histogram.cpp + ) + +set(dependencies cartographer cartographer_ros_msgs geometry_msgs @@ -115,85 +107,107 @@ ament_export_dependencies( rosbag2_cpp rosbag2_storage urdf - urdfdom_headers) - -# Lua -target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${LUA_INCLUDE_DIR}) - -# PCL -target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${PCL_INCLUDE_DIRS}) -target_link_libraries(${PROJECT_NAME} PUBLIC ${PCL_LIBRARIES}) -set(BLACKLISTED_PCL_DEFINITIONS " -march=native -msse4.2 -mfpmath=sse ") -foreach(DEFINITION ${PCL_DEFINITIONS}) - list (FIND BLACKLISTED_PCL_DEFINITIONS "${DEFINITION}" DEFINITIONS_INDEX) - if (${DEFINITIONS_INDEX} GREATER -1) - continue() - endif() - set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${DEFINITION}") -endforeach() - -# Eigen -target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC - "${EIGEN3_INCLUDE_DIR}") - -# Boost not in ros2 -target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC - "${Boost_INCLUDE_DIRS}") -target_link_libraries(${PROJECT_NAME} PUBLIC ${Boost_LIBRARIES}) - -## URDFDOM in ros2 1.0 -target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${urdfdom_headers_INCLUDE_DIRS}) - -# Add the binary directory first, so that port.h is included after it has -# been generated. -target_include_directories(${PROJECT_NAME} PUBLIC - $ - $ - $ + urdfdom ) +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} + ) +target_link_libraries(${PROJECT_NAME} cartographer ${PCL_LIBRARIES}) -set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${GOOG_CXX_FLAGS}") -set_target_properties(${PROJECT_NAME} PROPERTIES - COMPILE_FLAGS ${TARGET_COMPILE_FLAGS}) +# Executables +add_executable(cartographer_node src/node_main.cpp) +target_link_libraries(cartographer_node ${PROJECT_NAME}) +ament_target_dependencies(cartographer_node ${dependencies}) -install(DIRECTORY configuration_files urdf launch - DESTINATION share/${PROJECT_NAME}/ -) +add_executable(cartographer_occupancy_grid_node src/occupancy_grid_node_main.cpp) +target_link_libraries(cartographer_occupancy_grid_node ${PROJECT_NAME}) +ament_target_dependencies(cartographer_occupancy_grid_node ${dependencies}) -install( - FILES ${ALL_HEADERS} - DESTINATION include/${PROJECT_NAME} -) +add_executable(cartographer_offline_node src/offline_node_main.cpp) +target_link_libraries(cartographer_offline_node ${PROJECT_NAME}) +ament_target_dependencies(cartographer_offline_node ${dependencies}) + +add_executable(cartographer_assets_writer src/assets_writer_main.cpp) +target_link_libraries(cartographer_assets_writer ${PROJECT_NAME}) +ament_target_dependencies(cartographer_assets_writer ${dependencies}) + +add_executable(cartographer_pbstream_map_publisher src/pbstream_map_publisher_main.cpp) +target_link_libraries(cartographer_pbstream_map_publisher ${PROJECT_NAME}) +ament_target_dependencies(cartographer_pbstream_map_publisher ${dependencies}) + +add_executable(cartographer_pbstream_to_ros_map src/pbstream_to_ros_map_main.cpp) +target_link_libraries(cartographer_pbstream_to_ros_map ${PROJECT_NAME}) +ament_target_dependencies(cartographer_pbstream_to_ros_map ${dependencies}) + +add_executable(cartographer_rosbag_validate src/rosbag_validate_main.cpp) +target_link_libraries(cartographer_rosbag_validate ${PROJECT_NAME}) +ament_target_dependencies(cartographer_rosbag_validate ${dependencies}) -install( - TARGETS ${PROJECT_NAME} - EXPORT ${PROJECT_NAME}_targets + +install(TARGETS + ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin - INCLUDES DESTINATION include) +) -ament_export_targets(${PROJECT_NAME}_targets HAS_LIBRARY_TARGET) -ament_export_dependencies( - cartographer - cartographer_ros_msgs - geometry_msgs - nav_msgs - pcl_conversions - rclcpp - sensor_msgs - std_msgs - tf2 - tf2_eigen - tf2_msgs - tf2_ros - visualization_msgs - rosbag2_compression - rosbag2_cpp - rosbag2_storage - urdf - urdfdom_headers) -ament_export_libraries(${PROJECT_NAME}) -ament_export_include_directories(include) +install(TARGETS + cartographer_node + cartographer_occupancy_grid_node + cartographer_offline_node + cartographer_assets_writer + cartographer_pbstream_map_publisher + cartographer_pbstream_to_ros_map + cartographer_rosbag_validate + RUNTIME DESTINATION lib/${PROJECT_NAME} +) +install(DIRECTORY include/ + DESTINATION include/ +) + +install(DIRECTORY configuration_files urdf launch + DESTINATION share/${PROJECT_NAME}/ +) + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies(${dependencies}) ament_package() + +# Non converted bin: +#google_binary(cartographer_dev_pbstream_trajectories_to_rosbag +# SRCS +# dev/pbstream_trajectories_to_rosbag_main.cc +#) + +#google_binary(cartographer_dev_rosbag_publisher +# SRCS +# dev/rosbag_publisher_main.cc +#) + + +#google_binary(cartographer_dev_trajectory_comparison +# SRCS +# dev/trajectory_comparison_main.cc +#) + + +## TODO(cschuet): Add support for shared library case. +#if (${BUILD_GRPC}) +# google_binary(cartographer_grpc_node +# SRCS +# cartographer_grpc/node_grpc_main.cc +# ) + + +# google_binary(cartographer_grpc_offline_node +# SRCS +# cartographer_grpc/offline_node_grpc_main.cc +# ) + +# install(PROGRAMS +# ../scripts/cartographer_grpc_server.sh +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) +#endif() diff --git a/cartographer_ros/cartographer_ros/CMakeLists.txt b/cartographer_ros/cartographer_ros/CMakeLists.txt deleted file mode 100644 index 9252e9d0b..000000000 --- a/cartographer_ros/cartographer_ros/CMakeLists.txt +++ /dev/null @@ -1,97 +0,0 @@ -# Copyright 2016 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. - -google_binary(cartographer_node - SRCS - node_main.cc -) - -google_binary(cartographer_occupancy_grid_node - SRCS - occupancy_grid_node_main.cc -) - -google_binary(cartographer_offline_node - SRCS - offline_node_main.cc -) - -google_binary(cartographer_assets_writer - SRCS - assets_writer_main.cc - ros_map_writing_points_processor.h - ros_map_writing_points_processor.cc -) - -google_binary(cartographer_pbstream_map_publisher - SRCS - pbstream_map_publisher_main.cc -) - -google_binary(cartographer_pbstream_to_ros_map - SRCS - pbstream_to_ros_map_main.cc -) - -google_binary(cartographer_rosbag_validate - SRCS - rosbag_validate_main.cc -) - -install(TARGETS - cartographer_node - cartographer_occupancy_grid_node - cartographer_offline_node - cartographer_assets_writer - cartographer_pbstream_map_publisher - cartographer_pbstream_to_ros_map - cartographer_rosbag_validate - DESTINATION lib/${PROJECT_NAME}) - - -#google_binary(cartographer_dev_pbstream_trajectories_to_rosbag -# SRCS -# dev/pbstream_trajectories_to_rosbag_main.cc -#) - -#google_binary(cartographer_dev_rosbag_publisher -# SRCS -# dev/rosbag_publisher_main.cc -#) - - -#google_binary(cartographer_dev_trajectory_comparison -# SRCS -# dev/trajectory_comparison_main.cc -#) - - -## TODO(cschuet): Add support for shared library case. -#if (${BUILD_GRPC}) -# google_binary(cartographer_grpc_node -# SRCS -# cartographer_grpc/node_grpc_main.cc -# ) - - -# google_binary(cartographer_grpc_offline_node -# SRCS -# cartographer_grpc/offline_node_grpc_main.cc -# ) - -# install(PROGRAMS -# ../scripts/cartographer_grpc_server.sh -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) -#endif() diff --git a/cartographer_ros/cartographer_ros/assets_writer.h b/cartographer_ros/include/cartographer_ros/assets_writer.h similarity index 100% rename from cartographer_ros/cartographer_ros/assets_writer.h rename to cartographer_ros/include/cartographer_ros/assets_writer.h diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.h b/cartographer_ros/include/cartographer_ros/map_builder_bridge.h similarity index 100% rename from cartographer_ros/cartographer_ros/map_builder_bridge.h rename to cartographer_ros/include/cartographer_ros/map_builder_bridge.h diff --git a/cartographer_ros/cartographer_ros/metrics/family_factory.h b/cartographer_ros/include/cartographer_ros/metrics/family_factory.h similarity index 100% rename from cartographer_ros/cartographer_ros/metrics/family_factory.h rename to cartographer_ros/include/cartographer_ros/metrics/family_factory.h diff --git a/cartographer_ros/cartographer_ros/metrics/internal/counter.h b/cartographer_ros/include/cartographer_ros/metrics/internal/counter.h similarity index 100% rename from cartographer_ros/cartographer_ros/metrics/internal/counter.h rename to cartographer_ros/include/cartographer_ros/metrics/internal/counter.h diff --git a/cartographer_ros/cartographer_ros/metrics/internal/family.h b/cartographer_ros/include/cartographer_ros/metrics/internal/family.h similarity index 100% rename from cartographer_ros/cartographer_ros/metrics/internal/family.h rename to cartographer_ros/include/cartographer_ros/metrics/internal/family.h diff --git a/cartographer_ros/cartographer_ros/metrics/internal/gauge.h b/cartographer_ros/include/cartographer_ros/metrics/internal/gauge.h similarity index 100% rename from cartographer_ros/cartographer_ros/metrics/internal/gauge.h rename to cartographer_ros/include/cartographer_ros/metrics/internal/gauge.h diff --git a/cartographer_ros/cartographer_ros/metrics/internal/histogram.h b/cartographer_ros/include/cartographer_ros/metrics/internal/histogram.h similarity index 100% rename from cartographer_ros/cartographer_ros/metrics/internal/histogram.h rename to cartographer_ros/include/cartographer_ros/metrics/internal/histogram.h diff --git a/cartographer_ros/cartographer_ros/msg_conversion.h b/cartographer_ros/include/cartographer_ros/msg_conversion.h similarity index 100% rename from cartographer_ros/cartographer_ros/msg_conversion.h rename to cartographer_ros/include/cartographer_ros/msg_conversion.h diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/include/cartographer_ros/node.h similarity index 100% rename from cartographer_ros/cartographer_ros/node.h rename to cartographer_ros/include/cartographer_ros/node.h diff --git a/cartographer_ros/cartographer_ros/node_constants.h b/cartographer_ros/include/cartographer_ros/node_constants.h similarity index 100% rename from cartographer_ros/cartographer_ros/node_constants.h rename to cartographer_ros/include/cartographer_ros/node_constants.h diff --git a/cartographer_ros/cartographer_ros/node_options.h b/cartographer_ros/include/cartographer_ros/node_options.h similarity index 100% rename from cartographer_ros/cartographer_ros/node_options.h rename to cartographer_ros/include/cartographer_ros/node_options.h diff --git a/cartographer_ros/cartographer_ros/offline_node.h b/cartographer_ros/include/cartographer_ros/offline_node.h similarity index 100% rename from cartographer_ros/cartographer_ros/offline_node.h rename to cartographer_ros/include/cartographer_ros/offline_node.h diff --git a/cartographer_ros/cartographer_ros/playable_bag.h b/cartographer_ros/include/cartographer_ros/playable_bag.h similarity index 100% rename from cartographer_ros/cartographer_ros/playable_bag.h rename to cartographer_ros/include/cartographer_ros/playable_bag.h diff --git a/cartographer_ros/cartographer_ros/ros_log_sink.h b/cartographer_ros/include/cartographer_ros/ros_log_sink.h similarity index 100% rename from cartographer_ros/cartographer_ros/ros_log_sink.h rename to cartographer_ros/include/cartographer_ros/ros_log_sink.h diff --git a/cartographer_ros/cartographer_ros/ros_map.h b/cartographer_ros/include/cartographer_ros/ros_map.h similarity index 100% rename from cartographer_ros/cartographer_ros/ros_map.h rename to cartographer_ros/include/cartographer_ros/ros_map.h diff --git a/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h b/cartographer_ros/include/cartographer_ros/ros_map_writing_points_processor.h similarity index 100% rename from cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h rename to cartographer_ros/include/cartographer_ros/ros_map_writing_points_processor.h diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.h b/cartographer_ros/include/cartographer_ros/sensor_bridge.h similarity index 100% rename from cartographer_ros/cartographer_ros/sensor_bridge.h rename to cartographer_ros/include/cartographer_ros/sensor_bridge.h diff --git a/cartographer_ros/cartographer_ros/submap.h b/cartographer_ros/include/cartographer_ros/submap.h similarity index 100% rename from cartographer_ros/cartographer_ros/submap.h rename to cartographer_ros/include/cartographer_ros/submap.h diff --git a/cartographer_ros/cartographer_ros/tf_bridge.h b/cartographer_ros/include/cartographer_ros/tf_bridge.h similarity index 100% rename from cartographer_ros/cartographer_ros/tf_bridge.h rename to cartographer_ros/include/cartographer_ros/tf_bridge.h diff --git a/cartographer_ros/cartographer_ros/time_conversion.h b/cartographer_ros/include/cartographer_ros/time_conversion.h similarity index 100% rename from cartographer_ros/cartographer_ros/time_conversion.h rename to cartographer_ros/include/cartographer_ros/time_conversion.h diff --git a/cartographer_ros/cartographer_ros/trajectory_options.h b/cartographer_ros/include/cartographer_ros/trajectory_options.h similarity index 100% rename from cartographer_ros/cartographer_ros/trajectory_options.h rename to cartographer_ros/include/cartographer_ros/trajectory_options.h diff --git a/cartographer_ros/cartographer_ros/urdf_reader.h b/cartographer_ros/include/cartographer_ros/urdf_reader.h similarity index 100% rename from cartographer_ros/cartographer_ros/urdf_reader.h rename to cartographer_ros/include/cartographer_ros/urdf_reader.h diff --git a/cartographer_ros/cartographer_ros/.clang-format b/cartographer_ros/src/.clang-format similarity index 100% rename from cartographer_ros/cartographer_ros/.clang-format rename to cartographer_ros/src/.clang-format diff --git a/cartographer_ros/cartographer_ros/assets_writer.cc b/cartographer_ros/src/assets_writer.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/assets_writer.cc rename to cartographer_ros/src/assets_writer.cpp diff --git a/cartographer_ros/cartographer_ros/assets_writer_main.cc b/cartographer_ros/src/assets_writer_main.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/assets_writer_main.cc rename to cartographer_ros/src/assets_writer_main.cpp diff --git a/cartographer_ros/cartographer_ros/cartographer_grpc/node_grpc_main.cc b/cartographer_ros/src/cartographer_grpc/node_grpc_main.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/cartographer_grpc/node_grpc_main.cc rename to cartographer_ros/src/cartographer_grpc/node_grpc_main.cpp diff --git a/cartographer_ros/cartographer_ros/cartographer_grpc/offline_node_grpc_main.cc b/cartographer_ros/src/cartographer_grpc/offline_node_grpc_main.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/cartographer_grpc/offline_node_grpc_main.cc rename to cartographer_ros/src/cartographer_grpc/offline_node_grpc_main.cpp diff --git a/cartographer_ros/cartographer_ros/configuration_files_test.cc b/cartographer_ros/src/configuration_files_test.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/configuration_files_test.cc rename to cartographer_ros/src/configuration_files_test.cpp diff --git a/cartographer_ros/cartographer_ros/dev/pbstream_trajectories_to_rosbag_main.cc b/cartographer_ros/src/dev/pbstream_trajectories_to_rosbag_main.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/dev/pbstream_trajectories_to_rosbag_main.cc rename to cartographer_ros/src/dev/pbstream_trajectories_to_rosbag_main.cpp diff --git a/cartographer_ros/cartographer_ros/dev/rosbag_publisher_main.cc b/cartographer_ros/src/dev/rosbag_publisher_main.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/dev/rosbag_publisher_main.cc rename to cartographer_ros/src/dev/rosbag_publisher_main.cpp diff --git a/cartographer_ros/cartographer_ros/dev/trajectory_comparison_main.cc b/cartographer_ros/src/dev/trajectory_comparison_main.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/dev/trajectory_comparison_main.cc rename to cartographer_ros/src/dev/trajectory_comparison_main.cpp diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/src/map_builder_bridge.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/map_builder_bridge.cc rename to cartographer_ros/src/map_builder_bridge.cpp diff --git a/cartographer_ros/cartographer_ros/metrics/family_factory.cc b/cartographer_ros/src/metrics/family_factory.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/metrics/family_factory.cc rename to cartographer_ros/src/metrics/family_factory.cpp diff --git a/cartographer_ros/cartographer_ros/metrics/internal/family.cc b/cartographer_ros/src/metrics/internal/family.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/metrics/internal/family.cc rename to cartographer_ros/src/metrics/internal/family.cpp diff --git a/cartographer_ros/cartographer_ros/metrics/internal/histogram.cc b/cartographer_ros/src/metrics/internal/histogram.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/metrics/internal/histogram.cc rename to cartographer_ros/src/metrics/internal/histogram.cpp diff --git a/cartographer_ros/cartographer_ros/metrics/internal/metrics_test.cc b/cartographer_ros/src/metrics/internal/metrics_test.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/metrics/internal/metrics_test.cc rename to cartographer_ros/src/metrics/internal/metrics_test.cpp diff --git a/cartographer_ros/cartographer_ros/msg_conversion.cc b/cartographer_ros/src/msg_conversion.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/msg_conversion.cc rename to cartographer_ros/src/msg_conversion.cpp diff --git a/cartographer_ros/cartographer_ros/msg_conversion_test.cc b/cartographer_ros/src/msg_conversion_test.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/msg_conversion_test.cc rename to cartographer_ros/src/msg_conversion_test.cpp diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/src/node.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/node.cc rename to cartographer_ros/src/node.cpp diff --git a/cartographer_ros/cartographer_ros/node_constants.cc b/cartographer_ros/src/node_constants.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/node_constants.cc rename to cartographer_ros/src/node_constants.cpp diff --git a/cartographer_ros/cartographer_ros/node_main.cc b/cartographer_ros/src/node_main.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/node_main.cc rename to cartographer_ros/src/node_main.cpp diff --git a/cartographer_ros/cartographer_ros/node_options.cc b/cartographer_ros/src/node_options.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/node_options.cc rename to cartographer_ros/src/node_options.cpp diff --git a/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc b/cartographer_ros/src/occupancy_grid_node_main.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc rename to cartographer_ros/src/occupancy_grid_node_main.cpp diff --git a/cartographer_ros/cartographer_ros/offline_node.cc b/cartographer_ros/src/offline_node.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/offline_node.cc rename to cartographer_ros/src/offline_node.cpp diff --git a/cartographer_ros/cartographer_ros/offline_node_main.cc b/cartographer_ros/src/offline_node_main.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/offline_node_main.cc rename to cartographer_ros/src/offline_node_main.cpp diff --git a/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc b/cartographer_ros/src/pbstream_map_publisher_main.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc rename to cartographer_ros/src/pbstream_map_publisher_main.cpp diff --git a/cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc b/cartographer_ros/src/pbstream_to_ros_map_main.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc rename to cartographer_ros/src/pbstream_to_ros_map_main.cpp diff --git a/cartographer_ros/cartographer_ros/playable_bag.cc b/cartographer_ros/src/playable_bag.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/playable_bag.cc rename to cartographer_ros/src/playable_bag.cpp diff --git a/cartographer_ros/cartographer_ros/ros_log_sink.cc b/cartographer_ros/src/ros_log_sink.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/ros_log_sink.cc rename to cartographer_ros/src/ros_log_sink.cpp diff --git a/cartographer_ros/cartographer_ros/ros_map.cc b/cartographer_ros/src/ros_map.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/ros_map.cc rename to cartographer_ros/src/ros_map.cpp diff --git a/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc b/cartographer_ros/src/ros_map_writing_points_processor.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc rename to cartographer_ros/src/ros_map_writing_points_processor.cpp diff --git a/cartographer_ros/cartographer_ros/rosbag_validate_main.cc b/cartographer_ros/src/rosbag_validate_main.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/rosbag_validate_main.cc rename to cartographer_ros/src/rosbag_validate_main.cpp diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.cc b/cartographer_ros/src/sensor_bridge.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/sensor_bridge.cc rename to cartographer_ros/src/sensor_bridge.cpp diff --git a/cartographer_ros/cartographer_ros/submap.cc b/cartographer_ros/src/submap.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/submap.cc rename to cartographer_ros/src/submap.cpp diff --git a/cartographer_ros/cartographer_ros/tf_bridge.cc b/cartographer_ros/src/tf_bridge.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/tf_bridge.cc rename to cartographer_ros/src/tf_bridge.cpp diff --git a/cartographer_ros/cartographer_ros/time_conversion.cc b/cartographer_ros/src/time_conversion.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/time_conversion.cc rename to cartographer_ros/src/time_conversion.cpp diff --git a/cartographer_ros/cartographer_ros/time_conversion_test.cc b/cartographer_ros/src/time_conversion_test.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/time_conversion_test.cc rename to cartographer_ros/src/time_conversion_test.cpp diff --git a/cartographer_ros/cartographer_ros/trajectory_options.cc b/cartographer_ros/src/trajectory_options.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/trajectory_options.cc rename to cartographer_ros/src/trajectory_options.cpp diff --git a/cartographer_ros/cartographer_ros/urdf_reader.cc b/cartographer_ros/src/urdf_reader.cpp similarity index 100% rename from cartographer_ros/cartographer_ros/urdf_reader.cc rename to cartographer_ros/src/urdf_reader.cpp diff --git a/cartographer_rviz/CMakeLists.txt b/cartographer_rviz/CMakeLists.txt index c44454afb..957e855f1 100644 --- a/cartographer_rviz/CMakeLists.txt +++ b/cartographer_rviz/CMakeLists.txt @@ -1,4 +1,5 @@ # Copyright 2016 The Cartographer Authors +# Copyright 2022 Wyca Robotics (for the ROS2 conversion) # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -26,6 +27,8 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra) endif() +set(CMAKE_AUTOMOC ON) + find_package(Boost REQUIRED COMPONENTS system iostreams) find_package(cartographer REQUIRED) include("${CARTOGRAPHER_CMAKE_DIR}/functions.cmake") @@ -48,26 +51,24 @@ set(CMAKE_THREAD_PREFER_PTHREAD TRUE) set(THREADS_PREFER_PTHREAD_FLAG TRUE) find_package(Threads REQUIRED) +set(rviz_plugins_headers_to_moc + include/cartographer_rviz/drawable_submap.h + include/cartographer_rviz/ogre_slice.h + include/cartographer_rviz/submaps_display.h +) + include_directories( include - ".") - -file(GLOB_RECURSE ALL_SRCS "cartographer_rviz/*.cc" "cartographer_rviz/*.h") -set(CMAKE_AUTOMOC ON) - -add_library(${PROJECT_NAME} SHARED ${ALL_SRCS}) - -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDING_LIBRARY") - -# prevent pluginlib from using boost -target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + ) -pluginlib_export_plugin_description_file(rviz_common rviz_plugin_description.xml) -register_rviz_ogre_media_exports(DIRECTORIES "ogre_media/materials/glsl120" "ogre_media/materials/scripts") +add_library(${PROJECT_NAME} SHARED + src/drawable_submap.cpp + src/ogre_slice.cpp + src/submaps_display.cpp + ${rviz_plugins_headers_to_moc} + ) -ament_target_dependencies(${PROJECT_NAME} PUBLIC +set(dependencies rclcpp cartographer cartographer_ros @@ -76,81 +77,58 @@ ament_target_dependencies(${PROJECT_NAME} PUBLIC rviz_common pcl_conversions rcutils + ) + +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} ) -target_link_libraries(${PROJECT_NAME} PUBLIC ${PROTOBUF_LIBRARY} - ${cartographer_ros_LIBRARIES} - -lstdc++fs - -ldl - -lboost_thread - ) -ament_export_include_directories(include) -ament_export_dependencies(rosidl_default_runtime) - -# PCL -target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${PCL_INCLUDE_DIRS}) -target_link_libraries(${PROJECT_NAME} PUBLIC ${PCL_LIBRARIES}) -set(BLACKLISTED_PCL_DEFINITIONS " -march=native -msse4.2 -mfpmath=sse ") -foreach(DEFINITION ${PCL_DEFINITIONS}) - list (FIND BLACKLISTED_PCL_DEFINITIONS "${DEFINITION}" DEFINITIONS_INDEX) - if (${DEFINITIONS_INDEX} GREATER -1) - continue() - endif() - set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${DEFINITION}") -endforeach() - -message(STATUS "Using Qt5 based on the rviz2_QT_VERSION: ${rviz2_QT_VERSION}") -find_package(Qt5 ${rviz2_QT_VERSION} REQUIRED Core Widgets) -set(QT_LIBRARIES Qt5::Core Qt5::Widgets) -include_directories(${Qt5Widgets_INCLUDE_DIRS}) - -add_definitions(-DQT_NO_KEYWORDS) -target_link_libraries(${PROJECT_NAME} PUBLIC ${QT_LIBRARIES}) - -# Add the binary directory first, so that port.h is included after it has -# been generated. target_include_directories(${PROJECT_NAME} PUBLIC - $ - $ - $ + ${PCL_INCLUDE_DIRS} + ${Qt5Widgets_INCLUDE_DIRS} + ${OGRE_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} + ${Boost_INCLUDE_DIRS} ) -target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC - "${EIGEN3_INCLUDE_DIR}") -target_link_libraries(${PROJECT_NAME} PUBLIC ${EIGEN3_LIBRARIES}) - -target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC - "${Boost_INCLUDE_DIRS}") -target_link_libraries(${PROJECT_NAME} PUBLIC ${Boost_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} + cartographer + ${PCL_LIBRARIES} + ${QT_LIBRARIES} + ${EIGEN3_LIBRARIES} + ${Boost_LIBRARIES} + rviz_common::rviz_common + ) -set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${GOOG_CXX_FLAGS}") -set_target_properties(${PROJECT_NAME} PROPERTIES - COMPILE_FLAGS ${TARGET_COMPILE_FLAGS}) -target_link_libraries(${PROJECT_NAME} PUBLIC cartographer) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDING_LIBRARY") -install(FILES - rviz_plugin_description.xml - DESTINATION share/${PROJECT_NAME}) +# prevent pluginlib from using boost +target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") -install(DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/ogre_media" - DESTINATION "share/${PROJECT_NAME}" - ) +pluginlib_export_plugin_description_file(rviz_common rviz_plugin_description.xml) +register_rviz_ogre_media_exports(DIRECTORIES "ogre_media/materials/glsl120" "ogre_media/materials/scripts") install( TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) -# replaces catkin_package(LIBRARIES ${PROJECT_NAME}) -ament_export_libraries(${PROJECT_NAME}) +install( + DIRECTORY include/ + DESTINATION include/ +) -#LD_LIBRARY_PATH() -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() +ament_export_include_directories(include) +ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ${dependencies} +) ament_package() diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.h b/cartographer_rviz/include/cartographer_rviz/drawable_submap.h similarity index 100% rename from cartographer_rviz/cartographer_rviz/drawable_submap.h rename to cartographer_rviz/include/cartographer_rviz/drawable_submap.h diff --git a/cartographer_rviz/cartographer_rviz/ogre_slice.h b/cartographer_rviz/include/cartographer_rviz/ogre_slice.h similarity index 100% rename from cartographer_rviz/cartographer_rviz/ogre_slice.h rename to cartographer_rviz/include/cartographer_rviz/ogre_slice.h diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.h b/cartographer_rviz/include/cartographer_rviz/submaps_display.h similarity index 100% rename from cartographer_rviz/cartographer_rviz/submaps_display.h rename to cartographer_rviz/include/cartographer_rviz/submaps_display.h diff --git a/cartographer_rviz/cartographer_rviz/.clang-format b/cartographer_rviz/src/.clang-format similarity index 100% rename from cartographer_rviz/cartographer_rviz/.clang-format rename to cartographer_rviz/src/.clang-format diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/src/drawable_submap.cpp similarity index 100% rename from cartographer_rviz/cartographer_rviz/drawable_submap.cc rename to cartographer_rviz/src/drawable_submap.cpp diff --git a/cartographer_rviz/cartographer_rviz/ogre_slice.cc b/cartographer_rviz/src/ogre_slice.cpp similarity index 100% rename from cartographer_rviz/cartographer_rviz/ogre_slice.cc rename to cartographer_rviz/src/ogre_slice.cpp diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/src/submaps_display.cpp similarity index 100% rename from cartographer_rviz/cartographer_rviz/submaps_display.cc rename to cartographer_rviz/src/submaps_display.cpp From 061b6bd38c55bcdc980edeb442c9552080b580b8 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Mon, 7 Mar 2022 16:09:13 +0100 Subject: [PATCH 94/94] visualize pbstream launch Signed-off-by: Guillaume Doisy --- .../configuration_files/demo_2d.rviz | 237 +++++++++++++----- .../launch/visualize_pbstream.launch | 29 --- .../launch/visualize_pbstream.launch.py | 57 +++++ 3 files changed, 236 insertions(+), 87 deletions(-) delete mode 100644 cartographer_ros/launch/visualize_pbstream.launch create mode 100644 cartographer_ros/launch/visualize_pbstream.launch.py diff --git a/cartographer_ros/configuration_files/demo_2d.rviz b/cartographer_ros/configuration_files/demo_2d.rviz index 6db01920c..92e4708b9 100644 --- a/cartographer_ros/configuration_files/demo_2d.rviz +++ b/cartographer_ros/configuration_files/demo_2d.rviz @@ -5,14 +5,12 @@ Panels: Property Tree Widget: Expanded: - /Global Options1 - - /RobotModel1/Status1 - /RobotModel1/Description Topic1 - /TF1/Tree1 - - /TF1/Tree1/map1 - - /TF1/Tree1/map1/odom1 - /map1/Topic1 - /SubmapsDisplay1/Submaps1 - /Trajectories1/Namespaces1 + - /Constraints1/Namespaces1 Splitter Ratio: 0.42203986644744873 Tree Height: 1174 - Class: rviz_common/Selection @@ -37,7 +35,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: PointCloud2 + SyncSource: "" Visualization Manager: Class: "" Displays: @@ -59,25 +57,6 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - horizontal_laser_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - imu_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - vertical_laser_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true Name: RobotModel TF Prefix: "" Update Interval: 0 @@ -88,28 +67,13 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: false - base_link: - Value: false - horizontal_laser_link: - Value: true - imu_link: - Value: true - map: - Value: true - odom: - Value: true - vertical_laser_link: - Value: true Marker Scale: 0.30000001192092896 Name: TF Show Arrows: true Show Axes: true Show Names: true Tree: - map: - odom: - horizontal_laser_link: - {} + {} Update Interval: 0 Value: true - Alpha: 0.699999988079071 @@ -178,23 +142,180 @@ Visualization Manager: All: true All Submap Pose Markers: true Trajectory 0: - 0.70: true - 1.70: true - 10.70: true - 11.70: true - 12.70: true - 13.70: true - 14.70: true - 15.68: true - 16.33: true - 2.70: true - 3.70: true - 4.70: true - 5.70: true - 6.70: true - 7.70: true - 8.70: true - 9.70: true + 0.60: true + 1.60: true + 10.60: true + 100.60: true + 101.60: true + 102.60: true + 103.60: true + 104.60: true + 105.60: true + 106.60: true + 107.60: true + 108.60: true + 109.60: true + 11.60: true + 110.60: true + 111.60: true + 112.60: true + 113.60: true + 114.60: true + 115.60: true + 116.60: true + 117.60: true + 118.60: true + 119.60: true + 12.60: true + 120.60: true + 121.60: true + 122.60: true + 123.60: true + 124.60: true + 125.60: true + 126.60: true + 127.60: true + 128.60: true + 129.60: true + 13.60: true + 130.60: true + 131.60: true + 132.60: true + 133.60: true + 134.60: true + 135.60: true + 136.60: true + 137.60: true + 138.60: true + 139.60: true + 14.60: true + 140.60: true + 141.60: true + 142.60: true + 143.60: true + 144.60: true + 145.60: true + 146.60: true + 147.60: true + 148.60: true + 149.60: true + 15.60: true + 150.60: true + 151.60: true + 152.60: true + 153.60: true + 154.60: true + 155.60: true + 156.60: true + 157.60: true + 158.60: true + 159.60: true + 16.60: true + 160.60: true + 161.60: true + 162.60: true + 163.60: true + 164.60: true + 165.60: true + 166.60: true + 167.60: true + 168.60: true + 169.60: true + 17.60: true + 170.60: true + 171.60: true + 172.43: true + 173.13: true + 18.60: true + 19.60: true + 2.60: true + 20.60: true + 21.60: true + 22.60: true + 23.60: true + 24.60: true + 25.60: true + 26.60: true + 27.60: true + 28.60: true + 29.60: true + 3.60: true + 30.60: true + 31.60: true + 32.60: true + 33.60: true + 34.60: true + 35.60: true + 36.60: true + 37.60: true + 38.60: true + 39.60: true + 4.60: true + 40.60: true + 41.60: true + 42.60: true + 43.60: true + 44.60: true + 45.60: true + 46.60: true + 47.60: true + 48.60: true + 49.60: true + 5.60: true + 50.60: true + 51.60: true + 52.60: true + 53.60: true + 54.60: true + 55.60: true + 56.60: true + 57.60: true + 58.60: true + 59.60: true + 6.60: true + 60.60: true + 61.60: true + 62.60: true + 63.60: true + 64.60: true + 65.60: true + 66.60: true + 67.60: true + 68.60: true + 69.60: true + 7.60: true + 70.60: true + 71.60: true + 72.60: true + 73.60: true + 74.60: true + 75.60: true + 76.60: true + 77.60: true + 78.60: true + 79.60: true + 8.60: true + 80.60: true + 81.60: true + 82.60: true + 83.60: true + 84.60: true + 85.60: true + 86.60: true + 87.60: true + 88.60: true + 89.60: true + 9.60: true + 90.60: true + 91.60: true + 92.60: true + 93.60: true + 94.60: true + 95.60: true + 96.60: true + 97.60: true + 98.60: true + 99.60: true Submap Pose Markers: true Value: true Topic: @@ -204,7 +325,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /submap_list - Tracking frame: odom + Tracking frame: map Value: true - Class: rviz_default_plugins/MarkerArray Enabled: false @@ -294,7 +415,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/ThirdPersonFollower - Distance: 29.796886444091797 + Distance: 204.58663940429688 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 diff --git a/cartographer_ros/launch/visualize_pbstream.launch b/cartographer_ros/launch/visualize_pbstream.launch deleted file mode 100644 index cf5b7e034..000000000 --- a/cartographer_ros/launch/visualize_pbstream.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - diff --git a/cartographer_ros/launch/visualize_pbstream.launch.py b/cartographer_ros/launch/visualize_pbstream.launch.py new file mode 100644 index 000000000..859c3587a --- /dev/null +++ b/cartographer_ros/launch/visualize_pbstream.launch.py @@ -0,0 +1,57 @@ +""" + Copyright 2018 The Cartographer Authors + Copyright 2022 Wyca Robotics (for the ros2 conversion) + + 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. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, ExecuteProcess +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import Shutdown + +def generate_launch_description(): + ## ***** Launch arguments ***** + pbstream_filename_arg = DeclareLaunchArgument('pbstream_filename') + + ## ***** Nodes ***** + rviz_node = Node( + package = 'rviz2', + executable = 'rviz2', + on_exit = Shutdown(), + arguments = ['-d', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files/demo_2d.rviz'], + ) + + cartographer_node = Node( + package = 'cartographer_ros', + executable = 'cartographer_node', + arguments = [ + '-configuration_directory', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files', + '-configuration_basename', 'visualize_pbstream.lua', + '-load_state_filename', LaunchConfiguration('pbstream_filename'), + '-load_frozen_state=false', + '-start_trajectory_with_default_topics=false'], + output = 'screen' + ) + + return LaunchDescription([ + # Launch arguments + pbstream_filename_arg, + # Nodes + rviz_node, + cartographer_node + ])