<commit_before>#include "spruce.hh"

#include "drake/systems/LCMSystem.h"
#include "drake/systems/LinearSystem.h"
#include "drake/systems/pd_control_system.h"
#include "drake/systems/plants/BotVisualizer.h"
#include "drake/systems/plants/RigidBodySystem.h"
#include "drake/util/drakeAppUtil.h"
#include "lcmtypes/drake/lcmt_driving_control_cmd_t.hpp"

using namespace std;
using namespace Eigen;
using namespace Drake;

template <typename ScalarType = double>
class DrivingCommand {
 public:
  typedef drake::lcmt_driving_control_cmd_t LCMMessageType;
  static std::string channel() { return "DRIVING_COMMAND"; }

  DrivingCommand(void) : throttle(0), brake(0), steering_angle(0) {}

  template <typename Derived>
  DrivingCommand(  // NOLINT(runtime/explicit) per Drake::Vector.
      const Eigen::MatrixBase<Derived>& x)
      : steering_angle(x(0)), throttle(x(1)), brake(x(2)) {}

  template <typename Derived>
  DrivingCommand& operator=(const Eigen::MatrixBase<Derived>& x) {
    steering_angle = x(0);
    throttle = x(1);
    brake = x(2);
    return *this;
  }

  friend Eigen::Vector3d toEigen(const DrivingCommand<ScalarType>& vec) {
    Eigen::Vector3d x;
    x << vec.steering_angle, vec.throttle, vec.brake;
    return x;
  }

  friend std::string getCoordinateName(const DrivingCommand<ScalarType>& vec,
                                       unsigned int index) {
    switch (index) {
      case 0:
        return "steering_angle";
      case 1:
        return "throttle";
      case 2:
        return "brake";
    }
    return "error";
  }
  static const int RowsAtCompileTime = 3;

  ScalarType steering_angle;
  ScalarType throttle;
  ScalarType brake;
};

/**
 * A toString method for DrivingCommand.
 */
template <typename ScalarType = double>
std::ostream& operator<<(std::ostream& os,
                         const DrivingCommand<ScalarType>& dc) {
  return os << "[steering_angle = " << dc.steering_angle
            << ", throttle = " << dc.throttle << ", brake = " << dc.brake
            << "]";
}

bool decode(const drake::lcmt_driving_control_cmd_t& msg, double& t,
            DrivingCommand<double>& x) {
  t = double(msg.timestamp) / 1000.0;
  x.steering_angle = msg.steering_angle;
  x.throttle = msg.throttle_value;
  x.brake = msg.brake_value;
  return true;
}

/** Driving Simulator
 * Usage:  simulateLCM vehicle_model_file [world_model files ...]
 */

