diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index c5796c5d7e..1b30ab5a33 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 @@ -326,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; @@ -458,6 +468,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 +680,11 @@ 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 +972,21 @@ bool UserCommandsPrivate::VisualService(const msgs::Visual &_req, return true; } +////////////////////////////////////////////////// +void UserCommandsPrivate::OnCmdMaterialColor(const msgs::MaterialColor &_msg) +{ + auto msg = _msg.New(); + msg->CopyFrom(_msg); + 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; +} + ////////////////////////////////////////////////// bool UserCommandsPrivate::WheelSlipService( const msgs::WheelSlipParametersCmd &_req, @@ -1720,57 +1754,128 @@ 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())); + + // When size > 1, we don't know which entity to modify + if (entities.size() == 1) + { + visualEntity = entities[0]; + } + } - auto entities = - this->iface->ecm->ChildrenByComponents(parentEntity, - components::Name(visualMsg->name())); + if (visualEntity == kNullEntity) + { + gzerr << "Failed to find visual entity" << std::endl; + return false; + } - // When size > 1, we don't know which entity to modify - if (entities.size() == 1) + auto visualCmdComp = + this->iface->ecm->Component(visualEntity); + if (!visualCmdComp) { - visualEntity = entities[0]; + 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); } } - - if (visualEntity == kNullEntity) + else if (materialColorMsg != nullptr) { - gzerr << "Failed to find visual entity" << std::endl; - return false; - } + 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()); + // TODO(anyone) Enable setting shininess + 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(*visualMsg)); + 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; } diff --git a/test/integration/user_commands.cc b/test/integration/user_commands.cc index a6e3f2f340..281c6dbf9c 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,112 @@ 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", "material_color.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; + + // box + auto sphereVisualEntity = + ecm->EntityByComponents(components::Name("sphere_visual")); + ASSERT_NE(kNullEntity, sphereVisualEntity); + + // check box visual's initial values + 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"; + + // 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( + gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_ALL); + 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); + + EXPECT_EQ(math::Color(1.0f, 1.0f, 1.0f, 1.0f), + updatedVisual0->Data().Diffuse()); + EXPECT_EQ(math::Color(1.0f, 1.0f, 1.0f, 1.0f), + updatedVisual1->Data().Diffuse()); +} + ///////////////////////////////////////////////// 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..5458071dbd --- /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 + + + + + +