/** @brief Set the odometry topic. This overrides what was set in the constructor, if anything. * * This unsubscribes from the old topic (if any) and subscribes to the new one (if any). * * If odom_topic is the empty string, this just unsubscribes from the previous topic. */ voidsetOdomTopic(std::string odom_topic);
private: std::string odom_topic_; // we listen on odometry on the odom topic ros::Subscriber odom_sub_; nav_msgs::Odometry base_odom_; boost::mutex odom_mutex_; // global tf frame id std::string frame_id_; ///< The frame_id associated this data };
Checks if any obstacles in the costmap lie inside a convex footprint that is rasterized into the grid.
参数:
position The position of the robot in world coordinates
footprint The specification of the footprint of the robot in world coordinates
inscribed_radius The radius of the inscribed circle of the robot
circumscribed_radius The radius of the circumscribed circle of the robot
返回值: Positive if all the points lie outside the footprint, negative otherwise: -1 if footprint covers at least a lethal obstacle cell, or -2 if footprint covers at least a no-information cell, or -3 if footprint is [partially] outside of the map
doubleCostmapModel::footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint, double inscribed_radius, double circumscribed_radius ) { //used to put things into grid coordinates unsignedint cell_x, cell_y; //get the cell coord of the center point of the robot if(!costmap_.worldToMap(position.x, position.y, cell_x, cell_y)) return-3.0;
//省略 footprint的点少于3个，只取点位置的代价 if(footprint.size() < 3) { ...... } // 把足迹视为多边形，循环调用 lineCost 计算多边形各边的cell，注意首尾闭合，最后返回代价 // 对于圆形底盘，按16边形处理 // now we really have to lay down the footprint in the costmap grid unsignedint x0, x1, y0, y1; double line_cost = 0.0; double footprint_cost = 0.0; // we need to rasterize each line in the footprint for(unsignedint i = 0; i < footprint.size() - 1; ++i) { //get the cell coord of the first point if(!costmap_.worldToMap(footprint[i].x, footprint[i].y, x0, y0)) return-3.0;
//get the cell coord of the second point if(!costmap_.worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1)) return-3.0;
line_cost = lineCost(x0, x1, y0, y1); footprint_cost = std::max(line_cost, footprint_cost); //if there is an obstacle that hits the line, return false if(line_cost < 0) return line_cost; } // 以下是连接 footprint 的第一个和最后一个点 //get the cell coord of the last point if(!costmap_.worldToMap(footprint.back().x, footprint.back().y, x0, y0)) return-3.0;
//get the cell coord of the first point if(!costmap_.worldToMap(footprint.front().x, footprint.front().y, x1, y1)) return-3.0;
doubleCostmapModel::pointCost(int x, int y)const { unsignedchar cost = costmap_.getCost(x, y); //if the cell is in an obstacle the path is invalid if(cost == NO_INFORMATION) return-2; if(cost == LETHAL_OBSTACLE) return-1;
std::vector<geometry_msgs::Point> makeFootprintFromRadius(double radius) { std::vector<geometry_msgs::Point> points; // Loop over 16 angles around a circle making a point each time int N = 16; geometry_msgs::Point pt; for (int i = 0; i < N; ++i) { double angle = i * 2 * M_PI / N; pt.x = cos(angle) * radius; pt.y = sin(angle) * radius;
voidCostmap2DROS::updateMap() { if (!stop_updates_) { // get pose in global frame of costmap tf::Stamped < tf::Pose > pose; if (getRobotPose (pose)) { double x = pose.getOrigin().x(), y = pose.getOrigin().y(), yaw = tf::getYaw(pose.getRotation());
tf_static是静态转换， 和tf的格式一样， any transform on this topic can be considered true for all time. Internally any query for a static transform will return true.
tf_static的发布者使用latched topics, the tf2_rosstatic_transform_publisher does this correctly. Note: avoid multiple latched static transform publishers on /tf_static in the same process, because with multiple latched publishers within the same node only one will latch correctly.
