Mission code

Back to robobot.

Back to Robobot mission application.

Mission 1 code

This mission segment function is called every about 10ms until it returns true (finished). When the mission is in manual mode, then code is not called.

/**
* Run mission
* \param state is kept by caller, but is changed here
*              therefore defined as reference with the '&'.
*              State will be 0 at first call.
* \returns true, when finished. */
bool UMission::mission1(int & state)
{
 bool finished = false;
 // First commands to send to robobot in given mission
 // (robot sends event 1 after driving 1 meter)):
 switch (state)
 {
   case 0:
     // tell the operatior what to do
     printf("# press green to start.\n");
     system("espeak \"press green to start\" -ven+f4 -s130 -a5 2>/dev/null &"); 
     bridge->send("oled 5 press green to start");
     state++;
     break;
   case 1:
     if (bridge->joy->button[BUTTON_GREEN])
       state = 10;
     break;
   case 10: // first PART - wait for IR2 then go fwd and turn
     snprintf(lines[0], MAX_LEN, "vel=0 : ir2 < 0.3");
     // drive straight 0.6m - keep an acceleration limit of 1m/s2 (until changed)
     snprintf(lines[1], MAX_LEN, "vel=0.2,acc=1:dist=0.6");
     // stop and create an event when arrived at this line
     snprintf(lines[2], MAX_LEN, "event=1, vel=0");
     // add a line, so that the robot is occupied until next snippet has arrived
     snprintf(lines[3], MAX_LEN, ": dist=1");
     // send the 4 lines to the REGBOT
     sendAndActivateSnippet(lines, 4);
     // make sure event 1 is cleared
     bridge->event->isEventSet(1);
     // tell the operator
     printf("# case=%d sent mission snippet 1\n", state);
     system("espeak \"code snippet 1.\" -ven+f4 -s130 -a5 2>/dev/null &"); 
     bridge->send("oled 5 code snippet 1");
     //
     // play as we go
     play.setFile("../The_thing_goes_Bassim.mp3");
     play.setVolume(5); // % (0..100)
     play.start();
     // go to wait for finished
     state = 11;
     featureCnt = 0;
     break;
   case 11:
     // wait for event 1 (send when finished driving first part)
     if (bridge->event->isEventSet(1))
     { // finished first drive
       state = 999;
       play.stopPlaying();
     }
     break;
   case 999:
   default:
     printf("mission 1 ended \n");
     bridge->send("oled 5 \"mission 1 ended.\"");
     finished = true;
     break;
 }
 return finished;
}

On the first call, the 'state' variable is 0. The state is used in a case statement.

case 0, case 1, green button

