RTS路径规划 roborts_planner 学习笔记

学习日志 DAY 5

layer.h学习笔记

(~ end)

#ifndef ROBORTS_COSTMAP_LAYER_H
#define ROBORTS_COSTMAP_LAYER_H

#include <string>
#include <tf/tf.h>
#include <tf/transform_listener.h>

#include "costmap_2d.h"
#include "layered_costmap.h"

namespace roborts_costmap {

class CostmapLayers;

class Layer {
 public:
/**
 * @brief constructor
 */
  Layer();
/**
 * @简短的初始化
 * @param parent 分层成本图，即主网格。
 * @param name 此层名称
 * @param tf 一个提供变换的tf监听器
 * @brief initialize
 * @param parent the layered costmap, ie master grid
 * @param name this layer name
 * @param tf a tf listener providing transforms
 */
  void Initialize(CostmapLayers *parent, std::string name, tf::TransformListener *tf);

/**
 * @brief 这是由LayeredCostmap调用的，以轮询这个插件需要更新多少
 * 它需要更新多少成本图。每一层都可以增加
 * 这个界限的大小。*
 * @param robot_x
 * @param robot_y
 * @param robot_yaw 这些点是机器人在全局框架中的姿态
 * @param min_x
 * @param min_y
 * @param max_x
 * @param max_y 这些声明了更新的边界
 * @brief This is called by the LayeredCostmap to poll this plugin as to how
 *        much of the costmap it needs to update. Each layer can increase
 *        the size of this bounds. *
 * @param robot_x
 * @param robot_y
 * @param robot_yaw these point the pose of the robot in global frame
 * @param min_x
 * @param min_y
 * @param max_x
 * @param max_y these declare the updating boundary
 */
  virtual void UpdateBounds(double robot_x, double robot_y, double robot_yaw, double *min_x, double *min_y,
                            double *max_x, double *max_y) {}

/**
 * @brief 实际上是更新底层的成本图，只在UpdateBounds()计算的范围内进行。
 * 在UpdateBounds()中计算出来的。*
 * @param master_grid 是主图。
 * @param min_i
 * @param min_j
 * @param max_i
 * @param max_j 更新边界
 * @brief Actually update the underlying costmap, only within the bounds
 *        calculated during UpdateBounds(). *
 * @param master_grid the master map
 * @param min_i
 * @param min_j
 * @param max_i
 * @param max_j the update boundary
 */
  virtual void UpdateCosts(Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j) {}

  /**
   * @brief 停止
   * @brief Stop.
   */
  virtual void Deactivate() {}

  /**
   * @brief 重新启动，如果他们已经被停止
   * @brief Restart, if they've been stopped.
   */
  virtual void Activate() {}

  /**
   * @brief 重置该层
   * @brief Reset the layer
   */
  virtual void Reset() {}

  virtual ~Layer() {}

  /**
   * @brief 检查以确保该层中的所有数据都已更新。
   * @return 图层中的数据是否是最新的。
   * @brief Check to make sure all the data in the layer is update.
   * @return Whether the data in the layer is up to date.
   */
  bool IsCurrent() const {
    return is_current_;
  }

  /**
   * @brief 实现这一点，以使本层与父成本图的大小相匹配。
   * @brief Implement this to make this layer match the size of the parent costmap.
   */
  virtual void MatchSize() {}

  std::string GetName() const {
    return name_;
  }

  /**
   * @brief 为 layered_costmap_->GetFootprint()提供的便利函数。
   * @brief Convenience function for layered_costmap_->GetFootprint().
   */
  const std::vector<geometry_msgs::Point> &GetFootprint() const;

 virtual void OnFootprintChanged() {}

 protected:
  /**
   * @brief 这是在initialize()的末尾调用的。 
   * 覆盖以实现特定子类的初始化。
   * 当这个函数被调用时，tf_、name_和layered_costmap_都已经被设置了。
   * @brief This is called at the end of initialize().  
   * Override to implement subclass-specific initialization.
   * tf_, name_, and layered_costmap_ will all be set already when this is called.
   */
  virtual void OnInitialize() {}

  CostmapLayers *layered_costmap_;
  bool is_current_, is_enabled_, is_debug_;
  std::string name_;
  tf::TransformListener *tf_;

 private:
  std::vector<geometry_msgs::Point> footprint_spec_;
};

}  //namespace roborts_costmap
#endif  // ROBORTS_COSTMAP_LAYER_H

costmap_interface学习笔记

各种量：

类-CostmapInterface

