Add sim_control

modules/dreamview/backend/BUILD

@@ -15,6 +15,7 @@ cc_library(
         "//modules/common:apollo_app",
         "//modules/common/adapters:adapter_manager",
         "//modules/dreamview/backend/simulation_world:simulation_world_updater",
+        "//modules/dreamview/backend/sim_control",
         "//modules/dreamview/backend/websocket",
         "@civetweb//:civetweb++",
     ],

modules/dreamview/backend/common/dreamview_gflags.cc

@@ -28,12 +28,12 @@ DEFINE_string(static_file_dir, "modules/dreamview/frontend/dist",
 
 DEFINE_int32(server_port, 8888, "The port of backend webserver");
 
+// TODO(siyangy): Make this directly configurable by script/dreamview
 DEFINE_bool(
     enable_sim_control, false,
     "Whether to enable SimControl to publish localiztion and chassis message.");
 
-DEFINE_string(routing_request_file, "modules/map/data/garage_routing.pb.txt",
-              "File path of the routing request that SimControl will read the "
+DEFINE_string(routing_result_file, "modules/map/data/garage_routing.pb.txt",
+              "File path of the routing result that SimControl will read the "
               "start point from. If this is absent, SimControl will directly "
-              "take the RoutingResult directly from ROS to determine the "
-              "start point.");
+              "take the RoutingResult from ROS to determine the start point.");

modules/dreamview/backend/common/dreamview_gflags.h

@@ -29,6 +29,6 @@ DECLARE_int32(server_port);
 
 DECLARE_bool(enable_sim_control);
 
-DECLARE_string(routing_request_file);
+DECLARE_string(routing_result_file);
 
 #endif  // MODULES_DREAMVIEW_BACKEND_COMMON_DREAMVIEW_GFLAGS_H_

modules/dreamview/backend/dreamview.cc

@@ -39,6 +39,11 @@ Status Dreamview::Init() {
   AdapterManager::Init();
   VehicleConfigHelper::Init();
 
+  CHECK(AdapterManager::GetChassis()) << "Chassis is not initialized.";
+  CHECK(AdapterManager::GetPlanning()) << "Planning is not initialized.";
+  CHECK(AdapterManager::GetLocalization())
+      << "Localization is not initialized.";
+
   // Initialize and run the web server which serves the dreamview htmls and
   // javascripts and handles websocket requests.
   server_.reset(
@@ -50,20 +55,22 @@ Status Dreamview::Init() {
   map_service_.reset(new MapService(FLAGS_dreamview_map));
   sim_world_updater_.reset(
       new SimulationWorldUpdater(websocket_.get(), map_service_.get()));
+  sim_control_.reset(new SimControl());
 
   return Status::OK();
 }
 
 Status Dreamview::Start() {
-  // start ROS timer, one-shot = false, auto-start = true
-  timer_ = AdapterManager::CreateTimer(ros::Duration(kSimWorldTimeInterval),
-                                       &SimulationWorldUpdater::OnPushTimer,
-                                       sim_world_updater_.get());
+  sim_world_updater_->Start();
+  if (FLAGS_enable_sim_control) {
+    sim_control_->Start();
+  }
   return Status::OK();
 }
 
 void Dreamview::Stop() {
   server_->close();
+  sim_control_->Stop();
 }
 
 }  // namespace dreamview

modules/dreamview/backend/dreamview.h

@@ -26,6 +26,7 @@
 
 #include "modules/dreamview/backend/map/map_service.h"
 #include "modules/dreamview/backend/simulation_world/simulation_world_updater.h"
+#include "modules/dreamview/backend/sim_control/sim_control.h"
 #include "modules/dreamview/backend/websocket/websocket.h"
 
 /**
@@ -44,12 +45,9 @@ class Dreamview : public apollo::common::ApolloApp {
   virtual ~Dreamview() = default;
 
  private:
-  // Time interval, in seconds, between pushing SimulationWorld to frontend.
-  static constexpr double kSimWorldTimeInterval = 0.1;
-
-  ros::Timer timer_;
   std::unique_ptr<SimulationWorldUpdater> sim_world_updater_;
   std::unique_ptr<CivetServer> server_;
+  std::unique_ptr<SimControl> sim_control_;
   std::unique_ptr<WebSocketHandler> websocket_;
   std::unique_ptr<MapService> map_service_;
 };

modules/dreamview/backend/sim_control/BUILD

+load("//tools:cpplint.bzl", "cpplint")
+
+package(default_visibility = ["//visibility:public"])
+
+cc_library(
+    name = "sim_control",
+    srcs = [
+        "sim_control.cc",
+    ],
+    hdrs = [
+        "sim_control.h",
+    ],
+    deps = [
+        "//modules/common/adapters:adapter_manager",
+        "//modules/dreamview/backend/common:dreamview_gflags",
+    ],
+)
+
+cpplint()

modules/dreamview/backend/sim_control/sim_control.cc

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#include "modules/dreamview/backend/sim_control/sim_control.h"
+
+#include <cmath>
+
+#include "modules/common/math/math_utils.h"
+#include "modules/common/math/quaternion.h"
+#include "modules/common/time/time.h"
+#include "modules/common/util/file.h"
+
+namespace apollo {
+namespace dreamview {
+
+using apollo::common::adapter::AdapterManager;
+using apollo::common::math::HeadingToQuaternion;
+using apollo::common::math::NormalizeAngle;
+using apollo::common::time::Clock;
+using apollo::common::TrajectoryPoint;
+using apollo::common::util::GetProtoFromFile;
+using apollo::hdmap::RoutingResult;
+using apollo::localization::LocalizationEstimate;
+using apollo::canbus::Chassis;
+
+SimControl::SimControl() {
+  if (FLAGS_enable_sim_control) {
+    RoutingResult routing;
+    if (!GetProtoFromFile(FLAGS_routing_result_file, &routing)) {
+      AWARN << "Unable to read start point from file: "
+            << FLAGS_routing_result_file;
+      return;
+    }
+    SetStartPoint(routing, &next_point_);
+  }
+}
+
+void SimControl::SetStartPoint(const RoutingResult& routing,
+                               TrajectoryPoint* point) {
+  point->set_v(0.0);
+  point->set_a(0.0);
+
+  auto* p = point->mutable_path_point();
+
+  p->set_x(routing.routing_request().start().pose().x());
+  p->set_y(routing.routing_request().start().pose().y());
+  p->set_z(0.0);
+
+  // TODO(siyangy): Calculate the real theta when the API is ready.
+  p->set_theta(0.0);
+}
+
+void SimControl::Start() {
+  // Setup planning data callback.
+  AdapterManager::SetPlanningCallback(&SimControl::OnPlanning, this);
+
+  // Start timer to publish localiztion and chassis messages.
+  sim_control_timer_ = AdapterManager::CreateTimer(
+      ros::Duration(kSimControlInterval), &SimControl::TimerCallback, this);
+}
+
+void SimControl::Stop() {
+  sim_control_timer_.stop();
+}
+
+void SimControl::OnPlanning(const apollo::planning::ADCTrajectory& trajectory) {
+  // Reset current trajectory and the indices upon receiving a new trajectory.
+  current_trajectory_ = trajectory;
+  prev_point_index_ = 0;
+  next_point_index_ = 0;
+}
+
+void SimControl::Freeze() {
+  next_point_.set_v(0.0);
+  next_point_.set_a(0.0);
+  prev_point_ = next_point_;
+}
+
+double SimControl::AbsoluteTimeOfNextPoint() {
+  return current_trajectory_.header().timestamp_sec() +
+         current_trajectory_.trajectory_point(next_point_index_)
+             .relative_time();
+}
+
+bool SimControl::NextPointWithinRange() {
+  return next_point_index_ < current_trajectory_.trajectory_point_size() - 1;
+}
+
+void SimControl::TimerCallback(const ros::TimerEvent& event) {
+  AdapterManager::Observe();
+
+  // Result of the interpolation.
+  double lambda = 0;
+  auto current_time = apollo::common::time::ToSecond(Clock::Now());
+
+  if (AdapterManager::GetPlanning()->Empty()) {
+    prev_point_ = next_point_;
+  } else {
+    if (current_trajectory_.estop().is_estop() || !NextPointWithinRange()) {
+      // Freeze the car when there's an estop or the current trajectory has been
+      // exhausted.
+      Freeze();
+    } else {
+      // Determine the status of the car based on received planning message.
+      double timestamp = current_trajectory_.header().timestamp_sec();
+
+      while (NextPointWithinRange() &&
+             current_time > AbsoluteTimeOfNextPoint()) {
+        ++next_point_index_;
+      }
+
+      if (next_point_index_ == 0) {
+        AERROR << "First trajectory point is a future point!";
+        return;
+      }
+
+      if (current_time > AbsoluteTimeOfNextPoint()) {
+        prev_point_index_ = next_point_index_;
+      } else {
+        prev_point_index_ = next_point_index_ - 1;
+      }
+
+      next_point_ = current_trajectory_.trajectory_point(next_point_index_);
+      prev_point_ = current_trajectory_.trajectory_point(prev_point_index_);
+
+      // Calculate the ratio based on the the position of current time in
+      // between the previous point and the next point, where lamda =
+      // (current_point - prev_point) / (next_point - prev_point).
+      if (next_point_index_ != prev_point_index_) {
+        lambda = (current_time - timestamp - prev_point_.relative_time()) /
+                 (next_point_.relative_time() - prev_point_.relative_time());
+      }
+    }
+  }
+
+  PublishChassis(lambda);
+  PublishLocalization(lambda);
+}
+
+void SimControl::PublishChassis(double lambda) {
+  Chassis chassis;
+  AdapterManager::FillChassisHeader("SimControl", chassis.mutable_header());
+
+  chassis.set_engine_started(true);
+  chassis.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE);
+  chassis.set_gear_location(Chassis::GEAR_DRIVE);
+
+  double cur_speed = Interpolate(prev_point_.v(), next_point_.v(), lambda);
+  chassis.set_speed_mps(cur_speed);
+
+  AdapterManager::PublishChassis(chassis);
+}
+
+void SimControl::PublishLocalization(double lambda) {
+  LocalizationEstimate localization;
+  AdapterManager::FillLocalizationHeader("SimControl",
+                                         localization.mutable_header());
+
+  auto* pose = localization.mutable_pose();
+  auto prev = prev_point_.path_point();
+  auto next = next_point_.path_point();
+
+  // Set position
+  double cur_x = Interpolate(prev.x(), next.x(), lambda);
+  pose->mutable_position()->set_x(cur_x);
+  double cur_y = Interpolate(prev.y(), next.y(), lambda);
+  pose->mutable_position()->set_y(cur_y);
+  double cur_z = Interpolate(prev.z(), next.z(), lambda);
+  pose->mutable_position()->set_z(cur_z);
+
+  // Set orientation
+  double cur_theta = NormalizeAngle(
+      prev.theta() + lambda * NormalizeAngle(next.theta() - prev.theta()));
+
+  Eigen::Quaternion<double> cur_orientation =
+      HeadingToQuaternion<double>(cur_theta);
+  pose->mutable_orientation()->set_qx(cur_orientation.x());
+  pose->mutable_orientation()->set_qy(cur_orientation.y());
+  pose->mutable_orientation()->set_qz(cur_orientation.z());
+  pose->mutable_orientation()->set_qw(cur_orientation.w());
+
+  // Set linear_velocity
+  double cur_speed = Interpolate(prev_point_.v(), next_point_.v(), lambda);
+  pose->mutable_linear_velocity()->set_x(std::cos(cur_theta) * cur_speed);
+  pose->mutable_linear_velocity()->set_y(std::sin(cur_theta) * cur_speed);
+  pose->mutable_linear_velocity()->set_z(0);
+
+  // Set angular_velocity
+  double cur_curvature = Interpolate(prev.kappa(), next.kappa(), lambda);
+  pose->mutable_angular_velocity()->set_x(0);
+  pose->mutable_angular_velocity()->set_y(0);
+  pose->mutable_angular_velocity()->set_z(cur_speed * cur_curvature);
+
+  // Set linear_acceleration
+  double cur_acceleration_s =
+      Interpolate(prev_point_.a(), next_point_.a(), lambda);
+  auto* linear_acceleration = pose->mutable_linear_acceleration();
+  linear_acceleration->set_x(std::cos(cur_theta) * cur_acceleration_s);
+  linear_acceleration->set_y(std::sin(cur_theta) * cur_acceleration_s);
+  linear_acceleration->set_z(0);
+
+  AdapterManager::PublishLocalization(localization);
+}
+
+}  // namespace dreamview
+}  // namespace apollo

modules/dreamview/backend/sim_control/sim_control.h

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file
+ */
+
+#ifndef MODULES_DREAMVIEW_BACKEND_SIM_CONTROL_SIM_CONTROL_H_
+#define MODULES_DREAMVIEW_BACKEND_SIM_CONTROL_SIM_CONTROL_H_
+
+#include <string>
+
+#include "modules/common/adapters/adapter_manager.h"
+#include "modules/dreamview/backend/common/dreamview_gflags.h"
+
+/**
+ * @namespace apollo::dreamview
+ * @brief apollo::dreamview
+ */
+namespace apollo {
+namespace dreamview {
+
+/**
+ * @class SimControl
+ * @brief
+ */
+class SimControl {
+ public:
+  SimControl();
+
+  /**
+   * @brief Starts the timer to publish simulated localization and chassis
+   * messages.
+   */
+  void Start();
+
+  /**
+   * @brief Stops the timer.
+   */
+  void Stop();
+
+ private:
+  void OnPlanning(const apollo::planning::ADCTrajectory &trajectory);
+
+  void SetStartPoint(const apollo::hdmap::RoutingResult &routing,
+                     apollo::common::TrajectoryPoint *point);
+
+  void Freeze();
+
+  double AbsoluteTimeOfNextPoint();
+  bool NextPointWithinRange();
+
+  void TimerCallback(const ros::TimerEvent &event);
+
+  void PublishChassis(double lambda);
+  void PublishLocalization(double lambda);
+
+  template <typename T>
+  T Interpolate(T prev, T next, double lambda) {
+    return (1 - lambda) * prev + lambda * next;
+  }
+
+  // The timer to publish simulated localization and chassis messages.
+  ros::Timer sim_control_timer_;
+
+  // Time interval of the timer, in seconds.
+  static constexpr double kSimControlInterval = 0.01;
+
+  // The latest received planning trajectory.
+  apollo::planning::ADCTrajectory current_trajectory_;
+  // The index of the previous and next point with regard to the
+  // current_trajectory.
+  int prev_point_index_;
+  int next_point_index_;
+
+  apollo::common::TrajectoryPoint prev_point_;
+  apollo::common::TrajectoryPoint next_point_;
+};
+
+}  // namespace dreamview
+}  // namespace apollo
+
+#endif  // MODULES_DREAMVIEW_BACKEND_SIM_CONTROL_SIM_CONTROL_H_

modules/dreamview/backend/simulation_world/simulation_world_updater.cc

@@ -24,6 +24,7 @@
 namespace apollo {
 namespace dreamview {
 
+using apollo::common::adapter::AdapterManager;
 using google::protobuf::util::MessageToJsonString;
 using Json = nlohmann::json;
 
@@ -52,6 +53,13 @@ SimulationWorldUpdater::SimulationWorldUpdater(WebSocketHandler *websocket,
       });
 }
 
+void SimulationWorldUpdater::Start() {
+  // start ROS timer, one-shot = false, auto-start = true
+  timer_ =
+      AdapterManager::CreateTimer(ros::Duration(kSimWorldTimeInterval),
+                                  &SimulationWorldUpdater::OnPushTimer, this);
+}
+
 void SimulationWorldUpdater::OnPushTimer(const ros::TimerEvent &event) {
   sim_world_service_.Update();
   if (!sim_world_service_.ReadyToPush()) {

modules/dreamview/backend/simulation_world/simulation_world_updater.h

@@ -50,6 +50,12 @@ class SimulationWorldUpdater {
    */
   SimulationWorldUpdater(WebSocketHandler *websocket, MapService *map_service);
 
+  /**
+   * @brief Starts to push simulation_world to frontend.
+   */
+  void Start();
+
+ private:
   /**
    * @brief The callback function to get updates from SimulationWorldService,
    * and push them to the frontend clients via websocket when the periodic timer
@@ -58,7 +64,10 @@ class SimulationWorldUpdater {
    */
   void OnPushTimer(const ros::TimerEvent &event);
 
- private:
+  // Time interval, in seconds, between pushing SimulationWorld to frontend.
+  static constexpr double kSimWorldTimeInterval = 0.1;
+
+  ros::Timer timer_;
   SimulationWorldService sim_world_service_;
   MapService *map_service_;
   WebSocketHandler *websocket_;

modules/perception/obstacle/lidar/visualizer/BUILD

 load("//tools:cpplint.bzl", "cpplint")
 
+load("//tools:cpplint.bzl", "cpplint")
+
 package(default_visibility = ["//visibility:public"])
 
 cc_library(
@@ -21,4 +23,6 @@ cc_library(
         "@pcl//:pcl",
         "@vtk//:vtk",
     ],
-)
\ No newline at end of file
+)
+
+cpplint()

modules/perception/tool/BUILD

 load("//tools:cpplint.bzl", "cpplint")
 
+load("//tools:cpplint.bzl", "cpplint")
+
 package(default_visibility = ["//visibility:public"])
 
 cc_binary(
@@ -17,4 +19,6 @@ cc_binary(
         "//modules/perception/obstacle/onboard:perception_obstacle_lidar_process",
         "@eigen//:eigen",
     ],
-)
\ No newline at end of file
+)
+
+cpplint()