Section 5 SPaT Processing - ika-rwth-aachen/acdc GitHub Wiki

• Contents
• SPaT/MAP processing for trajectory planning
• Task 1: Configure the bridge so that you receive the messages (SPATEM/MAPEM) from the traffic light
• Task 2: Visualize the messages (SPATEM/MAPEM) in RVIZ
  • MAPEM visualization
  • SPATEM visualization
• Task 3: Modify the trajectory planner, so it can react to the traffic lights
  • Process MAPEM
  • Process SPATEM
• Task 4: Visualize other vehicles via Cooperative Awareness Messages (CAM)
• Wrap-up
• References

In contrast to the previous subsection, where your computer acted as a cloud and implemented a collective function, in the following tasks you will implement a supportive function that allows the trajectory planning to handle traffic lights.

For this purpose, we have added a pedestrian traffic light to the ika test track known from the previous sections. The main goal of this task is to visualize the SPaT/MAP data and adapt the trajectory planning from Section 4 so that it can react to the traffic light.

If not already done, you should first build your workspace.

catkin build
source devel/setup.bash # make sure to execute this line in each of the following terminals!

The connected traffic light publishs its state and the corresponding intersection topology as SPATEM resp. MAPEM via MQTT to the broker from the last subsection. The ASN.1 decoding has already been done on the server, so the messages are in the following formats:

• SPATEM: v2x_SPAT
• MAPEM: v2x_MAP

You can take a closer look at all ROS message formats in this folder.

To receive the messages you have to use the mqtt_client, which you should know from the last subsection. Analogous to the procedure described there, you have to create a new configuration or extend your existing one accordingly. Use the following information for setting the parameters:

• the broker runs on a public server with this address broker.hivemq.com
• you don't need any credentials to connect to the broker
• you can subscribe to the messages on the following topics:
  • Signal Phase and Timing Message (SPATEM): ika_acdc_22/SPATEM
  • MAP Message (MAPEM): ika_acdc_22/MAPEM

If you are done with that, you can start your launch file by:

roslaunch mqtt_launchpack ${YourLaunchFile}.launch # make sure you have included the right parameter file

If everything was done correctly, the node should have started with the information Connected to broker at 'tcp://broker.hivemq.com:1883'. Afterwards you can check with rqt if the messages arrive on the ROS topics you have chosen.

We have already prepared the nodes in the section 4 & section 5 folder, so you only have to implement the relevant parts.

To visualize the messages, you must first configure the etsi_visualization node with your chosen SPATEM/MAPEM ROS topic. You can set the necessary parameters in start_ETSIViz.launch.

Now we want to start visualizing our received MAP messages. Therefore you have to fill the gap at Line 29 in MAPViz.cpp.

// ### START CODE HERE ###
// Identify number of intersections in message
int n_intersections = 0; // Task
// ### END CODE HERE ###

If you don't know exactly how a MAPEM is structured, it's best to look at the definitions or the ETSI standard.

If you are done with that, you can rebuild your workspace and start the predefined V2X launchfile for the trajectory planner. It already includes the launchfile for the ETSI visualization.

catkin build
roslaunch trajectory_planner vehicle_guidance_v2x.launch

If everything works correctly, the visualization will look like this:

As you can see, the visualization has been extended with some markers for the MAPEM. The position of the traffic lights are shown by the circle markers, which are currently displayed in black, because you have not interpreted the SPaT message yet. The trajectory planner also still acts as in Section 4 and is not influenced by the traffic lights.

Now you will face the SPaT message. Therefore you have to fill the SignalGroup struct with the information from the received SPAT messages.

struct SignalGroup
{
  uint8_t sg_id; // signal group id
  uint8_t current_state; // current state of signal group
  uint16_t next_change; // time for next signal state change (timing_likelyTime)
};

Fill in the correct solution for the marked lines in SPATViz.cpp. You can use the expression for the current_state as an example.

// ### START CODE HERE ###
sg.current_state = spat_intsctn.states[m].state_time_speed[0].eventState; // Get first element of state_time_speed
sg.sg_id = 0; // Task
sg.next_change = 0; // Task
// ### END CODE HERE ###

If you don't know exactly how a SPATEM is structured, it's best to look at the definitions or the ETSI standard.

After that you also have to visualize the current state of the traffic light in place of the black circles. Therefore, solve the task at the other marked lines in SPATViz.cpp. If you don't know exactly which values the state of a signal group can take, have a look at the last video.

// ### START CODE HERE ###
// Set marker color depending on signal group state
// The signal group state is given by the function variable "state"
marker.color.r = 0.0;
marker.color.g = 0.0;
marker.color.b = 0.0;
// ### END CODE HERE ###

Afterwards you have to rebuild the workspace and restart the trajectory planner.

catkin build
roslaunch trajectory_planner vehicle_guidance_v2x.launch

If the implementation is successful, the visualization should show the same states for the four lanes and the opposite states for the two pedestrian directions as well as a timer with the time until the next signal change. For example like this:

Now that MAPEM and SPATEM of the traffic light have been visualized, you have to let the trajectory planner know how to react to it. For this, you have again to adjust the topics for the messages in vehicle_guidance_v2x.launch.

After that, you have to interpret the messages in their related subscribers. For that, open the v2x_planner_interface.cpp and fill in the marked gaps. Thereby you can use the implementations from the etsi_visualization as a reference. In sum, you have to transfer the received messages into a traffic light struct which is built like this:

struct TrafficLight
{
  int stationID;
  std::vector<geometry_msgs::Point> ingress_lane;
  int sig_id;
  int tl_id;
  bool red;
  ros::Time last_spat;
};

