C++ OccupancyGrid类代码示例

/* main */
int main (int argc, char** argv)
{
  
  if (argc < 3)
    usage(argv[0]);

  // settings
  double gamma =   1; // penalty for discontinuity in space
                        // (weight for probability difference pairwise cost)
                        // > 1 does not seem to make a difference
  double unknown = 0.2; // prior; below 0.5 makes sense

  // command line arguments
  if (argc >= 4)
    gamma = atof(argv[3]);
  if (argc >= 5)
    unknown = atof(argv[4]);

  // open file
  OccupancyGrid occgrid;
  occgrid.load(argv[1]);
  occgrid.graphcut(gamma, unknown);

  // save; use extension (.bt, .ot) to determine file format
  bool binary = (string(argv[2]).find_last_of(".bt") > -1);
  occgrid.save(argv[2], binary);
  cout << "Result saved as " << argv[2];
  if (binary)
    cout << " (binary)";
  cout << endl;

  return 0;
}

int main(int argc, const char *argv[])
{
  typedef double EnergyType;
  global_vis_.enable_show();
  global_vis2_.enable_show();
  // parse arguments ///////////////////////////////////////////
  // Declare the supported options.
  po::options_description desc("Run dual decomposition");
  desc.add_options()
    ("help", "produce help message")
    ("width", po::value<double>(), "Width")
    ("height", po::value<double>(), "Height")
    ("resolution", po::value<double>()->required(), "Size of square cell in the map")
    ("step", po::value<EnergyType>()->default_value(50), "step size for algorithm")
    ("dir", po::value<std::string>()->default_value("Data/player_sim_with_reflectance"), "Data directory")
    ("clock", po::value<double>()->default_value(400), "Max clock")
;

  po::positional_options_description pos;
  pos.add("resolution", 1);

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).options(desc).positional(pos).run(), vm);
  po::notify(vm);    

  EnergyType step = vm["step"].as<EnergyType>();
  std::string directory = vm["dir"].as<std::string>();
  double max_clock = CLOCKS_PER_SEC * vm["clock"].as<double>();
  // end of parse arguments ////////////////////////////////////
  OccupancyGrid occupancyGrid = loadOccupancyGrid(vm);

  global_vis_.init(occupancyGrid.height(), occupancyGrid.width());
  global_vis2_.init(occupancyGrid.height(), occupancyGrid.width(), "d");

  typedef gtsam::Index SampleSpaceType;
  typedef gtsam::Index vertex_descriptor;
  typedef G<OccupancyGrid, EnergyType, SampleSpaceType, vertex_descriptor> OccupancyGridGraph;
  typedef hash_property_map< std::pair<vertex_descriptor, vertex_descriptor>, SampleSpaceType> MultiAssignment;
  typedef DisagreementTracker<OccupancyGridGraph, MultiAssignment> DisagreementMap;
  typedef hash_property_map< typename OccupancyGridGraph::MessageTypes::key_type, EnergyType> Messages;
  typedef SlaveMinimizer<OccupancyGridGraph,
          DDLaserFactor<DisagreementMap, Messages>, Messages, DisagreementMap> SlaveMinimizer_;
  OccupancyGridGraph ogg(occupancyGrid);
  SlaveMinimizer_ slvmin(ogg);
  typedef typename OccupancyGridGraph::SampleSpaceMap SampleSpaceMap;
  SampleSpaceMap ssm = get(sample_space_t(), ogg);
  Messages messages(0);
  MultiAssignment multiassign(0);
  DisagreementMap disagreement_map(ogg, multiassign);
  typedef SubgradientDualDecomposition<OccupancyGridGraph, SlaveMinimizer_,
          SampleSpaceMap, DisagreementMap, Messages, SampleSpaceType, EnergyType> SubgradientDualDecompositionType;
  SubgradientDualDecompositionType subg_dd(ogg, slvmin, ssm, disagreement_map, messages, step);
  iterate_dualdecomposition<OccupancyGridGraph, SubgradientDualDecompositionType, DisagreementMap, SampleSpaceType>(ogg, subg_dd, disagreement_map, 70, max_clock);
  std::stringstream ss;
  ss << directory << "/dualdecomposition.png";
  global_vis_.save(ss.str());
}

