multi_robot_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));

  std::array<std::string, 5> names{{"Alice", "Bob", "Chris", "Donna", "Ed"}};

  message.num_links = 5;
  message.link.resize(5);
  for (int i = 0; i < 5; ++i) {
    message.link[i].name = names[i];
    message.link[i].robot_num = i;
    message.link[i].num_geom = 1;
    message.link[i].geom.resize(1);
    message.link[i].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.
  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);

  DrivingCommandTranslator driving_command_translator;
  for (int i = 0; i < 5; ++i) {
    auto command_subscriber =
        builder.template AddSystem<LcmSubscriberSystem>(
            "DRIVING_COMMAND_" + names[i], driving_command_translator, &lcm);
    SimpleCar<double>* car = builder.template AddSystem<SimpleCar>();
    builder.Connect(*command_subscriber, *car);
    builder.Connect(car->pose_output(), aggregator->AddSingleInput(names[i], i));
  }

  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();
}