From d49b820cb62e3767af8d6c203b0b6ef1f559011d Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Sun, 14 Jan 2024 11:54:11 -0500 Subject: [PATCH 01/12] Change an entities visual material color by topic. This change allows for a simpler mapping from MaterialColor msg to the much more cumbersome Visual msg, allong for users to rapidly change the colors of a entities visual material color components. This is especially useful for simulating status lighting. Signed-off-by: Benjamin Perseghetti --- src/systems/user_commands/UserCommands.cc | 59 ++++++++++++++++++++++- 1 file changed, 58 insertions(+), 1 deletion(-) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index c5796c5d7e..935b1a16f5 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -1,5 +1,7 @@ /* * Copyright (C) 2019 Open Source Robotics Foundation + * Copyright (C) 2024 CogniPilot Foundation + * Copyright (C) 2024 Rudis Laboratories LLC * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -33,6 +35,7 @@ #include #include #include +#include #include #include #include @@ -45,6 +48,7 @@ #include #include +#include #include #include @@ -458,6 +462,10 @@ class gz::sim::systems::UserCommandsPrivate /// \param[in] _msg Light message public: void OnCmdLight(const msgs::Light &_msg); + /// \brief Callback for MaterialColor subscription + /// \param[in] _msg MaterialColor message + public: void OnCmdMaterialColor(const msgs::MaterialColor &_msg); + /// \brief Callback for pose service /// \param[in] _req Request containing pose update of an entity. /// \param[out] _res True if message successfully received and queued. @@ -666,6 +674,10 @@ void UserCommands::Configure(const Entity &_entity, this->dataPtr->node.Subscribe(lightTopic, &UserCommandsPrivate::OnCmdLight, this->dataPtr.get()); + std::string materialColorTopic{"/world/" + validWorldName +"/material_color"}; + this->dataPtr->node.Subscribe(materialColorTopic, + &UserCommandsPrivate::OnCmdMaterialColor, this->dataPtr.get()); + // Physics service std::string physicsService{"/world/" + validWorldName + "/set_physics"}; this->dataPtr->node.Advertise(physicsService, @@ -953,6 +965,39 @@ bool UserCommandsPrivate::VisualService(const msgs::Visual &_req, return true; } +////////////////////////////////////////////////// +void UserCommandsPrivate::OnCmdMaterialColor(const msgs::MaterialColor &_msg) +{ + msgs::Visual _req; + auto msg = _req.New(); + msg->set_name(_msg.name()); + msg->set_parent_name(_msg.parent_name()); + msg->mutable_material()->mutable_ambient()->set_r(_msg.ambient().r()); + msg->mutable_material()->mutable_ambient()->set_g(_msg.ambient().g()); + msg->mutable_material()->mutable_ambient()->set_b(_msg.ambient().b()); + msg->mutable_material()->mutable_ambient()->set_a(_msg.ambient().a()); + msg->mutable_material()->mutable_diffuse()->set_r(_msg.diffuse().r()); + msg->mutable_material()->mutable_diffuse()->set_g(_msg.diffuse().g()); + msg->mutable_material()->mutable_diffuse()->set_b(_msg.diffuse().b()); + msg->mutable_material()->mutable_diffuse()->set_a(_msg.diffuse().a()); + msg->mutable_material()->mutable_specular()->set_r(_msg.specular().r()); + msg->mutable_material()->mutable_specular()->set_g(_msg.specular().g()); + msg->mutable_material()->mutable_specular()->set_b(_msg.specular().b()); + msg->mutable_material()->mutable_specular()->set_a(_msg.specular().a()); + msg->mutable_material()->mutable_emissive()->set_r(_msg.emissive().r()); + msg->mutable_material()->mutable_emissive()->set_g(_msg.emissive().g()); + msg->mutable_material()->mutable_emissive()->set_b(_msg.emissive().b()); + msg->mutable_material()->mutable_emissive()->set_a(_msg.emissive().a()); + + auto cmd = std::make_unique(msg, this->iface); + + // Push to pending + { + std::lock_guard lock(this->pendingMutex); + this->pendingCmds.push_back(std::move(cmd)); + } +} + ////////////////////////////////////////////////// bool UserCommandsPrivate::WheelSlipService( const msgs::WheelSlipParametersCmd &_req, @@ -1748,7 +1793,19 @@ bool VisualCommand::Execute() // When size > 1, we don't know which entity to modify if (entities.size() == 1) { - visualEntity = entities[0]; + auto subentities = + this->iface->ecm->ChildrenByComponents(entities[0], + components::Name(visualMsg->name())); + if (subentities.size() == 1) + { + visualEntity = subentities[0]; + gzmsg << "Using visual entity id: " << visualEntity << std::endl; + } + else + { + visualEntity = entities[0]; + gzmsg << "Using visual entity id: " << visualEntity << std::endl; + } } } From 9ffb05c869238bc0257cde5d5a7de4a45fd87a7f Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Mon, 15 Jan 2024 22:19:03 -0500 Subject: [PATCH 02/12] Update src/systems/user_commands/UserCommands.cc MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero Signed-off-by: Benjamin Perseghetti --- src/systems/user_commands/UserCommands.cc | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 935b1a16f5..cca4feb789 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -1799,13 +1799,12 @@ bool VisualCommand::Execute() if (subentities.size() == 1) { visualEntity = subentities[0]; - gzmsg << "Using visual entity id: " << visualEntity << std::endl; } else { visualEntity = entities[0]; - gzmsg << "Using visual entity id: " << visualEntity << std::endl; } + gzmsg << "Using visual entity id: " << visualEntity << std::endl; } } From 0d153bdc25d2a9bbcffb4fed76aae63376bc867d Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Mon, 15 Jan 2024 23:12:09 -0500 Subject: [PATCH 03/12] Add UserCommands MaterialColor test. Signed-off-by: Benjamin Perseghetti --- test/integration/user_commands.cc | 68 +++++++++++++++++++++++++++++++ test/worlds/material_color.sdf | 59 +++++++++++++++++++++++++++ 2 files changed, 127 insertions(+) create mode 100644 test/worlds/material_color.sdf diff --git a/test/integration/user_commands.cc b/test/integration/user_commands.cc index a6e3f2f340..4b0f456c8f 100644 --- a/test/integration/user_commands.cc +++ b/test/integration/user_commands.cc @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -1046,6 +1047,73 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Light)) spotLightComp->Data().Diffuse()); } +///////////////////////////////////////////////// +TEST_F(UserCommandsTest, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(MaterialColor)) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = gz::common::joinPaths( + std::string(PROJECT_SOURCE_PATH), "test", "worlds", "shapes.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system just to get the ECM + EntityComponentManager *ecm{nullptr}; + test::Relay testSystem; + testSystem.OnPreUpdate([&](const sim::UpdateInfo &, + sim::EntityComponentManager &_ecm) + { + ecm = &_ecm; + }); + + server.AddSystem(testSystem.systemPtr); + + // Run server and check we have the ECM + EXPECT_EQ(nullptr, ecm); + server.Run(true, 1, false); + EXPECT_NE(nullptr, ecm); + + transport::Node node; + + // Camera Ball + auto boxVisualEntity = + ecm->EntityByComponents(components::Name("box_visual")); + ASSERT_NE(kNullEntity, boxVisualEntity); + + // check box visual's initial values + auto boxVisualComp = ecm->Component(boxVisualEntity); + ASSERT_NE(nullptr, boxVisualComp); + EXPECT_EQ(math::Color(1.0f, 0.0f, 0.0f, 1.0f), + boxVisualComp->Data().Diffuse()); + + // Test material_color topic + const std::string materialColorTopic = "/world/material_color/material_color"; + + msgs::MaterialColor materialColorMsg; + materialColorMsg.set_name("box_visual"); + materialColorMsg.set_parent_name("box_link"); + gz::msgs::Set(materialColorMsg.mutable_diffuse(), + gz::math::Color(1.0f, 1.0f, 1.0f, 1.0f)); + + // Publish material color + auto pub = node.Advertise(materialColorTopic); + pub.Publish(materialColorMsg); + + server.Run(true, 100, false); + // Sleep for a small duration to allow Run thread to start + GZ_SLEEP_MS(10); + + auto boxVisCmdComp = ecm->Component(boxVisualEntity); + ASSERT_NE(nullptr, boxVisualComp); + EXPECT_FLOAT_EQ(0.0f, boxVisCmdComp->Data().material().diffuse().r()); + EXPECT_FLOAT_EQ(0.0f, boxVisCmdComp->Data().material().diffuse().g()); + EXPECT_FLOAT_EQ(1.0f, boxVisCmdComp->Data().material().diffuse().b()); + EXPECT_FLOAT_EQ(1.0f, boxVisCmdComp->Data().material().diffuse().a()); +} + ///////////////////////////////////////////////// TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Physics)) { diff --git a/test/worlds/material_color.sdf b/test/worlds/material_color.sdf new file mode 100644 index 0000000000..f9443bdb40 --- /dev/null +++ b/test/worlds/material_color.sdf @@ -0,0 +1,59 @@ + + + + + 0.001 + 0 + + + + + + + ogre2 + + + 0 0.0 0.0 0 0 0 + + + + 1 0 1.3 0 0 0 + + 1.047 + + 320 + 240 + + + 0.1 + 100 + + + 1 + 30 + false + camera + + + + + 0.5 + + + + 0.3 0.3 0.3 1 + 0.3 0.3 0.3 1 + 0.3 0.3 0.3 1 + 0.3 0.3 0.3 1 + + + + + + From 1cba71d6d103b6ab5804e8b675becac391f5ea17 Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Tue, 16 Jan 2024 21:30:29 -0500 Subject: [PATCH 04/12] Use MaterialColor Apply Enum and Entity. Signed-off-by: Benjamin Perseghetti --- src/systems/user_commands/UserCommands.cc | 80 ++++++++++++----------- test/integration/user_commands.cc | 3 +- test/worlds/material_color.sdf | 59 ----------------- 3 files changed, 44 insertions(+), 98 deletions(-) delete mode 100644 test/worlds/material_color.sdf diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index cca4feb789..ec9d6a4a91 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -969,32 +969,49 @@ bool UserCommandsPrivate::VisualService(const msgs::Visual &_req, void UserCommandsPrivate::OnCmdMaterialColor(const msgs::MaterialColor &_msg) { msgs::Visual _req; - auto msg = _req.New(); - msg->set_name(_msg.name()); - msg->set_parent_name(_msg.parent_name()); - msg->mutable_material()->mutable_ambient()->set_r(_msg.ambient().r()); - msg->mutable_material()->mutable_ambient()->set_g(_msg.ambient().g()); - msg->mutable_material()->mutable_ambient()->set_b(_msg.ambient().b()); - msg->mutable_material()->mutable_ambient()->set_a(_msg.ambient().a()); - msg->mutable_material()->mutable_diffuse()->set_r(_msg.diffuse().r()); - msg->mutable_material()->mutable_diffuse()->set_g(_msg.diffuse().g()); - msg->mutable_material()->mutable_diffuse()->set_b(_msg.diffuse().b()); - msg->mutable_material()->mutable_diffuse()->set_a(_msg.diffuse().a()); - msg->mutable_material()->mutable_specular()->set_r(_msg.specular().r()); - msg->mutable_material()->mutable_specular()->set_g(_msg.specular().g()); - msg->mutable_material()->mutable_specular()->set_b(_msg.specular().b()); - msg->mutable_material()->mutable_specular()->set_a(_msg.specular().a()); - msg->mutable_material()->mutable_emissive()->set_r(_msg.emissive().r()); - msg->mutable_material()->mutable_emissive()->set_g(_msg.emissive().g()); - msg->mutable_material()->mutable_emissive()->set_b(_msg.emissive().b()); - msg->mutable_material()->mutable_emissive()->set_a(_msg.emissive().a()); - - auto cmd = std::make_unique(msg, this->iface); - - // Push to pending + int numberOfEntities = 0; + auto entities = entitiesFromScopedName(_msg.entity().name(), + *this->iface->ecm); + if (entities.empty()) { - std::lock_guard lock(this->pendingMutex); - this->pendingCmds.push_back(std::move(cmd)); + gzwarn << "Entity name: " << _msg.entity().name() << ", is not found." + << std::endl; + return; + } + for (const Entity &id: entities) + { + if ((numberOfEntities > 0) && (_msg.apply() != + gz::msgs::MaterialColor::Apply::MaterialColor_Apply_ALL)) + { + break; + } + numberOfEntities++; + auto msg = _req.New(); + msg->set_id(id); + msg->mutable_material()->mutable_ambient()->set_r(_msg.ambient().r()); + msg->mutable_material()->mutable_ambient()->set_g(_msg.ambient().g()); + msg->mutable_material()->mutable_ambient()->set_b(_msg.ambient().b()); + msg->mutable_material()->mutable_ambient()->set_a(_msg.ambient().a()); + msg->mutable_material()->mutable_diffuse()->set_r(_msg.diffuse().r()); + msg->mutable_material()->mutable_diffuse()->set_g(_msg.diffuse().g()); + msg->mutable_material()->mutable_diffuse()->set_b(_msg.diffuse().b()); + msg->mutable_material()->mutable_diffuse()->set_a(_msg.diffuse().a()); + msg->mutable_material()->mutable_specular()->set_r(_msg.specular().r()); + msg->mutable_material()->mutable_specular()->set_g(_msg.specular().g()); + msg->mutable_material()->mutable_specular()->set_b(_msg.specular().b()); + msg->mutable_material()->mutable_specular()->set_a(_msg.specular().a()); + msg->mutable_material()->mutable_emissive()->set_r(_msg.emissive().r()); + msg->mutable_material()->mutable_emissive()->set_g(_msg.emissive().g()); + msg->mutable_material()->mutable_emissive()->set_b(_msg.emissive().b()); + msg->mutable_material()->mutable_emissive()->set_a(_msg.emissive().a()); + + auto cmd = std::make_unique(msg, this->iface); + + // Push to pending + { + std::lock_guard lock(this->pendingMutex); + this->pendingCmds.push_back(std::move(cmd)); + } } } @@ -1793,18 +1810,7 @@ bool VisualCommand::Execute() // When size > 1, we don't know which entity to modify if (entities.size() == 1) { - auto subentities = - this->iface->ecm->ChildrenByComponents(entities[0], - components::Name(visualMsg->name())); - if (subentities.size() == 1) - { - visualEntity = subentities[0]; - } - else - { - visualEntity = entities[0]; - } - gzmsg << "Using visual entity id: " << visualEntity << std::endl; + visualEntity = entities[0]; } } diff --git a/test/integration/user_commands.cc b/test/integration/user_commands.cc index 4b0f456c8f..23e457274d 100644 --- a/test/integration/user_commands.cc +++ b/test/integration/user_commands.cc @@ -1093,8 +1093,7 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(MaterialColor)) const std::string materialColorTopic = "/world/material_color/material_color"; msgs::MaterialColor materialColorMsg; - materialColorMsg.set_name("box_visual"); - materialColorMsg.set_parent_name("box_link"); + materialColorMsg.mutable_entity()->set_name("box::box_link::box_visual"); gz::msgs::Set(materialColorMsg.mutable_diffuse(), gz::math::Color(1.0f, 1.0f, 1.0f, 1.0f)); diff --git a/test/worlds/material_color.sdf b/test/worlds/material_color.sdf deleted file mode 100644 index f9443bdb40..0000000000 --- a/test/worlds/material_color.sdf +++ /dev/null @@ -1,59 +0,0 @@ - - - - - 0.001 - 0 - - - - - - - ogre2 - - - 0 0.0 0.0 0 0 0 - - - - 1 0 1.3 0 0 0 - - 1.047 - - 320 - 240 - - - 0.1 - 100 - - - 1 - 30 - false - camera - - - - - 0.5 - - - - 0.3 0.3 0.3 1 - 0.3 0.3 0.3 1 - 0.3 0.3 0.3 1 - 0.3 0.3 0.3 1 - - - - - - From f8f23f97489804cb6e55d6ba4548ee25c542a1cb Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Wed, 17 Jan 2024 16:28:40 -0500 Subject: [PATCH 05/12] Update MaterialCommand to use EntityMatch. Signed-off-by: Benjamin Perseghetti --- src/systems/user_commands/UserCommands.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index ec9d6a4a91..8b9d6c6ced 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -978,10 +978,10 @@ void UserCommandsPrivate::OnCmdMaterialColor(const msgs::MaterialColor &_msg) << std::endl; return; } - for (const Entity &id: entities) + for (const Entity &id : entities) { - if ((numberOfEntities > 0) && (_msg.apply() != - gz::msgs::MaterialColor::Apply::MaterialColor_Apply_ALL)) + if ((numberOfEntities > 0) && (_msg.entity_match() != + gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_ALL)) { break; } From 0e23d4262bd5f1f86f29b00c11bc25ff91f71a9d Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Fri, 19 Jan 2024 09:24:28 -0500 Subject: [PATCH 06/12] MaterialColor test world and line fix. Signed-off-by: Benjamin Perseghetti --- src/systems/user_commands/UserCommands.cc | 3 +- test/integration/user_commands.cc | 40 ++++++------ test/worlds/material_color.sdf | 76 +++++++++++++++++++++++ 3 files changed, 100 insertions(+), 19 deletions(-) create mode 100644 test/worlds/material_color.sdf diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 8b9d6c6ced..8dd284f7ae 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -674,7 +674,8 @@ void UserCommands::Configure(const Entity &_entity, this->dataPtr->node.Subscribe(lightTopic, &UserCommandsPrivate::OnCmdLight, this->dataPtr.get()); - std::string materialColorTopic{"/world/" + validWorldName +"/material_color"}; + std::string materialColorTopic{ + "/world/" + validWorldName + "/material_color"}; this->dataPtr->node.Subscribe(materialColorTopic, &UserCommandsPrivate::OnCmdMaterialColor, this->dataPtr.get()); diff --git a/test/integration/user_commands.cc b/test/integration/user_commands.cc index 23e457274d..b3e6a8c836 100644 --- a/test/integration/user_commands.cc +++ b/test/integration/user_commands.cc @@ -1053,7 +1053,7 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(MaterialColor)) // Start server ServerConfig serverConfig; const auto sdfFile = gz::common::joinPaths( - std::string(PROJECT_SOURCE_PATH), "test", "worlds", "shapes.sdf"); + std::string(PROJECT_SOURCE_PATH), "test", "worlds", "material_color.sdf"); serverConfig.SetSdfFile(sdfFile); Server server(serverConfig); @@ -1078,39 +1078,43 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(MaterialColor)) transport::Node node; - // Camera Ball - auto boxVisualEntity = - ecm->EntityByComponents(components::Name("box_visual")); - ASSERT_NE(kNullEntity, boxVisualEntity); + // box + auto sphereVisualEntity = + ecm->EntityByComponents(components::Name("sphere_visual")); + ASSERT_NE(kNullEntity, sphereVisualEntity); // check box visual's initial values - auto boxVisualComp = ecm->Component(boxVisualEntity); - ASSERT_NE(nullptr, boxVisualComp); - EXPECT_EQ(math::Color(1.0f, 0.0f, 0.0f, 1.0f), - boxVisualComp->Data().Diffuse()); + auto sphereVisualComp = + ecm->Component(sphereVisualEntity); + ASSERT_NE(nullptr, sphereVisualComp); + EXPECT_EQ(math::Color(0.3f, 0.3f, 0.3f, 1.0f), + sphereVisualComp->Data().Diffuse()); // Test material_color topic - const std::string materialColorTopic = "/world/material_color/material_color"; + const std::string materialColorTopic = + "/world/material_color/material_color"; msgs::MaterialColor materialColorMsg; - materialColorMsg.mutable_entity()->set_name("box::box_link::box_visual"); + materialColorMsg.mutable_entity()->set_name("sphere_visual"); + materialColorMsg.set_entity_match( + gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_ALL); gz::msgs::Set(materialColorMsg.mutable_diffuse(), gz::math::Color(1.0f, 1.0f, 1.0f, 1.0f)); // Publish material color auto pub = node.Advertise(materialColorTopic); pub.Publish(materialColorMsg); + GZ_SLEEP_MS(100); + pub.Publish(materialColorMsg); server.Run(true, 100, false); // Sleep for a small duration to allow Run thread to start - GZ_SLEEP_MS(10); + GZ_SLEEP_MS(100); - auto boxVisCmdComp = ecm->Component(boxVisualEntity); - ASSERT_NE(nullptr, boxVisualComp); - EXPECT_FLOAT_EQ(0.0f, boxVisCmdComp->Data().material().diffuse().r()); - EXPECT_FLOAT_EQ(0.0f, boxVisCmdComp->Data().material().diffuse().g()); - EXPECT_FLOAT_EQ(1.0f, boxVisCmdComp->Data().material().diffuse().b()); - EXPECT_FLOAT_EQ(1.0f, boxVisCmdComp->Data().material().diffuse().a()); + auto updatedVisual = + ecm->Component(sphereVisualEntity); + EXPECT_EQ(math::Color(1.0f, 1.0f, 1.0f, 1.0f), + updatedVisual->Data().Diffuse()); } ///////////////////////////////////////////////// diff --git a/test/worlds/material_color.sdf b/test/worlds/material_color.sdf new file mode 100644 index 0000000000..3a430ae806 --- /dev/null +++ b/test/worlds/material_color.sdf @@ -0,0 +1,76 @@ + + + + + 0.001 + 0 + + + + + + + ogre2 + + + + 0 0.0 0.0 0 0 0 + + + + 1 0 1.3 0 0 0 + + 1.047 + + 320 + 240 + + + 0.1 + 100 + + + 1 + 30 + false + camera + + + + + 0.5 + + + + 0.3 0.3 0.3 1 + 0.3 0.3 0.3 1 + 0.3 0.3 0.3 1 + + + + + + 0.5 1.0 0.0 0 0 0 + + + + + 0.5 + + + + 0.3 0.3 0.3 1 + 0.3 0.3 0.3 1 + 0.3 0.3 0.3 1 + + + + + + From 60cace15ab66a6ce718744f451b82b80373d6b14 Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Sun, 21 Jan 2024 09:54:51 -0500 Subject: [PATCH 07/12] MaterialColor increase codecov. Signed-off-by: Benjamin Perseghetti --- test/integration/user_commands.cc | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/test/integration/user_commands.cc b/test/integration/user_commands.cc index b3e6a8c836..19c1d7e761 100644 --- a/test/integration/user_commands.cc +++ b/test/integration/user_commands.cc @@ -1094,6 +1094,21 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(MaterialColor)) const std::string materialColorTopic = "/world/material_color/material_color"; + // Test first return logic (no direct compare as returns unordered set) + msgs::MaterialColor materialColorMsgFirst; + materialColorMsgFirst.mutable_entity()->set_name("sphere_visual"); + materialColorMsgFirst.set_entity_match( + gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_FIRST); + gz::msgs::Set(materialColorMsgFirst.mutable_diffuse(), + gz::math::Color(0.0f, 0.0f, 0.0f, 1.0f)); + + // Publish material color + auto pub = node.Advertise(materialColorTopic); + pub.Publish(materialColorMsgFirst); + server.Run(true, 100, false); + // Sleep for a small duration to allow Run thread to start + GZ_SLEEP_MS(100); + msgs::MaterialColor materialColorMsg; materialColorMsg.mutable_entity()->set_name("sphere_visual"); materialColorMsg.set_entity_match( @@ -1102,11 +1117,7 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(MaterialColor)) gz::math::Color(1.0f, 1.0f, 1.0f, 1.0f)); // Publish material color - auto pub = node.Advertise(materialColorTopic); pub.Publish(materialColorMsg); - GZ_SLEEP_MS(100); - pub.Publish(materialColorMsg); - server.Run(true, 100, false); // Sleep for a small duration to allow Run thread to start GZ_SLEEP_MS(100); From ed1b31ca378c99e9b4a945696cbf36d58d9b5a2e Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Mon, 22 Jan 2024 16:04:16 -0500 Subject: [PATCH 08/12] Update src/systems/user_commands/UserCommands.cc Co-authored-by: Addisu Z. Taddese Signed-off-by: Benjamin Perseghetti --- src/systems/user_commands/UserCommands.cc | 20 ++++---------------- 1 file changed, 4 insertions(+), 16 deletions(-) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 8dd284f7ae..abeb247576 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -989,22 +989,10 @@ void UserCommandsPrivate::OnCmdMaterialColor(const msgs::MaterialColor &_msg) numberOfEntities++; auto msg = _req.New(); msg->set_id(id); - msg->mutable_material()->mutable_ambient()->set_r(_msg.ambient().r()); - msg->mutable_material()->mutable_ambient()->set_g(_msg.ambient().g()); - msg->mutable_material()->mutable_ambient()->set_b(_msg.ambient().b()); - msg->mutable_material()->mutable_ambient()->set_a(_msg.ambient().a()); - msg->mutable_material()->mutable_diffuse()->set_r(_msg.diffuse().r()); - msg->mutable_material()->mutable_diffuse()->set_g(_msg.diffuse().g()); - msg->mutable_material()->mutable_diffuse()->set_b(_msg.diffuse().b()); - msg->mutable_material()->mutable_diffuse()->set_a(_msg.diffuse().a()); - msg->mutable_material()->mutable_specular()->set_r(_msg.specular().r()); - msg->mutable_material()->mutable_specular()->set_g(_msg.specular().g()); - msg->mutable_material()->mutable_specular()->set_b(_msg.specular().b()); - msg->mutable_material()->mutable_specular()->set_a(_msg.specular().a()); - msg->mutable_material()->mutable_emissive()->set_r(_msg.emissive().r()); - msg->mutable_material()->mutable_emissive()->set_g(_msg.emissive().g()); - msg->mutable_material()->mutable_emissive()->set_b(_msg.emissive().b()); - msg->mutable_material()->mutable_emissive()->set_a(_msg.emissive().a()); + msg->mutable_material()->mutable_ambient()->CopyFrom(_msg.ambient()); + msg->mutable_material()->mutable_diffuse()->CopyFrom(_msg.diffuse()); + msg->mutable_material()->mutable_specular()->CopyFrom(_msg.specular()); + msg->mutable_material()->mutable_emissive()->CopyFrom(_msg.emissive()); auto cmd = std::make_unique(msg, this->iface); From 89a63677ea2774008b27b7dcbd021f34d9356a38 Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Mon, 22 Jan 2024 16:42:49 -0500 Subject: [PATCH 09/12] Update user_commands test check all spheres. Signed-off-by: Benjamin Perseghetti --- test/integration/user_commands.cc | 27 ++++++++++++++++++++++++--- test/worlds/material_color.sdf | 6 +++--- 2 files changed, 27 insertions(+), 6 deletions(-) diff --git a/test/integration/user_commands.cc b/test/integration/user_commands.cc index 19c1d7e761..a298123114 100644 --- a/test/integration/user_commands.cc +++ b/test/integration/user_commands.cc @@ -1116,16 +1116,37 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(MaterialColor)) gz::msgs::Set(materialColorMsg.mutable_diffuse(), gz::math::Color(1.0f, 1.0f, 1.0f, 1.0f)); + Entity sphereEntity0 = + ecm->EntityByComponents(components::Name("sphere_0")); + Entity sphereEntity1 = + ecm->EntityByComponents(components::Name("sphere_1")); + auto sphereLinkEntity0 = + ecm->ChildrenByComponents(sphereEntity0, components::Name("sphere_link_0"))[0]; + auto sphereLinkEntity1 = + ecm->ChildrenByComponents(sphereEntity1, components::Name("sphere_link_1"))[0]; + auto sphereVisualEntity0 = + ecm->ChildrenByComponents(sphereLinkEntity0, components::Name("sphere_visual"))[0]; + auto sphereVisualEntity1 = + ecm->ChildrenByComponents(sphereLinkEntity1, components::Name("sphere_visual"))[0]; + auto updatedVisual0 = + ecm->Component(sphereVisualEntity0); + auto updatedVisual1 = + ecm->Component(sphereVisualEntity1); + EXPECT_TRUE((math::Color(0.0f, 0.0f, 0.0f, 1.0f) == + updatedVisual0->Data().Diffuse()) || + (math::Color(0.0f, 0.0f, 0.0f, 1.0f) == + updatedVisual1->Data().Diffuse())); + // Publish material color pub.Publish(materialColorMsg); server.Run(true, 100, false); // Sleep for a small duration to allow Run thread to start GZ_SLEEP_MS(100); - auto updatedVisual = - ecm->Component(sphereVisualEntity); EXPECT_EQ(math::Color(1.0f, 1.0f, 1.0f, 1.0f), - updatedVisual->Data().Diffuse()); + updatedVisual0->Data().Diffuse()); + EXPECT_EQ(math::Color(1.0f, 1.0f, 1.0f, 1.0f), + updatedVisual1->Data().Diffuse()); } ///////////////////////////////////////////////// diff --git a/test/worlds/material_color.sdf b/test/worlds/material_color.sdf index 3a430ae806..5458071dbd 100644 --- a/test/worlds/material_color.sdf +++ b/test/worlds/material_color.sdf @@ -19,9 +19,9 @@ ogre2 - + 0 0.0 0.0 0 0 0 - + 1 0 1.3 0 0 0 @@ -57,7 +57,7 @@ 0.5 1.0 0.0 0 0 0 - + From 15ecb18d85c78aae5e29d3bbd64eb086cf8f886d Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Tue, 23 Jan 2024 20:53:18 -0500 Subject: [PATCH 10/12] Update UserCommands MaterialColor to thread safe method. Co-authored-by: Addisu Z. Taddese Signed-off-by: Benjamin Perseghetti --- src/systems/user_commands/UserCommands.cc | 180 ++++++++++++++-------- 1 file changed, 115 insertions(+), 65 deletions(-) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index abeb247576..8412e487db 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -330,6 +330,12 @@ class VisualCommand : public UserCommandBase public: VisualCommand(msgs::Visual *_msg, std::shared_ptr &_iface); + /// \brief Constructor + /// \param[in] _msg Message containing the material color parameters. + /// \param[in] _iface Pointer to user commands interface. + public: VisualCommand(msgs::MaterialColor *_msg, + std::shared_ptr &_iface); + // Documentation inherited public: bool Execute() final; @@ -969,39 +975,16 @@ bool UserCommandsPrivate::VisualService(const msgs::Visual &_req, ////////////////////////////////////////////////// void UserCommandsPrivate::OnCmdMaterialColor(const msgs::MaterialColor &_msg) { - msgs::Visual _req; - int numberOfEntities = 0; - auto entities = entitiesFromScopedName(_msg.entity().name(), - *this->iface->ecm); - if (entities.empty()) + auto msg = _msg.New(); + msg->CopyFrom(_msg); + auto cmd = std::make_unique(msg, this->iface); + // Push to pending { - gzwarn << "Entity name: " << _msg.entity().name() << ", is not found." - << std::endl; - return; + std::lock_guard lock(this->pendingMutex); + this->pendingCmds.push_back(std::move(cmd)); } - for (const Entity &id : entities) - { - if ((numberOfEntities > 0) && (_msg.entity_match() != - gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_ALL)) - { - break; - } - numberOfEntities++; - auto msg = _req.New(); - msg->set_id(id); - msg->mutable_material()->mutable_ambient()->CopyFrom(_msg.ambient()); - msg->mutable_material()->mutable_diffuse()->CopyFrom(_msg.diffuse()); - msg->mutable_material()->mutable_specular()->CopyFrom(_msg.specular()); - msg->mutable_material()->mutable_emissive()->CopyFrom(_msg.emissive()); - - auto cmd = std::make_unique(msg, this->iface); - // Push to pending - { - std::lock_guard lock(this->pendingMutex); - this->pendingCmds.push_back(std::move(cmd)); - } - } + return; } ////////////////////////////////////////////////// @@ -1771,57 +1754,124 @@ VisualCommand::VisualCommand(msgs::Visual *_msg, { } +////////////////////////////////////////////////// +VisualCommand::VisualCommand(msgs::MaterialColor *_msg, + std::shared_ptr &_iface) + : UserCommandBase(_msg, _iface) +{ +} + ////////////////////////////////////////////////// bool VisualCommand::Execute() { auto visualMsg = dynamic_cast(this->msg); - if (nullptr == visualMsg) + auto materialColorMsg = dynamic_cast(this->msg); + if (visualMsg != nullptr) { - gzerr << "Internal error, null visual message" << std::endl; - return false; - } + Entity visualEntity = kNullEntity; + if (visualMsg->id() != kNullEntity) + { + visualEntity = visualMsg->id(); + } + else if (!visualMsg->name().empty() && !visualMsg->parent_name().empty()) + { + Entity parentEntity = + this->iface->ecm->EntityByComponents( + components::Name(visualMsg->parent_name())); - Entity visualEntity = kNullEntity; - if (visualMsg->id() != kNullEntity) - { - visualEntity = visualMsg->id(); - } - else if (!visualMsg->name().empty() && !visualMsg->parent_name().empty()) - { - Entity parentEntity = - this->iface->ecm->EntityByComponents( - components::Name(visualMsg->parent_name())); + auto entities = + this->iface->ecm->ChildrenByComponents(parentEntity, + components::Name(visualMsg->name())); - auto entities = - this->iface->ecm->ChildrenByComponents(parentEntity, - components::Name(visualMsg->name())); + // When size > 1, we don't know which entity to modify + if (entities.size() == 1) + { + visualEntity = entities[0]; + } + } - // When size > 1, we don't know which entity to modify - if (entities.size() == 1) + if (visualEntity == kNullEntity) { - visualEntity = entities[0]; + gzerr << "Failed to find visual entity" << std::endl; + return false; } - } - if (visualEntity == kNullEntity) - { - gzerr << "Failed to find visual entity" << std::endl; - return false; + auto visualCmdComp = + this->iface->ecm->Component(visualEntity); + if (!visualCmdComp) + { + this->iface->ecm->CreateComponent( + visualEntity, components::VisualCmd(*visualMsg)); + } + else + { + auto state = visualCmdComp->SetData(*visualMsg, this->visualEql) ? + ComponentState::OneTimeChange : ComponentState::NoChange; + this->iface->ecm->SetChanged( + visualEntity, components::VisualCmd::typeId, state); + } } - - auto visualCmdComp = - this->iface->ecm->Component(visualEntity); - if (!visualCmdComp) + else if (materialColorMsg != nullptr) { - this->iface->ecm->CreateComponent( - visualEntity, components::VisualCmd(*visualMsg)); + + Entity visualEntity = kNullEntity; + int numberOfEntities = 0; + auto entities = entitiesFromScopedName(materialColorMsg->entity().name(), + *this->iface->ecm); + if (entities.empty()) + { + gzwarn << "Entity name: " << materialColorMsg->entity().name() << ", is not found." + << std::endl; + return false; + } + for (const Entity &id : entities) + { + if ((numberOfEntities > 0) && (materialColorMsg->entity_match() != + gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_ALL)) + { + return true; + } + numberOfEntities++; + msgs::Visual visualMCMsg; + visualMCMsg.set_id(id); + visualMCMsg.mutable_material()->mutable_ambient()->CopyFrom(materialColorMsg->ambient()); + visualMCMsg.mutable_material()->mutable_diffuse()->CopyFrom(materialColorMsg->diffuse()); + visualMCMsg.mutable_material()->mutable_specular()->CopyFrom(materialColorMsg->specular()); + visualMCMsg.mutable_material()->mutable_emissive()->CopyFrom(materialColorMsg->emissive()); + + visualEntity = kNullEntity; + if (visualMCMsg.id() != kNullEntity) + { + visualEntity = visualMCMsg.id(); + } + if (visualEntity == kNullEntity) + { + gzerr << "Failed to find visual entity" << std::endl; + return false; + } + + auto visualCmdComp = + this->iface->ecm->Component(visualEntity); + if (!visualCmdComp) + { + this->iface->ecm->CreateComponent( + visualEntity, components::VisualCmd(visualMCMsg)); + } + else + { + auto state = visualCmdComp->SetData(visualMCMsg, this->visualEql) ? + ComponentState::OneTimeChange : ComponentState::NoChange; + this->iface->ecm->SetChanged( + visualEntity, components::VisualCmd::typeId, state); + } + } } else { - auto state = visualCmdComp->SetData(*visualMsg, this->visualEql) ? - ComponentState::OneTimeChange : ComponentState::NoChange; - this->iface->ecm->SetChanged( - visualEntity, components::VisualCmd::typeId, state); + gzerr << + "VisualCommand _msg does not match MaterialColor or Visual msg type." + << std::endl; + return false; } return true; } From b5e08dc8560966110274b77640319e5f84b81701 Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Thu, 25 Jan 2024 09:17:58 -0500 Subject: [PATCH 11/12] UserCommands fix whitespace. Signed-off-by: Benjamin Perseghetti --- src/systems/user_commands/UserCommands.cc | 16 ++++++++++------ test/integration/user_commands.cc | 12 ++++++++---- 2 files changed, 18 insertions(+), 10 deletions(-) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 8412e487db..abceaaf6fe 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -1813,14 +1813,14 @@ bool VisualCommand::Execute() } else if (materialColorMsg != nullptr) { - Entity visualEntity = kNullEntity; int numberOfEntities = 0; auto entities = entitiesFromScopedName(materialColorMsg->entity().name(), *this->iface->ecm); if (entities.empty()) { - gzwarn << "Entity name: " << materialColorMsg->entity().name() << ", is not found." + gzwarn << "Entity name: " << materialColorMsg->entity().name() + << ", is not found." << std::endl; return false; } @@ -1834,10 +1834,14 @@ bool VisualCommand::Execute() numberOfEntities++; msgs::Visual visualMCMsg; visualMCMsg.set_id(id); - visualMCMsg.mutable_material()->mutable_ambient()->CopyFrom(materialColorMsg->ambient()); - visualMCMsg.mutable_material()->mutable_diffuse()->CopyFrom(materialColorMsg->diffuse()); - visualMCMsg.mutable_material()->mutable_specular()->CopyFrom(materialColorMsg->specular()); - visualMCMsg.mutable_material()->mutable_emissive()->CopyFrom(materialColorMsg->emissive()); + visualMCMsg.mutable_material()->mutable_ambient()->CopyFrom( + materialColorMsg->ambient()); + visualMCMsg.mutable_material()->mutable_diffuse()->CopyFrom( + materialColorMsg->diffuse()); + visualMCMsg.mutable_material()->mutable_specular()->CopyFrom( + materialColorMsg->specular()); + visualMCMsg.mutable_material()->mutable_emissive()->CopyFrom( + materialColorMsg->emissive()); visualEntity = kNullEntity; if (visualMCMsg.id() != kNullEntity) diff --git a/test/integration/user_commands.cc b/test/integration/user_commands.cc index a298123114..281c6dbf9c 100644 --- a/test/integration/user_commands.cc +++ b/test/integration/user_commands.cc @@ -1121,13 +1121,17 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(MaterialColor)) Entity sphereEntity1 = ecm->EntityByComponents(components::Name("sphere_1")); auto sphereLinkEntity0 = - ecm->ChildrenByComponents(sphereEntity0, components::Name("sphere_link_0"))[0]; + ecm->ChildrenByComponents(sphereEntity0, + components::Name("sphere_link_0"))[0]; auto sphereLinkEntity1 = - ecm->ChildrenByComponents(sphereEntity1, components::Name("sphere_link_1"))[0]; + ecm->ChildrenByComponents(sphereEntity1, + components::Name("sphere_link_1"))[0]; auto sphereVisualEntity0 = - ecm->ChildrenByComponents(sphereLinkEntity0, components::Name("sphere_visual"))[0]; + ecm->ChildrenByComponents(sphereLinkEntity0, + components::Name("sphere_visual"))[0]; auto sphereVisualEntity1 = - ecm->ChildrenByComponents(sphereLinkEntity1, components::Name("sphere_visual"))[0]; + ecm->ChildrenByComponents(sphereLinkEntity1, + components::Name("sphere_visual"))[0]; auto updatedVisual0 = ecm->Component(sphereVisualEntity0); auto updatedVisual1 = From 22f1c02299eacdf8226524c31965320b419e850a Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Thu, 25 Jan 2024 09:44:57 -0500 Subject: [PATCH 12/12] Add shininess TODO. Signed-off-by: Benjamin Perseghetti --- src/systems/user_commands/UserCommands.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index abceaaf6fe..1b30ab5a33 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -1842,7 +1842,7 @@ bool VisualCommand::Execute() materialColorMsg->specular()); visualMCMsg.mutable_material()->mutable_emissive()->CopyFrom( materialColorMsg->emissive()); - + // TODO(anyone) Enable setting shininess visualEntity = kNullEntity; if (visualMCMsg.id() != kNullEntity) {