diff --git a/src/backend/automotive_simulator.cc b/src/backend/automotive_simulator.cc index bee5269ca..6ea8617cd 100644 --- a/src/backend/automotive_simulator.cc +++ b/src/backend/automotive_simulator.cc @@ -177,8 +177,8 @@ const RoadGeometry* AutomotiveSimulator::SetRoadGeometry( } template -const delphyne::AgentBase& AutomotiveSimulator::GetAgentById( - int agent_id) const { +const delphyne::AgentBase& +AutomotiveSimulator::GetAgentById(int agent_id) const { DELPHYNE_VALIDATE(agents_.count(agent_id) != 0, std::runtime_error, "No agent found with the given ID."); return *agents_.at(agent_id); @@ -348,9 +348,9 @@ void AutomotiveSimulator::Build() { // Connects the PoseBundleToSimpleCarState_v input and output. builder_->Connect( aggregator_->get_output_port(0), - pose_bundle_to_simple_car_state_v_->get_pose_bundle_input_port()); + pose_bundle_to_simple_car_state_v_->get_input_port(0)); builder_->Connect( - pose_bundle_to_simple_car_state_v_->get_simple_car_state_v_output(), + pose_bundle_to_simple_car_state_v_->get_output_port(0), agents_state_publisher_system->get_input_port(0)); diagram_ = builder_->Build(); diff --git a/src/translations/pose_bundle_to_simple_car_state_v.cc b/src/translations/pose_bundle_to_simple_car_state_v.cc index aaf760d12..dd853b1c4 100644 --- a/src/translations/pose_bundle_to_simple_car_state_v.cc +++ b/src/translations/pose_bundle_to_simple_car_state_v.cc @@ -8,55 +8,39 @@ #include #include "delphyne/protobuf/simple_car_state_v.pb.h" +#include "delphyne/protobuf/simple_car_state.pb.h" +#include "translations/drake_to_ign.h" namespace delphyne { -using drake::systems::rendering::PoseBundle; +const unsigned int kPoseBundleVectorSize{0}; -PoseBundleToSimpleCarState_V::PoseBundleToSimpleCarState_V() { - const int kPoseBundleVectorSize{0}; - // Declares input ports. - pose_bundle_input_port_index_ = - this->DeclareInputPort(drake::systems::kVectorValued, - kPoseBundleVectorSize) - .get_index(); - // Declares output port. - output_port_index_ = - this->DeclareAbstractOutputPort( - &PoseBundleToSimpleCarState_V::CalcSimpleCarState_V) - .get_index(); -} - -void PoseBundleToSimpleCarState_V::CalcSimpleCarState_V( - const drake::systems::Context& context, - ignition::msgs::SimpleCarState_V* output) const { - // Obtains the input and output. - const PoseBundle& pose_bundle = - drake::systems::System::EvalAbstractInput( - context, pose_bundle_input_port_index_) - ->template GetValue>(); +void PoseBundleToSimpleCarState_V::DoDrakeToIgnTranslation( + const drake::systems::rendering::PoseBundle& drake_message, + ignition::msgs::SimpleCarState_V* ign_message, int64_t time_ms) const { - // Clears the current SimpleCarState_V message from its previous content. - output->Clear(); + // Clears state from the previous call. + // @see DrakeToIgn::DoDrakeToIgnTranslation + ign_message->Clear(); - for (int i = 0; i < pose_bundle.get_num_poses(); ++i) { + for (int i = 0; i < drake_message.get_num_poses(); ++i) { // Gets the car's pose. - const drake::Isometry3& pose = pose_bundle.get_pose(i); + const drake::Isometry3& pose = drake_message.get_pose(i); // Translates pose from quaternion to euler. - const Eigen::Vector3d& euler_rotation = + const Eigen::Vector3d euler_rotation = pose.rotation().eulerAngles(0, 1, 2); // Calculates car's velocity. - const drake::systems::rendering::FrameVelocity& velocity = - pose_bundle.get_velocity(i); - const drake::multibody::SpatialVelocity& spatial_velocity = + const drake::systems::rendering::FrameVelocity velocity = + drake_message.get_velocity(i); + const drake::multibody::SpatialVelocity spatial_velocity = velocity.get_velocity(); - const double& velocity_norm = + const double velocity_norm = static_cast(spatial_velocity.translational().norm()); // Appends a new state to the vector. - auto* current_state = output->add_states(); - current_state->set_name("/agent/" + pose_bundle.get_name(i) + "/state"); + ignition::msgs::SimpleCarState* current_state = ign_message->add_states(); + current_state->set_name("/agent/" + drake_message.get_name(i) + "/state"); current_state->set_x(pose.translation().x()); current_state->set_y(pose.translation().y()); current_state->set_heading(euler_rotation(2)); diff --git a/src/translations/pose_bundle_to_simple_car_state_v.h b/src/translations/pose_bundle_to_simple_car_state_v.h index 46484db0e..2dbbe370c 100644 --- a/src/translations/pose_bundle_to_simple_car_state_v.h +++ b/src/translations/pose_bundle_to_simple_car_state_v.h @@ -2,43 +2,26 @@ #pragma once -#include +#include + +#include -#include "delphyne/protobuf/simple_car_state.pb.h" #include "delphyne/protobuf/simple_car_state_v.pb.h" +#include "translations/drake_to_ign.h" namespace delphyne { /// @brief A system that takes a PoseBundle and generates an array of /// SimpleCarStates (SimpleCarState_V). -class PoseBundleToSimpleCarState_V : public drake::systems::LeafSystem { - public: - PoseBundleToSimpleCarState_V(); - - /// @brief Returns the output port of the system. - const drake::systems::OutputPort& get_simple_car_state_v_output() - const { - return this->get_output_port(output_port_index_); - } - - /// @brief Returns the input port descriptor for the pose bundle. This port - /// expects a drake::sytems::rendering::PoseBundle. - const drake::systems::InputPortDescriptor& - get_pose_bundle_input_port() const { - return get_input_port(pose_bundle_input_port_index_); - } - +class PoseBundleToSimpleCarState_V + : public DrakeToIgn, + ignition::msgs::SimpleCarState_V> { protected: - /// @brief Calculates the SimpleCarState_V message based on the given context. - void CalcSimpleCarState_V(const drake::systems::Context& context, - ignition::msgs::SimpleCarState_V* output) const; - - private: - // @brief The pose bundle input port index assigned by the system. - int pose_bundle_input_port_index_{}; - - // @brief The abstract output port index assigned by the system. - int output_port_index_{}; + // @brief @see DrakeToIgn::DoDrakeToIgnTranslation. + void DoDrakeToIgnTranslation( + const drake::systems::rendering::PoseBundle& drake_message, + ignition::msgs::SimpleCarState_V* ign_message, + int64_t time_ms) const override; }; } // namespace delphyne diff --git a/test/regression/cpp/automotive_simulator_test.cc b/test/regression/cpp/automotive_simulator_test.cc index edbcf5d92..6722296ec 100644 --- a/test/regression/cpp/automotive_simulator_test.cc +++ b/test/regression/cpp/automotive_simulator_test.cc @@ -225,6 +225,8 @@ TEST_F(AutomotiveSimulatorTest, TestPriusSimpleCar) { kStateMessagesCount, kTimeoutMs, [this, &simulator]() { simulator->StepBy(kSmallTimeStep); })); + EXPECT_TRUE(ign_monitor.get_last_message().states_size() > 0); + ignition::msgs::SimpleCarState state_message = ign_monitor.get_last_message().states(0); EXPECT_LT(state_message.x(), 0.1); @@ -264,12 +266,15 @@ TEST_F(AutomotiveSimulatorTest, TestPriusSimpleCarInitialState) { kStateMessagesCount, kTimeoutMs, [this, &simulator]() { simulator->StepBy(kSmallTimeStep); })); + + EXPECT_TRUE(ign_monitor.get_last_message().states_size() > 0); + const ignition::msgs::SimpleCarState state_message = ign_monitor.get_last_message().states(0); - // A very minimal loss of accuracy is produced during the conversion from the - // PoseBundle to the SimpleCarState. Hence, a small tolerance is allowed when - // comparing the values from the SimpleCarState with the expected values. + // Computations of SimpleCarState from a PoseBundle incur minimal numerical + // precision loss. Hence, a small tolerance is allowed when comparing the + // values from the SimpleCarState with the expected values. const double kAccuracy = 1e-15; EXPECT_EQ(state_message.x(), kX);