void AddOpenNeighboursToList (location_t here)
{
if (HasExit (here, NORTH)) {
ListAdd (Neighbour (here, NORTH));
}
if (HasExit (here, EAST)) {
ListAdd (Neighbour (here, EAST));
}
if (HasExit (here, SOUTH)) {
ListAdd (Neighbour (here, SOUTH));
}
if (HasExit (here, WEST)) {
ListAdd (Neighbour (here, WEST));
}
}
ColorClasses::Color Color3DTree::Node::classify(const Vector3<int>& color, unsigned int maxdistance, unsigned int maxneighbours)
{
std::deque<Neighbour> nearestNeighbours;
for(unsigned i = 0; i < maxneighbours; i++)
nearestNeighbours.push_back(Neighbour(ClassifiedColor(ColorClasses::none, 0, 0, 0), maxdistance + 1));
mNearestNeighbours(color, 0, maxneighbours, nearestNeighbours);
unsigned int histogram[ColorClasses::numOfColors];
//I have absolute no idea why, but memset dies not always set the histogram to 0
//memset(histogram, 0, ColorClasses::numOfColors);
for(int i = 0; i < ColorClasses::numOfColors; ++i)
histogram[i] = 0;
std::deque<Neighbour>::const_iterator it = nearestNeighbours.begin();
std::deque<Neighbour>::const_iterator end = nearestNeighbours.end();
for(; it < end; it++)
{
const ClassifiedColor& cc = it->getColor();
unsigned idx = cc.getColorClass();
histogram[idx] += 1;
if(histogram[idx] > maxneighbours / 2)
return cc.getColorClass();
}
// Color could not be classified. Classify into none.
return ColorClasses::none;
}
void Color3DTree::Node::mNearestNeighbours(const Vector3<int>& color, unsigned int level,
unsigned int maxneighbours, std::deque<Neighbour>& nearestNeighbours) const
{
const int distance = this->color.distance(color);
const unsigned int component = level % 3;
const int d = color[component] - this->color.get(component);
insertSorted(nearestNeighbours, Neighbour(this->color, distance), maxneighbours);
const int lastDistance = nearestNeighbours.back().getDistance();
if(d < 0)
{
if(below)
{
below->mNearestNeighbours(color, level + 1, maxneighbours, nearestNeighbours);
}
if(above && (-d) < lastDistance)
{
above->mNearestNeighbours(color, level + 1, maxneighbours, nearestNeighbours);
}
}
else
{
if(above)
{
above->mNearestNeighbours(color, level + 1, maxneighbours, nearestNeighbours);
}
if(below && d < lastDistance)
{
below->mNearestNeighbours(color, level + 1, maxneighbours, nearestNeighbours);
}
}
}
int main(int argc, char **argv) {
// Initialise ros
ros::init(argc,argv,"Neighbour");
// Create ros handler for this node
ros::NodeHandle n;
std::string xPosArg = argv[1];
std::string yPosString = argv[2];
double yPos = atof(yPosString.c_str());
double xPos = atof(xPosArg.c_str());
neighbour=Neighbour(xPos,yPos);
neighbour.setOriginY(yPos);
neighbour.setOriginX(xPos);
neighbour.robotNode_stage_pub = n.advertise<geometry_msgs::Twist>("cmd_vel",1000);
neighbour.stageOdo_Sub = n.subscribe<nav_msgs::Odometry>("base_pose_ground_truth", 1000, &Neighbour::stageOdom_callback, &neighbour);
neighbour.Neighbour_status_pub = n.advertise<se306project::human_status>("status",1000);
//subscribe to laser
neighbour.baseScan_Sub = n.subscribe<sensor_msgs::LaserScan>("base_scan", 1000,callBackLaserScan);
ros::Rate loop_rate(10);
//Add initial movement
se306project::human_status status_msg;
neighbour.setStatus("Finding a robot");
neighbour.faceWest(1);
neighbour.addMovement("forward_x", -35, 1);
// ROS infinite loop
while (ros::ok()) {
ros::spinOnce();
loop_rate.sleep();
//determine the logic of neighbour movement
if (neighbour.getMovementQueueSize() == 0){
if(neighbour.getStatus().compare("Finding a robot")==0){
neighbour.faceEast(1);
neighbour.addMovement("forward_x", neighbour.getOriginX()-neighbour.getX(), 1);
neighbour.setStatus("Moving back");
}else{
neighbour.setStatus("Finding a robot");
neighbour.faceWest(1);
neighbour.addMovement("forward_x", -35, 1);
}
}
if(neighbour.getObstacleStatus().compare("Obstacle nearby")!=0){
neighbour.move();
}
// Publish neighbour status
status_msg.pos_x = neighbour.getX();
status_msg.pos_y = neighbour.getY();
status_msg.pos_theta = neighbour.getTheta();
status_msg.status = neighbour.getStatus();
status_msg.obstacle = neighbour.getObstacleStatus();
neighbour.Neighbour_status_pub.publish(status_msg);
}
}
direction_t SmallestNeighbourDirection (location_t loc)
{
direction_t result = NORTH;
location_t neighbour;
cost_t smallestCost;
cost_t cost;
smallestCost = MAX_COST;
if (HasExit (loc, NORTH)) {
neighbour = Neighbour (loc, NORTH);
cost = Cost (neighbour);
if (cost < smallestCost) {
smallestCost = cost;
result = NORTH;
}
}
if (HasExit (loc, EAST)) {
neighbour = Neighbour (loc, EAST);
cost = Cost (neighbour);
if (cost < smallestCost) {
smallestCost = cost;
result = EAST;
}
}
if (HasExit (loc, SOUTH)) {
neighbour = Neighbour (loc, SOUTH);
cost = Cost (neighbour);
if (cost < smallestCost) {
smallestCost = cost;
result = SOUTH;
}
}
if (HasExit (loc, WEST)) {
neighbour = Neighbour (loc, WEST);
cost = Cost (neighbour);
if (cost < smallestCost) {
smallestCost = cost;
result = WEST;
}
}
return result;
}
void ModifiedFlood (location_t here)
{
direction_t direction;
cost_t smallestCost;
ListReset();
ListAdd (here);
while (!ListIsEmpty()) {
here = ListStackPop();
if (Cost (here) == 0) {
continue;
}
direction = SmallestNeighbourDirection (here);
smallestCost = Cost (Neighbour (here, direction));
SetDirection (here, direction);
if (Cost (here) != smallestCost + 1) {
SetCost (here, smallestCost + 1);
AddOpenNeighboursToList (here);
}
}
}
Neighbour neighbour( const Neighbour& u ) const {
return Neighbour( u );
}
Neighbour neighbour( const Element& x ) const {
return Neighbour( x );
}
Neighbour neighbour( int i, int c ) const {
return Neighbour( i, c );
}
/* clear a single wall - looks after neighbours - set seen*/
void MazeRemoveWall (location_t location, direction_t direction)
{
WallClear (&_walls[location.row][location.col], direction);
location = Neighbour (location, direction);
WallClear (&_walls[location.row][location.col], Behind (direction));
}
/* set a single wall - looks after neighbours - set seen*/
void MazeAddWall (location_t location, direction_t direction)
{
WallSet (&_walls[location.row][location.col], direction);
location = Neighbour (location, direction);
WallSet (&_walls[location.row][location.col], Behind (direction));
}
void FloodMazeClassic (location_t target)
{
location_t here;
location_t nextLoc;
cost_t costHere;
cost_t costNext;
for (here.row = 0; here.row < MAZE_ROWS; here.row++) {
for (here.col = 0; here.col < MAZE_COLS; here.col++) {
SetCost (here, MAX_COST);
SetDirection (here, INVALID);
}
}
SetCost (target, 0);
ListReset();
ListAdd (target);
while (!ListIsEmpty()) {
/*
* TODO: show costs and directions for all combinations
* of testing against MAX_COST vs smaller than and stack vs queue
*/
here = ListQueueHead();
//here = ListStackPop();
costNext = Cost (here) + 1;
if (HasExit (here, NORTH)) {
nextLoc = Neighbour (here, NORTH);
if (Cost (nextLoc) > costNext) {
SetDirection (nextLoc, SOUTH);
SetCost (nextLoc, costNext);
ListAdd (nextLoc);
}
}
if (HasExit (here, EAST)) {
nextLoc = Neighbour (here, EAST);
if (Cost (nextLoc) > costNext) {
SetDirection (nextLoc, WEST);
SetCost (nextLoc, costNext);
ListAdd (nextLoc);
}
}
if (HasExit (here, SOUTH)) {
nextLoc = Neighbour (here, SOUTH);
if (Cost (nextLoc) > costNext) {
SetDirection (nextLoc, NORTH);
SetCost (nextLoc, costNext);
ListAdd (nextLoc);
}
}
if (HasExit (here, WEST)) {
nextLoc = Neighbour (here, WEST);
if (Cost (nextLoc) > costNext) {
SetDirection (nextLoc, EAST);
SetCost (nextLoc, costNext);
ListAdd (nextLoc);
}
}
}
//printf ("Max List Length = %d List additions = %d path cost = %d\n", ListMaxSize(), ListAdditions(), Cost (Home()));
}
void mpi_manager_3D::setup(NumArray<int> &nproc, NumArray<int> &mx) {
// Save number of processors in each dimension
for(int dir=0; dir<DIM; ++dir) {
this->nproc[dir] = nproc[dir];
}
// Determine the rank of the current task
MPI_Comm_rank(MPI_COMM_WORLD, &rank);
// Get number of ranks from MPI
int ntasks;
MPI_Comm_size(MPI_COMM_WORLD, &ntasks);
this->ntasks = ntasks;
// Set the distribution of processes:
if(ntasks != nproc[0]*nproc[1]*nproc[2]){
std::cerr << " Wrong number of processes " << std::endl;
std::cout << ntasks << " " << nproc[0]*nproc[1]*nproc[2] << std::endl;
Finalise();
}
if(rank==0) {
std::cout << " Number of tasks: " << ntasks << std::endl;
}
// Check if grid can be subdevided as desired
for(int dir = 0; dir < DIM; ++dir) {
if(mx[dir] < nproc[dir] && nproc[dir] > 1) {
if(rank == 0) {
std::cerr << " Wrong grid topology for dimension ";
std::cerr << dir << std::endl;
std::cerr << " mx[" << dir << "]:" << mx[dir] << std::endl;
std::cerr << " nproc[" << dir << "]:" << nproc[dir] << std::endl;
}
Finalise();
}
}
// Check if grid is a power of 2:
double eps = 1.e-12;
for(int dir = 0; dir < DIM; ++dir) {
double exponent = log(mx[dir])/log(2.);
int i_exponent = static_cast<int>(exponent+eps);
if(exponent - i_exponent > 2.*eps) {
if(rank == 0) {
std::cerr << " Error: grid must be of the form mx = 2^n ";
std::cerr << std::endl;
std::cerr << " Exiting " << std::endl;
}
Finalise();
}
}
// Grid is not periodic
int periods[3] = {false, false, false};
int reorder = false;
// If all is okay: Create new communicator "comm3d"
MPI_Cart_create(MPI_COMM_WORLD, DIM, nproc, periods, reorder, &comm3d);
// Retrieve the cartesian topology
if (rank == 0) {
int TopoType;
std::cout << " Cart topology: ";
MPI_Topo_test(comm3d, &TopoType);
switch (TopoType) {
case MPI_UNDEFINED :
std::cout << " MPI_UNDEFINED " << std::endl;
break;
case MPI_GRAPH :
std::cout << "MPI_GRAPH" << std::endl;
break;
case MPI_CART :
std::cout << "MPI_CART" << std::endl;
break;
}
}
// Determine rank again for cartesian communicator -> overwrite rank
MPI_Comm_rank(comm3d, &rank);
// std::cout << " my rank: " << rank << std::endl;
// Translate rank to coordinates
MPI_Cart_coords(comm3d, rank, DIM, coords);
// // Backwards translation
// int TranslateRank;
// MPI_Cart_rank(comm3d, coords, &TranslateRank);
// Find neighbouring ranks
// Syntax: comm3d, shift direction, displacement, source, destination
MPI_Cart_shift(comm3d, 0, 1, &left , &right);
MPI_Cart_shift(comm3d, 1, 1, &front, &back);
MPI_Cart_shift(comm3d, 2, 1, &bottom, &top);
// std::cout << " My rank " << rank << " " << left << " " << right << " " << front << " " << back << " " << bottom << " " << top << std::endl;
if(rank==0) {
std::cout << " nearby " << right << " " << back << " " << top << std::endl;
}
// Determine ranks of neighbour processes:
int shiftcoord[DIM];
int lbound[DIM],ubound[DIM];
for(int dim=0;dim<DIM;dim++){
lbound[dim]=-1;
ubound[dim]= 1;
}
Neighbour.resize(lbound,ubound);
Neighbour.clear();
for(int dim0=-1; dim0<=1; dim0++){
shiftcoord[0] = (coords[0]+dim0)%nproc[0];
if(shiftcoord[0] < 0) shiftcoord[0]+=nproc[0];
for(int dim1=-1; dim1<=1; dim1++){
shiftcoord[1] = (coords[1]+dim1)%nproc[1];
if(shiftcoord[1] < 0) shiftcoord[1]+=nproc[1];
for(int dim2=-1; dim2<=1; dim2++){
shiftcoord[2] = (coords[2]+dim2)%nproc[2];
if(shiftcoord[2] < 0) shiftcoord[2]+=nproc[2];
MPI_Cart_rank(comm3d, shiftcoord,&Neighbour(dim0,dim1,dim2));
}
}
}
// if(rank==1) {
// for(int dim0=-1; dim0<=1; dim0++){
// for(int dim1=-1; dim1<=1; dim1++){
// for(int dim2=-1; dim2<=1; dim2++){
// std::cout << " neighbour " << dim0 << " " << dim1 << " ";
// std::cout << dim2 << " " << Neighbour(dim0, dim1, dim2);
// std::cout << std::endl;
// }
// }
// }
// }
// Determine absolute position of any rank:
AllRanks.resize(Index::set(0,0,0),
Index::set(nproc[0]-1,nproc[1]-1,nproc[2]-1));
for(int dim0=0; dim0<nproc[0]; ++dim0) {
for(int dim1=0; dim1<nproc[1]; ++dim1) {
for(int dim2=0; dim2<nproc[2]; ++dim2) {
int coord[3] = {dim0, dim1, dim2};
MPI_Cart_rank(comm3d, coord, &AllRanks(dim0, dim1, dim2));
}
}
}
// if(rank==2) {
// std::cout << " Neigh: " << rank << " "<<Neighbour(0,0,0) << " " << AllRanks(2,0,0) << std::endl;
// }
// Now make additional mpi groups relating to planes:
int count(0);
int num_xy = nproc[0]*nproc[1];
int num_xz = nproc[0]*nproc[2];
int num_yz = nproc[1]*nproc[2];
NumMatrix<int,1> x_ranks[nproc[0]];
NumMatrix<int,1> y_ranks[nproc[1]];
NumMatrix<int,1> z_ranks[nproc[2]];
// Walk trough z-axis -- xy plane
for(int irz=0; irz<nproc[2]; irz++) {
count = 0;
z_ranks[irz].resize(Index::set(0), Index::set(num_xy));
for(int irx=0; irx<nproc[0]; irx++) {
for(int iry=0; iry<nproc[1]; iry++) {
z_ranks[irz](count) = AllRanks(irx,iry,irz);
count++;
}
}
}
// Walk trough y-axis -- xz plane
for(int iry=0; iry<nproc[1]; iry++) {
count = 0;
y_ranks[iry].resize(Index::set(0), Index::set(num_xz));
for(int irx=0; irx<nproc[0]; irx++) {
for(int irz=0; irz<nproc[2]; irz++) {
y_ranks[iry](count) = AllRanks(irx,iry,irz);
count++;
}
}
}
// Walk trough x-axis -- yz plane
for(int irx=0; irx<nproc[0]; irx++) {
count = 0;
x_ranks[irx].resize(Index::set(0), Index::set(num_yz));
for(int iry=0; iry<nproc[1]; iry++) {
for(int irz=0; irz<nproc[2]; irz++) {
x_ranks[irx](count) = AllRanks(irx,iry,irz);
count++;
}
}
}
// Build local communicator:
MPI_Group group_all, group_constz, group_consty, group_constx;
// Get standard group handle:
MPI_Comm_group(comm3d, &group_all);
// Devide tasks into groups based on z-position
MPI_Group_incl(group_all, num_xy, z_ranks[coords[2]], &group_constz);
// Devide tasks into groups based on z-position
MPI_Group_incl(group_all, num_xz, y_ranks[coords[1]], &group_consty);
// Devide tasks into groups based on x-position
MPI_Group_incl(group_all, num_yz, x_ranks[coords[0]], &group_constx);
// // Make corresponding communicators:
// MPI_Comm_create(comm3d, group_constz, &comm_plane_xy); // const z
// MPI_Comm_create(comm3d, group_consty, &comm_plane_xz); // const x
// MPI_Comm_create(comm3d, group_constx, &comm_plane_yz); // const x
// // Get corresponding rank
// MPI_Group_rank (group_constz, &rank_plane_xy);
// MPI_Group_rank (group_consty, &rank_plane_xz);
// MPI_Group_rank (group_constx, &rank_plane_yz);
int remain_dims[3];
// x-y plane:
remain_dims[0] = 1;
remain_dims[1] = 1;
remain_dims[2] = 0;
MPI_Cart_sub(comm3d, remain_dims, &comm_plane_xy);
MPI_Comm_rank(comm_plane_xy, &rank_plane_xy);
// x-z plane
remain_dims[0] = 1;
remain_dims[1] = 0;
remain_dims[2] = 1;
MPI_Cart_sub(comm3d, remain_dims, &comm_plane_xz);
MPI_Comm_rank(comm_plane_xz, &rank_plane_xz);
// y-z plane
remain_dims[0] = 0;
remain_dims[1] = 1;
remain_dims[2] = 1;
MPI_Cart_sub(comm3d, remain_dims, &comm_plane_yz);
MPI_Comm_rank(comm_plane_yz, &rank_plane_yz);
}
