Project

General

Profile

History

How to run the Fiacre/Hippo experiment with a monitor

Providing you have properly synthesized and compiled the fiacre/hippo executable, you can now run it in place of the regular genom3 modules.

cd ~/work/osmosis/scripts
./start-simu-hippo-pocolibs.sh

You will get the Gazebo and Rviz window again and the main window will look like:

Starting

The top pane is the Hippo pane where the 7 modules are now running in the same Hippo process. The bottom pane is the eltclsh pane similar to the one in the regular experiment.

Thus, you can run the experiment exactly like the regular GenoM one (see run).

Select the eltclsh pane, and type:

eltclsh > source start-simu.tcl
eltclsh > init

You can get the robot to navigate with commands like:

eltclsh > Navigation::GotoNode N2 &

The difference here is that if you inject a fault (which results in a delay longer than 200 ms in the LaserScan Scan port update), the robot will make an emergency stop.

eltclsh > LaserDriver::SetDelay 300000

Indeed, in the traces you will see:

  43.290238  42500 [*** FiacreModel ***] robotnik --PATCH-- scan_updated in LaserDriver Activity ReadROSTopicsWriteGenoMPorts codel LWGCBQ
  43.290351  42500 [*** FiacreModel ***] robotnik --PATCH-- monitor_wait entered
  43.290354  42500 [*** FiacreModel ***] robotnik --PATCH-- monitor_wait scan_updated
  43.291283 [robotnik:callback_scan] Starting 200000 delay.
  43.491258  42700 [*** FiacreModel ***] robotnik --PATCH-- monitor_wait entered
  43.491269  42700 [*** FiacreModel ***] robotnik --PATCH-- monitor_wait 200 ms elapsed
  43.491354 [robotnik:callback_scan] Ending 200000 delay.
  43.491379  42700 [*** FiacreModel ***] robotnik --PATCH-- monitor_error entered
  43.491383  42700 [*** FiacreModel ***] robotnik --PATCH-- monitor_error stopping Track
  43.491384  42700 [*** FiacreModel ***] robotnik --PATCH-- monitor_error to monitor_start
  43.492167  42701 [*** FiacreModel ***] robotnik --PATCH-- scan_updated in LaserDriver Activity ReadROSTopicsWriteGenoMPorts codel LWGCBQ
  43.492311  42701 [*** FiacreModel ***] robotnik --PATCH-- monitor_start entered
  43.492315  42701 [*** FiacreModel ***] robotnik --PATCH-- monitor_start scan_updated
  43.493669 [robotnik:StopSMAAO] SafetyPilot: Stopping the robot with an explicit Stop: l 0 m/s, a 0 deg/s.
  43.494283  42703 [*** FiacreModel ***] robotnik SafetyPilot Activity SpeedMergeAndStopIfObstacle DONE (ether), back to ET.
  43.494340  42703 [*** FiacreModel ***] robotnik SafetyPilot ET pilot activity returned ACT_ETHER.
  43.494545  42703 [*** FiacreModel ***] robotnik SafetyPilot CT processes pilot activities, activity report.
  43.498335  42707 [*** FiacreModel ***] robotnik PotentialField ET plan ***OVERSHOOT*** its period.
Note the first numnber: 43.290238, is the wall clock since the beginning of the execution, while the second number (on * FiacreModel * traces) 42500 is the logical clock in milliseconds of Hippo. These two clocks are not synced upon starting, hence the slight difference, but should evolve at the same rate).

This trace corresponds to this monitor being triggered, whose arguments are 3 shared variables.

  • scan_updated is automatically updated by the activity which grab the LaserScan: ReadROSTopicsWriteGenoMPorts in the codel LWGCBQ of the module LaserWriter.

  • &pilot_task_activities, an array which corresponds to the activity of the pilot task of SafetyPilot module

  • Track_index: is the index in this array of the service SpeedMergeAndStopIfObstacle which produces and export the speed executed by the platform. By forcing this activity to Stop, we force the transition in the automata to the stop codel of this activity: StopSMAAO which immediately produces a null speed.

An interesting side effect which is properly reported by hippo, is that the PotentialField plan execution task also overshoot its period due to this delay. This is indeed the case, as the error is being produced by delaying the port update. Other tasks in need of this port must also wait longer than expected and this my lead to task overshooting their regular period (50ms for PotentialField plan).

process Laser_Scans_robotnik_Track_Stopper(
       &scan_updated: bool,
       &pilot_task_activities:  Activities_SafetyPilot_pilot_Array,
       Track_index: act_inst_SafetyPilot_pilot_index_type) is

states monitor_start, monitor_wait, monitor_error

var ignorep:nat

from monitor_start
    ignorep := fiacre_c_print_patch_trace(6); //   {0, "monitor_start entered"} /* 6 */
    on (scan_updated);
    ignorep := fiacre_c_print_patch_trace(7); //   {0, "monitor_start scan_updated"},	/* 7 */
    scan_updated := false;
    to monitor_wait

from monitor_wait
    ignorep := fiacre_c_print_patch_trace(8); //   {0, "monitor_wait entered"} /* 8 */
    select
       wait [200,200];
       ignorep := fiacre_c_print_patch_trace(0); //   {0, "monitor_wait 200 ms elapsed"},	/* 0 */
       to monitor_error
    []
       on (scan_updated);
       ignorep := fiacre_c_print_patch_trace(1); //    {0, "monitor_wait scan_updated."},	/* 1 */
       scan_updated := false;
       to monitor_wait
    end

from monitor_error
   ignorep := fiacre_c_print_patch_trace(4); //    {0, "monitor_error entered"},/* 4 */
   if (pilot_task_activities[Track_index].status = ACT_RUN_FCR) then
       ignorep := fiacre_c_print_patch_trace(2); //   {0, "monitor_error stopping Track"},	/* 2 */
       pilot_task_activities[Track_index].stop := true
   else
       ignorep := fiacre_c_print_patch_trace(9) //   {0, "monitor_error nothing to stop"},	/* 9 */
   end;
   ignorep := fiacre_c_print_patch_trace(5); //  {0, "monitor_error to monitor_start"},/* 5 */
   to monitor_start