vhwanger

// simulate sensing the cells
for (int i = 0; i < (int)sensecells.size(); i++) {
	int x = CONTXY2DISC(startx, cellsize_m) + sensecells.at(i).x;
	int y = CONTXY2DISC(starty, cellsize_m) + sensecells.at(i).y;
	// ignore if outside the map
	if (x < 0 || x >= size_x || y < 0 || y >= size_y) {
		continue;
	}

	int index = x + y * size_x;

vhwanger

"""
Plots paths outputted from the xytheta example. 
 
Usage: 
    $ python visualize.py [cfg file] [solution file]
 
"""
import sys
import re
import numpy as np

vhwanger

#include <cstring>
#include <iostream>
#include <string>
 
using namespace std;
 
#include <sbpl/headers.h>
 
// creating the footprint
void createFootprint(vector<sbpl_2Dpt_t>& perimeter){

vhwanger

void planxythetalat(char* envCfgFilename, char* motPrimFilename){
    // set the perimeter of the robot   
    vector<sbpl_2Dpt_t> perimeter;
    createFootprint(perimeter);

    // initialize an environment
    EnvironmentNAVXYTHETALAT env;
    initializeEnv(env, perimeter, envCfgFilename, motPrimFilename);

    // specify a start and goal state