int main(int argc, char* argv[]) {
  if (argc < 2) {
    std::cerr << "Usage: " << argv[0] << " vehicle_model [world sdf files ...]"
              << std::endl;
    return 1;
  }

  // todo: consider moving this logic into the RigidBodySystem class so it can
  // be reused
  DrakeJoint::FloatingBaseType floating_base_type = DrakeJoint::QUATERNION;

  auto rigid_body_sys = make_shared<RigidBodySystem>();

  // The offset between Drake's world frame and the vehicle's world frame
  // depends on whether the vehicle model is an SDF file or a URDF file. The SDF
  // file internally specifies the offset, meaning z_offset to be zero.
  // The URDF cannot specify this offset internally, meaning z_offset should
  // be 0.378326 meters.
  double z_offset = 0;
  {
    spruce::path p(argv[1]);
    auto ext = p.extension();

    // Converts the file extension to be all lower case.
    std::transform(ext.begin(), ext.end(), ext.begin(), ::tolower);

    // Sets the Z-axis transform from Drake's world frame to the model's
    // world frame by 0.378326 meters if the model is a URDF.
    if (ext == ".urdf")
      z_offset = 0.378326;
  }

  // The following variable, weld_to_frame, is only needed if the model is a
  // URDF file. It is needed since URDF does not specify the location and
  // orientation of the car's root node in the world. If the model is an SDF,
  // weld_to_frame is ignored by the parser.
  auto weld_to_frame = allocate_shared<RigidBodyFrame>(
      aligned_allocator<RigidBodyFrame>(),
      // Weld the model to the world link.
      "world",

      // A pointer to a rigid body to which to weld the model is not needed
      // since the model will be welded to the world, which can by automatically
      // found within the rigid body tree.
      nullptr,

      // The following parameter specifies the X,Y,Z position of the car's root
      // link in the world's frame. The kinematics of the car model requires
      // that its root link be elevated along the Z-axis by 0.378326m.
      Eigen::Vector3d(0, 0, z_offset),

      // The following parameter specifies the roll, pitch, and yaw of the car's
      // root link in the world's frame.
      Eigen::Vector3d(0, 0, 0));

  rigid_body_sys->addRobotFromFile(argv[1], floating_base_type, weld_to_frame);

  auto const& tree = rigid_body_sys->getRigidBodyTree();
  for (int i = 2; i < argc; i++)
    tree->addRobotFromSDF(argv[i], DrakeJoint::FIXED);  // add environment

  if (argc < 3) {  // add flat terrain
    double box_width = 1000;
    double box_depth = 10;
    DrakeShapes::Box geom(Vector3d(box_width, box_width, box_depth));
    Isometry3d T_element_to_link = Isometry3d::Identity();
    T_element_to_link.translation() << 0, 0,
        -box_depth / 2;  // top of the box is at z=0
    auto& world = tree->bodies[0];
    Vector4d color;
    color << 0.9297, 0.7930, 0.6758,
        1;  // was hex2dec({'ee','cb','ad'})'/256 in matlab
    world->addVisualElement(<commit_msg>Modifications from round 1 review.<commit_after>#include "spruce.hh"

#include "drake/systems/LCMSystem.h"
#include "drake/systems/LinearSystem.h"
#include "drake/systems/pd_control_system.h"
#include "drake/systems/plants/BotVisualizer.h"
#include "drake/systems/plants/RigidBodySystem.h"
#include "drake/util/drakeAppUtil.h"
#include "lcmtypes/drake/lcmt_driving_control_cmd_t.hpp"

using namespace std;
using namespace Eigen;
using namespace Drake;

template <typename ScalarType = double>
class DrivingCommand {
 public:
  typedef drake::lcmt_driving_control_cmd_t LCMMessageType;
  static std::string channel() { return "DRIVING_COMMAND"; }

  DrivingCommand(void) : throttle(0), brake(0), steering_angle(0) {}

  template <typename Derived>
  DrivingCommand(  // NOLINT(runtime/explicit) per Drake::Vector.
      const Eigen::MatrixBase<Derived>& x)
      : steering_angle(x(0)), throttle(x(1)), brake(x(2)) {}

  template <typename Derived>
  DrivingCommand& operator=(const Eigen::MatrixBase<Derived>& x) {
    steering_angle = x(0);
    throttle = x(1);
    brake = x(2);
    return *this;
  }

  friend Eigen::Vector3d toEigen(const DrivingCommand<ScalarType>& vec) {
    Eigen::Vector3d x;
    x << vec.steering_angle, vec.throttle, vec.brake;
    return x;
  }

  friend std::string getCoordinateName(const DrivingCommand<ScalarType>& vec,
                                       unsigned int index) {
    switch (index) {
      case 0:
        return "steering_angle";
      case 1:
        return "throttle";
      case 2:
        return "brake";
    }
    return "error";
  }
  static const int RowsAtCompileTime = 3;

  ScalarType steering_angle;
  ScalarType throttle;
  ScalarType brake;
};

/**
 * A toString method for DrivingCommand.
 */
template <typename ScalarType = double>
std::ostream& operator<<(std::ostream& os,
                         const DrivingCommand<ScalarType>& dc) {
  return os << "[steering_angle = " << dc.steering_angle
            << ", throttle = " << dc.throttle << ", brake = " << dc.brake
            << "]";
}

bool decode(const drake::lcmt_driving_control_cmd_t& msg, double& t,
            DrivingCommand<double>& x) {
  t = double(msg.timestamp) / 1000.0;
  x.steering_angle = msg.steering_angle;
  x.throttle = msg.throttle_value;
  x.brake = msg.brake_value;
  return true;
}

/** Driving Simulator
 * Usage:  simulateLCM vehicle_model_file [world_model files ...]
 */

int main(int argc, char* argv[]) {
  if (argc < 2) {
    std::cerr << "Usage: " << argv[0] << " vehicle_model [world sdf files ...]"
              << std::endl;
    return 1;
  }

  // todo: consider moving this logic into the RigidBodySystem class so it can
  // be reused
  DrakeJoint::FloatingBaseType floating_base_type = DrakeJoint::QUATERNION;

  auto rigid_body_sys = make_shared<RigidBodySystem>();

  // The offset between Drake's world frame and the vehicle's world frame
  // depends on whether the vehicle model is an SDF file or a URDF file. The SDF
  // file internally specifies the offset, meaning z_offset to be zero.
  // The URDF cannot specify this offset internally, meaning z_offset should
  // be 0.378326 meters.
  double z_offset = 0;

  // TODO(liangfok): Once PR 2171 is merged, modify prius.urdf to contain a
  // world link and proper offset of the chassis_floor. For more information,
  // see: https://github.com/RobotLocomotion/drake/pull/2171 and
  // https://github.com/RobotLocomotion/drake/issues/2247
  if (std::string(argv[1]).find("prius.urdf") != std::string::npos)
    z_offset = 0.378326;

  // The following variable, weld_to_frame, is only needed if the model is a
  // URDF file. It is needed since URDF does not specify the location and
  // orientation of the car's root node in the world. If the model is an SDF,
  // weld_to_frame is ignored by the parser.
  auto weld_to_frame = allocate_shared<RigidBodyFrame>(
      aligned_allocator<RigidBodyFrame>(),
      // Weld the model to the world link.
      "world",

      // A pointer to a rigid body to which to weld the model is not needed
      // since the model will be welded to the world, which can by automatically
      // found within the rigid body tree.
      nullptr,

      // The following parameter specifies the X,Y,Z position of the car's root
      // link in the world's frame.
      Eigen::Vector3d(0, 0, z_offset),

      // The following parameter specifies the roll, pitch, and yaw of the car's
      // root link in the world's frame.
      Eigen::Vector3d(0, 0, 0));

  rigid_body_sys->addRobotFromFile(argv[1], floating_base_type, weld_to_frame);

  auto const& tree = rigid_body_sys->getRigidBodyTree();
  for (int i = 2; i < argc; i++)
    tree->addRobotFromSDF(argv[i], DrakeJoint::FIXED);  // add environment

  if (argc < 3) {  // add flat terrain
    double box_width = 1000;
    double box_depth = 10;
    DrakeShapes::Box geom(Vector3d(box_width, box_width, box_depth));
    Isometry3d T_element_to_link = Isometry3d::Identity();
    T_element_to_link.translation() << 0, 0,
        -box_depth / 2;  // top of the box is at z=0
    auto& world = tree->bodies[0];
    Vector4d color;
    color << 0.9297, 0.7930, 0.6758,
        1;  // was hex2dec({'ee','cb','ad'})'/256 in matlab
    world->addVisualElement(
{'fraction_non_alphanumeric': 0.11683913452492944, 'fraction_numerical': 0.018344308560677328, 'mean_word_length': 3.1953433307024466, 'pattern_counts': {'":': 0, '<': 95, '<?xml version=': 0, '>': 39, 'https://': 2, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 2, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4210178', 'n_tokens_mistral': 3711, 'n_tokens_neox': 3435, 'n_words': 1013}