# Eigen parallelise itself, OPENMP is experimental. We experienced some slowdown with it set(G2O_USE_OPENMP OFF CACHE BOOL "Build g2o with OpenMP support (EXPERIMENTAL)") if(G2O_USE_OPENMP) find_package(OpenMP) if(OPENMP_FOUND) MESSAGE(" ----------------- Using OpenMP ! ----------------- ") set (G2O_OPENMP 1) set(g2o_C_FLAGS "${g2o_C_FLAGS}${OpenMP_C_FLAGS}") set(g2o_CXX_FLAGS "${g2o_CXX_FLAGS} -DEIGEN_DONT_PARALLELIZE ${OpenMP_CXX_FLAGS}") message(STATUS "Compiling with OpenMP support") endif(OPENMP_FOUND) endif(G2O_USE_OPENMP)
/usr/bin/ld: CMakeFiles/ls_slam_g2o.dir/src/main.cpp.o: undefined reference to symbol 'omp_set_lock@@OMP_3.0' //usr/lib/x86_64-linux-gnu/libgomp.so.1: error adding symbols: DSO missing from command line collect2: error: ld returned 1 exit status
/** @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());