int main(int argc, char** argv) {

    std::string inputfile;
    float radius;
    float initx, inity, initTheta, goalx, goaly, goalr, goalTheta;
    int depth;
    bool heuristic;
    float inflate;

    goalTheta = 0;

    parseArgs(argc, argv, inputfile, radius, initx, inity, initTheta, goalx, goaly, goalr, goalTheta, depth, heuristic, inflate);

    // Load grid
    OccupancyGrid grid;
    grid.load(inputfile);

    if (grid.empty()) {
        std::cerr << "error loading grid!\n";
        return 1;
    }

    // expand obstacles
    if (radius > 0) {
        OccupancyGrid tmp;
        expandObstacles(grid, radius, tmp, false);
        grid = tmp;
    }

    // Run search
    DubinSearch helper;
    GridChecker* checker = new GridChecker(&grid);
    Dubin* goal = helper.search(initx, inity, initTheta, goalx, goaly, goalr, goalTheta, checker, depth, heuristic, inflate);
    if (goal==NULL) {
        cout<<"Search failed"<<endl;
        std::string filename = "DebugImage.svg";
        helper.makeSVG(NULL,filename,grid,goalx,goaly,goalr);

        return -1;
    }
    else {
        cout<<"Search successful after "<<helper.getCount()<<" expansions"<<endl;
        std::string filename = "DebugImage.svg";
        helper.makeSVG(goal,filename,grid,goalx,goaly,goalr);
    }

    return 0;
}

void OccupancyPredictor::updatePrediction(const OccupancyGrid &grid)
{
    int minX, maxX, minY, maxY;
    grid.getBounds(minX, maxX, minY, maxY);
    for (int x = minX; x < maxX; ++x)
    {
        for (int y = minY; y < maxY; ++y)
        {
            if (grid.getCellProbability(QPointF(x, y)) > 0.8)
            {
                _prediction[x][y].occupied++;
            }
            else
            {
                _prediction[x][y].free++;
            }
        }
    }
}

//Updates an occupancy map by modifying cells along the beam using beamval and modifying the end-cell using endval.
void _mapUpdateTrace(OccupancyGrid& occ_map, GridRayTrace& trace, logit_val beamval, logit_val endval)
{
	int i, j;
	while(trace.getNextPoint(i, j))
	{
		//ROS_WARN("%d, %d", i, j);
		if(0 <= i && i < occ_map.getWidth() && 0 <= j && j < occ_map.getHeight())
		{
			if(trace.hasNext())
			{
				occ_map.valueAt(i, j) += beamval;
			}
			else
			{
				occ_map.valueAt(i, j) += endval;
			}
		}
	}
}

GridRayTrace::GridRayTrace(tf::Vector3 start, tf::Vector3 end, const OccupancyGrid& grid_ref)
	: _x_store(), _y_store()
{
	//Transform (x,y) into map frame
	start = grid_ref.toGridFrame(start);
	end = grid_ref.toGridFrame(end);
	
	double x0 = start.getX(), y0 = start.getY();
	double x1 = end.getX(), y1 = end.getY();
	
	//ROS_WARN("start: (%f, %f) -> end: (%f, %f)", x0, y0, x1, y1);
	
	bresenham
		(
			floor(x0), floor(y0), 
			floor(x1), floor(y1), 
			_x_store, _y_store
		);
	
	_cur_idx = 0;
	_max_idx = std::min(_x_store.size(), _y_store.size());
}

    bool can_move(TetPiece const& p, int dx, int dy, OccupancyGrid const& og)
	{
		Point2 pos = p.get_position();
		pos.x -= 1;
		pos.y -= 1;

		vector<Point2> const& blocks = p.piece_blocks();
		for (size_t i = 0; i < blocks.size(); i++)
		{
			int r_index = pos.y + dy + blocks[i].y;
			int c_index = pos.x + dx + blocks[i].x;
			
			if (!og.is_valid_cell(r_index, c_index))
				return false;

			occupancy_cell cell = og.get_cell(r_index, c_index);
			if (cell.v != -1)
				return false;
		}
		
		return true;
	}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "occupancy_grid");

    OccupancyGrid map;
    _map = ↦

    map.init();
    map.mapInit();

    ros::Rate loop_rate(20.0);

    while(map.n.ok())
    {
        ros::spinOnce();
        map.gridVisualize();
        loop_rate.sleep();
    }

    save_maps(1);

    return 0;
}

