Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[infra] drop the unused lcm interface #408

Merged
merged 4 commits into from
May 15, 2018
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 1 addition & 22 deletions backend/automotive_simulator.cc
Original file line number Diff line number Diff line change
Expand Up @@ -19,16 +19,13 @@
#include "drake/automotive/prius_vis.h"
#include "drake/common/drake_throw.h"
#include "drake/common/text_logging.h"
#include "drake/lcm/drake_lcm.h"
#include "drake/lcmt_viewer_draw.hpp"
#include "drake/multibody/joints/floating_base_types.h"
#include "drake/multibody/parsers/urdf_parser.h"
#include "drake/multibody/rigid_body_plant/create_load_robot_message.h"
#include "drake/systems/analysis/runge_kutta2_integrator.h"
#include "drake/systems/framework/basic_vector.h"
#include "drake/systems/framework/context.h"
#include "drake/systems/framework/system.h"
#include "drake/systems/lcm/lcmt_drake_signal_translator.h"
#include "drake/systems/primitives/multiplexer.h"

#include "backend/agent_plugin_loader.h"
Expand All @@ -52,21 +49,15 @@ using drake::maliput::api::RoadGeometry;
using drake::maliput::api::RoadGeometryId;
using drake::multibody::joints::kRollPitchYaw;
using drake::systems::AbstractValue;
using drake::systems::lcm::LcmPublisherSystem;
using drake::systems::OutputPort;
using drake::systems::rendering::PoseBundle;
using drake::systems::RungeKutta2Integrator;
using drake::systems::System;
using drake::systems::SystemOutput;

template <typename T>
AutomotiveSimulator<T>::AutomotiveSimulator()
: AutomotiveSimulator(std::make_unique<drake::lcm::DrakeLcm>()) {}

