RayTraceIterRange rayTrace (const nm::MapMetaData& info, const gm::Point& p1, const gm::Point& p2,
bool project_onto_grid, bool project_source_onto_grid,
float max_range)
{
gm::Point np2 = p2;
if (max_range > 0) {
double distance = euclideanDistance(p1,p2);
if (distance > max_range) {
np2.x = ((p2.x - p1.x) * max_range/distance) + p1.x;
np2.y = ((p2.y - p1.y) * max_range/distance) + p1.y;
}
}
Cell c1 = pointCell(info, p1);
Cell c2 = pointCell(info, np2);
ROS_DEBUG_STREAM_NAMED ("ray_trace", "Ray tracing between " << c1.x <<
", " << c1.y << " and " << c2.x << ", " << c2.y);
const RayTraceIterator done(c1, c1, true);
const RayTraceIterRange empty_range(done, done);
if (!withinBounds(info, c1)) {
if (project_source_onto_grid) {
const optional<Cell> c = rayTraceOntoGrid(info, c2, c1);
if (c)
c1 = *c;
else
return empty_range;
}
else
throw PointOutOfBoundsException(p1);
}
if (!withinBounds(info, np2)) {
if (project_onto_grid) {
const optional<Cell> c = rayTraceOntoGrid(info, c1, c2);
if (c)
c2 = *c;
else
return empty_range;
}
else {
throw PointOutOfBoundsException(np2);
}
}
ROS_DEBUG_STREAM_NAMED ("ray_trace", "Projected ray trace endpoints to " << c1.x <<
", " << c1.y << " and " << c2.x << ", " << c2.y);
return rayTrace(c1, c2);
}
unsigned int GameBoard::destinationCheck(const uint8_t tMask, const uint8_t oMask, const Move &i_move, const unsigned int v_pos, const unsigned int h_pos)
{
if (withinBounds(v_pos, h_pos))
{
if((i_move.move_board[v_pos][h_pos] & tMask) == oMask)
{
// Square has opponent piece
return 1;
}
else if (i_move.move_board[v_pos][h_pos] == 0)
{
// Square is empty
return 0;
}
else
{
// Square is same team
return 2;
}
}
else
{
// Square is out of bounds
return 3;
}
}
static void
create_path(circuitstruct * wp, int n)
{
int col, row;
int count = 0;
int dir, prob;
int nextcol, nextrow, i;
#ifdef RANDOMSTART
/* Path usually "mushed" in a corner */
col = NRAND(wp->ncols) + 1;
row = NRAND(wp->nrows) + 1;
#else
/* Start from center */
col = wp->ncols / 2;
row = wp->nrows / 2;
#endif
wp->mincol = col - 1, wp->minrow = row - 2;
wp->maxcol = col + 1, wp->maxrow = row + 2;
dir = NRAND(wp->neighbors) * ANGLES / wp->neighbors;
*(wp->newcells + col + row * wp->bncols) = HEAD;
while (++count < n) {
prob = NRAND(wp->prob_array[wp->neighbors - 1]);
i = 0;
while (prob > wp->prob_array[i])
i++;
dir = ((dir * wp->neighbors / ANGLES + i) %
wp->neighbors) * ANGLES / wp->neighbors;
nextcol = col;
nextrow = row;
positionOfNeighbor(wp, dir, &nextcol, &nextrow);
if (withinBounds(wp, nextcol, nextrow)) {
col = nextcol;
row = nextrow;
if (col == wp->mincol && col > 2)
wp->mincol--;
if (row == wp->minrow && row > 2)
wp->minrow--;
else if (row == wp->minrow - 1 && row > 3)
wp->minrow -= 2;
if (col == wp->maxcol && col < wp->bncols - 3)
wp->maxcol++;
if (row == wp->maxrow && row < wp->bnrows - 3)
wp->maxrow++;
else if (row == wp->maxrow + 1 && row < wp->bnrows - 4)
wp->maxrow += 2;
if (!*(wp->newcells + col + row * wp->bncols))
*(wp->newcells + col + row * wp->bncols) = WIRE;
} else {
if (wp->neighbors == 3)
break; /* There is no reverse step */
dir = ((dir * wp->neighbors / ANGLES + wp->neighbors / 2) %
wp->neighbors) * ANGLES / wp->neighbors;
}
}
*(wp->newcells + col + row * wp->bncols) = HEAD;
}
RayTraceIterRange rayTrace (const nm::MapMetaData& info, const gm::Point& p1, const gm::Point& p2,
bool project_onto_grid, bool project_source_onto_grid)
{
Cell c1 = pointCell(info, p1);
Cell c2 = pointCell(info, p2);
ROS_DEBUG_STREAM_NAMED ("ray_trace", "Ray tracing between " << c1.x <<
", " << c1.y << " and " << c2.x << ", " << c2.y);
const RayTraceIterator done(c1, c1, true);
const RayTraceIterRange empty_range(done, done);
if (!withinBounds(info, c1)) {
if (project_source_onto_grid) {
const optional<Cell> c = rayTraceOntoGrid(info, c2, c1);
if (c)
c1 = *c;
else
return empty_range;
}
else
throw PointOutOfBoundsException(p1);
}
if (!withinBounds(info, p2)) {
if (project_onto_grid) {
const optional<Cell> c = rayTraceOntoGrid(info, c1, c2);
if (c)
c2 = *c;
else
return empty_range;
}
else {
throw PointOutOfBoundsException(p2);
}
}
ROS_DEBUG_STREAM_NAMED ("ray_trace", "Projected ray trace endpoints to " << c1.x <<
", " << c1.y << " and " << c2.x << ", " << c2.y);
return rayTrace(c1, c2);
}
/**
* If the current waypoint has been reached then advance to the next.
* If no new waypoint then return true, otherwise false.
*/
bool MoveArm::goalReached(){
for(unsigned int i = currentWaypoint; i < plan.path.get_states_size(); i++){
if(withinBounds(i)){
std::cout << "Accomplished waypoint " << i << std::endl;
currentWaypoint++;
}
else {
return false;
}
}
std::cout << "Goal Reached" << std::endl;
return true;
}
void addKnownFreePoint (OverlayClouds* overlay, const gm::Point& p, const double r)
{
const nm::MapMetaData& geom = overlay->grid.info;
const int cell_radius = floor(r/geom.resolution);
const Cell c = pointCell(geom, p);
for (int x= c.x-cell_radius; x<=c.x+cell_radius; x++)
{
for (int y=c.y-cell_radius; y<=c.y+cell_radius; y++)
{
const Cell c2(x, y);
if (withinBounds(geom, c2))
{
const index_t ind = cellIndex(geom, c2);
overlay->hit_counts[ind] = 0;
overlay->pass_through_counts[ind] = overlay->min_pass_through+1;
}
}
}
}
//! Set the coords for a coil.
bool rtCathDataObject::setCoilCoords(int coilID, double cx, double cy, double cz) {
if(!m_coilIDList.contains(coilID)) return false;
if (m_cathGuiSetup.gateBox->isChecked() && !withinBounds(rtApplication::instance().getTimeManager()->getTriggerDelay()))
return false;
if (m_coilList.contains(coilID)) {
m_coilList[coilID].cx = cx;
m_coilList[coilID].cy = cy;
m_coilList[coilID].cz = cz;
emit updateCoilTableSignal();
return true;
} else if (m_discardCoilList.contains(coilID)) {
m_discardCoilList[coilID].cx = cx;
m_discardCoilList[coilID].cy = cy;
m_discardCoilList[coilID].cz = cz;
emit updateCoilTableSignal();
return true;
}
return false;
}
//Update all openGL visuals of the bird object (structure)
void Bird::updateObjectDisplays(MatrixStack projectionStack) {
birdCentre[0] = objects[3]->getTranslation()[0];
birdCentre[1] = objects[3]->getTranslation()[1];
birdCentre[2] = objects[3]->getTranslation()[2];
//Check if we've hit the ground
if(birdCentre[1] <= -flyingBounds[1]) {
for(int i = 0; i < objects.size(); i++) {
objects[i]->resetFall();
if(!staticDrop)
objects[i]->initiateCrash();
}
staticDrop = false;
falling = false;
}
//Go through objects and display.
for(int i = 0; i < objects.size(); i++) {
objects[i]->updateDisplay(projectionStack, falling, birdCentre[1] >= flyingBounds[1],
staticDrop, withinBounds(), birdCentre);
}
}
std::set<Cell> tileCells (const nm::MapMetaData& info, const float d,
const Pred& pred)
{
ROS_DEBUG_NAMED ("tile", "Tiling %ux%u map", info.height, info.width);
Cells cells;
Cells forbidden;
int rad = ceil(d/info.resolution);
for (size_t x=0; x<info.width; x++)
{
for (size_t y=0; y<info.height; y++)
{
const Cell c(x, y);
if (!pred(c))
continue;
ROS_DEBUG_STREAM_NAMED("tile", "Cell " << c << " satisfies condition");
if (forbidden.find(c)!=forbidden.end())
continue;
ROS_DEBUG_STREAM_NAMED ("tile", " Sufficiently far");
cells.insert(c);
ROS_DEBUG_STREAM_NAMED ("tile", " Inserted");
for (int dx=0; dx<=rad; dx++)
{
for (int dy=-rad; dy<=rad; dy++)
{
const Cell c2(int(x)+dx, int(y)+dy);
if (dx*dx+dy*dy <= rad*rad && withinBounds(info, c2))
{
ROS_DEBUG_STREAM_NAMED ("tile", " Blocking " << c2);
forbidden.insert(c2);
}
}
}
}
}
ROS_DEBUG_NAMED("tile", "Done tiling");
return cells;
}
/**
* @brief Finds the next Cell for a LocalCoords object along a trajectory
* defined by some angle (in radians from 0 to pi).
* @details The method will update the LocalCoords passed in as an argument
* to be the one at the boundary of the next Cell crossed along the
* given trajectory. It will do this by recursively building a linked
* list of LocalCoords from the LocalCoords passed in as an argument
* down to the lowest level Cell found. In the process it will set
* the local coordinates for each LocalCoords in the linked list for
* the Lattice or Universe that it is in. If the LocalCoords is
* outside the bounds of the Lattice or on the boundaries this method
* will return NULL; otherwise it will return a pointer to the Cell
* that the LocalCoords will reach next along its trajectory.
* @param coords pointer to a LocalCoords object
* @param angle the angle of the trajectory
* @param universes a map of all of the Universes passed in by the Geometry
* @return a pointer to a Cell if found, NULL if no cell found
*/
Cell* Lattice::findNextLatticeCell(LocalCoords* coords, double angle,
std::map<int, Universe*> universes) {
/* Tests the upper, lower, left and right Lattice Cells adjacent to
* the LocalCoord and uses the one with the shortest distance from
* the current location of the LocalCoord */
/* Initial distance is infinity */
double distance = std::numeric_limits<double>::infinity();
/* Current minimum distance */
double d;
/* Properties of the current LocalCoords */
double x0 = coords->getX();
double y0 = coords->getY();
int lattice_x = coords->getLatticeX();
int lattice_y = coords->getLatticeY();
/* Slope of trajectory */
double m = sin(angle) / cos(angle);
/* Properties of the new location for LocalCoords */
/* Current point of minimum distance */
double x_curr, y_curr;
/* x-coordinate on new Lattice cell */
double x_new = x0;
/* y-coordinate on new Lattice cell */
double y_new = x0;
/* New x Lattice cell index */
int new_lattice_x;
/* New y Lattice cell index */
int new_lattice_y;
/* Test Point for computing distance */
Point test;
/* Check lower Lattice cell */
if (lattice_y >= 0 && angle >= M_PI) {
y_curr = (lattice_y - _num_y/2.0) * _width_y;
x_curr = x0 + (y_curr - y0) / m;
test.setCoords(x_curr, y_curr);
/* Check if the test Point is within the bounds of the Lattice */
if (withinBounds(&test)) {
d = test.distanceToPoint(coords->getPoint());
/* Check if distance to test Point is current minimum */
if (d < distance) {
distance = d;
x_new = x_curr;
y_new = y_curr;
}
}
}
/* Upper Lattice cell */
if (lattice_y <= _num_y-1 && angle <= M_PI) {
y_curr = (lattice_y - _num_y/2.0 + 1) * _width_y;
x_curr = x0 + (y_curr - y0) / m;
test.setCoords(x_curr, y_curr);
/* Check if the test Point is within the bounds of the Lattice */
if (withinBounds(&test)) {
d = test.distanceToPoint(coords->getPoint());
/* Check if distance to test Point is current minimum */
if (d < distance) {
distance = d;
x_new = x_curr;
y_new = y_curr;
}
}
}
/* Left Lattice cell */
if (lattice_x >= 0 && (angle >= M_PI/2 && angle <= 3*M_PI/2)) {
x_curr = (lattice_x - _num_x/2.0) * _width_x;
y_curr = y0 + m * (x_curr - x0);
test.setCoords(x_curr, y_curr);
/* Check if the test Point is within the bounds of the Lattice */
if (withinBounds(&test)) {
d = test.distanceToPoint(coords->getPoint());
/* Check if distance to test Point is current minimum */
if (d < distance) {
distance = d;
x_new = x_curr;
y_new = y_curr;
}
}
}
/* Right Lattice cell */
if (lattice_x <= _num_x-1 && (angle <= M_PI/2 || angle >= 3*M_PI/2)) {
x_curr = (lattice_x - _num_x/2.0 + 1) * _width_x;
y_curr = y0 + m * (x_curr - x0);
test.setCoords(x_curr, y_curr);
/* Check if the test Point is within the bounds of the Lattice */
if (withinBounds(&test)) {
d = test.distanceToPoint(coords->getPoint());
/* Check if distance to test Point is current minimum */
if (d < distance) {
distance = d;
x_new = x_curr;
y_new = y_curr;
}
}
}
/* If no Point was found on the Lattice cell, then the LocalCoords was
* already on the boundary of the Lattice */
if (distance == INFINITY)
return NULL;
/* Otherwise a Point was found inside a new Lattice cell */
else {
/* Update the Localcoords location to the Point on the new Lattice cell
* plus a small bit to ensure that its coordinates are inside cell */
double delta_x = (x_new - coords->getX()) + cos(angle) * TINY_MOVE;
double delta_y = (y_new - coords->getY()) + sin(angle) * TINY_MOVE;
coords->adjustCoords(delta_x, delta_y);
/* Compute the x and y indices for the new Lattice cell */
new_lattice_x = (int)floor((coords->getX() - _origin.getX())/_width_x);
new_lattice_y = (int)floor((coords->getY() - _origin.getY())/_width_y);
/* Check if the LocalCoord is on the lattice boundaries and if so adjust
* x or y Lattice cell indices i */
if (fabs(fabs(coords->getX()) - _num_x*_width_x*0.5) <
ON_LATTICE_CELL_THRESH) {
if (coords->getX() > 0)
new_lattice_x = _num_x - 1;
else
new_lattice_x = 0;
}
if (fabs(fabs(coords->getY()) - _num_y*_width_y*0.5) <
ON_LATTICE_CELL_THRESH) {
if (coords->getY() > 0)
new_lattice_y = _num_y - 1;
else
new_lattice_y = 0;
}
/* Check if new Lattice cell indices are within the bounds, if not,
* new LocalCoords is now on the boundary of the Lattice */
if (new_lattice_x >= _num_x || new_lattice_x < 0)
return NULL;
else if (new_lattice_y >= _num_y || new_lattice_y < 0)
return NULL;
/* New LocalCoords is still within the interior of the Lattice */
else {
/* Update the LocalCoords Lattice cell indices */
coords->setLatticeX(new_lattice_x);
coords->setLatticeY(new_lattice_y);
/* Move to next lowest level Universe */
coords->prune();
Universe* univ = _universes.at(new_lattice_y).at(new_lattice_x).second;
LocalCoords* next_coords;
/* Compute local position of Point in next level Universe */
double nextX = coords->getX() - (_origin.getX()
+ (new_lattice_x + 0.5) * _width_x);
double nextY = coords->getY() - (_origin.getY()
+ (new_lattice_y + 0.5) * _width_y);
/* Set the coordinates at the next level LocalCoord */
next_coords = new LocalCoords(nextX, nextY);
next_coords->setPrev(coords);
coords->setNext(next_coords);
next_coords->setUniverse(univ->getId());
/* Search lower level Universe */
return findCell(coords, universes);
}
}
}
void processMotors(struct OUTPUT_STRUCT output){
#if defined(SPYDER_EN)
int op1 = output.throttle + output.roll - output.pitch + output.yaw;
int op2 = output.throttle - output.roll - output.pitch - output.yaw;
int op3 = output.throttle - output.roll + output.pitch + output.yaw;
int op4 = output.throttle + output.roll + output.pitch - output.yaw;
withinBounds(op1, THROTTLE_MAXIMUM, THROTTLE_MINIMUM);
withinBounds(op2, THROTTLE_MAXIMUM, THROTTLE_MINIMUM);
withinBounds(op3, THROTTLE_MAXIMUM, THROTTLE_MINIMUM);
withinBounds(op4, THROTTLE_MAXIMUM, THROTTLE_MINIMUM);
if (output.throttle > THROTTLE_CUTOFF){
output1.writeMicroseconds(op1);
output2.writeMicroseconds(op2);
output3.writeMicroseconds(op3);
output4.writeMicroseconds(op4);
} else {
output1.writeMicroseconds(THROTTLE_MINIMUM);
output2.writeMicroseconds(THROTTLE_MINIMUM);
output3.writeMicroseconds(THROTTLE_MINIMUM);
output4.writeMicroseconds(THROTTLE_MINIMUM);
}
#elif defined(TRI_EN)
int op1 = output.throttle + output.roll - 0.8 * output.pitch;
int op2 = output.throttle - output.roll - 0.8 * output.pitch;
int op3 = output.throttle + output.pitch;
int op4 = SERVO_MIDPOINT + output.yaw + 30;
double tailConv = intMap(op4, TAIL_SERVO_MIN, TAIL_SERVO_MAX, TAIL_SERVO_MIN_DEGREE, TAIL_SERVO_MAX_DEGREE) - TAIL_SERVO_OFFSET;
op3 = ((op3 - SERVO_MINIMUM) / sin(radians(tailConv))) + SERVO_MINIMUM;
withinBounds(op1, THROTTLE_MAXIMUM, THROTTLE_MINIMUM);
withinBounds(op2, THROTTLE_MAXIMUM, THROTTLE_MINIMUM);
withinBounds(op3, THROTTLE_MAXIMUM, THROTTLE_MINIMUM);
withinBounds(op4, THROTTLE_MAXIMUM, THROTTLE_MINIMUM);
if (output.throttle > THROTTLE_CUTOFF){
output1.writeMicroseconds(op1);
output2.writeMicroseconds(op2);
output3.writeMicroseconds(op3);
output4.writeMicroseconds(op4);
} else {
output1.writeMicroseconds(THROTTLE_MINIMUM);
output2.writeMicroseconds(THROTTLE_MINIMUM);
output3.writeMicroseconds(THROTTLE_MINIMUM);
if (TRI_EN){
output4.writeMicroseconds(SERVO_MIDPOINT);
}
else {
output4.writeMicroseconds(THROTTLE_MINIMUM);
}
}
#elif defined(ROVER_EN)
int op2 = output.throttle;
int op4 = output.yaw;
withinBounds(op2, THROTTLE_MAXIMUM, THROTTLE_MINIMUM);
withinBounds(op4, THROTTLE_MAXIMUM, THROTTLE_MINIMUM);
output2.writeMicroseconds(op2);
output4.writeMicroseconds(op4);
#endif
}
void Astar_planning(){
ROS_INFO("A star planning");
std::priority_queue<PQItem> open_list;
const unsigned num_cells = local_map->info.height*local_map->info.width;
std::vector<bool> seen(num_cells); // Default initialized to all false
const occupancy_grid_utils::index_t dest_ind = occupancy_grid_utils::pointIndex(local_map->info,goal.point);
const occupancy_grid_utils::index_t src_ind = occupancy_grid_utils::pointIndex(local_map->info,statePoint(startState));
open_list.push(PQItem(src_ind, 0, h(src_ind),src_ind));
std::vector<occupancy_grid_utils::index_t> parent(num_cells,0);
while (!open_list.empty()) {
const PQItem current = open_list.top();
open_list.pop();
const occupancy_grid_utils::Cell c = occupancy_grid_utils::indexCell(local_map->info, current.ind);
if (seen[current.ind])
continue;
parent[current.ind] = current.parent_ind;
seen[current.ind] = true;
ROS_DEBUG_STREAM_NAMED ("shortest_path_internal", " Considering " << c << " with cost " <<
current.g_cost << " + " << current.h_cost);
if (current.ind == dest_ind) {
std::cout << "solution found" << std::endl;
std::cout << "Visited " << std::count(seen.begin(), seen.end(), true) << " states out of " << num_cells << std::endl;;
std::deque<occupancy_grid_utils::index_t> path;
path.push_back(dest_ind);
occupancy_grid_utils::index_t last = dest_ind;
while (parent[last]!=src_ind){
path.push_front(parent[last]);
last=parent[last];
}
/*for(std::deque<occupancy_grid_utils::index_t>::iterator it=path.begin(),end=path.end();it!=end;++it){
std::cout << occupancy_grid_utils::indexCell(local_map->info, *it) << " -- ";
}
std::cout << std::endl;*/
current_path_raw.resize(path.size());
std::transform(path.begin(), path.end(), current_path_raw.begin(), &indexState);
/*std::cout << "Path length: " << current_path_raw.size() << std::endl;
std::cout << "Path cost: " << current.g_cost << std::endl;
std::cout << "Path :" << std::endl;
for(std::deque<State<2>>::iterator it=current_path_raw.begin(),end=current_path_raw.end();it!=end;++it){
std::cout << (*it) << " -- ";
}
std::cout << std::endl;*/
planned=true;
return;
}
for (int d=-1; d<=1; d+=2) {
for (int vertical=0; vertical<2; vertical++) {
const int cx = c.x + d*(1-vertical);
const int cy = c.y + d*vertical;
if (cx>=0 && cy>=0) {
const occupancy_grid_utils::Cell c2((occupancy_grid_utils::coord_t) cx, (occupancy_grid_utils::coord_t) cy);
if (withinBounds(local_map->info, c2)) {
const occupancy_grid_utils::index_t ind = cellIndex(local_map->info, c2);
if (!isObstacle(pointState(indexPoint(ind))) && !seen[ind]) {
open_list.push(PQItem(ind, current.g_cost + g(ind), h(ind), current.ind));
}
//ROS_DEBUG_STREAM_COND_NAMED (g.data[ind]!=UNOCCUPIED, "shortest_path_internal",
// " Skipping cell " << indexCell(g.info, ind) <<
// " with cost " << (unsigned) g.data[ind]);
}
}
}
}
}
planning=false;
ROS_INFO("Planning failed");
}
