Fix cutin ego scenario

[rodrigoqueiroz]

Cut In Scenario is now working (server side)

• Added fix to lane change from the other branch.

• Added new check to abort cut-in if there is no vehicle on target lane and testing cut-in completion that was commented out.

• New Behavior Tree btrees/sdv/st_cut_in_ego.btree and fixed scenario files anm_scenarios/gs_ringroad_modified_with_ego.osm

• Scenario contains an additional vehicle vid=2 acting as placeholder. Switch its type to gs=vehicle to work as a placeholder for Ego while prototyping a scenario server side. Switch to gs=placeholder when running with Autonomy stack and it will be ignored at scenario startup.

[mantkiew]

I ported the scenario to https://git.uwaterloo.ca/wise-lab/wise-sim-scenarios/-/merge_requests/15 (I removed the ego placeholder and ego start. Ego start is in the base scenario file). While the vid=5 drives in GSS and ego drives in WISE sim, ego vid=99 does not show up in GSS.

[mantkiew]

Is it required to have ego_start in the geoscenarioserver part? When it's present, vid=99 shows up in GSS but it does not drive. When it's absent, wid=99 does not show up in GSS dash.

[mantkiew]

I just like to note that in wise-sim-scenarios scenario ring_road_ccw the GSS vehicles are visible in WISE sim; however, ego is not visible in GSS. I wonder why in ring_road_section_cut_in the GSS vehicle does not even show up in WISE Sim?

[rodrigoqueiroz]

If there is Ego start in the scenario, the Server creates an EV with and expect that a vehicle of same ID in the client will send its pose. We are currently using id 99 for Ego. All EVs show up as green in the dash (if it is not inside the field of view, it still shows up in the table).

A more generic form is to not have Ego Start, create a Vehicle of type EV and give it the same ID Ego has in the client.

If Ego is not moving in any scenario, the updated pose is being lost (could be in the client? does Ego actor have the flags with same ID? Is the client finding the connection on the way back?)

[mantkiew]

@WAEliasmith @rodrigoqueiroz @mchlswyr The shared memory now works properly for vehicles. Specific fixes:

SimTraffic.py:111 - fix pattern matching to get number of pedestrians SimSharedMemory.py:107-111 - catch a potential header parsing exception SimSharedMemory.py:116 - reduce the range to prevent out of bounds or getting a pedestrian SimSharedMemory.py:134-137 - catch a potential vehicle state parsing exception SimSharedMemory.py:159-161 - catch a potential pedestrian state parsing exception GSClient.cpp:161 - get all vehicles (was missing the last one previously) GSClient.cpp:220 - process SDV or TV the same way GSClient.cpp:264 - be more specific and process EV only GSClient.cpp:322 - write number of pedestrians into the header

Testing

1. checkout this branch and build & package the dev simulator.
2. Use https://git.uwaterloo.ca/wise-lab/wise-sim-scenarios/-/merge_requests/15 scenarios: ring_road_section_cut_in and ring_road_ccw.
3. Copy scripts wise_sim_switch.bash and gss_switch.bash from anm_unreal_test_suite/scripts to wise-sim-scenarios/scripts. Switch both wise-sim and gss to their dev versions using the scripts

Test 1:

$ slaunch ring_road_ccw -s

• should see 10 GSS vehicles and GSS dash should show vehicle 99 (ego) moving once you hit "drive"

Test 2:

$ slaunch ring_road_section_cut_in -s

• hit drive immediately
• should see one GSS vehicle passing the ego and GSS dash should show vehicle 99 (ego) moving once you hit "drive"
• currently, the GSS vehicle does not properly perform the cut-in. Need to tweak it's behavior

[rodrigoqueiroz]

Not related with this patch, but building the local project is giving this error. E: Failed to fetch https://apt.kitware.com/ubuntu/pool/main/c/cmake/cmake-data_3.19.2-0kitware1_all.deb . Are we fetching this or is it part of the Unreal cleanup?

[mantkiew]

@rodrigoqueiroz when I click on this link, I can actually download the file. Could have been an intermittent internet problem. Can you download that file manually?

Also, the cmake version on my system is cmake is already the newest version (3.20.0-0kitware1). If you still have 3.19, feel free to remove it and just rerun the road-demo installer or road-demo prepare-target.bash. We have to upgrade cmake on Ubuntu 18.04 because of known bugs with the default cmake version.

[mantkiew]

This is now ready to test again.

Pull your anm_unreal_sim(master) and anm_sim_platform(master) to get the gentle screen session shutdown. It works smoothly. After you press <ctlr>+c wait a bit to see a message

...
^C
Closing screen sessions. Please wait...
Closed session 'simulator'.
Closed session 'gss'.

that means the TRAP catching the interrupt was entered and proper shutdown is being executed.

You have to source

$ source ~/anm_unreal_sim/submodules/anm_sim_platform/scripts/short_launch.bash

to get that behavior as it is not released yet.

HINT: The system state window is introducing a delay after you press <ctrl>+c. I always close that window manually after I press drive and then the shutdown of the scenario is very quick.

HINT2: execute ipcs to see if the shared memory blocks of size 2048 and semaphore arrays are present, similar to this:

