void newSheep::StageLaser_callback(sensor_msgs::LaserScan msg)
{
distance = msg.ranges[20];
se306_example::IdentityRequest request;
if(distance <= 5) {
RobotNode_cmdvel.linear.x = linear_x;
RobotNode_cmdvel.angular.z = angular_z;
//RobotNode_stage_pub.publish(RobotNode_cmdvel);
request.sender = robot_name;
std::list<double> pose = calculateTheta(theta, distance);
double x = pose.front();
double y = pose.back();
request.px = this->px+x+(width/2.0);
//pose.pop_front();
request.py = py+y+(length/2.0);
//pose.pop_front();
Request_pub.publish(request);
replyReceived = false;
requestSent = true;
}
}
// Basic Right wall following code
void wallFollowingR()
{
int rightSideIR, leftSideIR, rightFrontIR, leftFrontIR, distAhead, i;
while (1)
//for(i = 0; i < 170; i++)
{
if (thetaAcc > 40)
{ break; }
get_front_ir_dists(&leftFrontIR, &rightFrontIR);
get_side_ir_dists(&leftSideIR, &rightSideIR);
distAhead = get_us_dist();
int lbump, rbump;
check_bumpers(&lbump, &rbump);
if (lbump){
set_motors(SPEED+20, SPEED-20);
}
if (rbump){
set_motors(SPEED-20, SPEED+20);
}
if (distAhead < 35){ //in the L shape
if (leftFrontIR - rightFrontIR > 0){
set_motors(-SPEED, SPEED);
}
else {
set_motors(+20,-20);
}
}
else if (rightFrontIR > 30)
{ set_motors(SPEED+10, SPEED-20); } //turn right
else if (rightFrontIR < 20)
{ set_motors (SPEED-25, SPEED+15); } //turn left
else
{set_motors(SPEED+7, SPEED+7); } // Straight
calculateTheta();
}
}
void cartesianToPolar(float x, float y, double *radiusPtr, double *thetaPtr)
{
// Es radiusPtr diferente de NULL?
if(radiusPtr) {
// Guardamos el valor del radio con esta formula
*radiusPtr = sqrt(x * x + y * y);
}
// Es thetaPtr NULL?
if(!thetaPtr) {
// Saltarnos la funcion entera
return;
}
// Calculamos theta
*thetaPtr = calculateTheta(x, y);
}
bool KMeans::trainModel(MatrixFloat &data){
if( numClusters == 0 ){
errorLog << "trainModel(MatrixFloat &data) - Failed to train model. NumClusters is zero!" << std::endl;
return false;
}
if( clusters.getNumRows() != numClusters ){
errorLog << "trainModel(MatrixFloat &data) - Failed to train model. The number of rows in the cluster matrix does not match the number of clusters! You should need to initalize the clusters matrix first before calling this function!" << std::endl;
return false;
}
if( clusters.getNumCols() != numInputDimensions ){
errorLog << "trainModel(MatrixFloat &data) - Failed to train model. The number of columns in the cluster matrix does not match the number of input dimensions! You should need to initalize the clusters matrix first before calling this function!" << std::endl;
return false;
}
Timer timer;
UINT currentIter = 0;
UINT numChanged = 0;
bool keepTraining = true;
Float theta = 0;
Float lastTheta = 0;
Float delta = 0;
Float startTime = 0;
thetaTracker.clear();
finalTheta = 0;
numTrainingIterationsToConverge = 0;
trained = false;
converged = false;
//Scale the data if needed
ranges = data.getRanges();
if( useScaling ){
data.scale(0,1);
}
//Init the assign and count Vectors
//Assign is set to K+1 so that the nChanged values in the eStep at the first iteration will be updated correctly
for(UINT m=0; m<numTrainingSamples; m++) assign[m] = numClusters+1;
for(UINT k=0; k<numClusters; k++) count[k] = 0;
//Run the training loop
timer.start();
while( keepTraining ){
startTime = timer.getMilliSeconds();
//Compute the E step
numChanged = estep( data );
//Compute the M step
mstep( data );
//Update the iteration counter
currentIter++;
//Compute theta if needed
if( computeTheta ){
theta = calculateTheta(data);
delta = lastTheta - theta;
lastTheta = theta;
}else theta = delta = 0;
//Check convergance
if( numChanged == 0 && currentIter > minNumEpochs ){ converged = true; keepTraining = false; }
if( currentIter >= maxNumEpochs ){ keepTraining = false; }
if( fabs( delta ) < minChange && computeTheta && currentIter > minNumEpochs ){ converged = true; keepTraining = false; }
if( computeTheta ) thetaTracker.push_back( theta );
trainingLog << "Epoch: " << currentIter << "/" << maxNumEpochs;
trainingLog << " Epoch time: " << (timer.getMilliSeconds()-startTime)/1000.0 << " seconds";
trainingLog << " Theta: " << theta << " Delta: " << delta << std::endl;
}
trainingLog << "Model Trained at epoch: " << currentIter << " with a theta value of: " << theta << std::endl;
finalTheta = theta;
numTrainingIterationsToConverge = currentIter;
trained = true;
//Setup the cluster labels
clusterLabels.resize(numClusters);
for(UINT i=0; i<numClusters; i++){
clusterLabels[i] = i+1;
}
clusterLikelihoods.resize(numClusters,0);
clusterDistances.resize(numClusters,0);
return true;
}
//Basic wall following code
void wallFollowing()
{
//Sensor readings
int rightSideIR, leftSideIR, rightFrontIR, leftFrontIR;
int ultrasound;
get_front_ir_dists(&leftFrontIR, &rightFrontIR);
get_side_ir_dists(&leftSideIR, &rightSideIR);
inputMotors();
while (1)
{
if (thetaAcc > 40)
{ break; }
int lbump, rbump;
check_bumpers(&lbump, &rbump);
if (lbump){
set_motors(SPEED+20, SPEED-20);
}
if (rbump){
set_motors(SPEED-20, SPEED+20);
}
get_front_ir_dists(&leftFrontIR, &rightFrontIR);
get_side_ir_dists(&leftSideIR, &rightSideIR);
ultrasound = get_us_dist();
//Hit wall
if (ultrasound < 30){
int i;
/*
for(i=0; i<20; i++){
set_motors(-SPEED, -SPEED);
ultrasound = get_us_dist();
}
*/
//set_ir_angle(RIGHT, 45);
//set_ir_angle(LEFT, -45);
get_front_ir_dists(&leftFrontIR, &rightFrontIR);
//for (i=0; i<15; i++){
if (leftFrontIR - rightFrontIR > 0){
set_motors(-SPEED, -SPEED);
//set_motors(-SPEED, SPEED);
set_motors(-25,25);
}
else{
set_motors(-SPEED, -SPEED);
set_motors(SPEED, -SPEED);
}
get_front_ir_dists(&leftFrontIR, &rightFrontIR);
//}
setAngles();
}
/*
//Deadlock
if (ultrasound < 30 && leftFrontIR < 40 && rightFrontIR < 40)
{
int i;
//while ((ultrasound < 80 && leftFrontIR < 45) || (ultrasound < 80 && rightFrontIR < 45))
while (ultrasound < 87)
{ set_motors(-SPEED, -SPEED);
//get_side_ir_dists(&leftFrontIR, &rightSideIR);
ultrasound = get_us_dist();
}
set_ir_angle(RIGHT, 45);
set_ir_angle(LEFT, -45);
get_front_ir_dists(&leftFrontIR, &rightFrontIR);
//printf("%d %d\n", leftFrontIR, rightFrontIR);
if (leftFrontIR - rightFrontIR > 0){
//for (i=0; i<20; i++)
set_motors(-SPEED-20, -SPEED+20);
//turnLeft(50);
}
else if (rightFrontIR - leftFrontIR > 0){
//for (i=0; i<20; i++)
set_motors(-SPEED+20, -SPEED-20);
//turnRight(50);
}
setAngles();
//Make 90 deg left turn
//for (i=0; i<35; i++)
//{ set_motors(SPEED, -SPEED); }
}
*/
else{
if (leftFrontIR > rightFrontIR)
{
set_motors(SPEED-25, SPEED+25);
//addNode(pointer, LEFT);
}
else if (rightFrontIR > leftFrontIR )
{
set_motors(SPEED+25, SPEED-25);
//addNode(pointer, RIGHT);
}
else
{
set_motors(SPEED, SPEED);
//addNode(pointer, STRAIGHT);
}
}
calculateTheta();
}
}