PoseBundleToDrawMessage demo program

pose_bundle_draw_demo.cc

#include "drake/automotive/gen/driving_command_translator.h"
#include "drake/automotive/simple_car.h"
#include "drake/lcm/drake_lcm.h"
#include "drake/lcmt_viewer_draw.hpp"
#include "drake/systems/analysis/simulator.h"
#include "drake/systems/framework/diagram.h"
#include "drake/systems/framework/diagram_builder.h"
#include "drake/systems/lcm/lcm_publisher_system.h"
#include "drake/systems/lcm/lcm_subscriber_system.h"
#include "drake/systems/lcm/serializer.h"
#include "drake/systems/rendering/drake_visualizer_client.h"
#include "drake/systems/rendering/pose_aggregator.h"
#include "drake/systems/rendering/pose_bundle_to_draw_message.h"
#include "drake/lcmtypes/drake/lcmt_viewer_load_robot.hpp"

#include <Eigen/Dense>

namespace drake {

using lcm::DrakeLcm;
using systems::lcm::LcmPublisherSystem;
using systems::lcm::LcmSubscriberSystem;
using systems::lcm::Serializer;
using systems::DiagramBuilder;
using systems::rendering::MakeGeometryData;
using systems::rendering::PoseAggregator;
using systems::rendering::PoseBundleToDrawMessage;

namespace automotive {


int do_main() {
  DrakeLcm lcm;
  DiagramBuilder<double> builder;

  // Emit a one-time load message.
  Serializer<lcmt_viewer_load_robot> load_serializer;
  std::vector<uint8_t> message_bytes;

  lcmt_viewer_load_robot message;
  DrakeShapes::VisualElement car_vis(DrakeShapes::Box(Eigen::Vector3d(1.0, 1.0, 1.0)),
                                     Eigen::Isometry3d::Identity(),
                                     Eigen::Vector4d(0.7, 0.7, 0.7, 1));
  message.num_links = 1;
  message.link.resize(1);
  message.link[0].name = "car";
  message.link[0].robot_num = 0;
  message.link[0].num_geom = 1;
  message.link[0].geom.resize(1);
  message.link[0].geom[0] = MakeGeometryData(car_vis);

  const int message_length = message.getEncodedSize();
  message_bytes.resize(message_length);
  message.encode(message_bytes.data(), 0, message_length);
  lcm.Publish("DRAKE_VIEWER_LOAD_ROBOT", message_bytes.data(), message_bytes.size());

  // Build the simulation diagram.
  DrivingCommandTranslator driving_command_translator;
  auto command_subscriber =
      builder.template AddSystem<LcmSubscriberSystem>(
          "DRIVING_COMMAND", driving_command_translator, &lcm);
  SimpleCar<double>* car = builder.template AddSystem<SimpleCar>();
  PoseAggregator<double>* aggregator = builder.template AddSystem<PoseAggregator>();
  PoseBundleToDrawMessage* converter =
      builder.template AddSystem<PoseBundleToDrawMessage>();
  LcmPublisherSystem* publisher =
      builder.template AddSystem<LcmPublisherSystem>("DRAKE_VIEWER_DRAW",
                                                     std::make_unique<Serializer<lcmt_viewer_draw>>(),
                                                     &lcm);
  publisher->set_publish_period(0.1);

  builder.Connect(*command_subscriber, *car);
  builder.Connect(car->pose_output(), aggregator->AddSingleInput("car"));
  builder.Connect(*aggregator, *converter);
  builder.Connect(*converter, *publisher);
  auto diagram = builder.Build();

  auto context = diagram->CreateDefaultContext();
  systems::Simulator<double> simulator(*diagram, std::move(context));
  simulator.set_target_realtime_rate(1.0);
  double t = 1.0;
  lcm.StartReceiveThread();
  while (true) {
    simulator.StepTo(t);
    t += 1.0;
  }
}


}  // namespace automotive
}  // namespace drake

int main (int argc, char* argv[]) {
  return drake::automotive::do_main();
}