From a4f38509121a26525e5f037ee2b4efe7e7b2f96d Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Thu, 25 Oct 2018 14:43:43 +0200 Subject: [PATCH] Transform landmark poses to the tracking frame. (#1076) * Transform landmark poses to the tracking frame. * Address the comment. --- .../cartographer_ros/map_builder_bridge.cc | 4 ++-- cartographer_ros/cartographer_ros/sensor_bridge.cc | 13 ++++++++++++- 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index 26b89a2..13384c9 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -30,7 +30,7 @@ namespace { using ::cartographer::transform::Rigid3d; constexpr double kTrajectoryLineStripMarkerScale = 0.07; -constexpr double kLandmarkMarkerScale = 0.3; +constexpr double kLandmarkMarkerScale = 0.2; constexpr double kConstraintMarkerScale = 0.025; ::std_msgs::ColorRGBA ToMessage(const cartographer::io::FloatColor& color) { @@ -75,7 +75,7 @@ visualization_msgs::Marker CreateLandmarkMarker(int landmark_index, visualization_msgs::Marker marker; marker.ns = "Landmarks"; marker.id = landmark_index; - marker.type = visualization_msgs::Marker::CUBE; + marker.type = visualization_msgs::Marker::SPHERE; marker.header.stamp = ::ros::Time::now(); marker.header.frame_id = frame_id; marker.scale.x = kLandmarkMarkerScale; diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.cc b/cartographer_ros/cartographer_ros/sensor_bridge.cc index 525c585..3254d98 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.cc +++ b/cartographer_ros/cartographer_ros/sensor_bridge.cc @@ -100,7 +100,18 @@ void SensorBridge::HandleNavSatFixMessage( void SensorBridge::HandleLandmarkMessage( const std::string& sensor_id, const cartographer_ros_msgs::LandmarkList::ConstPtr& msg) { - trajectory_builder_->AddSensorData(sensor_id, ToLandmarkData(*msg)); + auto landmark_data = ToLandmarkData(*msg); + + auto tracking_from_landmark_sensor = tf_bridge_.LookupToTracking( + landmark_data.time, CheckNoLeadingSlash(msg->header.frame_id)); + if (tracking_from_landmark_sensor != nullptr) { + for (auto& observation : landmark_data.landmark_observations) { + observation.landmark_to_tracking_transform = + *tracking_from_landmark_sensor * + observation.landmark_to_tracking_transform; + } + } + trajectory_builder_->AddSensorData(sensor_id, landmark_data); } std::unique_ptr SensorBridge::ToImuData(