template <typename T>
AutomotiveSimulator<T>::AutomotiveSimulator(
std::unique_ptr<drake::lcm::DrakeLcmInterface> lcm)
: lcm_(std::move(lcm)) {
AutomotiveSimulator<T>::AutomotiveSimulator() {
aggregator_ =
builder_
->template AddSystem<drake::systems::rendering::PoseAggregator<T>>();
Expand All @@ -84,18 +75,6 @@ AutomotiveSimulator<T>::AutomotiveSimulator(
bundle_to_draw_->set_name("bundle_to_draw");
}

template <typename T>
AutomotiveSimulator<T>::~AutomotiveSimulator() {
// Forces the LCM instance to be destroyed before any of the subscribers are
// destroyed.
lcm_.reset();
}

template <typename T>
drake::lcm::DrakeLcmInterface* AutomotiveSimulator<T>::get_lcm() {
return lcm_.get();
}

template <typename T>
drake::systems::DiagramBuilder<T>* AutomotiveSimulator<T>::get_builder() {
DELPHYNE_DEMAND(!has_started());
Expand Down
9 changes: 0 additions & 9 deletions backend/automotive_simulator.h
Original file line number Diff line number Diff line change
Expand Up @@ -24,13 +24,10 @@
#include "drake/automotive/simple_car.h"
#include "drake/automotive/trajectory_car.h"
#include "drake/common/drake_copyable.h"
#include "drake/lcm/drake_lcm_interface.h"
#include "drake/lcmt_viewer_load_robot.hpp"
#include "drake/multibody/rigid_body_tree.h"
#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/rendering/pose_aggregator.h"
#include "drake/systems/rendering/pose_bundle.h"
#include "drake/systems/rendering/pose_bundle_to_draw_message.h"
Expand Down Expand Up @@ -61,12 +58,6 @@ class AutomotiveSimulator {
/// A constructor that configures this object to use DrakeLcm, which
/// encapsulates a _real_ LCM instance.
AutomotiveSimulator();
explicit AutomotiveSimulator(
std::unique_ptr<drake::lcm::DrakeLcmInterface> lcm);
~AutomotiveSimulator();

/// Returns the LCM object used by this AutomotiveSimulator.
drake::lcm::DrakeLcmInterface* get_lcm();

/// Returns the DiagramBuilder.
/// @pre Start() has NOT been called.
Expand Down
30 changes: 15 additions & 15 deletions backend/translation_systems/drake_to_ign.h
Original file line number Diff line number Diff line change
Expand Up @@ -78,34 +78,34 @@ class DrakeToIgn : public drake::systems::LeafSystem<double> {
static const unsigned int kPositionVectorSize{3};
static const unsigned int kOrientationVectorSize{4};

// @brief Converts an array of floats (LCM's type for positions) to an
// @brief Converts an array of floats to an
// ignition position message.
//
// @param[in] lcm_position The LCM position array.
// @param[in] position The position array
// @param[out] ign_position The ignition position message.
static void LcmPositionToIgnition(const float lcm_position[3],
static void PositionArrayToIgnition(const float position[3],
ignition::msgs::Vector3d* ign_position) {
DELPHYNE_DEMAND(ign_position != nullptr);

ign_position->set_x(lcm_position[0]);
ign_position->set_y(lcm_position[1]);
ign_position->set_z(lcm_position[2]);
ign_position->set_x(position[0]);
ign_position->set_y(position[1]);
ign_position->set_z(position[2]);
}

// @brief Converts an array of floats (LCM's type for quaternions) to an
// ignition quaterion message.
// @brief Converts an array of floats to an
// ignition quaternion message.
//
// @param[in] lcm_quaternion The LCM orientation array.
// @param[in] quaternion The orientation array.
// @param[out] ign_quaternion The ign quaternion message.
static void LcmQuaternionToIgnition(
const float lcm_quaternion[4],
static void QuaternionArrayToIgnition(
const float quaternion[4],
ignition::msgs::Quaternion* ign_quaternion) {
DELPHYNE_DEMAND(ign_quaternion != nullptr);

ign_quaternion->set_w(lcm_quaternion[0]);
ign_quaternion->set_x(lcm_quaternion[1]);
ign_quaternion->set_y(lcm_quaternion[2]);
ign_quaternion->set_z(lcm_quaternion[3]);
ign_quaternion->set_w(quaternion[0]);
ign_quaternion->set_x(quaternion[1]);
ign_quaternion->set_y(quaternion[2]);
ign_quaternion->set_z(quaternion[3]);
}

// @brief Converts an array of floats (LCM's type for a color) to an
Expand Down
4 changes: 2 additions & 2 deletions backend/translation_systems/lcm_viewer_draw_to_ign_model_v.cc
Original file line number Diff line number Diff line change
Expand Up @@ -47,12 +47,12 @@ void LcmViewerDrawToIgnModelV::DoDrakeToIgnTranslation(

// Checks position size and translates.
DELPHYNE_DEMAND(lcm_message.position[i].size() == kPositionVectorSize);
LcmPositionToIgnition(lcm_message.position[i].data(),
PositionArrayToIgnition(lcm_message.position[i].data(),
pose->mutable_position());

// Checks orientation size and translates.
DELPHYNE_DEMAND(lcm_message.quaternion[i].size() == kOrientationVectorSize);
LcmQuaternionToIgnition(lcm_message.quaternion[i].data(),
QuaternionArrayToIgnition(lcm_message.quaternion[i].data(),
pose->mutable_orientation());
}
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -55,8 +55,8 @@ void LcmViewerLoadRobotToIgnModelV::DoDrakeToIgnTranslation(
LcmGeometryToIgnition(geometry, new_visual->mutable_geometry());

ignition::msgs::Pose* pose = new_visual->mutable_pose();
LcmPositionToIgnition(geometry.position, pose->mutable_position());
LcmQuaternionToIgnition(geometry.quaternion,
PositionArrayToIgnition(geometry.position, pose->mutable_position());
QuaternionArrayToIgnition(geometry.quaternion,
pose->mutable_orientation());

ignition::msgs::Material* material = new_visual->mutable_material();
Expand Down
84 changes: 9 additions & 75 deletions test/regression/cpp/automotive_simulator_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -18,14 +18,8 @@
#include "drake/automotive/maliput/dragway/road_geometry.h"
#include "drake/automotive/prius_vis.h"
#include "drake/common/find_resource.h"
#include "drake/lcm/drake_mock_lcm.h"
#include "drake/lcmt_simple_car_state_t.hpp"
#include "drake/lcmt_viewer_draw.hpp"
#include "drake/systems/framework/basic_vector.h"
#include "drake/systems/framework/diagram_context.h"
#include "drake/systems/lcm/lcm_publisher_system.h"
#include "drake/systems/lcm/lcm_subscriber_system.h"
#include "drake/systems/lcm/lcmt_drake_signal_translator.h"
#include "drake/systems/rendering/pose_bundle.h"

#include "protobuf/simple_car_state.pb.h"
Expand Down Expand Up @@ -120,21 +114,9 @@ TEST_F(AutomotiveSimulatorTest, TestGetScene) {
// Simple touches on the getters.
TEST_F(AutomotiveSimulatorTest, BasicTest) {
auto simulator = std::make_unique<AutomotiveSimulator<double>>();
EXPECT_NE(nullptr, simulator->get_lcm());
EXPECT_NE(nullptr, simulator->get_builder());
}

// Obtains the serialized version of the last message transmitted on LCM channel
// @p channel. Uses @p translator to decode the message into @p result.
void GetLastPublishedSimpleCarState(
const std::string& channel,
const drake::systems::lcm::LcmAndVectorBaseTranslator& translator,
const drake::lcm::DrakeMockLcm* mock_lcm, SimpleCarState<double>* result) {
const std::vector<uint8_t>& message =
mock_lcm->get_last_published_message(channel);
translator.Deserialize(message.data(), message.size(), result);
}

// Covers simple-car, Start and StepBy
TEST_F(AutomotiveSimulatorTest, TestPriusSimpleCar) {
ignition::msgs::SimpleCarState state_message;
Expand All @@ -149,8 +131,7 @@ TEST_F(AutomotiveSimulatorTest, TestPriusSimpleCar) {
node.Subscribe<ignition::msgs::SimpleCarState>("agents/0/state", callback);

// Set up a basic simulation with just a Prius SimpleCar.
auto simulator = std::make_unique<AutomotiveSimulator<double>>(
std::make_unique<drake::lcm::DrakeMockLcm>());
auto simulator = std::make_unique<AutomotiveSimulator<double>>();

auto initial_state = std::make_unique<SimpleCarState<double>>();
std::map<std::string, linb::any> simple_params;
Expand Down Expand Up @@ -195,8 +176,7 @@ TEST_F(AutomotiveSimulatorTest, TestPriusSimpleCar) {

// Tests the ability to initialize a SimpleCar to a non-zero initial state.
TEST_F(AutomotiveSimulatorTest, TestPriusSimpleCarInitialState) {
auto simulator = std::make_unique<AutomotiveSimulator<double>>(
std::make_unique<drake::lcm::DrakeMockLcm>());
auto simulator = std::make_unique<AutomotiveSimulator<double>>();
const double kX{10};
const double kY{5.5};
const double kHeading{M_PI_2};
Expand Down Expand Up @@ -234,12 +214,7 @@ TEST_F(AutomotiveSimulatorTest, TestPriusSimpleCarInitialState) {

TEST_F(AutomotiveSimulatorTest, TestMobilControlledSimpleCar) {
// Set up a basic simulation with a MOBIL- and IDM-controlled SimpleCar.
auto simulator = std::make_unique<AutomotiveSimulator<double>>(
std::make_unique<drake::lcm::DrakeMockLcm>());

drake::lcm::DrakeMockLcm* lcm =
dynamic_cast<drake::lcm::DrakeMockLcm*>(simulator->get_lcm());
ASSERT_NE(lcm, nullptr);
auto simulator = std::make_unique<AutomotiveSimulator<double>>();

const drake::maliput::api::RoadGeometry* road{};
EXPECT_NO_THROW(
Expand Down Expand Up @@ -348,8 +323,7 @@ TEST_F(AutomotiveSimulatorTest, TestMobilControlledSimpleCar) {
// // start at position zero; the first has a speed of 1 m/s, while the other
// is
// // stationary. They both follow a straight 100 m long line.
// auto simulator = std::make_unique<AutomotiveSimulator<double>>(
// std::make_unique<drake::lcm::DrakeMockLcm>());
// auto simulator = std::make_unique<AutomotiveSimulator<double>>();
//
// std::map<std::string, linb::any> traj_params;
// traj_params["curve"] = curve;
Expand Down Expand Up @@ -469,8 +443,7 @@ double GetXPosition(const ignition::msgs::Model_V& message, double y) {
}

TEST_F(AutomotiveSimulatorTest, TestBadMaliputRailcars) {
auto simulator = std::make_unique<AutomotiveSimulator<double>>(
std::make_unique<drake::lcm::DrakeMockLcm>());
auto simulator = std::make_unique<AutomotiveSimulator<double>>();

const double kR{0.5};
MaliputRailcarParams<double> params;
Expand Down Expand Up @@ -527,12 +500,7 @@ TEST_F(AutomotiveSimulatorTest, TestBadMaliputRailcars) {

// Covers AddMaliputRailcar().
TEST_F(AutomotiveSimulatorTest, TestMaliputRailcar) {
auto simulator = std::make_unique<AutomotiveSimulator<double>>(
std::make_unique<drake::lcm::DrakeMockLcm>());

drake::lcm::DrakeMockLcm* lcm =
dynamic_cast<drake::lcm::DrakeMockLcm*>(simulator->get_lcm());
ASSERT_NE(lcm, nullptr);
auto simulator = std::make_unique<AutomotiveSimulator<double>>();
const double kR{0.5};
MaliputRailcarParams<double> params;
params.set_r(kR);
Expand Down Expand Up @@ -597,8 +565,7 @@ TEST_F(AutomotiveSimulatorTest, TestMaliputRailcar) {
// LcmPublisherSystem are instantiated in AutomotiveSimulator's Diagram and
// collectively result in the correct ignition messages being published.
TEST_F(AutomotiveSimulatorTest, TestLcmOutput) {
auto simulator = std::make_unique<AutomotiveSimulator<double>>(
std::make_unique<drake::lcm::DrakeMockLcm>());
auto simulator = std::make_unique<AutomotiveSimulator<double>>();

std::map<std::string, linb::any> simple_params;
auto state1 = std::make_unique<SimpleCarState<double>>();
Expand Down Expand Up @@ -684,8 +651,7 @@ TEST_F(AutomotiveSimulatorTest, TestLcmOutput) {
// Verifies that exceptions are thrown if a vehicle with a non-unique name is
// added to the simulation.
TEST_F(AutomotiveSimulatorTest, TestDuplicateVehicleNameException) {
auto simulator = std::make_unique<AutomotiveSimulator<double>>(
std::make_unique<drake::lcm::DrakeMockLcm>());
auto simulator = std::make_unique<AutomotiveSimulator<double>>();

auto state1 = std::make_unique<SimpleCarState<double>>();
auto state2 = std::make_unique<SimpleCarState<double>>();
Expand Down Expand Up @@ -730,8 +696,7 @@ TEST_F(AutomotiveSimulatorTest, TestDuplicateVehicleNameException) {
// Verifies that the velocity outputs of the MaliputRailcars are connected to
// the PoseAggregator, which prevents a regression of #5894.
TEST_F(AutomotiveSimulatorTest, TestRailcarVelocityOutput) {
auto simulator = std::make_unique<AutomotiveSimulator<double>>(
std::make_unique<drake::lcm::DrakeMockLcm>());
auto simulator = std::make_unique<AutomotiveSimulator<double>>();

const double kR{0.5};
MaliputRailcarParams<double> params;
Expand Down Expand Up @@ -820,37 +785,6 @@ TEST_F(AutomotiveSimulatorTest, TestBuild2) {
EXPECT_NO_THROW(simulator->GetDiagram());
}

// Verifies that messages are no longer being published in LCM
// DRAKE_VIEWER_LOAD_ROBOT and DRAKE_VIEWER_DRAW channels.
TEST_F(AutomotiveSimulatorTest, TestNoLcm) {
auto simulator = std::make_unique<AutomotiveSimulator<double>>(
std::make_unique<drake::lcm::DrakeMockLcm>());

auto simple_state = std::make_unique<SimpleCarState<double>>();

std::map<std::string, linb::any> simple_params;
simulator->AddLoadableAgent("simple-car", simple_params, "Model1",
std::move(simple_state));

simulator->Start();
simulator->StepBy(1e-3);

const drake::lcm::DrakeLcmInterface* lcm = simulator->get_lcm();
ASSERT_NE(lcm, nullptr);

const drake::lcm::DrakeMockLcm* mock_lcm =
dynamic_cast<const drake::lcm::DrakeMockLcm*>(lcm);
ASSERT_NE(mock_lcm, nullptr);

// There should be no message published in DRAKE_VIEWER_LOAD_ROBOT
EXPECT_THROW(mock_lcm->get_last_published_message("DRAKE_VIEWER_LOAD_ROBOT"),
std::runtime_error);

// There should be no message published in DRAKE_VIEWER_DRAW
EXPECT_THROW(mock_lcm->get_last_published_message("DRAKE_VIEWER_DRAW"),
std::runtime_error);
}

// Tests that AddLoadableAgent basically works.
TEST_F(AutomotiveSimulatorTest, TestAddLoadableAgentBasic) {
auto simulator = std::make_unique<AutomotiveSimulator<double>>();
Expand Down