On the first state, state 0, the operator is told what to do:

 switch (state)
 {
   case 0:
     // tell the operatior what to do
     printf("# press green to start.\n");
     system("espeak \"press green to start\" -ven+f4 -s130 -a5 2>/dev/null &"); 
     bridge->send("oled 5 press green to start");
     state++;
     break;
   case 1:
     if (bridge->joy->button[BUTTON_GREEN])
       state = 10;
     break;

The 'printf(...)' prints to the console.

Synthetic speach

The line ' system("espeak \"press green to start\" -ven+f4 -s130 -a5 2>/dev/null &"); ' will announce, using the speaker, what to do. Note that the text is in quotes '"' that are escaped by a backslash. The parameter '-ven+f4' is to use a female f4 voice. The '-s130' makes the speak a bit faster (130%). The '-a5' sets the volume to 5%. and the '2>/dev/null' pipes any error messages to the bit bucked (null device).

Google espeak for more details.

O-LED information

Further the line 'bridge->send("oled 5 press...");' will display a line (on line 5) on the small o-led display.

Next state

Once these messages are announced, then state is changed to 1.

case 1, wait for button

On the next call to the function, there is a check to see if the green button is pressed, if it is, then state is changed to 10.

case 10, drive snippet

When the state is 10 the following code is executed:

   case 10: // first PART - wait for IR2 then go fwd and turn
     snprintf(lines[0], MAX_LEN, "vel=0 : ir2 < 0.3");
     // drive straight 0.6m - keep an acceleration limit of 1m/s2 (until changed)
     snprintf(lines[1], MAX_LEN, "vel=0.2,acc=1:dist=0.6");
     // stop and create an event when arrived at this line
     snprintf(lines[2], MAX_LEN, "event=1, vel=0");
     // add a line, so that the robot is occupied until next snippet has arrived
     snprintf(lines[3], MAX_LEN, ": dist=1");
     // send the 4 lines to the REGBOT
     sendAndActivateSnippet(lines, 4);
     // make sure event 1 is cleared
     bridge->event->isEventSet(1);
     ...

There is an array of C-strings that is used to build the mission snippet.

The line 'snprintf(lines[0], MAX_LEN, "vel=0 : ir2 < 0.3");' prepares the command 'vel=0 : ir2 < 0.3', velocity zero and then wait until the IR2 sensor has a detection that is closer than 0.3m.

The next line is 'vel=0.2,acc=1:dist=0.6' sets velocity to 0.2m/s with a safe acceleration of 1m/s^2. This line then holds until 0.6m is driven.

The third line is 'event=1, vel=0' it will set the speed to 0 and generate an event, event 1. There is no ':' and thus the REGBOT will continue to the next line right away.

The fourth line is ': dist=1' this line has a ':' and will wait until the robot has moved 1m. This will not happen, as the velocity is zero, so the REGBOT will wait for further instructions.

Now 4 lines are prepared, ready to send to the REGBOT and activated.

sendAndActivateSnippet(lines, 4);

It will take some time before the drive is finished and event 1 generated. To be on the safe side the event 1 flag is cleared with the 'isEventSet(1);' (could have been set by a previous mission).

Music gimmick

Now that the robot has speakers, it could be a gimmick to play some music.

There is a couple of lines to play:

     // play as we go
     play.setFile("../The_thing_goes_Bassim.mp3");
     play.setVolume(5); // % (0..100)
     play.start();
     // go to wait for finished
     state = 11;
     featureCnt = 0;
     break;

The class has a variable called 'play' that has the code to start and stop music files. I have found a royalty-free clip called 'The_thing_goes_Bassim.mp3', so this filename is set to the play object, then the volume is set to just 5% - not to be too loud. And then finally started.

The state is then changed to 11.

case 11, wait for mission snippet event

The 4 lines of REGBOT mission snippet is sent and activated, now we should wait until it has finished. This is what state 11 is doing.

   case 11:
     // wait for event 1 (send when finished driving first part)
     if (bridge->event->isEventSet(1))
     { // finished first drive
       state = 999;
       play.stopPlaying();
     }
     break;
   case 999:
   default:
     printf("mission 1 ended \n");
     bridge->send("oled 5 \"mission 1 ended.\"");
     finished = true;
     break;

When the event 1 is detected, the state is set to 999. State 999 is also the default state, and will just set the 'finished' flag to true. This will tell the mission loop that this segment is finished, and will then reset the state to 0 and call the next mission segment instead.

Mission 2 code

This part will look for an ArUco code marker in a camera image. If found then move to the marker, if not then turn a bit.

bool UMission::mission2(int & state)
{
 bool finished = false;
 switch (state)
 {
   case 0:
     system("espeak \"looking for ArUco\" -ven+f4 -s130 -a5 2>/dev/null &"); 
     bridge->send("oled 5 looking 4 ArUco");
     state=11;
     break;
   case 11:
     if (fabsf(bridge->motor->getVelocity()) < 0.001 and bridge->imu->turnrate() < (2*180/M_PI))
     { // finished first drive and turnrate is zero'ish
       state = 12;
       usleep(35000);
       // start aruco analysis 
       cam->arUcos->setNewFlagToFalse();
       cam->doArUcoAnalysis = true;
     }
     break;
   case 12:
     if (not cam->doArUcoAnalysis)
     { // aruco processing finished
       if (cam->arUcos->getMarkerCount(true) > 0)
       { // found a marker - go to marker (any marker)
         state = 30;
       }
       else
       { // turn a bit (more)
         state = 20;
       }
     }
     break;
   case 20: 
     { // turn a bit and then look for a marker again
       int line = 0;
       snprintf(lines[line++], MAX_LEN, "vel=0.25, tr=0.15: turn=10,time=10");
       snprintf(lines[line++], MAX_LEN, "vel=0,event=2:dist=1");
       sendAndActivateSnippet(lines, line);
       // make sure event 2 is cleared
       bridge->event->isEventSet(2);
       state = 21;
       break;
     }
   case 21: // wait until manoeuvre has finished
     if (bridge->event->isEventSet(2))
     {// repeat looking (until all 360 degrees are tested)
       if (featureCnt < 36)
         state = 11;
       else
         state = 999;
       featureCnt++;
     }
     break;
   case 30:
     { // found marker
       // if stop marker, then exit
       ArUcoVal * v = cam->arUcos->getID(6);
       if (v != NULL and v->isNew)
       { // sign to stop
         state = 999;
         break;
       }
       // use the first (assumed only one)
       v = cam->arUcos->getFirstNew();
       // marker position in robot coordinates
       float xm = v->markerPosition.at<float>(0,0);
       float ym = v->markerPosition.at<float>(0,1);
       float hm = v->markerAngle;
       // stop some distance in front of marker
       float dx = 0.3; // distance to stop in front of marker
       float dy = 0.0; // distance to the left of marker
       xm += - dx*cos(hm) + dy*sin(hm);
       ym += - dx*sin(hm) - dy*cos(hm);
       // limits
       float acc = 1.0; // max allowed acceleration - linear and turn
       float vel = 0.3; // desired velocity
       // set parameters
       // end at 0 m/s velocity
       UPose2pose pp4(xm, ym, hm, 0.0);
       printf("\n");
       // calculate turn-straight-turn (Angle-Line-Angle)  manoeuvre
       bool isOK = pp4.calculateALA(vel, acc);
       // use only if distance to destination is more than 3cm
       if (isOK and (pp4.movementDistance() > 0.03))
       { // a solution is found - and more that 3cm away.
         // debug print manoeuvre details
         pp4.printMan();
         printf("\n");
         // debug end
         int line = 0;
         if (pp4.initialBreak > 0.01)
         { // there is a starting straight part
           snprintf(lines[line++], MAX_LEN, "vel=%.3f,acc=%.1f :dist=%.3f", 
                    pp4.straightVel, acc, pp4.straightVel);
         }
         snprintf(lines[line++], MAX_LEN,   "vel=%.3f,tr=%.3f :turn=%.1f", 
                  pp4.straightVel, pp4.radius1, pp4.turnArc1 * 180 / M_PI);
         snprintf(lines[line++], MAX_LEN,   ":dist=%.3f", pp4.straightDist);
         snprintf(lines[line++], MAX_LEN,   "tr=%.3f :turn=%.1f", 
                  pp4.radius2, pp4.turnArc2 * 180 / M_PI);
         if (pp4.finalBreak > 0.01)
         { // there is a straight break distance
           snprintf(lines[line++], MAX_LEN,   "vel=0 : time=%.2f", 
                    sqrt(2*pp4.finalBreak));
         }
         snprintf(lines[line++], MAX_LEN,   "vel=0, event=2: dist=1");
         sendAndActivateSnippet(lines, line);
         bridge->event->isEventSet(2);
       }
       else
       { // look again for marker
         state = 11;
       }
       // wait for movement to finish
       state = 31;
     }
     break;
   case 31:
     // wait for event 2 (send when finished driving)
     if (bridge->event->isEventSet(2))
     { // look for next marker
       state = 11;
       // no, stop
       state = 999;
     }
     break;
   case 999:
   default:
     printf("mission 1 ended \n");
     bridge->send("oled 5 \"mission 1 ended.\"");
     finished = true;
     play.stopPlaying();
     break;
 }
 // printf("# mission1 return (state=%d, finished=%d, )\n", state, finished);
 return finished;
}

Case 0:

The first state 'case 0:' do nothing - just set the state to 11.

case 11:

  case 11:
    if (fabsf(bridge->motor->getVelocity()) < 0.001 and bridge->imu->turnrate() < (2*180/M_PI))
    { // finished first drive and turnrate is zero'ish
      state = 12;
      usleep(35000);
      // start aruco analysis 
      cam->arUcos->setNewFlagToFalse();
      cam->doArUcoAnalysis = true;
    }
    break;

This state checks if the robot is stationary by checking if the velocity is zero and the gyro do not detect any movement (less than 2 degrees per second).

If the robot is steady, then it waits a bit (35ms) to allow the camera to start another frame. Then the flag to start ArUco analyses is set, and all previously detected markers have their 'new' flag cleared.

case 12:

   case 12:
     if (not cam->doArUcoAnalysis)
     { // aruco processing finished
       if (cam->arUcos->getMarkerCount(true) > 0)
       { // found a marker - go to marker (any marker)
         state = 30;
         // tell the operator
         printf("# case=%d found marker\n", state);
         system("espeak \"found marker.\" -ven+f4 -s130 -a5 2>/dev/null &"); 
         bridge->send("oled 5 found marker");
       }
       else
       { // turn a bit (more)
         state = 20;
       }
     }
     break;

This state waits until the ArUco flag is cleared (is cleared, when the camera process is finished).

Then it checks the result, if the marker count is not zero, then the state is set to 30, if not then to 20 - to make a slight turn.

case 20: and case 21:

   case 20: 
     { // turn a bit and then look for a marker again
       int line = 0;
       snprintf(lines[line++], MAX_LEN, "vel=0.25, tr=0.15: turn=10,time=10");
       snprintf(lines[line++], MAX_LEN, "vel=0,event=2:dist=1");
       sendAndActivateSnippet(lines, line);
       // make sure event 2 is cleared
       bridge->event->isEventSet(2);
       // tell the operator
       printf("# case=%d sent mission turn a bit\n", state);
       system("espeak \"turn.\" -ven+f4 -s130 -a5 2>/dev/null &"); 
       bridge->send("oled 5 code turn a bit");
       state = 21;
       break;
     }
   case 21: // wait until manoeuvre has finished
     if (bridge->event->isEventSet(2))
     {// repeat looking (until all 360 degrees are tested)
       if (featureCnt < 36)
         state = 11;
       else
         state = 999;
       featureCnt++;
     }
     break;

The case 20 sends a mission snippet to REGBOT with a turn of 10 degrees and a turning radius of 15cm. And ask REGBOT to emit an event 2 when finished and the velocity set to 0.

'Case 21' waits for event 2 and sets the state back to 11, initiating another image search. There is further a test to see if the robot has turned 36 times.

case 30:

A marker is found and the code is a bit longer and consist of:

• extract the marker position,
• calculate manoeuver required to face the marker,
• translate the manoeuver to a REGBOT snippet.

extract marker position

The same image can hold many markers, but here we go to any marker, except a marker with ID=6.

       ArUcoVal * v = cam->arUcos->getID(6);
       if (v != NULL and v->isNew)
       { // sign to stop
         state = 999;
         break;
       }
       // use the first (assumed only one)
       v = cam->arUcos->getFirstNew();
       // marker position in robot coordinates
       float xm = v->markerPosition.at<float>(0,0);
       float ym = v->markerPosition.at<float>(0,1);
       float hm = v->markerAngle;

The marker details are in an object of class type 'ArUcoVal', and we can get a pointer to the object with ID 6 using the 'cam->arUcos->getID(6);' call. Marker 6 is interpreted as a stop sign, and if found the state is set to 999 that will terminate the mission.

Any other marker is taken as destination, and the x,y position is taken from the marker vector (of type cv::Mat - the OpenCv matrix type).

The position and orientation are used only - see Robobot_mission#ArUco for more details on ArUco marker detection.

Manoeuver calculation

There is a library that can calculate the required manoeuver from one pose to another using a turn, a straight part and an end turn. The library allows a turning radius different from zero and limitation in the allowed acceleration, both lateral and while turning.

The code is

       // stop some distance in front of marker
       float dx = 0.3; // distance to stop in front of marker
       float dy = 0.0; // distance to the left of marker
       xm += - dx*cos(hm) + dy*sin(hm);
       ym += - dx*sin(hm) - dy*cos(hm);
       // limits
       float acc = 1.0; // max allowed acceleration - linear and turn
       float vel = 0.3; // desired velocity
       // set parameters
       // end at 0 m/s velocity
       UPose2pose pp4(xm, ym, hm, 0.0);
       // calculate turn-straight-turn (Angle-Line-Angle)  manoeuvre
       bool isOK = pp4.calculateALA(vel, acc);

First, the end pose is adjusted to 30cm in front of the marker (xm, ym, hm).

The library is defined in a class called 'UPose2pose' and the object of that type is here called 'pp4' , and is created with the desired end pose.

To calculate, the function requires to know the desired velocity and the maximum tolerated acceleration - here 1 m/sec^2 and 0.3 m/s. and the call to 'pp4.calculateALA(vel, acc);' returns true if successful.

Translate to manoeuver snippet

The result of the calculated manoeuver is available as global variables of the object and is translated to REGBOT mission snippet as:

         if (pp4.initialBreak > 0.01)
         { // there is a starting straight part
           snprintf(lines[line++], MAX_LEN, "vel=%.3f,acc=%.1f :dist=%.3f", 
                    pp4.straightVel, acc, pp4.straightVel);
         }
         snprintf(lines[line++], MAX_LEN,   "vel=%.3f,tr=%.3f :turn=%.1f", 
                  pp4.straightVel, pp4.radius1, pp4.turnArc1 * 180 / M_PI);
         snprintf(lines[line++], MAX_LEN,   ":dist=%.3f", pp4.straightDist);
         snprintf(lines[line++], MAX_LEN,   "tr=%.3f :turn=%.1f", 
                  pp4.radius2, pp4.turnArc2 * 180 / M_PI);
         if (pp4.finalBreak > 0.01)
         { // there is a straight break distance
           snprintf(lines[line++], MAX_LEN,   "vel=0 : time=%.2f", 
                    sqrt(2*pp4.finalBreak));
         }
         snprintf(lines[line++], MAX_LEN,   "vel=0, event=2: dist=1");
         sendAndActivateSnippet(lines, line);

The global variables to use is

pp4.straightVel    An initial break distance to get to the to a velocity allowed during the turn
pp4.radius1        Turn radius of first turn
pp4.turnArc1       Turn angle (in radians) for the first turn
pp4.straightDist   The stratight part
pp4.radius2        Turn radius of second turn
pp4.turnArc2       Turn angle of second turn
pp4.finalBreak     The final break distance to get to a final velocity of 0m/s

Potentially the code allows for an initial break part (to get to a velocity resulting in an allowed acceleration during the first turn). The relevant velocity is to aim for is 'pp4.straightVel' , and this velocity is assumed in both turns.

The code creates the needed REGBOT mission snipped to implement the manoeuver.

The last break is terminated using time rather than distance as the end velocity is zero, and may never reach the calculated distance. The time needed is from s = 0.5 a t^2.

When the manoeuver is finished, the REGBOT is asked to emit an event 2.

case 31

The state 31 just waits for REGBOT to finish (event 2) and then finishes the mission segment.