From 07806f1152f7b624654ff7e3ee6470f408fb3ac5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Thu, 8 Jun 2017 13:55:15 +0200 Subject: [PATCH] Trajectory visualization (#360) Based on code originally by @domeinzer. --- cartographer_ros/CMakeLists.txt | 1 + .../cartographer_ros/map_builder_bridge.cc | 26 +++++++++++++++++++ .../cartographer_ros/map_builder_bridge.h | 2 ++ .../cartographer_ros/msg_conversion.cc | 12 ++++++--- .../cartographer_ros/msg_conversion.h | 2 ++ cartographer_ros/cartographer_ros/node.cc | 16 ++++++++++++ cartographer_ros/cartographer_ros/node.h | 3 +++ .../cartographer_ros/node_options.cc | 2 ++ .../cartographer_ros/node_options.h | 1 + .../configuration_files/backpack_2d.lua | 1 + .../configuration_files/backpack_3d.lua | 1 + .../configuration_files/demo_2d.rviz | 8 ++++++ .../configuration_files/demo_3d.rviz | 8 ++++++ cartographer_ros/configuration_files/pr2.lua | 1 + .../configuration_files/revo_lds.lua | 1 + .../configuration_files/taurob_tracker.lua | 1 + cartographer_ros/package.xml | 1 + docs/source/configuration.rst | 4 +++ 18 files changed, 88 insertions(+), 3 deletions(-) diff --git a/cartographer_ros/CMakeLists.txt b/cartographer_ros/CMakeLists.txt index 5cfdf87..f7a4cd9 100644 --- a/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/CMakeLists.txt @@ -32,6 +32,7 @@ set(PACKAGE_DEPENDENCIES tf2_eigen tf2_ros urdf + visualization_msgs ) find_package(cartographer REQUIRED) diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index 47a28ec..88719dc 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -23,6 +23,8 @@ namespace cartographer_ros { +constexpr double kTrajectoryLineStripMarkerScale = 0.07; + MapBuilderBridge::MapBuilderBridge(const NodeOptions& node_options, tf2_ros::Buffer* const tf_buffer) : node_options_(node_options), @@ -192,6 +194,30 @@ MapBuilderBridge::GetTrajectoryStates() { return trajectory_states; } +visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodesList() { + visualization_msgs::MarkerArray trajectory_nodes_list; + const auto trajectory_nodes = + map_builder_.sparse_pose_graph()->GetTrajectoryNodes(); + int marker_id = 0; + for (const auto& single_trajectory : trajectory_nodes) { + visualization_msgs::Marker marker; + marker.id = marker_id++; + marker.type = visualization_msgs::Marker::LINE_STRIP; + marker.header.stamp = ::ros::Time::now(); + marker.header.frame_id = node_options_.map_frame; + marker.color.b = 1.0; + marker.color.a = 1.0; + marker.scale.x = kTrajectoryLineStripMarkerScale; + marker.pose.orientation.w = 1.0; + for (const auto& node : single_trajectory) { + marker.points.push_back(ToGeometryMsgPoint( + (node.pose * node.constant_data->tracking_to_pose).translation())); + } + trajectory_nodes_list.markers.push_back(marker); + } + return trajectory_nodes_list; +} + SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) { return sensor_bridges_.at(trajectory_id).get(); } diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.h b/cartographer_ros/cartographer_ros/map_builder_bridge.h index 36d0687..cc7d019 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.h +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.h @@ -31,6 +31,7 @@ #include "cartographer_ros_msgs/SubmapList.h" #include "cartographer_ros_msgs/SubmapQuery.h" #include "nav_msgs/OccupancyGrid.h" +#include "visualization_msgs/MarkerArray.h" namespace cartographer_ros { @@ -60,6 +61,7 @@ class MapBuilderBridge { cartographer_ros_msgs::SubmapList GetSubmapList(); std::unique_ptr BuildOccupancyGrid(); std::unordered_map GetTrajectoryStates(); + visualization_msgs::MarkerArray GetTrajectoryNodesList(); SensorBridge* sensor_bridge(int trajectory_id); diff --git a/cartographer_ros/cartographer_ros/msg_conversion.cc b/cartographer_ros/cartographer_ros/msg_conversion.cc index 7720e5a..85b7857 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion.cc +++ b/cartographer_ros/cartographer_ros/msg_conversion.cc @@ -225,9 +225,7 @@ geometry_msgs::Transform ToGeometryMsgTransform(const Rigid3d& rigid3d) { geometry_msgs::Pose ToGeometryMsgPose(const Rigid3d& rigid3d) { geometry_msgs::Pose pose; - pose.position.x = rigid3d.translation().x(); - pose.position.y = rigid3d.translation().y(); - pose.position.z = rigid3d.translation().z(); + pose.position = ToGeometryMsgPoint(rigid3d.translation()); pose.orientation.w = rigid3d.rotation().w(); pose.orientation.x = rigid3d.rotation().x(); pose.orientation.y = rigid3d.rotation().y(); @@ -235,4 +233,12 @@ geometry_msgs::Pose ToGeometryMsgPose(const Rigid3d& rigid3d) { return pose; } +geometry_msgs::Point ToGeometryMsgPoint(const Eigen::Vector3d& vector3d) { + geometry_msgs::Point point; + point.x = vector3d.x(); + point.y = vector3d.y(); + point.z = vector3d.z(); + return point; +} + } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/msg_conversion.h b/cartographer_ros/cartographer_ros/msg_conversion.h index c7dae2c..456b024 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion.h +++ b/cartographer_ros/cartographer_ros/msg_conversion.h @@ -44,6 +44,8 @@ geometry_msgs::Transform ToGeometryMsgTransform( geometry_msgs::Pose ToGeometryMsgPose( const ::cartographer::transform::Rigid3d& rigid3d); +geometry_msgs::Point ToGeometryMsgPoint(const Eigen::Vector3d& vector3d); + ::cartographer::sensor::PointCloudWithIntensities ToPointCloudWithIntensities( const sensor_msgs::LaserScan& msg); diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 4311a9d..5f8244c 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -40,6 +40,7 @@ #include "ros/serialization.h" #include "sensor_msgs/PointCloud2.h" #include "tf2_eigen/tf2_eigen.h" +#include "visualization_msgs/MarkerArray.h" namespace cartographer_ros { @@ -103,6 +104,9 @@ Node::Node(const NodeOptions& node_options, tf2_ros::Buffer* const tf_buffer) submap_list_publisher_ = node_handle_.advertise<::cartographer_ros_msgs::SubmapList>( kSubmapListTopic, kLatestOnlyPublisherQueueSize); + trajectory_nodes_list_publisher_ = + node_handle_.advertise<::visualization_msgs::MarkerArray>( + kTrajectoryNodesListTopic, kLatestOnlyPublisherQueueSize); service_servers_.push_back(node_handle_.advertiseService( kSubmapQueryServiceName, &Node::HandleSubmapQuery, this)); service_servers_.push_back(node_handle_.advertiseService( @@ -131,6 +135,9 @@ Node::Node(const NodeOptions& node_options, tf2_ros::Buffer* const tf_buffer) wall_timers_.push_back(node_handle_.createWallTimer( ::ros::WallDuration(node_options_.pose_publish_period_sec), &Node::PublishTrajectoryStates, this)); + wall_timers_.push_back(node_handle_.createWallTimer( + ::ros::WallDuration(node_options_.trajectory_publish_period_sec), + &Node::PublishTrajectoryNodesList, this)); } Node::~Node() { @@ -220,6 +227,15 @@ void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) { } } +void Node::PublishTrajectoryNodesList( + const ::ros::WallTimerEvent& unused_timer_event) { + carto::common::MutexLocker lock(&mutex_); + if (trajectory_nodes_list_publisher_.getNumSubscribers() > 0) { + trajectory_nodes_list_publisher_.publish( + map_builder_bridge_.GetTrajectoryNodesList()); + } +} + void Node::SpinOccupancyGridThreadForever() { for (;;) { std::this_thread::sleep_for(std::chrono::milliseconds(1000)); diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index 9dc2acd..8f0ab09 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -51,6 +51,7 @@ constexpr char kSubmapListTopic[] = "submap_list"; constexpr char kSubmapQueryServiceName[] = "submap_query"; constexpr char kStartTrajectoryServiceName[] = "start_trajectory"; constexpr char kWriteAssetsServiceName[] = "write_assets"; +constexpr char kTrajectoryNodesListTopic[] = "trajectory_nodes_list"; // Wires up ROS topics to SLAM. class Node { @@ -90,6 +91,7 @@ class Node { int trajectory_id); void PublishSubmapList(const ::ros::WallTimerEvent& timer_event); void PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event); + void PublishTrajectoryNodesList(const ::ros::WallTimerEvent& timer_event); void SpinOccupancyGridThreadForever(); bool ValidateTrajectoryOptions(const TrajectoryOptions& options); bool ValidateTopicName(const ::cartographer_ros_msgs::SensorTopics& topics, @@ -104,6 +106,7 @@ class Node { ::ros::NodeHandle node_handle_; ::ros::Publisher submap_list_publisher_; + ::ros::Publisher trajectory_nodes_list_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_options.cc b/cartographer_ros/cartographer_ros/node_options.cc index 3bfe2f0..f487ac9 100644 --- a/cartographer_ros/cartographer_ros/node_options.cc +++ b/cartographer_ros/cartographer_ros/node_options.cc @@ -34,6 +34,8 @@ NodeOptions CreateNodeOptions( lua_parameter_dictionary->GetDouble("submap_publish_period_sec"); options.pose_publish_period_sec = lua_parameter_dictionary->GetDouble("pose_publish_period_sec"); + options.trajectory_publish_period_sec = + lua_parameter_dictionary->GetDouble("trajectory_publish_period_sec"); return options; } diff --git a/cartographer_ros/cartographer_ros/node_options.h b/cartographer_ros/cartographer_ros/node_options.h index 8e09b50..431d80f 100644 --- a/cartographer_ros/cartographer_ros/node_options.h +++ b/cartographer_ros/cartographer_ros/node_options.h @@ -32,6 +32,7 @@ struct NodeOptions { double lookup_transform_timeout_sec; double submap_publish_period_sec; double pose_publish_period_sec; + double trajectory_publish_period_sec; }; NodeOptions CreateNodeOptions( diff --git a/cartographer_ros/configuration_files/backpack_2d.lua b/cartographer_ros/configuration_files/backpack_2d.lua index 534b85e..9279d3c 100644 --- a/cartographer_ros/configuration_files/backpack_2d.lua +++ b/cartographer_ros/configuration_files/backpack_2d.lua @@ -30,6 +30,7 @@ options = { lookup_transform_timeout_sec = 0.2, submap_publish_period_sec = 0.3, pose_publish_period_sec = 5e-3, + trajectory_publish_period_sec = 30e-3, } MAP_BUILDER.use_trajectory_builder_2d = true diff --git a/cartographer_ros/configuration_files/backpack_3d.lua b/cartographer_ros/configuration_files/backpack_3d.lua index 15d2941..3a6296b 100644 --- a/cartographer_ros/configuration_files/backpack_3d.lua +++ b/cartographer_ros/configuration_files/backpack_3d.lua @@ -30,6 +30,7 @@ options = { lookup_transform_timeout_sec = 0.2, submap_publish_period_sec = 0.3, pose_publish_period_sec = 5e-3, + trajectory_publish_period_sec = 30e-3, } TRAJECTORY_BUILDER_3D.scans_per_accumulation = 160 diff --git a/cartographer_ros/configuration_files/demo_2d.rviz b/cartographer_ros/configuration_files/demo_2d.rviz index 8de036e..3db172f 100644 --- a/cartographer_ros/configuration_files/demo_2d.rviz +++ b/cartographer_ros/configuration_files/demo_2d.rviz @@ -170,6 +170,14 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /trajectory_nodes_list + Name: MarkerArray + Namespaces: + "": true + Queue Size: 100 + Value: true Enabled: true Global Options: Background Color: 100; 100; 100 diff --git a/cartographer_ros/configuration_files/demo_3d.rviz b/cartographer_ros/configuration_files/demo_3d.rviz index f7742b2..22349ed 100644 --- a/cartographer_ros/configuration_files/demo_3d.rviz +++ b/cartographer_ros/configuration_files/demo_3d.rviz @@ -232,6 +232,14 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /trajectory_nodes_list + Name: MarkerArray + Namespaces: + "": true + Queue Size: 100 + Value: true Enabled: true Global Options: Background Color: 100; 100; 100 diff --git a/cartographer_ros/configuration_files/pr2.lua b/cartographer_ros/configuration_files/pr2.lua index 7eec98c..3c2ba22 100644 --- a/cartographer_ros/configuration_files/pr2.lua +++ b/cartographer_ros/configuration_files/pr2.lua @@ -30,6 +30,7 @@ options = { lookup_transform_timeout_sec = 0.2, submap_publish_period_sec = 0.3, pose_publish_period_sec = 5e-3, + trajectory_publish_period_sec = 30e-3, } MAP_BUILDER.use_trajectory_builder_2d = true diff --git a/cartographer_ros/configuration_files/revo_lds.lua b/cartographer_ros/configuration_files/revo_lds.lua index 7896208..4be039c 100644 --- a/cartographer_ros/configuration_files/revo_lds.lua +++ b/cartographer_ros/configuration_files/revo_lds.lua @@ -30,6 +30,7 @@ options = { lookup_transform_timeout_sec = 0.2, submap_publish_period_sec = 0.3, pose_publish_period_sec = 5e-3, + trajectory_publish_period_sec = 30e-3, } MAP_BUILDER.use_trajectory_builder_2d = true diff --git a/cartographer_ros/configuration_files/taurob_tracker.lua b/cartographer_ros/configuration_files/taurob_tracker.lua index f7fe1db..847aae4 100644 --- a/cartographer_ros/configuration_files/taurob_tracker.lua +++ b/cartographer_ros/configuration_files/taurob_tracker.lua @@ -30,6 +30,7 @@ options = { lookup_transform_timeout_sec = 0.2, submap_publish_period_sec = 0.3, pose_publish_period_sec = 5e-3, + trajectory_publish_period_sec = 30e-3, } TRAJECTORY_BUILDER_3D.scans_per_accumulation = 180 diff --git a/cartographer_ros/package.xml b/cartographer_ros/package.xml index 50d09f9..91a5e0c 100644 --- a/cartographer_ros/package.xml +++ b/cartographer_ros/package.xml @@ -58,6 +58,7 @@ tf2_eigen tf2_ros urdf + visualization_msgs yaml-cpp rosunit diff --git a/docs/source/configuration.rst b/docs/source/configuration.rst index 35492a5..8e0b209 100644 --- a/docs/source/configuration.rst +++ b/docs/source/configuration.rst @@ -81,6 +81,10 @@ pose_publish_period_sec Interval in seconds at which to publish poses, e.g. 5e-3 for a frequency of 200 Hz. +trajectory_publish_period_sec + Interval in seconds at which to publish the trajectory markers, e.g. 30e-3 + for 30 milliseconds. + .. _REP 105: http://www.ros.org/reps/rep-0105.html .. _ROS Names: http://wiki.ros.org/Names .. _nav_msgs/OccupancyGrid: http://docs.ros.org/api/nav_msgs/html/msg/OccupancyGrid.html