Use ignition Vector3d as intermediate type

Use an ignition::math::Vector3d for position and linear velocity when serializing to string since there is an easy conversion method from ignition::msgs.
maliput · Apr 29, 2021 · 04490de · 04490de
1 parent a24e5de
commit 04490de
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 16 deletions.
diff --git a/delphyne_gui/visualizer/display_plugins/AgentInfoDisplay.qml b/delphyne_gui/visualizer/display_plugins/AgentInfoDisplay.qml
@@ -9,7 +9,7 @@ Rectangle {
   Layout.minimumHeight: 110
   Layout.fillWidth: true
 
-  // Checkbox to toggle agent info text visibility.
+  // Checkbox to toggle axes visibility.
   RowLayout {
     CheckBox {
       id: visibilityCheckbox

diff --git a/delphyne_gui/visualizer/display_plugins/agent_info_display.cc b/delphyne_gui/visualizer/display_plugins/agent_info_display.cc
@@ -138,43 +138,36 @@ std::shared_ptr<AgentInfoText> AgentInfoDisplay::CreateAgentText(const std::stri
 /////////////////////////////////////////////////
 void AgentInfoDisplay::UpdateAgentLabel(const ignition::msgs::AgentState& _agent, const std::string& _agentName,
                                         std::shared_ptr<AgentInfoText> _agentInfoText) {
-  double x = 0.0;
-  double y = 0.0;
-  double z = 0.0;
+  ignition::math::Vector3d pos;
   double roll = 0.0;
   double pitch = 0.0;
   double yaw = 0.0;
-  double vx = 0.0;
-  double vy = 0.0;
-  double vz = 0.0;
+  ignition::math::Vector3d linear_velocity;
 
   if (_agent.has_position()) {
-    x = _agent.position().x();
-    y = _agent.position().y();
-    z = _agent.position().z();
+    pos = ignition::msgs::Convert(_agent.position());
   }
   if (_agent.has_orientation()) {
     roll = _agent.orientation().roll();
     pitch = _agent.orientation().pitch();
     yaw = _agent.orientation().yaw();
   }
   if (_agent.has_linear_velocity()) {
-    vx = _agent.linear_velocity().x();
-    vy = _agent.linear_velocity().y();
-    vz = _agent.linear_velocity().z();
+    linear_velocity = ignition::msgs::Convert(_agent.linear_velocity());
   }
 
   std::stringstream ss;
-  ss << _agentName << ":\n pos:(x:" << std::setprecision(2) << x << ",y:" << y << ",z:" << z << ",yaw:" << yaw << ")"
-     << "\n vel:(x:" << vx << ",y:" << vy << ",z:" << vz << ")";
+  ss << std::setprecision(2);
+  ss << _agentName << ":\n pos:(" << pos << "), yaw:(" << yaw << ")\n vel:(" << linear_velocity << ")";
   _agentInfoText->text->SetTextString(ss.str());
-  _agentInfoText->textVis->SetLocalPose(ignition::math::Pose3d(x, y, z + 2.6, roll, pitch, yaw));
+  _agentInfoText->textVis->SetLocalPose(ignition::math::Pose3d(pos.X(), pos.Y(), pos.Z() + 2.6, roll, pitch, yaw));
 }
 
 /////////////////////////////////////////////////
 void AgentInfoDisplay::ChangeAgentInfoVisibility() {
   const bool newIsVisibleValue = isVisible;
   for (auto& nameToAgentInfoText : mapAgentInfoText) {
+    auto name = nameToAgentInfoText.first;
     auto agentInfoText = nameToAgentInfoText.second;
     if (agentInfoText && agentInfoText->textVis) {
       agentInfoText->textVis->SetVisible(newIsVisibleValue);