class CostmapInterface {
 public:
  /**
   * @brief Constructor
   * @param map_name The costmap name
   * @param tf The tf listener
   * @param map_update_frequency The frequency to update costmap
   */
  CostmapInterface(std::string map_name, tf::TransformListener& tf, std::string config_file);//构造函数：地图名，监听坐标变换，配置文件
  ~CostmapInterface();//析构函数
  /**
   * @brief Start the costmap processing.
   */
  void Start();//开始
  /**
   * @breif Stop the costmap.
   */
  void Stop();//停止
  /**
   * @breif If costmap is stoped or paused, it can be resumed.
   */
  void Resume();//继续
  /**
   * @breif The core function updating the costmap with all layers.
   */
  void UpdateMap();//更新地图
  /**
   * @brief Function to pause the costmap disabling the update.
   */
  void Pause();//暂停
  /**
   * @breif Reset costmap with every layer reset.
   */
  void ResetLayers();//重置地图层
  /**
   * @brief Tell whether the costmap is updated.
   * @return True if every layer is current.
   */
  bool IsCurrent() {
    return layered_costmap_->IsCurrent();
  }//检测地图是否为最新
  /**
   * @brief Get the robot pose in global frame.
   * @param global_pose
   * @return True if got successfully.
   */
  bool GetRobotPose(tf::Stamped<tf::Pose>& global_pose) const;// 获取全局框架中的机器人姿势，参数 global_pose，如果成功获取，则返回True。检测机器人(检索异常、连接异常、外推异常)
  /**
   * @brief Get the costmap.
   * @return The class containing costmap data.
   */
  Costmap2D* GetCostMap() const {
    return layered_costmap_->GetCostMap();
  }//返回代价地图
  /**
   * @brief Get robot pose with time stamped.
   * @param global_pose
   * @return True if success.
   */
  bool GetRobotPose(geometry_msgs::PoseStamped & global_pose) const;//获取带有时间戳的机器人姿势，检测参数： global_pose，如果成功则返回True
  /**
   * @brief Get the global map frame.
   * @return The global map frame name.
   */
  std::string GetGlobalFrameID() {
    return global_frame_;
  }//获取全局地图框架，返回全局地图框架名称
  /**
   * @brief Get the base frame.
   * @return The base frame name.
   */
  std::string GetBaseFrameID() {
    return robot_base_frame_;
  }//获取基本框架，返回基本框架的名称
  /**
   * @brief Get the layered costmap
   * @return The class including layers
   */
  CostmapLayers* GetLayeredCostmap() {
    return layered_costmap_;
  }//获取代价地图层，返回包含图层的类
  /**
   * @brief Get the footprint polygon which are already padded.
   * @return The footprint polygon.
   */
  geometry_msgs::Polygon GetRobotFootprintPolygon() {
    return ToPolygon(padded_footprint_);
  }//获取已填充的足迹多边形，返回足迹多边形
  /**
   * @brief Get the footprint points which are already padded.
   * @return The vector of footprint point.
   */
  std::vector<geometry_msgs::Point> GetRobotFootprint() {
    return padded_footprint_;
  }//获取已填充的足迹点，return 足迹点的vector
  /**
   * @brief Get the footprint which are not padded.
   * @return The vector of footprint point.
   */
  std::vector<geometry_msgs::Point> GetUnpaddedRobotFootprint() {
    return unpadded_footprint_;
  }//获取未填充的足迹，返回足迹点的向量 
  /**
   * @brief Get the oriented footprint in global map.
   * @param oriented_footprint
   */

  //
  void GetOrientedFootprint(std::vector<geometry_msgs::Point>& oriented_footprint) const;//获取全局地图中的定向足迹，参数 oriented_footprint 
  /**
   * @brief Set up the robot footprint.
   * @param points Input vector of points to setup the robot footprint.
   */
  void SetUnpaddedRobotFootprint(const std::vector<geometry_msgs::Point>& points);//设置机器人足迹，参数点 输入点向量以设置机器人足迹 
  /**
   * @brief Set up the robot footprint.
   * @param footprint Input po"CostmapInterface"lygon to setup the robot footprint.
   */
  void SetUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon& footprint);//设置机器人足迹，参数footprint 输入“CostmapInterface”多边形来设置机器人足迹

  /**
   * @brief Output the original footprint.
   * @param footprint Vector of Eigen::Vector3f.
   */
  void GetFootprint(std::vector<Eigen::Vector3f> & footprint); 输出原始足迹，参数足迹vector的 Eigen::Vector3f
  /**
   * @brief Get the oriented footprint.
   * @param footprint Vector of Eigen::Vector3f
   */
  void GetOrientedFootprint(std::vector<Eigen::Vector3f> & footprint);//获取 Eigen::Vector3f 的定向足迹、参数足迹 Vector 
  /**
   * @brief Get robot pose in self-defined format.
   * @param pose Pose defined by RM team.
   * @return True if get successfully
   */
  bool GetRobotPose(RobotPose & pose);//获取自定义格式的机器人姿态，参数由RM团队定义的姿态，获取成功返回True 
  /**
   * @brief Get the costmap value array.
   * @return The pointer to the costmap value array.
   */
  unsigned char* GetCharMap() const;//获取costmap值数组，返回costmap值数组的指针
  /**
   * @brief Get the stamped pose message.
   * @param pose_msg
   * @return PoseStamped message.
   */
  geometry_msgs::PoseStamped Pose2GlobalFrame(const geometry_msgs::PoseStamped& pose_msg);//获取标记的姿势消息
  void ClearCostMap();
  void ClearLayer(CostmapLayer* costmap_layer_ptr, double pose_x, double pose_y);
 protected:
  void LoadParameter();
  std::vector<geometry_msgs::Point> footprint_points_;
  CostmapLayers* layered_costmap_;
  std::string name_, config_file_, config_file_inflation_;
  tf::TransformListener& tf_;
  std::string global_frame_, robot_base_frame_;
  double transform_tolerance_, dist_behind_robot_threshold_to_care_obstacles_;
  nav_msgs::OccupancyGrid grid_;
  char* cost_translation_table_ = new char[256];

  ros::Publisher costmap_pub_;

 private:
  void DetectMovement(const ros::TimerEvent &event);
  void MapUpdateLoop(double frequency);
  std::vector<geometry_msgs::Point> unpadded_footprint_, padded_footprint_;
  float footprint_padding_;
  bool map_update_thread_shutdown_, stop_updates_, initialized_, stopped_, robot_stopped_, got_footprint_, is_debug_, \
       is_track_unknown_, is_rolling_window_, has_static_layer_, has_obstacle_layer_;
  double map_update_frequency_, map_width_, map_height_, map_origin_x_, map_origin_y_, map_resolution_;
  std::thread* map_update_thread_;
  ros::Timer timer_;
  ros::Time last_publish_;
  tf::Stamped<tf::Pose> old_pose_;
};