Added a PoseBundle to SimpleCarState_V system. #497

apojomovsky · 2018-07-05T15:45:22Z

Fixes #485

By following the second approach proposed in this comment, this PR implements a PoseBundleToSimpleCarState_V system that, as its name suggests, feeds from the PoseBundle message published by the PoseAggregator (which includes the poses of all the agents in the simulation) and generates an array of SimpleCarState.

Solved issues:

The Topic Viewer wasn't displaying the non-ignition messages' payload. Solved in delphyne-gui's #136

Current issues:

The Topic Viewer cannot display the content of repeated (vector) elements from an ignition message. This can be appreciated in the /visualizer/scene_update topic, as well as in the just added /agents/state topic, both shown in the figure below:

Open to discussion:

In case we decide to go ahead with this PR, we'd need to fix the issue with the Topic Viewer.
As an option, we could go with the first approach proposed here, although it have its own difficulties that might not worth it.
Otherwise, I'm completely open to new proposals/options.

stonier · 2018-07-06T00:58:41Z

In case we decide to go ahead with this PR, we'd need to fix the issue with the Topic Viewer.

Whether we go ahead with this PR, we might want to fix that. Someone using a vector ... will be inevitable. I can imagine that there are some difficulties around time-varying lengths when it comes to displaying them though - let alone handling dragging the topics to a plotter. Ask in an issue to the appropriate ignition forum. A side question - do they handle this for plotting or viewing ROS topics?

apojomovsky · 2018-07-06T14:20:26Z

Sure @stonier , I think this problem deserves its own issue, so I just created it here, and there I'll be answering the questions raised here as I get them.

hidmic

@apojomovsky First pass, I left a couple comments.

hidmic · 2018-07-10T13:41:19Z

src/backend/automotive_simulator.cc

