cartographer source code interpretation - 2D&3D data flow - trajectory addition (taking 2D Radar and wheel speed odometer as an example)

1, Track addition

cartographer source code interpretation - 2D & 3D data flow - trajectory addition (taking 2D Radar and wheel speed odometer as an example)

Entrance: node_main.cc(cartographer_ros)

void Run() { constexpr double kTfBufferCacheTimeInSeconds = 10.; //Build tf_buffer tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)}; tf2_ros::TransformListener tf(tf_buffer); //Relationship between coordinate systems //Read node and trajectory parameters NodeOptions node_options; TrajectoryOptions trajectory_options; std::tie(node_options, trajectory_options) = LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename); //Constructing map_builder auto map_builder = cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);//Create a map builder, with the argument map_builder //Construct node Node node(node_options, std::move(map_builder), &tf_buffer, FLAGS_collect_metrics);//Create a node with a map builder //Load the state(pbstream file) saved offline from proto if (!FLAGS_load_state_filename.empty()) { node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state); } //Enable trajectory with default parameters if (FLAGS_start_trajectory_with_default_topics) { node.StartTrajectoryWithDefaultTopics(trajectory_options); } ::ros::spin();//Continue to enter the subscription function callback (sensor) //Tasks performed at the end of node //1:finish trajectory node.FinishAllTrajectories();//Stop all tracks after stopping node //2: Final optimization (last time) node.RunFinalOptimization(); //3: Optional: if you specify to save the final state, save the PoseGraph if (!FLAGS_save_state_filename.empty()) { node.SerializeState(FLAGS_save_state_filename, true /* include_unfinished_submaps */); } } } // namespace } // namespace cartographer_ros

The following three things are done in the run() entry function:

1. Before node execution, read the parameters and construct node (construct map_builder and tf at the same time);

2. Execute the node and start the task: Start Trajectory With Default Topics (use the default topic name) and LoadState (use for positioning);

3. At the end of the node task, perform three tasks: finish trajectory, the last optimization, and save PoseGraph (optional).

Node class: node.h(cartographer_ros)

class Node { public: //Constructor Node(const NodeOptions& node_options, std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder, tf2_ros::Buffer* tf_buffer, bool collect_metrics); ~Node(); Node(const Node&) = delete; Node& operator=(const Node&) = delete; // Finishes all yet active trajectories. void FinishAllTrajectories(); // Finishes a single given trajectory. Returns false if the trajectory did not // exist or was already finished. bool FinishTrajectory(int trajectory_id); // Runs final optimization. All trajectories have to be finished when calling. void RunFinalOptimization(); // Starts the first trajectory with the default topics. void StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options); // Returns unique SensorIds for multiple input bag files based on // their TrajectoryOptions. // 'SensorId::id' is the expected ROS topic name. std::vector< std::set<::cartographer::mapping::TrajectoryBuilderInterface::SensorId>> ComputeDefaultSensorIdsForMultipleBags( const std::vector<TrajectoryOptions>& bags_options) const; // Adds a trajectory for offline processing, i.e. not listening to topics. int AddOfflineTrajectory( const std::set< cartographer::mapping::TrajectoryBuilderInterface::SensorId>& expected_sensor_ids, const TrajectoryOptions& options); // 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); void HandleNavSatFixMessage(int trajectory_id, const std::string& sensor_id, const sensor_msgs::NavSatFix::ConstPtr& msg); void HandleLandmarkMessage( int trajectory_id, const std::string& sensor_id, const cartographer_ros_msgs::LandmarkList::ConstPtr& msg); void HandleImuMessage(int trajectory_id, const std::string& sensor_id, const sensor_msgs::Imu::ConstPtr& msg); void HandleLaserScanMessage(int trajectory_id, const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg); void HandleMultiEchoLaserScanMessage( int trajectory_id, const std::string& sensor_id, const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg); void HandlePointCloud2Message(int trajectory_id, const std::string& sensor_id, const sensor_msgs::PointCloud2::ConstPtr& msg); // Serializes the complete Node state void SerializeState(const std::string& filename, const bool include_unfinished_submaps); // Loads a serialized SLAM state from a .pbstream file. void LoadState(const std::string& state_filename, bool load_frozen_state); //What is this ::ros::NodeHandle* node_handle(); private: struct Subscriber { ::ros::Subscriber subscriber; // ::ros::Subscriber::getTopic() does not necessarily return the same // std::string // it was given in its constructor. Since we rely on the topic name as the // unique identifier of a subscriber, we remember it ourselves. std::string topic; }; //Handle some service s of cartographer 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); // 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; // Add track int AddTrajectory(const TrajectoryOptions& options); // Handler for track subscription message void LaunchSubscribers(const TrajectoryOptions& options, int trajectory_id); // Add an extrapolator for the track. Note: This extrapolator has nothing to do with the front-end extrapolator. This extrapolator is used to publish the trajectory state // That is, the function: PublishTrajectoryStates. The extrapolator will also receive wheel tachometer and IMU data void AddExtrapolator(int trajectory_id, const TrajectoryOptions& options); // Adds a counter (sensor sampling) that receives data for the track void AddSensorSamplers(int trajectory_id, const TrajectoryOptions& options); //Release relevant news void PublishSubmapList(const ::ros::WallTimerEvent& timer_event); 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); bool ValidateTrajectoryOptions(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&); // 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_; absl::Mutex mutex_; std::unique_ptr<cartographer_ros::metrics::FamilyFactory> metrics_registry_; MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_);//The Node member variable map_builder_bridge_is of type MapBuilderBridge // Release relevant news ::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_; ::ros::Publisher scan_matched_point_cloud_publisher_; // These ros::ServiceServers need to live for the lifetime of the node. std::vector<::ros::ServiceServer> service_servers_; struct TrajectorySensorSamplers { TrajectorySensorSamplers(const double rangefinder_sampling_ratio, const double odometry_sampling_ratio, const double fixed_frame_pose_sampling_ratio, const double imu_sampling_ratio, const double landmark_sampling_ratio) : rangefinder_sampler(rangefinder_sampling_ratio), odometry_sampler(odometry_sampling_ratio), fixed_frame_pose_sampler(fixed_frame_pose_sampling_ratio), imu_sampler(imu_sampling_ratio), landmark_sampler(landmark_sampling_ratio) {} ::cartographer::common::FixedRatioSampler rangefinder_sampler; ::cartographer::common::FixedRatioSampler odometry_sampler; ::cartographer::common::FixedRatioSampler fixed_frame_pose_sampler; ::cartographer::common::FixedRatioSampler imu_sampler; ::cartographer::common::FixedRatioSampler landmark_sampler; }; // These are keyed with 'trajectory_id'. // Each track corresponds to an extrapolator std::map<int, ::cartographer::mapping::PoseExtrapolator> extrapolators_; std::map<int, ::ros::Time> last_published_tf_stamps_; // Counter for track receiving data std::unordered_map<int, TrajectorySensorSamplers> sensor_samplers_; // Subscription message std::unordered_map<int, std::vector<Subscriber>> subscribers_; std::unordered_set<std::string> subscribed_topics_; std::unordered_set<int> 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_; };

The functions of Node include:

1. Manage tracks: add tracks, finish tracks (one or all), add offline tracks (no longer listening topics), optimize all tracks (called at the end of node);

2. Receive sensor messages: IMU, Odometry, Landmark, LaserScan, PointCloud, MultiEchoLaserScan, NavSatFix, and process sensor messages: frequency reduction (the role of sensor_samplers_), extrapolator transmitted to node (the role of extraplorators) for pose estimation, transmission to corresponding trajectories, and SLAM process;

3. Publish relevant information: TrajectoryStates, TrajectoryNodeList, LandmarkPosesList, ConstraintList, SubmapList, scan_matched_point_cloud.

4. Processing related service s: SubmapQuery, StartTrajectory, finishtractory, WriteState

5. Test related: add offline tracks, verify TrajectoryOptions and TopicNames.

Starting from node.h, this paper analyzes the function and implementation of node In node.cc, the Node constructor Node::Node(): map_builder_bridge_is constructed by map_builder and tf

Node::Node( const NodeOptions& node_options, std::unique_ptr<cartographer::mapping::MapBuilderInterface> 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) {} //Node::Node (read parameters in node_options configuration file, map builder, coordinate system: initialize the list, and give map_builder to map_builder_bridge_)

The topic data name entry starts with the StartTrajectoryWithDefaultTopics() function of node.h

void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) { absl::MutexLock lock(&mutex_); //Verify that trajectoryOptions has an exception parameter CHECK(ValidateTrajectoryOptions(options)); //If not, this opens a new Node::AddTrajectory() function AddTrajectory(options); } //The TrajectoryOptions of the incoming data is in trajectory_options.h; //Another: defaultsensortopics (the default sensor topic is defined in node_constants.h)

StartTrajectoryWithDefaultTopics() function body of this function:

The function body of validatetrjectoryoptions (options) is

bool Node::ValidateTrajectoryOptions(const TrajectoryOptions& options) { //2D if (node_options_.map_builder_options.use_trajectory_builder_2d()) { return options.trajectory_builder_options .has_trajectory_builder_2d_options(); } //3D if (node_options_.map_builder_options.use_trajectory_builder_3d()) { return options.trajectory_builder_options .has_trajectory_builder_3d_options(); } return false; }

Node::AddTrajectory(options) add tracks

int Node::AddTrajectory(const TrajectoryOptions& options) { // Get the topics name required for the track const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId> expected_sensor_ids = ComputeExpectedSensorIds(options); // Gets the trajectory_id of the track const int trajectory_id = map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);//Let map_builder_bridge_ add tracks // Add a pose extrapolator to this track AddExtrapolator(trajectory_id, options); // add counter AddSensorSamplers(trajectory_id, options); // Turn on the callback function for this track LaunchSubscribers(options, trajectory_id); wall_timers_.push_back(node_handle_.createWallTimer( ::ros::WallDuration(kTopicMismatchCheckDelaySec), &Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true)); for (const auto& sensor_id : expected_sensor_ids) { subscribed_topics_.insert(sensor_id.id); } return trajectory_id; }

Computeexpectedsensors (options) function body (node.cc): calculates which data is received according to the parameters

std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId> Node::ComputeExpectedSensorIds(const TrajectoryOptions& options) const { using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId; using SensorType = SensorId::SensorType; std::set<SensorId> expected_topics; // Subscribe to all laser scan, multi echo laser scan, and point cloud topics. for (const std::string& topic : ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) { expected_topics.insert(SensorId); } for (const std::string& topic : ComputeRepeatedTopicNames( kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) { expected_topics.insert(SensorId); } for (const std::string& topic : ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) { expected_topics.insert(SensorId); } // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is // required. if (node_options_.map_builder_options.use_trajectory_builder_3d() || (node_options_.map_builder_options.use_trajectory_builder_2d() && options.trajectory_builder_options.trajectory_builder_2d_options() .use_imu_data())) { expected_topics.insert(SensorId); } // Odometry is optional. if (options.use_odometry) { expected_topics.insert(SensorId); } // NavSatFix is optional. if (options.use_nav_sat) { expected_topics.insert( SensorId); } // Landmark is optional. if (options.use_landmarks) { expected_topics.insert(SensorId); } return expected_topics; }

MapBuilderBridge - add track map_builder_bridge_.AddTrajectory(expected_sensor_ids, options) function, call MapBuilderBridge function and add track (Node.cc). Note: AddTrajectory here is a member function under map_builder_bridge_, not a member function in Node here

int MapBuilderBridge::AddTrajectory( const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>& expected_sensor_ids, const TrajectoryOptions& trajectory_options) { //Laser sensor data id,this is λ expression const int trajectory_id = map_builder_->AddTrajectoryBuilder( expected_sensor_ids, trajectory_options.trajectory_builder_options, [this](const int trajectory_id, const ::cartographer::common::Time time, const Rigid3d local_pose, ::cartographer::sensor::RangeData range_data_in_local, const std::unique_ptr< const ::cartographer::mapping::TrajectoryBuilderInterface:: InsertionResult>) { OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local); }); LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'."; // Make sure there is no trajectory with 'trajectory_id' yet. CHECK_EQ(sensor_bridges_.count(trajectory_id), 0); sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>( trajectory_options.num_subdivisions_per_laser_scan, trajectory_options.tracking_frame, node_options_.lookup_transform_timeout_sec, tf_buffer_, map_builder_->GetTrajectoryBuilder(trajectory_id));//sensor_bridges_ got the GetTrajectoryBuilder of map_builder auto emplace_result = trajectory_options_.emplace(trajectory_id, trajectory_options); CHECK(emplace_result.second == true); return trajectory_id; }

AddExtrapolator() function body: add the extrapolator PoseExtrapolator(node.cc) for the track in Node::extrapolators)

void Node::AddExtrapolator(const int trajectory_id, const TrajectoryOptions& options) { constexpr double kExtrapolationEstimationTimeSec = 0.001; // 1 ms CHECK(extrapolators_.count(trajectory_id) == 0); const double gravity_time_constant = node_options_.map_builder_options.use_trajectory_builder_3d() ? options.trajectory_builder_options.trajectory_builder_3d_options() .imu_gravity_time_constant() : options.trajectory_builder_options.trajectory_builder_2d_options() .imu_gravity_time_constant(); extrapolators_.emplace( std::piecewise_construct, std::forward_as_tuple(trajectory_id), std::forward_as_tuple( ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec), gravity_time_constant)); }

Node::AddSensorSamplers: add counters TrajectorySensorSamplers for tracks in Node::sensor_samplers

void Node::AddSensorSamplers(const int trajectory_id, const TrajectoryOptions& options) { CHECK(sensor_samplers_.count(trajectory_id) == 0); sensor_samplers_.emplace( std::piecewise_construct, std::forward_as_tuple(trajectory_id), std::forward_as_tuple( options.rangefinder_sampling_ratio, options.odometry_sampling_ratio, options.fixed_frame_pose_sampling_ratio, options.imu_sampling_ratio, options.landmarks_sampling_ratio)); }

Node::LaunchSubscribers: Subscribe related topic s for the track and start the callback function

void Node::LaunchSubscribers(const TrajectoryOptions& options, const int trajectory_id) { for (const std::string& topic : ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) { subscribers_[trajectory_id].push_back( ); } for (const std::string& topic : ComputeRepeatedTopicNames( kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) { subscribers_[trajectory_id].push_back( ); } for (const std::string& topic : ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) { subscribers_[trajectory_id].push_back( ); } // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is // required. if (node_options_.map_builder_options.use_trajectory_builder_3d() || (node_options_.map_builder_options.use_trajectory_builder_2d() && options.trajectory_builder_options.trajectory_builder_2d_options() .use_imu_data())) { subscribers_[trajectory_id].push_back( ); } if (options.use_odometry) { subscribers_[trajectory_id].push_back( ); } if (options.use_nav_sat) { subscribers_[trajectory_id].push_back( ); } if (options.use_landmarks) { subscribers_[trajectory_id].push_back( ); } }

Looking back: add track - map_builder_bridge_.AddTrajectory(), click to view the definition, and find that there are two definitions of AddTrajectory. Click MapBuilderBridge::AddTrajectory(), enter map_builder_bridge.cc, and then click MapBuilderBridge class to see what is in this class? (in map_builder_bridge.h) MapBuilderBridge manages a map_builder_, vector and TF; the functions called by MapBuilderBridge are basically MapBuilder functions, that is, it is the shell of MapBuilder.

namespace cartographer_ros { class MapBuilderBridge { public: // After the front end solves the pose, TrajectoryState is generated for sending positioning information struct LocalTrajectoryData { // Contains the trajectory data received from local SLAM, after // it had processed accumulated 'range_data_in_local' and estimated // current 'local_pose' at 'time'. struct LocalSlamData { //Laser frame timestamp ::cartographer::common::Time time; //The pose of the frame in the Local SLAM coordinate system ::cartographer::transform::Rigid3d local_pose; //The Transport Bureau of this frame point in the Local SLAM coordinate system ::cartographer::sensor::RangeData range_data_in_local; }; std::shared_ptr<const LocalSlamData> local_slam_data; //Conversion from Local SLAM to Global SLAM coordinate system cartographer::transform::Rigid3d local_to_map; //Publishing coordinate system to tracking coordinate system conversion std::unique_ptr<cartographer::transform::Rigid3d> published_to_tracking; //Loading track parameters TrajectoryOptions trajectory_options; }; //Constructor MapBuilderBridge( const NodeOptions& node_options, std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder, tf2_ros::Buffer* tf_buffer); MapBuilderBridge(const MapBuilderBridge&) = delete; MapBuilderBridge& operator=(const MapBuilderBridge&) = delete; //Load state from proto void LoadState(const std::string& state_filename, bool load_frozen_state); //Add track int AddTrajectory( const std::set< ::cartographer::mapping::TrajectoryBuilderInterface::SensorId>& expected_sensor_ids, const TrajectoryOptions& trajectory_options); //End track void FinishTrajectory(int trajectory_id); //Perform the last optimization void RunFinalOptimization(); //Write State to proto bool SerializeState(const std::string& filename, const bool include_unfinished_submaps); // Service: node:: handlesubmapquery function calls this function 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<int /* trajectory_id */, ::cartographer::mapping::PoseGraphInterface::TrajectoryState> GetTrajectoryStates(); // Functions called by node publish related functions cartographer_ros_msgs::SubmapList GetSubmapList(); std::unordered_map<int, LocalTrajectoryData> GetLocalTrajectoryData() LOCKS_EXCLUDED(mutex_); visualization_msgs::MarkerArray GetTrajectoryNodeList(); visualization_msgs::MarkerArray GetLandmarkPosesList(); visualization_msgs::MarkerArray GetConstraintList(); //Get the sensor_bridge of a track SensorBridge* sensor_bridge(int trajectory_id); private: //Callback function of front-end localSLAM void OnLocalSlamResult(const int trajectory_id, const ::cartographer::common::Time time, const ::cartographer::transform::Rigid3d local_pose, ::cartographer::sensor::RangeData range_data_in_local) LOCKS_EXCLUDED(mutex_); // lock absl::Mutex mutex_; const NodeOptions node_options_; // Track State std::unordered_map<int, std::shared_ptr<const LocalTrajectoryData::LocalSlamData>> local_slam_data_ GUARDED_BY(mutex_); // Member variable map_builder of MapBuilderBridge_ std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_; // tf tf2_ros::Buffer* const tf_buffer_; // landmark related std::unordered_map<std::string /* landmark ID */, int> landmark_to_index_; // These are keyed with 'trajectory_id'. // Store each track parameter std::unordered_map<int, TrajectoryOptions> trajectory_options_; // Each track corresponds to a SensorBridge std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_; // landmark related std::unordered_map<int, size_t> trajectory_to_highest_marker_id_; }; } // namespace cartographer_ros

Only the member function AddTrajectory() function body of MapBuilderBridge class is mentioned here:

int MapBuilderBridge::AddTrajectory( const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>& expected_sensor_ids, const TrajectoryOptions& trajectory_options) { // Laser sensor data id,this is λ expression // Call MapBuilder's AddTrajectoryBuilder function to add tracks const int trajectory_id = map_builder_->AddTrajectoryBuilder( expected_sensor_ids, trajectory_options.trajectory_builder_options, [this](const int trajectory_id, const ::cartographer::common::Time time, const Rigid3d local_pose, ::cartographer::sensor::RangeData range_data_in_local, const std::unique_ptr< const ::cartographer::mapping::TrajectoryBuilderInterface:: InsertionResult>) { OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local); }); LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'."; // Make sure there is no trajectory with 'trajectory_id' yet. CHECK_EQ(sensor_bridges_.count(trajectory_id), 0); // Add SensorBridge to this track sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>( trajectory_options.num_subdivisions_per_laser_scan, trajectory_options.tracking_frame, node_options_.lookup_transform_timeout_sec, tf_buffer_, map_builder_->GetTrajectoryBuilder(trajectory_id));//sensor_bridges_ got the GetTrajectoryBuilder of map_builder auto emplace_result = trajectory_options_.emplace(trajectory_id, trajectory_options); CHECK(emplace_result.second == true); return trajectory_id; }

be careful:

1. The member variable map_builder_in MapBuilderBridge class is constructed by map_builder and tf;

2. When constructing the SensorBridge, the tf of the Node is passed to the SensorBridge, and the TrajectoryBuilder in the map_builder is passed to the SensorBridge. In the SensorBridge, the data of the track is processed in the SensorBridge and passed to its corresponding TrajectoryBuilder
3. Note: defined map_builder is STD:: unique_ Ptrcartgrapher:: mapping:: MapBuilderInterface is the MapBuilderInterface class under the cartographer file, but the defined name is map_builder, and MapBuilder is a subclass of MapBuilder interface. You can go to cartographer to see the specific information

Add track MapBuilder(cartographer)

MapBuilder class (in map_builder.h)

class MapBuilder : public MapBuilderInterface { public: explicit MapBuilder(const proto::MapBuilderOptions &options); ~MapBuilder() override {} MapBuilder(const MapBuilder &) = delete; MapBuilder &operator=(const MapBuilder &) = delete; int AddTrajectoryBuilder( const std::set<SensorId> &expected_sensor_ids, const proto::TrajectoryBuilderOptions &trajectory_options, LocalSlamResultCallback local_slam_result_callback) override; int AddTrajectoryForDeserialization( const proto::TrajectoryBuilderOptionsWithSensorIds &options_with_sensor_ids_proto) override; void FinishTrajectory(int trajectory_id) override; std::string SubmapToProto(const SubmapId &submap_id, proto::SubmapQuery::Response *response) override; void SerializeState(bool include_unfinished_submaps, io::ProtoStreamWriterInterface *writer) override; bool SerializeStateToFile(bool include_unfinished_submaps, const std::string &filename) override; std::map<int, int> LoadState(io::ProtoStreamReaderInterface *reader, bool load_frozen_state) override; std::map<int, int> LoadStateFromFile(const std::string &filename, const bool load_frozen_state) override; mapping::PoseGraphInterface *pose_graph() override { return pose_graph_.get(); } int num_trajectory_builders() const override { return trajectory_builders_.size(); } mapping::TrajectoryBuilderInterface *GetTrajectoryBuilder( int trajectory_id) const override { return trajectory_builders_.at(trajectory_id).get(); } const std::vector<proto::TrajectoryBuilderOptionsWithSensorIds> &GetAllTrajectoryBuilderOptions() const override { return all_trajectory_builder_options_; } private: const proto::MapBuilderOptions options_; // Thread pool common::ThreadPool thread_pool_; // A back-end pose_graph_ std::unique_ptr<PoseGraph> pose_graph_; //Data distributor std::unique_ptr<sensor::CollatorInterface> sensor_collator_; // Front end of multiple tracks std::vector<std::unique_ptr<mapping::TrajectoryBuilderInterface>> trajectory_builders_; std::vector<proto::TrajectoryBuilderOptionsWithSensorIds> all_trajectory_builder_options_; };

Add track AddTrajectoryBuilder() function body (map_builder.cc)

int MapBuilder::AddTrajectoryBuilder( const std::set<SensorId>& expected_sensor_ids, const proto::TrajectoryBuilderOptions& trajectory_options, LocalSlamResultCallback local_slam_result_callback) { // Returns the id of the Trajectory to be created const int trajectory_id = trajectory_builders_.size(); absl::optional<MotionFilter> pose_graph_odometry_motion_filter; if (trajectory_options.has_pose_graph_odometry_motion_filter()) { LOG(INFO) << "Using a motion filter for adding odometry to the pose graph."; pose_graph_odometry_motion_filter.emplace( MotionFilter(trajectory_options.pose_graph_odometry_motion_filter())); } // 3Dslam if (options_.use_trajectory_builder_3d()) { // 3D front end std::unique_ptr<LocalTrajectoryBuilder3D> local_trajectory_builder; if (trajectory_options.has_trajectory_builder_3d_options()) { local_trajectory_builder = absl::make_unique<LocalTrajectoryBuilder3D>( trajectory_options.trajectory_builder_3d_options(), SelectRangeSensorIds(expected_sensor_ids)); } // 3D backend // sensor_collator_ Data distributor // The TrajectoryBuilder in MapBuilder is CollatedBuilder, // The CollatedBuilder is composed of a data distributor and a GlobalTrajectoryBuilder, // The GlobalTrajectoryBuilder is constructed by the front-end localtrajectorybuilder 3D and the back-end PoseGraph3D DCHECK(dynamic_cast<PoseGraph3D*>(pose_graph_.get())); trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>( trajectory_options, sensor_collator_.get(), trajectory_id, expected_sensor_ids, CreateGlobalTrajectoryBuilder3D( std::move(local_trajectory_builder), trajectory_id, static_cast<PoseGraph3D*>(pose_graph_.get()), local_slam_result_callback, pose_graph_odometry_motion_filter))); } else { // 2D SLAM // 2D front end std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder; if (trajectory_options.has_trajectory_builder_2d_options()) { local_trajectory_builder = absl::make_unique<LocalTrajectoryBuilder2D>( trajectory_options.trajectory_builder_2d_options(), SelectRangeSensorIds(expected_sensor_ids)); } // Same as 3D DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get())); trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>( trajectory_options, sensor_collator_.get(), trajectory_id, expected_sensor_ids, CreateGlobalTrajectoryBuilder2D( std::move(local_trajectory_builder), trajectory_id, static_cast<PoseGraph2D*>(pose_graph_.get()), local_slam_result_callback, pose_graph_odometry_motion_filter))); } MaybeAddPureLocalizationTrimmer(trajectory_id, trajectory_options, pose_graph_.get()); // Initial pose of trajectory if (trajectory_options.has_initial_trajectory_pose()) { const auto& initial_trajectory_pose = trajectory_options.initial_trajectory_pose(); pose_graph_->SetInitialTrajectoryPose( trajectory_id, initial_trajectory_pose.to_trajectory_id(), transform::ToRigid3(initial_trajectory_pose.relative_pose()), common::FromUniversal(initial_trajectory_pose.timestamp())); } proto::TrajectoryBuilderOptionsWithSensorIds options_with_sensor_ids_proto; for (const auto& sensor_id : expected_sensor_ids) { *options_with_sensor_ids_proto.add_sensor_id() = ToProto(sensor_id); } *options_with_sensor_ids_proto.mutable_trajectory_builder_options() = trajectory_options; all_trajectory_builder_options_.push_back(options_with_sensor_ids_proto); CHECK_EQ(trajectory_builders_.size(), all_trajectory_builder_options_.size()); return trajectory_id; }

Note: on the 2D and 3D backend, TrajectoryBuilder is CollatedBuilder

// 3D backend // sensor_collator_ Data distributor // The TrajectoryBuilder in MapBuilder is CollatedBuilder, // The CollatedBuilder is provided by the data distributor sensor_collator_.get() and GlobalTrajectoryBuilder, // The GlobalTrajectoryBuilder is constructed by the front-end localtrajectorybuilder 3D and the back-end PoseGraph3D DCHECK(dynamic_cast<PoseGraph3D*>(pose_graph_.get())); trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>( trajectory_options, sensor_collator_.get(), trajectory_id, expected_sensor_ids, CreateGlobalTrajectoryBuilder3D( std::move(local_trajectory_builder), trajectory_id, static_cast<PoseGraph3D*>(pose_graph_.get()), local_slam_result_callback, pose_graph_odometry_motion_filter)));

Click CollatedTrajectoryBuilder to enter CollatedTrajectoryBuilder_ Trajectory_ Builder. H to see what variables and methods this class has

class CollatedTrajectoryBuilder : public TrajectoryBuilderInterface { public: using SensorId = TrajectoryBuilderInterface::SensorId; // Constructor by MapBuilder's data distributor sensor_collator and global_trajectory_builder construction // wrapped_trajectory_builder is global_trajectory_builder has the same data type STD:: unique_ ptr<TrajectoryBuilderInterface> CollatedTrajectoryBuilder( const proto::TrajectoryBuilderOptions& trajectory_options, sensor::CollatorInterface* sensor_collator, int trajectory_id, const std::set<SensorId>& expected_sensor_ids, std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder); ~CollatedTrajectoryBuilder() override {} CollatedTrajectoryBuilder(const CollatedTrajectoryBuilder&) = delete; CollatedTrajectoryBuilder& operator=(const CollatedTrajectoryBuilder&) = delete; // By sensor_ The HandleRangefinder function of the bridge calls the sensor_ The laser data processed in the bridge is transmitted to the CollatedTrajectoryBuilder void AddSensorData( const std::string& sensor_id, const sensor::TimedPointCloudData& timed_point_cloud_data) override { AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data)); } void AddSensorData(const std::string& sensor_id, const sensor::ImuData& imu_data) override { AddData(sensor::MakeDispatchable(sensor_id, imu_data)); } // By sensor_ The HandleOdometryMessage function of the bridge calls the sensor_ The processed wheel speedometer data in the bridge is transmitted to the CollatedTrajectoryBuilder void AddSensorData(const std::string& sensor_id, const sensor::OdometryData& odometry_data) override { AddData(sensor::MakeDispatchable(sensor_id, odometry_data)); } void AddSensorData( const std::string& sensor_id, const sensor::FixedFramePoseData& fixed_frame_pose_data) override { if (collate_fixed_frame_) { AddData(sensor::MakeDispatchable(sensor_id, fixed_frame_pose_data)); return; } wrapped_trajectory_builder_->AddSensorData(sensor_id, fixed_frame_pose_data); } void AddSensorData(const std::string& sensor_id, const sensor::LandmarkData& landmark_data) override { if (collate_landmarks_) { AddData(sensor::MakeDispatchable(sensor_id, landmark_data));//Distribute data return; } wrapped_trajectory_builder_->AddSensorData(sensor_id, landmark_data); } void AddLocalSlamResultData(std::unique_ptr<mapping::LocalSlamResultData> local_slam_result_data) override { AddData(std::move(local_slam_result_data)); } private: void AddData(std::unique_ptr<sensor::Data> data); void HandleCollatedSensorData(const std::string& sensor_id, std::unique_ptr<sensor::Data> data); // Data distributor, by MapBuilder's sensor_collator_ Construct the sensor_collator_ Type of: // sensor::Collator or sensor::TrajectoryCollator. Generally, it is sensor::Collator sensor::CollatorInterface* const sensor_collator_; const bool collate_landmarks_; const bool collate_fixed_frame_; const int trajectory_id_; // The wrapped_trajectory_builder_ It's GlobalTrajectoryBuilder std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder_; // Time at which we last logged the rates of incoming sensor data. std::chrono::steady_clock::time_point last_logging_time_; std::map<std::string, common::RateTimer<>> rate_timers_; }; } // namespace mapping } // namespace cartographer

Enter its constructor collapsed_ Trajectory_ Builder.cc look

CollatedTrajectoryBuilder::CollatedTrajectoryBuilder( const proto::TrajectoryBuilderOptions& trajectory_options, sensor::CollatorInterface* const sensor_collator, const int trajectory_id, const std::set<SensorId>& expected_sensor_ids, std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder) : sensor_collator_(sensor_collator), collate_landmarks_(trajectory_options.collate_landmarks()), collate_fixed_frame_(trajectory_options.collate_fixed_frame()), trajectory_id_(trajectory_id), wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)), last_logging_time_(std::chrono::steady_clock::now()) { absl::flat_hash_set<std::string> expected_sensor_id_strings; for (const auto& sensor_id : expected_sensor_ids) { if (sensor_id.type == SensorId::SensorType::LANDMARK && !collate_landmarks_) { continue; } if (sensor_id.type == SensorId::SensorType::FIXED_FRAME_POSE && !collate_fixed_frame_) { continue; } expected_sensor_id_strings.insert(sensor_id.id); } // Through lambda( λ) The expression passes the data of different sensor_id s of the track through HandleCollatedSensorData //It is passed to the data distributor sensor_collator_ (from MapBuilder). It should be noted that a callback function callback is passed in, //In this callback function, the data is transmitted to the GlobalTrajectoryBuilder constructed in MapBuilder. After the data distributor receives the data, //The data is passed to the global trajectorybuilder sensor_collator_->AddTrajectory( trajectory_id, expected_sensor_id_strings, [this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data) { HandleCollatedSensorData(sensor_id, std::move(data)); }); } void CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data) { // The wheel tachometer data will be uniformly sent to the data distributor through this function sensor_collator_->AddSensorData(trajectory_id_, std::move(data)); } // Callback function void CollatedTrajectoryBuilder::HandleCollatedSensorData( const std::string& sensor_id, std::unique_ptr<sensor::Data> data) { auto it = rate_timers_.find(sensor_id); if (it == rate_timers_.end()) { it = rate_timers_ .emplace( std::piecewise_construct, std::forward_as_tuple(sensor_id), std::forward_as_tuple( common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds))) .first; } it->second.Pulse(data->GetTime()); if (std::chrono::steady_clock::now() - last_logging_time_ > common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) { for (const auto& pair : rate_timers_) { LOG(INFO) << pair.first << " rate: " << pair.second.DebugString(); } last_logging_time_ = std::chrono::steady_clock::now(); } // In this callback function, the data is passed to the GlobalTrajectoryBuilder constructed in MapBuilder data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get()); } } // namespace mapping } // namespace cartographer

This completes the addition of the track

Reference link: https://blog.csdn.net/yeluohanchan/article/details/108672133.

cartographer source code interpretation - 2D&3D data flow - trajectory addition (taking 2D Radar and wheel speed odometer as an example)