At best, start this task with the MAPEM callback function. With the help of the MAPEM, you can get information about the ingress_lane, sig_id and tl_id. This is important so that the vehicle knows where the traffic lights are and to which lane(s) they refer. So open the v2x_planner_interface.cpp at line 73 and fill the two gaps.

// ### START CODE HERE ###
// Identify number of intersections in message
int n_intersections = 0; // Task
// ### END CODE HERE ###

// Loop all intersections in message
for(int i = 0; i < n_intersections; i++) {
  definitions::v2x_MAP_Intersection intsctn = msg.intersections[i];

  // Loop all lanes to get signal groups and traffic light positions
  for(int m = 0; m < intsctn.adjacent_lanes.size(); m++) {

    definitions::v2x_MAP_Lane lane = intsctn.adjacent_lanes[m];
    
    // ### START CODE HERE ###
    // only ingress lanes can consider traffic signals -> skip all egress lanes
    bool is_egress_lane = true; // Task          
    if (is_egress_lane){
      continue;
    }
    // ### END CODE HERE ###

    ...

  }
}

After solving this first part of the task (and rebuilding the workspace), the vehicle should always stop before the traffic light, but not start again.

The final part of this task is to also interpret the SPaT messages so that the vehicle will move on again. Therefore open the v2x_planner_interface.cpp at line 41 and fill the last two gaps.

// ### START CODE HERE ###
// Identify number of intersections in message
int n_intersections = 0; // Task
// ### END CODE HERE ###

// Loop all intersections in message
for(int i = 0; i < n_intersections; i++) {
  definitions::v2x_SPAT_IntersectionState spat_intsctn = msg.spatData_intersections[i];
  // Loop all movement states to get signal groups
  for(int m = 0; m < spat_intsctn.states.size(); m++) {
    //Loop all traffic lights stored in the map data
    for(int k = 0; k<trafficlights.size(); k++)
    {
      if(trafficlights[k].stationID == msg.header_stationID && trafficlights[k].sig_id == spat_intsctn.states[m].signalGroup)
      {
        trafficlights[k].last_spat = ros::Time::now();

        // ### START CODE HERE ###
        // Check if signal state is red or not (hint: use an if condition)
        trafficlights[k].red = true; // Task
        // ### END CODE HERE ###
      }
    }
  }
}

Remember: If you have problems with the implementation, you can use the SPATEM/MAPEM subscriber from the spat visualization as a reference. You can also look again in the last video how a SPATEM is defined, or look at the received SPATEM/MAPEM in rqt.

When you are ready with your implementation, save your code rebuild the workspace and start the trajectory planner:

catkin build
roslaunch trajectory_planner vehicle_guidance_v2x.launch

The trajectory planner should now react correctly to the traffic lights! The visualization will look something like this:

If you notice incorrect behavior of the trajectory planner, feel free to adjust the weights of the cost terms and see how the behavior changes. This will let you change the parameterization during the simulation:

rosrun rqt_reconfigure rqt_reconfigure

In this last task of this workshop, we will focus on the Cooperative Awareness Message (CAM). Therefore you have to know that, using vehicle_guidance_v2x.launch, you already start the node which publishes your live position as CAM to the known MQTT broker when driving on the ika test track (etsi_message_generation). Your task will now be to visualize the CAMs of other students (their live position) in RVIZ. Since we also have a simulation running permanently and therefore also send a CAM to the broker, you should always see at least one other vehicle on the track - if implemented correctly.

First you have to make sure that you receive the CAMs of the others. For this, extend the mqtt_client configuration from Task 1. You can subscribe to the messages on the following MQTT topic:

• ika_acdc_22/CAM Set /CAM as corresponding ROS topic. You can also chose another one, but then note that you also have to change it in the start_ETSIViz.launch accordingly.

Now start your launchfile from Task 1 with the extended configuration and check in rqt if there are any messages on the CAM ROS topic.

roslaunch mqtt_launchpack ${YourLaunchFile}.launch # make sure you have included the right parameter file

If this is the case, these messages must now be interpreted. For this you have to fill the marked code lines in ETSIViz.cpp with the content from the received CAMs.

Note: The velocity is only given absolutely in the CAM, but must be split into x and y direction for our object definition. Use trigonometric functions for this. The following picture will help you.

// ### START CODE HERE
// fill with information from message
// use helping comments from Wiki
obj.IdInternal = 0; // stationID               // Task
float lon = 0;      // longitude (x)           // Task
float lat = 0;      // latitude (y)            // Task
float v_x = 0;      // velocity in x direction // Task
float v_y = 0;      // velocity in y direction // Task
// ### END CODE HERE

If you don't know exactly how a CAM is structured, it's best to look at the definitions or the ETSI standard.

If everything was implemented correctly, you should now always see at least one other vehicle including its speed vector (light blue) driving on the track. The more students are driving on the track at the same time, the more vehicles you will see!

Note: The CAMs are only visualized, the trajectory planner does not react to the other vehicles.

Your visualization could look like this, for example:

In this workshop you learned...

• how to interpret different ETSI messages.
  • SPAT
  • MAPEM
  • CAM
• how to use these messages to improve the trajectory planner from section 4.

• mqtt_client
• broker.hivemq.com
• ETSI standards
• ROS
• Flatland
• Control Toolbox

@article{adrlCT,
  title={The control toolbox — An open-source C++ library for robotics, optimal and model predictive control},
  author={Markus Giftthaler and Michael Neunert and Markus St{\"a}uble and Jonas Buchli},
  journal={2018 IEEE International Conference on Simulation, Modeling, and Programming for Autonomous Robots (SIMPAR)},
  year={2018},
  pages={123-129}
}