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
}