$ ipcs
...
------ Shared Memory Segments --------
key        shmid      owner      perms      bytes      nattch     status      
...
0x0001e240 31         ma         666        2048       14                      
0x00051877 32         ma         666        2048       14                      

------ Semaphore Arrays --------
key        semid      owner      perms      nsems     
0x000549c5 0          ma         600        1         
0x000e3e39 1          ma         600        1         

They should not be present after the scenario has properly shut down as the GSS is supposed to remove them in a destructor.

[rodrigoqueiroz]

I got it working, but still very inconsistent here. Can we add an auto set to drive to the scenario launch? (publish system state = drive to ros). That can speed up the start up and help syncing both vehicles

[mantkiew]

I got it working, but still very inconsistent here.

Really, even with the "closing screen sessions"?

Can we add an auto set to drive to the scenario launch? (publish system state = drive to ros). That can speed up the start up and help syncing both vehicles

Yes, you can run it as a benchmark, that sends autodrive. I pushed a gss benchmark to wise-sim-scenarios PR. You can run it using

$ rosrun anm_sim_platform benchmark.py _name:=gss

Benchmark adds much more consistency to scenario execution. It always waits 10s before hitting Drive. That 10s is necessary for the ROS system to stabilize and get all frequencies correct (also localizer has to initilalize properly, etc).

Can we add the 10s delay somehow to GSS? Or GSS could wait until ego hits drive?

Now, since this is maxing out all cores, I'm not sure how much consistency we can get on these weak machines.

Also, I noticed that while shared memory works well for the first scenario in the benchmark, it does not work anymore for the second scenario. It has to do how the benchmark is terminating the script launch.bash (it's not sending <ctrl>+c, it just kills it and the memory does not get cleaned up).

[mantkiew]

I have a suggestion: when GSS recognizes it's in a co-simulation (vehicle 99 exists), GSS will wait 10s to start all traffic.

[rodrigoqueiroz]

Really, even with the "closing screen sessions"?

That updates in anm_unreal_sim(master) and anm_sim_platform(master) just need checkout? Do I need run any install script? (other than source and reopen the terminal)

Can we add the 10s delay somehow to GSS? Or GSS could wait until ego hits drive?

Yes. We can make it wait from the behavior tree. However, the scenario was first designed for them to start in parallel. For the vehicle to start before Ego and catch up it needs to be different. I will make the change.

Now, since this is maxing out all cores, I'm not sure how much consistency we can get on these weak machines.

Consistency with the shared memory. Just with the cut-in scenario and 2 vehicles. After working, it always fails in the next attempt. The Ego position will be different (apparently reading some garbage) and not updating.

[rodrigoqueiroz]

Regarding the delay. When I was running the experiments for the paper, I had a ROS node monitoring the SystemState Drive, and just then launching the GSServer with Popen

# Launch the gss when the drive command has been received
if system_state_msg.state == SystemState.ST_DRIVE:
    rospy.loginfo("Starting GS Server")

    gs_command = GSS_COMMAND_FORMAT.format(rospy.get_param('~scenario'))
    self.gs_process = subprocess.Popen(shlex.split(gs_command), stdout=self.gss_log, stderr=subprocess.STDOUT)

[mchlswyr]

That updates in anm_unreal_sim(master) and anm_sim_platform(master) just need checkout? Do I need run any install script? (other than source and reopen the terminal)

You will just have to pull the latest master. The updates were only in the launch script.

[mantkiew]

You will just have to pull the latest master. The updates were only in the launch script.

But you have to source the short_launch.bash from submodules/anm_sim_platform/scripts not from /opt.

After you press <ctrl>+c, you should see

...
^C
Closing screen sessions. Please wait...
Closed session 'simulator'.
Closed session 'gss'.

That indicates that you are using the correct launch.bash from source.

[mantkiew]

See testing in https://github.com/wavelab/anm_sim_platform/pull/34 to properly test this PR using a benchmark.

Now, we have to add the 10s delay when in co-simulation.

[rodrigoqueiroz]

Getting many of these errors: Client state error: No. client vehicles (0) not the same as server vehicles (2). W0401 11:49:18.843827 10031 SimSharedMemory.py:142] 40 0.0247685 0 0 the prints empty bytes

The garbage is still there.

[rodrigoqueiroz]

I added a condition to hold start of planners if the shared memory is active and there is at least one External Vehicle until a valid state is read (meaning Client is connected and has valid states). That doesn't solve the problem with Ego taking too long to start. We need a clear signal from the client for when to start. The GeoScenarioServer must be tool and subject system independent, so adding 10 seconds complicates things for other uses.

[rodrigoqueiroz]

• I added an option in SimConfig, so WAIT_FOR_CLIENT in cosimulation is now optional. When we have time, we can work on a better signal for starting other than having a valid state (for example, send a message when Ego is actually driving)
• Also added a conversion inside shared memory to make sure we always write tick count as int. The shared memory needs a better structure to separate and parse attributes. The float was pushing values out of their expected order on the cpp client.
• Changed the scenario a bit. If needed, add a delay using the tmin=X in line 7. I'm not using any delay, but with the WAIT_FOR_CLIENT the vehicle does perform the cut-in.

condition c_time(sim_time(tmin=0))