void DubinSearch::makeSVG(Dubin* goal, std::string filename, OccupancyGrid& grid, 
							float goalx, float goaly, float goalr, int zoom){

    //TODO
    FILE* svg = fopen(filename.c_str(), "w");
    if (!svg) {
        std::cerr << "error opening svg for output! \n";
        return;
    }

    int width = grid.nx() * zoom;
    int height = grid.ny() * zoom;

    fprintf(svg, "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n");
    fprintf(svg, "<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\"\n");
    fprintf(svg, "  \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n");
    fprintf(svg, "<svg version=\"1.1\" xmlns=\"http://www.w3.org/2000/svg\"\n");
    fprintf(svg, "     xmlns:xlink=\"http://www.w3.org/1999/xlink\"\n");
    fprintf(svg, "     width=\"%upx\" height=\"%upx\" viewBox=\"0 0 %u %u\" >\n", 
          width, height, width, height);


    // Draw background and obstacle
    int f;
    for (size_t y=0; y<grid.ny(); ++y) {
        for (size_t x=0; x<grid.nx(); ++x) {

            if (grid(x,y)==0){
                f = 0;
            }
            else{
                f = 255;
            }
                    
            fprintf(svg, "  <rect x=\"%d\" y=\"%d\" width=\"%d\" height=\"%d\" "
                "fill=\"#%02x%02x%02x\" />\n", 
                (int)x*zoom, (int)y*zoom, zoom, zoom, f, f, f);
        }
    }
	
	fprintf(svg, "  <circle cx=\"%f\" cy=\"%f\" r=\"%f\" stroke=\"red\" stroke-width=\"2\" fill=\"none\"/>\n",
				goalx*zoom, goaly*zoom, goalr*zoom);
   
    fprintf(svg, "  <g stroke=\"#bfbfbf\" stroke-width=\"1.5\">\n");

    // Draw nodes and lines expanded
    Dubin* curr;
    Dubin* pred;
    while (!_poppedNodes.empty()){
        curr = _poppedNodes.back();
        float cx = (curr->x + 0.5)*zoom;
        float cy = (curr->y + 0.5)*zoom;
        pred = curr->predecessor;
        float r = 0.2 * zoom;
        
        if(pred!=NULL){
            float cxp = (pred->x + 0.5)*zoom;
            float cyp = (pred->y + 0.5)*zoom;
			float r = curr->archRadius * zoom;
			if (r ==0) {
				fprintf(svg, "    <line x1=\"%f\" y1=\"%f\" x2=\"%f\" y2=\"%f\" %s/>\n",
					cx, cy, cxp, cyp, "");
			} else {
				fprintf(svg, "    <path d=\"M %f %f A %f %f 0 0 %d %f %f \" fill=\"none\"/>\n",
					cxp, cyp, fabs(r), fabs(r), int(r>0), cx, cy);
			}
				

        }

        fprintf(svg, "    <circle cx=\"%f\" cy=\"%f\" r=\"%f\" fill=\"%s\" %s/>\n",
                cx, cy, r, "#ffffff","");
        

        _poppedNodes.pop_back();
    }
	
	while (!_queue.empty()){
        curr = _queue.back();
        float cx = (curr->x + 0.5)*zoom;
        float cy = (curr->y + 0.5)*zoom;
        pred = curr->predecessor;
        float r = 0.5 * zoom;
        
        if(pred!=NULL){
            float cxp = (pred->x + 0.5)*zoom;
            float cyp = (pred->y + 0.5)*zoom;
			float r = curr->archRadius * zoom;
			if (r ==0) {
				fprintf(svg, "    <line x1=\"%f\" y1=\"%f\" x2=\"%f\" y2=\"%f\" %s/>\n",
					cx, cy, cxp, cyp, "");
			} else {
				fprintf(svg, "    <path d=\"M %f %f A %f %f 0 0 %d %f %f \" fill=\"none\"/>\n",
					cxp, cyp, fabs(r), fabs(r), int(r>0), cx, cy);
			}
				

        }
		fprintf(svg, "  </g>\n");
		fprintf(svg, "  <g stroke=\"#0000ff\" stroke-width=\"1\">\n");
//.........这里部分代码省略.........

bool valid(const vec2i& p) {
  return ( p.x() >= 0 && 
           p.y() >= 0 && 
           size_t(p.x()) < grid.nx() && 
           size_t(p.y()) < grid.ny() &&
           grid(p.x(), p.y()) );
}

int main(int argc, char** argv) {

  glutInit(&argc, argv);
  glutInitDisplayMode(GLUT_DOUBLE | GLUT_DEPTH | GLUT_RGB);
  glutInitWindowSize(640, 480);

  if (argc < 2) { usage(1); }

  grid.load(argv[1]);
  if (grid.empty()) { 
    std::cerr << "Error loading grid: " << argv[1] << "\n";
    exit(1);
  }

  glutCreateWindow("Biped plan demo");
  glutDisplayFunc(display);
  glutReshapeFunc(reshape);
  glutKeyboardFunc(keyboard);
  glutMouseFunc(mouse);
  glutMotionFunc(motion);
  
  glClearColor(1,1,1,1);

  glGenTextures(1, &grid_texture);
  glBindTexture(GL_TEXTURE_2D, grid_texture);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);

  std::vector<unsigned char> rgbbuf(grid.nx() * grid.ny() * 4);
  int offs = 0;
  for (size_t y=0; y<grid.ny(); ++y) {
    for (size_t x=0; x<grid.nx(); ++x) {
      rgbbuf[offs++] = grid(x,y);
      rgbbuf[offs++] = grid(x,y);
      rgbbuf[offs++] = grid(x,y);
      rgbbuf[offs++] = 255;
    }
  }
    
  glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA,
               grid.nx(), grid.ny(), 0, GL_RGBA,
               GL_UNSIGNED_BYTE, &(rgbbuf[0]));

  quadric = gluNewQuadric();

  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  glutMainLoop();

  return 0;

}

