From f422c02b25fb3049a18061e9524a1abfd5370a75 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 22 Mar 2021 12:33:10 -0700 Subject: [PATCH] Scenebroadcaster sensors (#698) * Add sensors to scene broadcaster Signed-off-by: Nate Koenig * Update src/systems/scene_broadcaster/SceneBroadcaster.cc Co-authored-by: Michael Carroll * Fix codecheck Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig Co-authored-by: Michael Carroll --- .../scene_broadcaster/SceneBroadcaster.cc | 51 +++++++++++ test/integration/scene_broadcaster_system.cc | 51 +++++++++++ test/worlds/altimeter_with_pose.sdf | 91 +++++++++++++++++++ 3 files changed, 193 insertions(+) create mode 100644 test/worlds/altimeter_with_pose.sdf diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index ac93da86b6..e0d7ac0b79 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -38,6 +38,7 @@ #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/Static.hh" #include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/components/World.hh" @@ -124,6 +125,15 @@ class ignition::gazebo::systems::SceneBroadcasterPrivate public: static void AddVisuals(msgs::Link *_msg, const Entity _entity, const SceneGraphType &_graph); + /// \brief Adds sensors to a msgs::Link object based on the contents of + /// the scene graph + /// \param[inout] _msg Pointer to msg object to which the sensors will be + /// added. + /// \param[in] _entity Parent entity in the graph + /// \param[in] _graph Scene graph + public: static void AddSensors(msgs::Link *_msg, const Entity _entity, + const SceneGraphType &_graph); + /// \brief Recursively remove entities from the graph /// \param[in] _entity Entity /// \param[in/out] _graph Scene graph @@ -734,6 +744,26 @@ void SceneBroadcasterPrivate::SceneGraphAddEntities( return true; }); + // Sensors + _manager.EachNew( + [&](const Entity &_entity, const components::Sensor *, + const components::Name *_nameComp, + const components::ParentEntity *_parentComp, + const components::Pose *_poseComp) -> bool + { + auto sensorMsg = std::make_shared(); + sensorMsg->set_id(_entity); + sensorMsg->set_parent_id(_parentComp->Data()); + sensorMsg->set_name(_nameComp->Data()); + sensorMsg->mutable_pose()->CopyFrom(msgs::Convert(_poseComp->Data())); + + // Add to graph + newGraph.AddVertex(_nameComp->Data(), sensorMsg, _entity); + newGraph.AddEdge({_parentComp->Data(), _entity}, true); + newEntity = true; + return true; + }); // Update the whole scene graph from the new graph { @@ -878,6 +908,24 @@ void SceneBroadcasterPrivate::AddVisuals(msgs::Link *_msg, const Entity _entity, } } +////////////////////////////////////////////////// +void SceneBroadcasterPrivate::AddSensors(msgs::Link *_msg, const Entity _entity, + const SceneGraphType &_graph) +{ + if (!_msg) + return; + + for (const auto &vertex : _graph.AdjacentsFrom(_entity)) + { + auto sensorMsg = std::dynamic_pointer_cast( + vertex.second.get().Data()); + if (!sensorMsg) + continue; + + _msg->add_sensor()->CopyFrom(*sensorMsg); + } +} + ////////////////////////////////////////////////// void SceneBroadcasterPrivate::AddLinks(msgs::Model *_msg, const Entity _entity, const SceneGraphType &_graph) @@ -900,6 +948,9 @@ void SceneBroadcasterPrivate::AddLinks(msgs::Model *_msg, const Entity _entity, // Lights AddLights(msgOut, vertex.second.get().Id(), _graph); + + // Sensors + AddSensors(msgOut, vertex.second.get().Id(), _graph); } } diff --git a/test/integration/scene_broadcaster_system.cc b/test/integration/scene_broadcaster_system.cc index ad564bf5ad..1e46f8b645 100644 --- a/test/integration/scene_broadcaster_system.cc +++ b/test/integration/scene_broadcaster_system.cc @@ -220,6 +220,57 @@ TEST_P(SceneBroadcasterTest, SceneTopic) EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(msg, scene)); } +///////////////////////////////////////////////// +/// Test whether the scene topic is published only when new entities are added +TEST_P(SceneBroadcasterTest, SceneTopicSensors) +{ + // Start server + ignition::gazebo::ServerConfig serverConfig; + serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/altimeter_with_pose.sdf"); + + gazebo::Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + EXPECT_EQ(12u, *server.EntityCount()); + + // Create requester + transport::Node node; + + std::vector sceneMsgs; + std::function collectMsgs = + [&sceneMsgs](const msgs::Scene &_msg) + { + sceneMsgs.push_back(_msg); + }; + + node.Subscribe("/world/altimeter_sensor/scene/info", collectMsgs); + + // Run server + server.Run(true, 10, false); + + // Should only have one scene even though the simulation ran multiple times + ASSERT_EQ(1u, sceneMsgs.size()); + + // Compare this scene with one from a service request + msgs::Scene &scene = sceneMsgs.front(); + + bool result{false}; + unsigned int timeout{5000}; + ignition::msgs::Scene msg; + + EXPECT_TRUE(node.Request("/world/altimeter_sensor/scene/info", + timeout, msg, result)); + EXPECT_TRUE(result); + EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(msg, scene)); + + EXPECT_EQ(1, msg.model(1).link(0).sensor_size()); + EXPECT_EQ("altimeter_sensor", msg.model(1).link(0).sensor(0).name()); + EXPECT_DOUBLE_EQ(0.1, msg.model(1).link(0).sensor(0).pose().position().x()); + EXPECT_DOUBLE_EQ(0.2, msg.model(1).link(0).sensor(0).pose().position().y()); + EXPECT_DOUBLE_EQ(0.3, msg.model(1).link(0).sensor(0).pose().position().z()); +} + ///////////////////////////////////////////////// /// Test whether the scene topic is published only when new entities are added TEST_P(SceneBroadcasterTest, DeletedTopic) diff --git a/test/worlds/altimeter_with_pose.sdf b/test/worlds/altimeter_with_pose.sdf new file mode 100644 index 0000000000..fda866b333 --- /dev/null +++ b/test/worlds/altimeter_with_pose.sdf @@ -0,0 +1,91 @@ + + + + + 0.001 + 1.0 + + + + + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 4 0 3.0 0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + 0.1 0.2 0.3 0 0 0 + 1 + 30 + true + + + + + link + 2 + + + + +