Mission code
Back to robobot.
Back to Robobot mission application.
Contents |
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.
