Dreamview: plot station_error graph in control monitor

modules/dreamview/backend/dreamview.cc

@@ -37,7 +37,9 @@ using apollo::common::time::Clock;
 using apollo::common::util::PathExists;
 using apollo::hdmap::BaseMapFile;
 
-std::string Dreamview::Name() const { return FLAGS_dreamview_module_name; }
+std::string Dreamview::Name() const {
+  return FLAGS_dreamview_module_name;
+}
 
 void Dreamview::TerminateProfilingMode(const ros::TimerEvent& event) {
   Stop();
@@ -62,6 +64,8 @@ Status Dreamview::Init() {
 
   // Check the expected adapters are initialized.
   CHECK(AdapterManager::GetChassis()) << "ChassisAdapter is not initialized.";
+  CHECK(AdapterManager::GetControlCommand())
+      << "ControlCommandAdapter is not initialized.";
   CHECK(AdapterManager::GetGps()) << "GpsAdapter is not initialized.";
   CHECK(AdapterManager::GetPlanning()) << "PlanningAdapter is not initialized.";
   CHECK(AdapterManager::GetLocalization())

modules/dreamview/backend/simulation_world/simulation_world_service.cc

@@ -52,10 +52,11 @@ using apollo::common::adapter::AdapterManager;
 using apollo::common::monitor::MonitorMessage;
 using apollo::common::monitor::MonitorMessageItem;
 using apollo::common::time::Clock;
-using apollo::common::time::millis;
 using apollo::common::time::ToSecond;
+using apollo::common::time::millis;
 using apollo::common::util::DownsampleByAngle;
 using apollo::common::util::GetProtoFromFile;
+using apollo::control::ControlCommand;
 using apollo::hdmap::Path;
 using apollo::localization::Gps;
 using apollo::localization::LocalizationEstimate;
@@ -253,6 +254,8 @@ void SimulationWorldService::Update() {
   UpdateWithLatestObserved("PredictionObstacles",
                            AdapterManager::GetPrediction());
   UpdateWithLatestObserved("Planning", AdapterManager::GetPlanning());
+  UpdateWithLatestObserved("ControlCommand",
+                           AdapterManager::GetControlCommand());
   for (const auto &kv : obj_map_) {
     *world_.add_object() = kv.second;
   }
@@ -912,5 +915,23 @@ void SimulationWorldService::RegisterMessageCallbacks() {
       &SimulationWorldService::UpdateSimulationWorld, this);
 }
 
+template <>
+void SimulationWorldService::UpdateSimulationWorld(
+    const ControlCommand &control_command) {
+  auto *control_data = world_.mutable_control_data();
+  control_data->set_timestamp_sec(control_command.header().timestamp_sec());
+
+  if (control_command.has_debug()) {
+    auto &debug = control_command.debug();
+    if (debug.has_simple_lon_debug() &&
+        debug.simple_lon_debug().has_station_error()) {
+      control_data->set_station_error(debug.simple_lon_debug().station_error());
+    } else if (debug.has_simple_mpc_debug() &&
+               debug.simple_mpc_debug().has_station_error()) {
+      control_data->set_station_error(debug.simple_mpc_debug().station_error());
+    }
+  }
+}
+
 }  // namespace dreamview
 }  // namespace apollo

modules/dreamview/conf/adapter.conf

@@ -13,6 +13,11 @@ config {
   mode: RECEIVE_ONLY
   message_history_limit: 1
 }
+config {
+  type: CONTROL_COMMAND
+  mode: RECEIVE_ONLY
+  message_history_limit: 1
+}
 config {
   type: GPS
   mode: RECEIVE_ONLY

modules/dreamview/frontend/proto_bundle/sim_world_proto_bundle.json

@@ -387,6 +387,18 @@
                 }
               }
             },
+            "ControlData": {
+              "fields": {
+                "timestampSec": {
+                  "type": "double",
+                  "id": 1
+                },
+                "stationError": {
+                  "type": "double",
+                  "id": 2
+                }
+              }
+            },
             "SimulationWorld": {
               "fields": {
                 "timestamp": {
@@ -475,6 +487,10 @@
                 "laneMarker": {
                   "type": "apollo.perception.LaneMarkers",
                   "id": 21
+                },
+                "controlData": {
+                  "type": "ControlData",
+                  "id": 22
                 }
               }
             }

modules/dreamview/frontend/src/components/PNCMonitor/ControlMonitor.js

@@ -34,6 +34,7 @@ export default class ControlMonitor extends React.Component {
                 {this.generateScatterGraph('speedGraph', data.speedGraph)}
                 {this.generateScatterGraph('accelerationGraph', data.accelerationGraph)}
                 {this.generateScatterGraph('curvatureGraph', data.curvatureGraph)}
+                {this.generateScatterGraph('stationErrorGraph', data.stationErrorGraph)}
             </div>
         );
     }

modules/dreamview/frontend/src/store/config/ControlGraph.yml

@@ -153,3 +153,21 @@ accelerationGraph:
                 pointRadius: 4
                 fill: false
                 showLine: true
+stationErrorGraph:
+    title: 'Station Error'
+    options:
+        legend:
+            display: false
+        axes:
+            x:
+                labelString: 't (second)'
+            y:
+                labelString: 'error (m)'
+    properties:
+        lines:
+            error:
+                color: 'rgba(0, 106, 255, 1)' # blue
+                borderWidth: 2
+                pointRadius: 0
+                fill: false
+                showLine: ture
\ No newline at end of file

modules/dreamview/frontend/src/store/control_data.js

@@ -40,9 +40,38 @@ export default class ControlData {
                 real: [],
                 autoModeZone: []
             },
+            stationErrorGraph: {
+                error: [],
+            }
         };
     }
 
+    updateStationErrorGraph(controlData) {
+        if (!controlData.stationError) {
+            return;
+        }
+
+        const graph = this.data.stationErrorGraph;
+        const currentTimestamp = controlData.timestampSec;
+
+        // clean up data if needed
+        const removeAllPoints = graph.error.length > 0 &&
+              currentTimestamp < graph.error[graph.error.length - 1].x;
+        const removeOldestPoint = (graph.length >= MAX_HISTORY_POINTS);
+        if (removeAllPoints) {
+            graph.error = [];
+        } else if (removeOldestPoint) {
+            graph.error.shift();
+        }
+
+        // add new data
+        const hasNewData = graph.error.length === 0 ||
+            currentTimestamp !== graph.error[graph.error.length - 1].x;
+        if (hasNewData) {
+            graph.error.push({x: currentTimestamp, y: controlData.stationError});
+        }
+    }
+
     updateSteerCurve(graph, adc) {
         const steeringAngle = adc.steeringAngle / adc.steeringRatio;
         let R = null;
@@ -168,5 +197,9 @@ export default class ControlData {
 
             this.updateTime(world.planningTime);
         }
+
+        if (world.controlData) {
+            this.updateStationErrorGraph(world.controlData);
+        }
     }
 }
\ No newline at end of file

modules/dreamview/proto/simulation_world.proto

@@ -169,7 +169,12 @@ message MapElementIds {
   repeated string clear_area = 9;
 }
 
-// Next-id: 22
+message ControlData {
+  optional double timestamp_sec = 1;
+  optional double station_error = 2;
+}
+
+// Next-id: 23
 message SimulationWorld {
   // Timestamp in milliseconds
   optional double timestamp = 1;
@@ -224,4 +229,7 @@ message SimulationWorld {
 
   // Lane Markers from perception
   optional apollo.perception.LaneMarkers lane_marker = 21;
+
+  // Control data
+  optional ControlData control_data = 22;
 }