-const delphyne::AgentBase<T>&
-AutomotiveSimulator<T>::GetAgentById(int agent_id) const {
+const delphyne::AgentBase<T>& AutomotiveSimulator<T>::GetAgentById(
+    int agent_id) const {


@apojomovsky why the change?

Hmm, seems like clang-format has been doing his thing here, which is weird since this code should have been already compliant. Just returned to its original state.

Hmm, seems like clang-format has been doing his thing here, which is weird since this code should have been already compliant. Just returned to its original state.

Yeah, it seems to keep changing its mind; very often when I run it, it comes up with new differences. Not quite sure what is going on with it.

hidmic · 2018-07-10T13:46:56Z

src/translations/pose_bundle_to_simple_car_state_v.cc

+    const drake::Isometry3<double>& pose = pose_bundle.get_pose(i);
+    // Translates pose from quaternion to euler.
+    const Eigen::Vector3d& euler_rotation =
+        pose.rotation().eulerAngles(0, 1, 2);


@apojomovsky Hmm, eulerAngles() seems to be returning by-value. I wouldn't tie that temporary to a reference. Consider removing that extra &.

Done, thanks!

hidmic · 2018-07-10T13:48:50Z

src/translations/pose_bundle_to_simple_car_state_v.cc

+    const drake::multibody::SpatialVelocity<double>& spatial_velocity =
+        velocity.get_velocity();
+    const double& velocity_norm =
+        static_cast<double>(spatial_velocity.translational().norm());


@apojomovsky Same as above for the last two statements.

Done, thanks!

hidmic · 2018-07-10T13:49:50Z

src/translations/pose_bundle_to_simple_car_state_v.cc

+        static_cast<double>(spatial_velocity.translational().norm());
+
+    // Appends a new state to the vector.
+    auto* current_state = output->add_states();


@apojomovsky Does auto* infer SimpleCarState*? If so, it'd be great if you could be explicit about the type.

Done, thanks!

hidmic · 2018-07-10T13:50:56Z

src/translations/pose_bundle_to_simple_car_state_v.h

+
+/// @brief A system that takes a PoseBundle and generates an array of
+/// SimpleCarStates (SimpleCarState_V).
+class PoseBundleToSimpleCarState_V : public drake::systems::LeafSystem<double> {


@apojomovsky meta: why can't PoseBundleToSimpleCarState_V be a DrakeToIgn subclass?

Hmm you're right! Just did that. Thanks!

hidmic · 2018-07-10T13:53:34Z

test/regression/cpp/automotive_simulator_test.cc

-      ign_monitor.get_last_message();
+      ign_monitor.get_last_message().states(0);
+
+  // A very minimal loss of accuracy is produced during the conversion from the


@apojomovsky consider Computations of SimpleCarState from a PoseBundle incur minimal numerical precision loss. Hence....

Done, thanks for the suggestion! Sounds a lot better!

hidmic · 2018-07-10T13:54:31Z

test/regression/cpp/automotive_simulator_test.cc


  // Shortly after starting, we should have not have moved much.
  const int kStateMessagesCount{1};
  EXPECT_TRUE(ign_monitor.do_until(
      kStateMessagesCount, kTimeoutMs,
      [this, &simulator]() { simulator->StepBy(kSmallTimeStep); }));

-  ignition::msgs::SimpleCarState state_message = ign_monitor.get_last_message();
+  ignition::msgs::SimpleCarState state_message =
+      ign_monitor.get_last_message().states(0);


@apojomovsky should we be checking that there's at least one state in the array?

Done, thanks!

apojomovsky · 2018-07-12T19:25:59Z

Thanks for the review @hidmic , I just finished addressing your first round of comments. PTAL.

apojomovsky added the status: do not merge label Jul 5, 2018

apojomovsky requested review from clalancette, basicNew and hidmic July 5, 2018 15:45

apojomovsky force-pushed the apojomovsky/pose_bundle_to_simple_car_state branch from afccc2e to 5bf6afe Compare July 5, 2018 17:39

hidmic mentioned this pull request Jul 6, 2018

Add a SimpleCarStatePublisher system that feeds from the PoseAggregator #485

Closed

hidmic suggested changes Jul 10, 2018

View reviewed changes

Alexis Pojomovsky added 2 commits July 10, 2018 11:01

Added a PoseBundle to SimpleCarState_V system.

b953292

Fixed failing tests

2fa50f9

apojomovsky added status: do not review status: do not merge and removed status: do not merge status: do not review labels Jul 11, 2018

apojomovsky force-pushed the apojomovsky/pose_bundle_to_simple_car_state branch from 5bf6afe to 63a4029 Compare July 12, 2018 19:14

Addressed comments from Michel

1b4f971

apojomovsky force-pushed the apojomovsky/pose_bundle_to_simple_car_state branch from 63a4029 to 1b4f971 Compare July 12, 2018 19:20

hidmic approved these changes Jul 13, 2018

View reviewed changes

apojomovsky removed the status: do not merge label Jul 13, 2018

apojomovsky merged commit a296593 into master Jul 13, 2018

apojomovsky deleted the apojomovsky/pose_bundle_to_simple_car_state branch July 13, 2018 16:59

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Added a PoseBundle to SimpleCarState_V system. #497

Added a PoseBundle to SimpleCarState_V system. #497

apojomovsky commented Jul 5, 2018

stonier commented Jul 6, 2018 •

edited

Loading

apojomovsky commented Jul 6, 2018 •

edited

Loading

hidmic left a comment

hidmic Jul 10, 2018

apojomovsky Jul 12, 2018 •

edited

Loading

clalancette Jul 12, 2018

hidmic Jul 10, 2018

apojomovsky Jul 12, 2018

hidmic Jul 10, 2018

apojomovsky Jul 12, 2018

hidmic Jul 10, 2018

apojomovsky Jul 12, 2018

hidmic Jul 10, 2018

apojomovsky Jul 12, 2018

hidmic Jul 10, 2018

apojomovsky Jul 12, 2018

hidmic Jul 10, 2018

apojomovsky Jul 12, 2018

apojomovsky commented Jul 12, 2018

Added a PoseBundle to SimpleCarState_V system. #497

Added a PoseBundle to SimpleCarState_V system. #497

Conversation

apojomovsky commented Jul 5, 2018

stonier commented Jul 6, 2018 • edited Loading

apojomovsky commented Jul 6, 2018 • edited Loading

hidmic left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

apojomovsky Jul 12, 2018 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

apojomovsky commented Jul 12, 2018

stonier commented Jul 6, 2018 •

edited

Loading

apojomovsky commented Jul 6, 2018 •

edited

Loading

apojomovsky Jul 12, 2018 •

edited

Loading