void state_driving(emc::FSMInterface& fsm, emc::IO& io, void* user_data) { // The FSM engine does not know what kind of user data will be put into it, so it stores it as a // 'void pointer'. A void pointer is simply a pointer to data of which the type is unknown. But // we DO know the type! Therefore we can 'cast' it to our MyData type that we defined above. You // don't have to exactly know what is going on here, just know that the resulting 'my_data' is a // pointer to our data, with the correct type. MyData* my_data = static_cast<MyData*>(user_data); emc::LaserData scan; if (!io.readLaserData(scan)) return; // No data, so not much to do double min_dist = calculateMinimumDistance(scan); if (min_dist < my_data->max_obstacle_distance) // magic number! { fsm.raiseEvent("obstacle_near"); return; } // No obstacles near, so let's go! io.sendBaseReference(0.3, 0, 0); // We're driving...! std::cout << "driving" << std::endl; }
void state_waiting(emc::FSMInterface& fsm, emc::IO& io, void* user_data) { // Cast the data to our MyData type! (See explanation in state_driving) MyData* my_data = static_cast<MyData*>(user_data); // Stop the base! io.sendBaseReference(0, 0, 0); // We're waiting..! std::cout << "waiting" << std::endl; emc::LaserData scan; if (!io.readLaserData(scan)) return; // No data, so not much to do double min_dist = calculateMinimumDistance(scan); if (min_dist > my_data->max_obstacle_distance) { // All clear! fsm.raiseEvent("all_clear"); return; } }
//------------------------------------------------------------------------------------------------------------------------------ void ImplTreetorDistance::calculateTree( HTree & tree, const HMultipleAlignment & mali ) const { debug_func_cerr( 5 ); startUp( tree, mali ); DistanceMatrixSize swap_temp; // for swapping indices #define SWAP(x,y) { swap_temp = x; x = y; y = swap_temp; } //------------------------------------------------------------------------------------------ // start up DistanceMatrixSize width = mWorkMatrix->getWidth(); DistanceMatrixSize i; /* in this algorithm I assume that the matrix I use is only a half-matrix using the lower diagonal */ tree->setNumLeaves( width ); // allocate memory for tree mIndices = new Node[width]; /* allocate array to keep track of indices */ for (i = 0; i < width; i++) mIndices[i] = i; //---------------------------------------------------------------------------------------------------- // Perform hierarchical clustering DistanceMatrixSize last_row = width - 1; /* shrink distance matrix, until it contains only a single cluster */ while (last_row > 0) { debug_cerr( 6, "Last row " << last_row ); debug_cerr( 6, "Work matrix " << endl << *mWorkMatrix ); // find minimum distance in matrix /* - -- --- ---- -x--- ------ ------- -------- */ calculateMinimumDistance(); DistanceMatrixSize min_row = mMinimumCoord.row; DistanceMatrixSize min_col = mMinimumCoord.col; debug_cerr( 5, "Joining nodes -> " << "minimum distance :" << mMinimumValue << " " << "node 1: " << mIndices[min_row] << " (" << min_row << ") " << "node 2: " << mIndices[min_col] << " (" << min_col << ") " ); //------------------------------------------------------------------------------------------------------ // move rows around, so that the last two joined cluster are in the two last rows /* - - y- -- -y- --- -y-- -> ---- xOxx- ----- -y--x- ------ -y--x-- xxxxxxx -y--x--- yyyyyyyy */ DistanceMatrixSize second_row = last_row - 1; // second to last row // exchange row with last row mWorkMatrix->swap( min_row, last_row); SWAP( mIndices[min_row], mIndices[last_row]); swapHelpers( min_row, last_row); // exchange col with second to last row mWorkMatrix->swap( min_col, second_row); SWAP( mIndices[min_col], mIndices[second_row]); swapHelpers( min_col, second_row); //------------------------------------------------------------------------------------------------------ // join the two nodes in the tree giving the correct edge weights Node new_node = joinNodes( tree, second_row, last_row ); debug_cerr( 6, "-> new node " << new_node ); //------------------------------------------------------------------------------------------------------ // calculate distance to new cluster and put them in second to last row updateDistanceMatrix( tree, second_row, last_row); /* delete last row, update mIndices, so that the last row now contains the number of the new cluster id */ /* - - -- -- --- --- ---- -> ---- ----- ----- ------ ------ xxxxxxx xxxxxxx yyyyyyyy */ mWorkMatrix->shrink(); mIndices[last_row - 1] = new_node; last_row = mWorkMatrix->getWidth() - 1; } delete [] mIndices; cleanUp(); // cleans up mWorkMatrix }