Class MappingNode

mapping/src/mapping_node.cpp:44–154

}

class MappingNode : public rclcpp::Node
{
public:
  explicit MappingNode(const rclcpp::NodeOptions & options)
  // BEGIN STUDENT CODE
  : rclcpp::Node("mapping_node", options)
    // END STUDENT CODE
  {
    map_frame_id_ = declare_parameter<std::string>("map_frame", "map");
    robot_frame_id_ = declare_parameter<std::string>("robot_frame", "base_link");
    distance_coefficient_ = declare_parameter<double>("distance_coefficient", 0.01);
    hit_log_odds_ = toLogOdds(declare_parameter<double>("hit_probability", 0.7));
    miss_log_odds_ = toLogOdds(declare_parameter<double>("miss_probability", 0.3));

    const auto map_width = declare_parameter<double>("map_width", 1.2192);  // meters
    const auto map_height = declare_parameter<double>("map_height", 0.762);  // meters
    const auto map_resolution = declare_parameter<double>("map_resolution", 0.01);  // meters/cell

    map_info_.origin.position.x = -map_width / 2.0;
    map_info_.origin.position.y = -map_height / 2.0;
    map_info_.height = map_height / map_resolution;
    map_info_.width = map_width / map_resolution;
    map_info_.resolution = map_resolution;

    const auto map_data_size = map_info_.height * map_info_.width;
    map_data_.reserve(map_data_size);
    std::fill_n(std::back_inserter(map_data_), map_data_size, 0.0);

    map_publisher_ = create_publisher<nav_msgs::msg::OccupancyGrid>(
      "~/map",
      rclcpp::SystemDefaultsQoS());

    // BEGIN STUDENT CODE
    // END STUDENT CODE
  }

private:
  // BEGIN STUDENT CODE
  // END STUDENT CODE
  std::string map_frame_id_;
  std::string robot_frame_id_;
  double distance_coefficient_;
  double hit_log_odds_;
  double miss_log_odds_;
  std::vector<double> map_data_;
  nav_msgs::msg::MapMetaData map_info_;
  rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr map_publisher_;
  rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr obstacles_subscription_;

  void ObstaclesCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr obstacles_msg)
  {
    // BEGIN STUDENT CODE
    // END STUDENT CODE

    AddObstaclesToMap(*obstacles_msg);

    nav_msgs::msg::OccupancyGrid map_msg;
    map_msg.header.frame_id = map_frame_id_;