学习日志 DAY 4
global_planner_node.cpp学习笔记
(~end)
void GlobalPlannerNode::PathVisualization(const std::vector<geometry_msgs::PoseStamped> &path) {//路径可视化
path_.poses = path;
path_pub_.publish(path_);
new_path_ = true;
}
double GlobalPlannerNode::GetDistance(const geometry_msgs::PoseStamped &pose1,
const geometry_msgs::PoseStamped &pose2) {
const geometry_msgs::Point point1 = pose1.pose.position;
const geometry_msgs::Point point2 = pose2.pose.position;
const double dx = point1.x - point2.x;
const double dy = point1.y - point2.y;
return std::sqrt(dx * dx + dy * dy);
}//距离计算
double GlobalPlannerNode::GetAngle(const geometry_msgs::PoseStamped &pose1,
const geometry_msgs::PoseStamped &pose2) {
const geometry_msgs::Quaternion quaternion1 = pose1.pose.orientation;
const geometry_msgs::Quaternion quaternion2 = pose2.pose.orientation;
tf::Quaternion rot1, rot2;
tf::quaternionMsgToTF(quaternion1, rot1);
tf::quaternionMsgToTF(quaternion2, rot2);
return rot1.angleShortestPath(rot2);//角度最短路径
}
GlobalPlannerNode::~GlobalPlannerNode() {
StopPlanning();//析构函数，结束规划行为
}
} //namespace roborts_global_planner
int main(int argc, char **argv) {
ros::init(argc, argv, "global_planner_node");//初始化规划节点
roborts_global_planner::GlobalPlannerNode global_planner;//创建对象
ros::spin();//等待消息传入
return 0;
}
costmap_2d学习笔记
（~line 342）
class Costmap2D {
public:
/**
* @brief 成本图的构造函数
* @param cells_size_x 以单元格为单位的地图的x尺寸
* @param cells_size_y 地图的y尺寸，以单元格为单位。
* @param resolution 地图的分辨率，以米/单元为单位。
* @param origin_x 地图的X原点，单位是米。
* @param origin_y 地图的Y轴原点，单位是米。
* @param default_value 默认值
*/
Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
double origin_x, double origin_y, unsigned char default_value = 0);
/**
* @brief 成本图的拷贝构造函数，有效地创建一个副本。
* @param map 要复制的成本图
* @brief Copy constructor for a costmap, creates a copy efficiently
* @param map The costmap to copy
*/
Costmap2D(const Costmap2D &map);
/**
* @brief赋值构造函数
* @param map 要复制的成本图
* @return 复制完成后对地图的引用
* @brief Overloaded assignment operator
* @param map The costmap to copy
* @return A reference to the map after the copy has finished
*/
Costmap2D &operator=(const Costmap2D &map);
/**
* @brief 把这个成本图变成一个传入的成本图的窗口的副本
* @param map 要复制的成本图
* @param win_origin_x 要复制的窗口的x原点（左下角），单位是米。
* @param win_origin_y 要复制的窗口的y原点（左下角），以米为单位。
* @param win_size_x 窗口的X尺寸，单位是米。
* @param win_size_y 窗口的y尺寸，以米为单位。
* @brief Turn this costmap into a copy of a window of a costmap passed in
* @param map The costmap to copy
* @param win_origin_x The x origin (lower left corner) for the window to copy, in meters
* @param win_origin_y The y origin (lower left corner) for the window to copy, in meters
* @param win_size_x The x size of the window, in meters
* @param win_size_y The y size of the window, in meters
*/
bool CopyCostMapWindow(const Costmap2D &map, double win_origin_x, double win_origin_y, double win_size_x,
double win_size_y);
/**
* 默认构造函数
* @brief Default constructor
*/
Costmap2D();
/**
* 析构函数
* @brief Destructor
*/
virtual ~Costmap2D();
/**
* @brief 获取成本图中一个单元的成本
* @param mx 单元的x坐标
* @param my 该单元的y坐标
* @return 该单元的成本
* @brief Get the cost of a cell in the costmap
* @param mx The x coordinate of the cell
* @param my The y coordinate of the cell
* @return The cost of the cell
*/
unsigned char GetCost(unsigned int mx, unsigned int my) const;
/**
* @brief 设置成本图中的一个单元的成本
* @param mx 单元的x坐标
* @param my 该单元的y坐标
* @param cost 为单元格设置成本
* @brief Set the cost of a cell in the costmap
* @param mx The x coordinate of the cell
* @param my The y coordinate of the cell
* @param cost The cost to set the cell to
*/
void SetCost(unsigned int mx, unsigned int my, unsigned char cost);
/**
* @brief 从地图坐标转换为世界坐标
* @param mx 地图的x坐标
* @param my 地图的y坐标
* @param wx 将被设置为相关的世界x坐标
* @param wy 将被设置为相关的世界y坐标
* @brief Convert from map coordinates to world coordinates
* @param mx The x map coordinate
* @param my The y map coordinate
* @param wx Will be set to the associated world x coordinate
* @param wy Will be set to the associated world y coordinate
*/
void Map2World(unsigned int mx, unsigned int my, double &wx, double &wy) const;
/**
* @brief 从世界坐标转换为地图坐标
* @param wx 世界的x坐标
* @param wy 世界坐标的y坐标
* @param mx 将被设置为相关的地图x坐标
* @param my 将被设置为相关地图的y坐标
* @return True 如果转换成功（合法边界） false 否则
* @brief Convert from world coordinates to map coordinates
* @param wx The x world coordinate
* @param wy The y world coordinate
* @param mx Will be set to the associated map x coordinate
* @param my Will be set to the associated map y coordinate
* @return True if the conversion was successful (legal bounds) false otherwise
*/
bool World2Map(double wx, double wy, unsigned int &mx, unsigned int &my) const;
/**
* @brief 将世界坐标转换为地图坐标，无需检查合法边界
* @param wx 世界的x坐标
* @param wy 世界坐标的y坐标
* @param mx 将被设置为相关地图的x坐标
* @param my 将被设置为相关地图的y坐标。
* @注意 返回的地图坐标并不保证位于地图内。
* @brief Convert from world coordinates to map coordinates without checking for legal bounds
* @param wx The x world coordinate
* @param wy The y world coordinate
* @param mx Will be set to the associated map x coordinate
* @param my Will be set to the associated map y coordinate
* @note The returned map coordinates are not guaranteed to lie within the map.
*/
void World2MapNoBoundary(double wx, double wy, int &mx, int &my) const;
/**
* @brief 将世界坐标转换为地图坐标，将结果限制在合法范围内。
* @param wx 世界的x坐标
* @param wy 世界的y坐标
* @param mx 将被设置为相关地图的x坐标
* @param my 将被设置为相关地图的y坐标。
* @注意 返回的地图坐标保证位于地图中。
* @brief Convert from world coordinates to map coordinates, constraining results to legal bounds.
* @param wx The x world coordinate
* @param wy The y world coordinate
* @param mx Will be set to the associated map x coordinate
* @param my Will be set to the associated map y coordinate
* @note The returned map coordinates are guaranteed to lie in the map.
*/
void World2MapWithBoundary(double wx, double wy, int &mx, int &my) const;
/**
* @brief 给出两个地图坐标...计算相关的索引
* @param mx x坐标
* @param my y坐标
*@return 相关索引
* @brief Given two map coordinates... compute the associated index
* @param mx The x coordinate
* @param my The y coordinate
* @return The associated index
*/
inline unsigned int GetIndex(unsigned int mx, unsigned int my) const {
return my * size_x_ + mx;
}
/**
* @brief 给定一个索引，计算相关的地图坐标。
* @param index 指数
* @param mx 将被设置为x坐标
* @param my 将被设置为y坐标
* @brief Given an index, compute the associated map coordinates
* @param index The index
* @param mx Will be set to the x coordinate
* @param my Will be set to the y coordinate
*/
inline void Index2Cells(unsigned int index, unsigned int &mx, unsigned int &my) const {
my = index / size_x_;
mx = index - (my * size_x_);
}
/**
* @brief 将返回一个指向作为成本图的底层无符号字符数组的指针
* @return 一个指向存储成本值的底层无符号字符数组的指针
* @brief Will return a pointer to the underlying unsigned char array used as the costmap
* @return A pointer to the underlying unsigned char array storing cost values
*/
unsigned char *GetCharMap() const;
/**
* @brief 以单元格为单位的costmap的x大小的访问器
* @return 以单元格为单位的成本图的X尺寸
* @brief Accessor for the x size of the costmap in cells
* @return The x size of the costmap in cells
*/
unsigned int GetSizeXCell() const;
/**
* @brief 以单元格为单位的costmap的y大小的访问器
* @return 以单元格为单位的成本图的Y尺寸
* @brief Accessor for the y size of the costmap in cells
* @return The y size of the costmap in cells
*/
unsigned int GetSizeYCell() const;
/**
* @brief 成本图的x尺寸的访问器，单位是米
* @return 成本地图的x尺寸（返回地图中最后一个合法单元的中心点）
* @brief Accessor for the x size of the costmap in meters
* @return The x size of the costmap (returns the centerpoint of the last legal cell in the map)
*/
double GetSizeXWorld() const;
/**
* @brief 成本图的y尺寸的访问器，单位是米
* @return Costmap的y尺寸（返回地图中最后一个合法单元的中心点）
* @brief Accessor for the y size of the costmap in meters
* @return The y size of the costmap (returns the centerpoint of the last legal cell in the map)
*/
double GetSizeYWorld() const;
/**
* @brief 成本图的x原点的访问器
* @return 成本图的x原点
* @brief Accessor for the x origin of the costmap
* @return The x origin of the costmap
*/
double GetOriginX() const;
/**
* @brief 成本图的y原点访问器
* @return 成本图的y原点
* @brief Accessor for the y origin of the costmap
* @return The y origin of the costmap
*/
double GetOriginY() const;
/**
* @brief 成本图的分辨率的访问器
* @return 成本图的分辨率
* @brief Accessor for the resolution of the costmap
* @return The resolution of the costmap
*/
double GetResolution() const;
/**
* @brief 设置成本图的默认地图
* @param c 要设置的成本值
* @brief Set the default map of the costmap
* @param c the cost value to set
*/
void SetDefaultValue(unsigned char c) {
default_value_ = c;
}
/**
* @brief 获取成本图的默认值
* 返回地图的默认值
* @brief Get the default value of the costmap
* @return the default value of the map
*/
unsigned char GetDefaultValue() const {
return default_value_;
}
/**
* @brief 将一个凸形多边形的成本设定为一个期望值
* @param polygon 要执行操作的多边形
* @param cost_value 要设置成本的值
* 如果多边形被填充，则返回True，如果不能被填充，则返回False。
* @brief Sets the cost of a convex polygon to a desired value
* @param polygon The polygon to perform the operation on
* @param cost_value The value to set costs to
* @return True if the polygon was filled, false if it could not be filled
*/
bool SetConvexRegionCost(const std::vector<geometry_msgs::Point> &polygon_edge_world, unsigned char value);