<commit_before>/******************************************************************************
 * 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::QuaternionToHeading;
using apollo::common::math::NormalizeAngle;
using apollo::common::time::Clock;
using apollo::common::TrajectoryPoint;
using apollo::common::util::GetProtoFromFile;
using apollo::routing::RoutingResponse;
using apollo::localization::LocalizationEstimate;
using apollo::canbus::Chassis;

SimControl::SimControl()
    : prev_point_index_(0),
      next_point_index_(0),
      received_planning_(false),
      initial_start_(true),
      enabled_(FLAGS_enable_sim_control) {
  if (enabled_) {
    RoutingResponse routing;
    if (!GetProtoFromFile(FLAGS_routing_result_file, &routing)) {
      AWARN << "Unable to read start point from file: "
            << FLAGS_routing_result_file;
      return;
    }
    SetStartPoint(routing);
  }
}

void SimControl::SetStartPoint(const RoutingResponse& routing) {
  next_point_.set_v(0.0);
  next_point_.set_a(0.0);

  auto* p = next_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);
  p->set_kappa(0.0);
  p->set_s(0.0);

  prev_point_index_ = next_point_index_ = 0;
  received_planning_ = false;

  if (enabled_) {
    Start();
  }
}

void SimControl::Start() {
  if (initial_start_) {
    // Setup planning and routing result data callback.
    AdapterManager::AddPlanningCallback(&SimControl::OnPlanning, this);
    AdapterManager::AddRoutingResponseCallback(&SimControl::SetStartPoint, this);

    // Start timer to publish localiztion and chassis messages.
    sim_control_timer_ = AdapterManager::CreateTimer(
        ros::Duration(kSimControlInterval), &SimControl::TimerCallback, this);

    initial_start_ = false;
  } else {
    sim_control_timer_.start();
  }
}

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;
  received_planning_ = true;
}

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) {
  // Result of the interpolation.
  double lambda = 0;
  auto current_time = apollo::common::time::ToSecond(Clock::Now());

  if (!received_planning_) {
    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);

  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);<commit_msg>Fix a sim_control frontend render error. (#419)<commit_after>/******************************************************************************
 * 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::QuaternionToHeading;
using apollo::common::math::NormalizeAngle;
using apollo::common::time::Clock;
using apollo::common::TrajectoryPoint;
using apollo::common::util::GetProtoFromFile;
using apollo::routing::RoutingResponse;
using apollo::localization::LocalizationEstimate;
using apollo::canbus::Chassis;

SimControl::SimControl()
    : prev_point_index_(0),
      next_point_index_(0),
      received_planning_(false),
      initial_start_(true),
      enabled_(FLAGS_enable_sim_control) {
  if (enabled_) {
    RoutingResponse routing;
    if (!GetProtoFromFile(FLAGS_routing_result_file, &routing)) {
      AWARN << "Unable to read start point from file: "
            << FLAGS_routing_result_file;
      return;
    }
    SetStartPoint(routing);
  }
}

void SimControl::SetStartPoint(const RoutingResponse& routing) {
  next_point_.set_v(0.0);
  next_point_.set_a(0.0);

  auto* p = next_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);
  p->set_kappa(0.0);
  p->set_s(0.0);

  prev_point_index_ = next_point_index_ = 0;
  received_planning_ = false;

  if (enabled_) {
    Start();
  }
}

void SimControl::Start() {
  if (initial_start_) {
    // Setup planning and routing result data callback.
    AdapterManager::AddPlanningCallback(&SimControl::OnPlanning, this);
    AdapterManager::AddRoutingResponseCallback(&SimControl::SetStartPoint, this);

    // Start timer to publish localiztion and chassis messages.
    sim_control_timer_ = AdapterManager::CreateTimer(
        ros::Duration(kSimControlInterval), &SimControl::TimerCallback, this);

    initial_start_ = false;
  } else {
    sim_control_timer_.start();
  }
}

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;
  received_planning_ = true;
}

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) {
  // Result of the interpolation.
  double lambda = 0;
  auto current_time = apollo::common::time::ToSecond(Clock::Now());

  if (!received_planning_) {
    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);

  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);
  chassis.set_throttle_percentage(0.0);
  chassis.set_brake_percentage(0.0);

  AdapterManager::PublishChassis(chassis);
{'fraction_non_alphanumeric': 0.1361404676707997, 'fraction_numerical': 0.006074727469418324, 'mean_word_length': 4.369973190348525, 'pattern_counts': {'":': 0, '<': 19, '<?xml version=': 0, '>': 21, 'https://': 0, 'lorem ipsum': 0, 'www.': 2, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '7386511', 'n_tokens_mistral': 3817, 'n_tokens_neox': 3638, 'n_words': 791}