void reshape(int w, int h) {

  width = w;
  height = h;

  glViewport(0, 0, width, height);

  float ga = float(grid.nx())/grid.ny();
  float wa = float(w)/h;

  float wh = grid.ny();
  float ww = grid.nx();

  if (ga > wa) {
    wh *= ga/wa;
  } else {
    ww *= wa/ga;
  }

  float cx = 0.5*grid.nx();
  float cy = 0.5*grid.ny();

  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();

  gluOrtho2D(cx - 0.5 * ww, cx + 0.5*ww, 
             cy + 0.5 * wh, cy - 0.5*wh);

  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();

  glGetIntegerv(GL_VIEWPORT, viewport);
  glGetDoublev(GL_PROJECTION_MATRIX, projection);
  glGetDoublev(GL_MODELVIEW_MATRIX, modelview);

}

void display() {

  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  glEnable(GL_TEXTURE_2D);
  glBindTexture(GL_TEXTURE_2D, grid_texture);

  glColor3ub(255,255,255);

  glBegin(GL_QUADS);

  glTexCoord2f(1, 0);
  glVertex2f(grid.nx(), 0);

  glTexCoord2f(0, 0);
  glVertex2f(0, 0);

  glTexCoord2f(0, 1);
  glVertex2f(0, grid.ny());

  glTexCoord2f(1, 1);
  glVertex2f(grid.nx(), grid.ny());

  glEnd();

  glDisable(GL_TEXTURE_2D);

  if (valid(init)) {
    glPushMatrix();

    glColor3ub(63,255,63);
    glTranslated(init.x(), init.y(), 0);
    gluDisk(quadric, 0, goalr, 32, 1);

    glRotated(initTheta*180/M_PI, 0, 0, 1);
    glBegin(GL_TRIANGLES);
    glVertex2f(6, 0);
    glVertex2f(0, 2);
    glVertex2f(0, -2);
    glEnd();

    glPopMatrix();

  }

  if (valid(goal)) {
    glPushMatrix();
    glColor3ub(255,63,63);
    glTranslated(goal.x(), goal.y(), 0);
    gluDisk(quadric, 0, goalr, 32, 1);
    glPopMatrix();
  }

  draw(searchResult, true);

  glMatrixMode(GL_PROJECTION);
  glPushMatrix();
  glLoadIdentity();
  gluOrtho2D(0, width, 0, height);

  std::ostringstream ostr;
  ostr << "Goal pos:     (" << goal.x() << ", " << goal.y() << ")\n"
       << "Init pos:     (" << init.x() << ", " << init.y() << ")\n"
       << "Init theta:   " << initTheta*180/M_PI << "\n"
       << "XY inflation: " << inflate_h << "\n"
       << "Z inflation:  " << inflate_z << "\n"
       << "Max depth:    " << maxDepth << "\n"
       << "View depth:   " << viewDepth << "\n"
       << "Auto-plan:    " << (auto_plan ? "on" : "off") << "\n"
       << "\n"
       << "Plan cost: " << (searchResult ? searchResult->costToCome : 0) << "\n"
       << "Plan time: " << (searchResult ? planTime : 0) << "\n";

  if (show_help) { 
    ostr << "\n\n"
         << " Left-click init pos\n"
         << "Shift+click init theta\n"
         << "Right-click goal pos\n\n"
         << "        1/2 max depth\n"
         << "        3/4 view depth\n"
         << "        -/+ XY inflation\n"
         << "          A auto-plan\n"
         << "      Enter re-plan\n"
         << "        ESC quit\n"
         << "          ? hide help";
  } else {
    ostr << "\nPress ? to toggle help.";
  }
    


  std::string str = ostr.str();
  
  void* font = GLUT_BITMAP_8_BY_13;
  const int tx = 8;
  const int ty = 13;
  const int th = ty+2;
  int maxwidth = 0;
  int linewidth = 0;
  int rh = th;
//.........这里部分代码省略.........

int main(int argc, char** argv){
    double cruising_speed = 0.1;
    char* baseframe;
    char* childframe;
    if (argc < 4) {
        ROS_ERROR("(%i) usage: hallwaydrive <speed> <base frame> <child frame>\n", argc);
        return -1;    
    }
    else {    
        cruising_speed = atof(argv[1]);
        baseframe = argv[2];
        childframe = argv[3];
    }
    ros::init(argc, argv, "odometry_publisher");
    ros::NodeHandle n;
    ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
    tf::TransformBroadcaster odom_broadcaster;
    ros::Rate loop_rate(5);
    ros::Time current_time;
    
    ros::Publisher speedController = n.advertise<Speeds>("speeds_bus", 50);
    
    /*message_filters::Subscriber<Position2D> pos2DSub(n, "/pos2d_bus", 1);
    pos2DSub.registerCallback(roombaOdomCallback);
    
    message_filters::Subscriber<LaserScan> laserSub(n, "/scan", 1);
    laserSub.registerCallback(laserCallback);*/
    
    ros::Subscriber pos2DSub = n.subscribe("pos2d_bus", 100, roombaOdomCallback);
    ros::Subscriber laserSub = n.subscribe("scan", 100, laserCallback);

    while(n.ok()){
        current_time = ros::Time::now();

        //since all odometry is 6DOF we'll need a quaternion created from yaw
        geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(pose.angle);

        //first, we'll publish the transform over tf
        geometry_msgs::TransformStamped odom_trans;
        odom_trans.header.stamp = current_time;
        odom_trans.header.frame_id = baseframe;
        odom_trans.child_frame_id = childframe;

        odom_trans.transform.translation.x = pose.x;
        odom_trans.transform.translation.y = pose.y;
        odom_trans.transform.translation.z = 0.0;
        odom_trans.transform.rotation = odom_quat;

        //send the transform
        odom_broadcaster.sendTransform(odom_trans);

        //next, we'll publish the odometry message over ROS
        nav_msgs::Odometry odom;
        odom.header.stamp = current_time;
        odom.header.frame_id = baseframe;

        //set the position
        odom.pose.pose.position.x = pose.x;
        odom.pose.pose.position.y = pose.y;
        odom.pose.pose.position.z = 0.0;
        odom.pose.pose.orientation = odom_quat;


        //ROS_INFO("Odometry: (%.3lf, %.3lf) at %.3lf radians\n", pose.x, pose.y, pose.angle);

        double speed = 0.0, angle = 0.0;
        /*if (grid.closeObstacle) {
            double closestAngle = grid.getObstacleLaserAngle();
            ROS_INFO("Close Obstacle at angle %.3lf at dist %lf", closestAngle, grid.minDist);
            //If there's an obstacle really close, turn away from that obstacle
            if (closestAngle < 0)
                angle = closestAngle + 3.141/2.0;
            else
                angle = closestAngle - 3.141/2.0;
            angle /= (1.0 + grid.minDist*4.0);
        }
        else {*/
            Point centroid = grid.GetCentroid();
            angle = centroid.getAngle();
            angle = angle / 3.141;
            angle = angle*angle*angle;
            angle *= 3.141;
        //}
        speed = cruising_speed / (1.0 + 5.0 * fabs(angle));

        //Publish the speed message
        Speeds speedMsg;
        speedMsg.forward = speed;
        speedMsg.rotate = angle;
        
        if(!n.hasParam("/hallwaydrive/automatic")) {
            ROS_ERROR("Unable to find parameter automatic");
            return -1;
        }
        
        int automatic;
        n.param("/hallwaydrive/automatic", automatic, 0);
        //ROS_INFO("automatic = (%i)", automatic);
        if (automatic == 1) { //Only send commands if the user wants
        //automatic control; otherwise they will interfere with the controller GUI
//.........这里部分代码省略.........

void laserCallback(const boost::shared_ptr<const LaserScan>& s) {
    grid.fill(s);
    grid.findCloseObstacleBin(s);
}