ROS2FrameComponentTest.cpp 23 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530
  1. /*
  2. * Copyright (c) Contributors to the Open 3D Engine Project.
  3. * For complete copyright and license terms please see the LICENSE at the root of this distribution.
  4. *
  5. * SPDX-License-Identifier: Apache-2.0 OR MIT
  6. *
  7. */
  8. #include "Frame/NamespaceComputation.h"
  9. #include "Frame/ROS2FrameGameSystemComponent.h"
  10. #include <AzCore/Asset/AssetManagerComponent.h>
  11. #include <AzCore/Component/ComponentApplication.h>
  12. #include <AzCore/Component/ComponentApplicationBus.h>
  13. #include <AzCore/Component/Entity.h>
  14. #include <AzCore/Component/EntityId.h>
  15. #include <AzCore/RTTI/RTTIMacros.h>
  16. #include <AzCore/Serialization/SerializeContext.h>
  17. #include <AzCore/Settings/SettingsRegistry.h>
  18. #include <AzCore/Slice/SliceAssetHandler.h>
  19. #include <AzCore/UserSettings/UserSettingsComponent.h>
  20. #include <AzCore/std/containers/array.h>
  21. #include <AzCore/std/string/string_view.h>
  22. #include <AzQtComponents/Utilities/QtPluginPaths.h>
  23. #include <AzTest/GemTestEnvironment.h>
  24. #include <AzToolsFramework/Entity/EditorEntityContextComponent.h>
  25. #include <AzToolsFramework/ToolsComponents/TransformComponent.h>
  26. #include <AzToolsFramework/UnitTest/AzToolsFrameworkTestHelpers.h>
  27. #include <AzToolsFramework/UnitTest/ToolsTestApplication.h>
  28. #include <Clients/ROS2SystemComponent.h>
  29. #include <QApplication>
  30. #include <ROS2/Frame/ROS2FrameComponent.h>
  31. #include <ROS2/Frame/ROS2FrameComponentBus.h>
  32. #include <ROS2/Frame/ROS2FrameTrackingInterface.h>
  33. #include <ROS2/ROS2Bus.h>
  34. #include <gtest/gtest.h>
  35. #include <memory>
  36. #include <string>
  37. #include <string_view>
  38. namespace UnitTest
  39. {
  40. class ROS2FrameComponentTestEnvironment : public AZ::Test::GemTestEnvironment
  41. {
  42. // AZ::Test::GemTestEnvironment overrides ...
  43. void AddGemsAndComponents() override;
  44. AZ::ComponentApplication* CreateApplicationInstance() override;
  45. void PostSystemEntityActivate() override;
  46. public:
  47. ROS2FrameComponentTestEnvironment() = default;
  48. ~ROS2FrameComponentTestEnvironment() override = default;
  49. };
  50. void ROS2FrameComponentTestEnvironment::AddGemsAndComponents()
  51. {
  52. AddActiveGems(AZStd::to_array<AZStd::string_view>({ "ROS2" }));
  53. AddDynamicModulePaths({});
  54. AddComponentDescriptors(
  55. AZStd::initializer_list<AZ::ComponentDescriptor*>{ ROS2::ROS2FrameComponent::CreateDescriptor(),
  56. ROS2::ROS2SystemComponent::CreateDescriptor(),
  57. ROS2::ROS2FrameGameSystemComponent::CreateDescriptor() });
  58. AddRequiredComponents(
  59. AZStd::to_array<AZ::TypeId const>(
  60. { ROS2::ROS2SystemComponent::TYPEINFO_Uuid(), ROS2::ROS2FrameGameSystemComponent::TYPEINFO_Uuid() }));
  61. }
  62. AZ::ComponentApplication* ROS2FrameComponentTestEnvironment::CreateApplicationInstance()
  63. {
  64. // Using ToolsTestApplication to have AzFramework and AzToolsFramework components.
  65. return aznew UnitTest::ToolsTestApplication("ROS2FrameComponent");
  66. }
  67. void ROS2FrameComponentTestEnvironment::PostSystemEntityActivate()
  68. {
  69. AZ::UserSettingsComponentRequestBus::Broadcast(&AZ::UserSettingsComponentRequests::DisableSaveOnFinalize);
  70. }
  71. class ROS2FrameComponentFixture : public ::testing::Test
  72. {
  73. };
  74. // Simple test, default configuration, no hierarchy, no namespace change
  75. TEST_F(ROS2FrameComponentFixture, TestNamespaceResolvementDefault)
  76. {
  77. AZStd::vector<AZStd::pair<AZStd::string, ROS2::ROS2FrameConfiguration>> configurations;
  78. ROS2::ROS2FrameConfiguration config;
  79. config.m_namespaceConfiguration.m_namespaceStrategy = ROS2::NamespaceConfiguration::NamespaceStrategy::Default;
  80. configurations.emplace_back("root", config);
  81. const auto namespaceName = ROS2::ComputeNamespace(configurations);
  82. EXPECT_STREQ(namespaceName.c_str(), "root");
  83. }
  84. TEST_F(ROS2FrameComponentFixture, TestNamespaceResolvementCustom)
  85. {
  86. AZStd::vector<AZStd::pair<AZStd::string, ROS2::ROS2FrameConfiguration>> configurations;
  87. ROS2::ROS2FrameConfiguration config;
  88. config.m_namespaceConfiguration.m_namespaceStrategy = ROS2::NamespaceConfiguration::NamespaceStrategy::Custom;
  89. config.m_namespaceConfiguration.m_customNamespace = "foo";
  90. configurations.emplace_back("root", config);
  91. const auto namespaceName = ROS2::ComputeNamespace(configurations);
  92. EXPECT_STREQ(namespaceName.c_str(), "foo");
  93. }
  94. TEST_F(ROS2FrameComponentFixture, TestNamespaceResolvementHierachy1)
  95. {
  96. AZStd::vector<AZStd::pair<AZStd::string, ROS2::ROS2FrameConfiguration>> configurations;
  97. ROS2::ROS2FrameConfiguration config;
  98. config.m_namespaceConfiguration.m_namespaceStrategy = ROS2::NamespaceConfiguration::NamespaceStrategy::Default;
  99. configurations.emplace_back("child1", config);
  100. configurations.emplace_back("root", config);
  101. const auto namespaceName = ROS2::ComputeNamespace(configurations);
  102. EXPECT_STREQ(namespaceName.c_str(), "root");
  103. }
  104. TEST_F(ROS2FrameComponentFixture, TestNamespaceResolvementHierachy2)
  105. {
  106. AZStd::vector<AZStd::pair<AZStd::string, ROS2::ROS2FrameConfiguration>> configurations;
  107. ROS2::ROS2FrameConfiguration config;
  108. config.m_namespaceConfiguration.m_namespaceStrategy = ROS2::NamespaceConfiguration::NamespaceStrategy::Custom;
  109. config.m_namespaceConfiguration.m_customNamespace = "foo";
  110. configurations.emplace_back("child1", config);
  111. config.m_namespaceConfiguration.m_namespaceStrategy = ROS2::NamespaceConfiguration::NamespaceStrategy::Default;
  112. configurations.emplace_back("root", config);
  113. const auto namespaceName = ROS2::ComputeNamespace(configurations);
  114. EXPECT_STREQ(namespaceName.c_str(), "root/foo");
  115. }
  116. TEST_F(ROS2FrameComponentFixture, TestNamespaceResolvementHierachy4)
  117. {
  118. AZStd::vector<AZStd::pair<AZStd::string, ROS2::ROS2FrameConfiguration>> configurations;
  119. ROS2::ROS2FrameConfiguration config;
  120. config.m_namespaceConfiguration.m_namespaceStrategy = ROS2::NamespaceConfiguration::NamespaceStrategy::Custom;
  121. config.m_namespaceConfiguration.m_customNamespace = "bar";
  122. configurations.emplace_back("child2", config);
  123. config.m_namespaceConfiguration.m_namespaceStrategy = ROS2::NamespaceConfiguration::NamespaceStrategy::Custom;
  124. config.m_namespaceConfiguration.m_customNamespace = "foo";
  125. configurations.emplace_back("child1", config);
  126. config.m_namespaceConfiguration.m_namespaceStrategy = ROS2::NamespaceConfiguration::NamespaceStrategy::Default;
  127. configurations.emplace_back("root", config);
  128. const auto namespaceName = ROS2::ComputeNamespace(configurations);
  129. EXPECT_STREQ(namespaceName.c_str(), "root/foo/bar");
  130. }
  131. TEST_F(ROS2FrameComponentFixture, SingleFrameDefault)
  132. {
  133. ROS2::ROS2FrameConfiguration config;
  134. AZ::Entity entity;
  135. const std::string entityName = entity.GetName().c_str();
  136. entity.CreateComponent<AzFramework::TransformComponent>();
  137. auto frame = entity.CreateComponent<ROS2::ROS2FrameComponent>(config);
  138. entity.Init();
  139. entity.Activate();
  140. const std::string jointName(frame->GetNamespacedJointName().c_str());
  141. const std::string frameId(frame->GetNamespacedFrameID().c_str());
  142. EXPECT_EQ(entity.GetState(), AZ::Entity::State::Active);
  143. EXPECT_STRCASEEQ(jointName.c_str(), (entityName + "/").c_str());
  144. EXPECT_STRCASEEQ(frameId.c_str(), (entityName + "/sensor_frame").c_str());
  145. }
  146. TEST_F(ROS2FrameComponentFixture, ThreeFramesDefault)
  147. {
  148. ROS2::ROS2FrameConfiguration config;
  149. AZStd::vector<AZStd::unique_ptr<AZ::Entity>> entities;
  150. AZStd::vector<const ROS2::ROS2FrameComponent*> frames;
  151. constexpr int numOfEntities = 3;
  152. for (int i = 0; i < numOfEntities; i++)
  153. {
  154. entities.push_back(AZStd::make_unique<AZ::Entity>());
  155. }
  156. for (int i = 0; i < numOfEntities; i++)
  157. {
  158. entities[i]->CreateComponent<AzFramework::TransformComponent>();
  159. entities[i]->SetName(("entity" + std::to_string(i)).c_str());
  160. entities[i]->Init();
  161. entities[i]->Activate();
  162. if (i != 0)
  163. {
  164. AZ::TransformBus::Event(entities[i]->GetId(), &AZ::TransformBus::Events::SetParent, entities[i - 1]->GetId());
  165. }
  166. entities[i]->Deactivate();
  167. auto frame = entities[i]->CreateComponent<ROS2::ROS2FrameComponent>(config);
  168. frames.push_back(frame);
  169. entities[i]->Activate();
  170. }
  171. for (int i = 0; i < numOfEntities; i++)
  172. {
  173. const std::string jointName(frames[i]->GetNamespacedJointName().c_str());
  174. const std::string frameId(frames[i]->GetNamespacedFrameID().c_str());
  175. EXPECT_EQ(entities[i]->GetState(), AZ::Entity::State::Active);
  176. EXPECT_STRCASEEQ(jointName.c_str(), (entities[0]->GetName() + "/").c_str());
  177. EXPECT_STRCASEEQ(frameId.c_str(), (entities[0]->GetName() + "/sensor_frame").c_str());
  178. }
  179. }
  180. TEST_F(ROS2FrameComponentFixture, UpdateNamespace)
  181. {
  182. ROS2::ROS2FrameConfiguration config;
  183. AZ::Entity entity;
  184. const std::string entityName = entity.GetName().c_str();
  185. entity.CreateComponent<AzFramework::TransformComponent>();
  186. auto frame = entity.CreateComponent<ROS2::ROS2FrameComponent>(config);
  187. entity.Init();
  188. entity.Activate();
  189. auto rosifiedName = entityName;
  190. ASSERT_STREQ(frame->GetNamespace().c_str(), rosifiedName.c_str());
  191. // Note that namespace parameter is only applied using Custom strategy
  192. {
  193. auto configuration = frame->GetConfiguration();
  194. entity.Deactivate();
  195. configuration.m_namespaceConfiguration.m_customNamespace = "MyCustomNamespace";
  196. configuration.m_namespaceConfiguration.m_namespaceStrategy = ROS2::NamespaceConfiguration::NamespaceStrategy::Custom;
  197. frame->SetConfiguration(configuration);
  198. entity.Activate();
  199. EXPECT_STREQ(frame->GetNamespace().c_str(), "MyCustomNamespace");
  200. }
  201. // Empty strategy clears the namespace
  202. {
  203. auto configuration = frame->GetConfiguration();
  204. configuration.m_namespaceConfiguration.m_namespaceStrategy = ROS2::NamespaceConfiguration::NamespaceStrategy::Empty;
  205. entity.Deactivate();
  206. frame->SetConfiguration(configuration);
  207. entity.Activate();
  208. EXPECT_STREQ(frame->GetNamespace().c_str(), "");
  209. }
  210. // From Entity Name strategy uses rosified version of Entity Name
  211. {
  212. auto configuration = frame->GetConfiguration();
  213. configuration.m_namespaceConfiguration.m_namespaceStrategy = ROS2::NamespaceConfiguration::NamespaceStrategy::FromEntityName;
  214. entity.Deactivate();
  215. frame->SetConfiguration(configuration);
  216. entity.Activate();
  217. EXPECT_STREQ(frame->GetNamespace().c_str(), rosifiedName.c_str());
  218. }
  219. // Default strategy is like From Entity Name strategy if the entity is on top of hierarchy ...
  220. {
  221. auto configuration = frame->GetConfiguration();
  222. configuration.m_namespaceConfiguration.m_namespaceStrategy = ROS2::NamespaceConfiguration::NamespaceStrategy::Default;
  223. entity.Deactivate();
  224. frame->SetConfiguration(configuration);
  225. entity.Activate();
  226. EXPECT_STREQ(frame->GetNamespace().c_str(), rosifiedName.c_str());
  227. }
  228. AZ::Entity newRootEntity;
  229. AZStd::string newRootName = "new_root";
  230. newRootEntity.SetName(newRootName);
  231. newRootEntity.CreateComponent<AzFramework::TransformComponent>();
  232. newRootEntity.CreateComponent<ROS2::ROS2FrameComponent>(config);
  233. newRootEntity.Init();
  234. newRootEntity.Activate();
  235. AZ::TransformBus::Event(entity.GetId(), &AZ::TransformBus::Events::SetParent, newRootEntity.GetId());
  236. // If there is a parent Default strategy concatenates parent's namespace with rosified name
  237. {
  238. auto configuration = frame->GetConfiguration();
  239. configuration.m_namespaceConfiguration.m_namespaceStrategy = ROS2::NamespaceConfiguration::NamespaceStrategy::Default;
  240. entity.Deactivate();
  241. frame->SetConfiguration(configuration);
  242. entity.Activate();
  243. EXPECT_STREQ(frame->GetNamespace().c_str(), newRootName.c_str());
  244. }
  245. }
  246. TEST_F(ROS2FrameComponentFixture, ComponentBusInterface)
  247. {
  248. ROS2::ROS2FrameConfiguration config;
  249. config.m_frameName = "test_frame";
  250. config.m_jointName = "test_joint";
  251. AZ::Entity entity;
  252. entity.SetName("test_entity");
  253. entity.CreateComponent<AzFramework::TransformComponent>();
  254. entity.CreateComponent<ROS2::ROS2FrameComponent>(config);
  255. entity.Init();
  256. entity.Activate();
  257. // Test component bus interface
  258. AZStd::string namespace_result;
  259. AZStd::string frameId_result;
  260. AZStd::string jointName_result;
  261. AZStd::string jointNameRaw_result;
  262. AZStd::string frameName_result;
  263. AZStd::string globalFrameName_result;
  264. ROS2::ROS2FrameComponentBus::EventResult(namespace_result, entity.GetId(), &ROS2::ROS2FrameComponentRequests::GetNamespace);
  265. ROS2::ROS2FrameComponentBus::EventResult(frameId_result, entity.GetId(), &ROS2::ROS2FrameComponentRequests::GetNamespacedFrameID);
  266. ROS2::ROS2FrameComponentBus::EventResult(
  267. jointName_result, entity.GetId(), &ROS2::ROS2FrameComponentRequests::GetNamespacedJointName);
  268. ROS2::ROS2FrameComponentBus::EventResult(jointNameRaw_result, entity.GetId(), &ROS2::ROS2FrameComponentRequests::GetJointName);
  269. ROS2::ROS2FrameComponentBus::EventResult(frameName_result, entity.GetId(), &ROS2::ROS2FrameComponentRequests::GetFrameName);
  270. ROS2::ROS2FrameComponentBus::EventResult(
  271. globalFrameName_result, entity.GetId(), &ROS2::ROS2FrameComponentRequests::GetGlobalFrameID);
  272. const AZStd::string globalNamespace{ "odom" };
  273. if (auto* registry = AZ::SettingsRegistry::Get())
  274. {
  275. registry->Set("/O3DE/ROS2/GlobalFrameName", globalNamespace);
  276. }
  277. EXPECT_EQ(namespace_result, "test_entity");
  278. EXPECT_EQ(frameId_result, "test_entity/test_frame");
  279. EXPECT_EQ(jointName_result, "test_entity/test_joint");
  280. EXPECT_EQ(jointNameRaw_result, "test_joint");
  281. EXPECT_EQ(frameName_result, "test_frame");
  282. EXPECT_EQ(globalFrameName_result, "test_entity/" + globalNamespace); // Uses default global frame name
  283. }
  284. TEST_F(ROS2FrameComponentFixture, FrameTrackingInterfaceBasic)
  285. {
  286. auto trackingInterface = ROS2::ROS2FrameTrackingInterface::Get();
  287. ASSERT_NE(trackingInterface, nullptr);
  288. // Initially no frames should be registered
  289. EXPECT_EQ(trackingInterface->GetRegisteredFrameCount(), 0);
  290. EXPECT_TRUE(trackingInterface->GetRegisteredFrameEntityIds().empty());
  291. EXPECT_TRUE(trackingInterface->GetAllNamespacedFrameIds().empty());
  292. // Create and activate a frame component
  293. ROS2::ROS2FrameConfiguration config;
  294. config.m_frameName = "tracked_frame";
  295. AZ::Entity entity;
  296. entity.SetName("tracked_entity");
  297. entity.CreateComponent<AzFramework::TransformComponent>();
  298. entity.CreateComponent<ROS2::ROS2FrameComponent>(config);
  299. entity.Init();
  300. entity.Activate();
  301. // Frame should now be registered
  302. EXPECT_EQ(trackingInterface->GetRegisteredFrameCount(), 1);
  303. EXPECT_TRUE(trackingInterface->IsFrameRegistered(entity.GetId()));
  304. EXPECT_FALSE(trackingInterface->GetRegisteredFrameEntityIds().empty());
  305. EXPECT_EQ(trackingInterface->GetRegisteredFrameEntityIds().size(), 1);
  306. EXPECT_TRUE(trackingInterface->GetRegisteredFrameEntityIds().contains(entity.GetId()));
  307. // Check namespaced frame ID lookup
  308. auto namespacedFrameId = trackingInterface->GetNamespacedFrameId(entity.GetId());
  309. ASSERT_FALSE(namespacedFrameId.empty());
  310. EXPECT_STREQ(namespacedFrameId.c_str(), "tracked_entity/tracked_frame");
  311. auto allFrameIds = trackingInterface->GetAllNamespacedFrameIds();
  312. EXPECT_EQ(allFrameIds.size(), 1);
  313. EXPECT_TRUE(allFrameIds.contains("tracked_entity/tracked_frame"));
  314. // Check reverse lookup
  315. auto foundEntityId = trackingInterface->GetFrameEntityByNamespacedId("tracked_entity/tracked_frame");
  316. ASSERT_TRUE(foundEntityId.IsValid());
  317. EXPECT_EQ(foundEntityId, entity.GetId());
  318. // Deactivate entity and check frame is unregistered
  319. entity.Deactivate();
  320. EXPECT_EQ(trackingInterface->GetRegisteredFrameCount(), 0);
  321. EXPECT_FALSE(trackingInterface->IsFrameRegistered(entity.GetId()));
  322. EXPECT_TRUE(trackingInterface->GetRegisteredFrameEntityIds().empty());
  323. EXPECT_TRUE(trackingInterface->GetAllNamespacedFrameIds().empty());
  324. }
  325. TEST_F(ROS2FrameComponentFixture, FrameTrackingInterfaceMultipleFrames)
  326. {
  327. auto trackingInterface = ROS2::ROS2FrameTrackingInterface::Get();
  328. ASSERT_NE(trackingInterface, nullptr);
  329. // Create multiple frame entities
  330. constexpr int numFrames = 3;
  331. AZStd::vector<AZStd::unique_ptr<AZ::Entity>> entities;
  332. AZStd::vector<AZStd::string> expectedFrameIds;
  333. for (int i = 0; i < numFrames; ++i)
  334. {
  335. ROS2::ROS2FrameConfiguration config;
  336. config.m_frameName = AZStd::string::format("frame_%d", i);
  337. auto entity = AZStd::make_unique<AZ::Entity>();
  338. entity->SetName(AZStd::string::format("entity_%d", i));
  339. entity->CreateComponent<AzFramework::TransformComponent>();
  340. entity->CreateComponent<ROS2::ROS2FrameComponent>(config);
  341. expectedFrameIds.push_back(AZStd::string::format("entity_%d/frame_%d", i, i));
  342. entity->Init();
  343. entity->Activate();
  344. entities.push_back(AZStd::move(entity));
  345. }
  346. // Check all frames are registered
  347. EXPECT_EQ(trackingInterface->GetRegisteredFrameCount(), numFrames);
  348. EXPECT_EQ(trackingInterface->GetRegisteredFrameEntityIds().size(), numFrames);
  349. EXPECT_EQ(trackingInterface->GetAllNamespacedFrameIds().size(), numFrames);
  350. // Check each frame individually
  351. for (int i = 0; i < numFrames; ++i)
  352. {
  353. EXPECT_TRUE(trackingInterface->IsFrameRegistered(entities[i]->GetId()));
  354. auto namespacedFrameId = trackingInterface->GetNamespacedFrameId(entities[i]->GetId());
  355. ASSERT_FALSE(namespacedFrameId.empty());
  356. EXPECT_EQ(namespacedFrameId, expectedFrameIds[i]);
  357. auto foundEntityId = trackingInterface->GetFrameEntityByNamespacedId(expectedFrameIds[i]);
  358. ASSERT_TRUE(foundEntityId.IsValid());
  359. EXPECT_EQ(foundEntityId, entities[i]->GetId());
  360. }
  361. // Deactivate one frame and check tracking updates
  362. entities[1]->Deactivate();
  363. EXPECT_EQ(trackingInterface->GetRegisteredFrameCount(), numFrames - 1);
  364. EXPECT_FALSE(trackingInterface->IsFrameRegistered(entities[1]->GetId()));
  365. EXPECT_TRUE(trackingInterface->IsFrameRegistered(entities[0]->GetId()));
  366. EXPECT_TRUE(trackingInterface->IsFrameRegistered(entities[2]->GetId()));
  367. // Deactivate remaining frames
  368. entities[0]->Deactivate();
  369. entities[2]->Deactivate();
  370. EXPECT_EQ(trackingInterface->GetRegisteredFrameCount(), 0);
  371. EXPECT_TRUE(trackingInterface->GetRegisteredFrameEntityIds().empty());
  372. }
  373. TEST_F(ROS2FrameComponentFixture, FrameTrackingInterfaceEdgeCases)
  374. {
  375. auto trackingInterface = ROS2::ROS2FrameTrackingInterface::Get();
  376. ASSERT_NE(trackingInterface, nullptr);
  377. // Test with invalid entity ID
  378. AZ::EntityId invalidEntityId;
  379. EXPECT_FALSE(trackingInterface->IsFrameRegistered(invalidEntityId));
  380. auto invalidNamespacedFrameId = trackingInterface->GetNamespacedFrameId(invalidEntityId);
  381. EXPECT_TRUE(invalidNamespacedFrameId.empty());
  382. // Test lookup with non-existent namespaced frame ID
  383. auto invalidEntityLookup = trackingInterface->GetFrameEntityByNamespacedId("non_existent/frame");
  384. EXPECT_FALSE(invalidEntityLookup.IsValid());
  385. // Test with entity that doesn't have a frame component
  386. AZ::Entity entityWithoutFrame;
  387. entityWithoutFrame.CreateComponent<AzFramework::TransformComponent>();
  388. entityWithoutFrame.Init();
  389. entityWithoutFrame.Activate();
  390. EXPECT_FALSE(trackingInterface->IsFrameRegistered(entityWithoutFrame.GetId()));
  391. auto noFrameNamespacedId = trackingInterface->GetNamespacedFrameId(entityWithoutFrame.GetId());
  392. EXPECT_TRUE(noFrameNamespacedId.empty());
  393. }
  394. TEST_F(ROS2FrameComponentFixture, FrameTrackingInterfaceNamespaceChanges)
  395. {
  396. auto trackingInterface = ROS2::ROS2FrameTrackingInterface::Get();
  397. ASSERT_NE(trackingInterface, nullptr);
  398. ROS2::ROS2FrameConfiguration config;
  399. config.m_frameName = "dynamic_frame";
  400. config.m_namespaceConfiguration.m_namespaceStrategy = ROS2::NamespaceConfiguration::NamespaceStrategy::Custom;
  401. config.m_namespaceConfiguration.m_customNamespace = "initial_namespace";
  402. AZ::Entity entity;
  403. entity.SetName("dynamic_entity");
  404. entity.CreateComponent<AzFramework::TransformComponent>();
  405. auto frame = entity.CreateComponent<ROS2::ROS2FrameComponent>(config);
  406. entity.Init();
  407. entity.Activate();
  408. // Check initial registration
  409. EXPECT_TRUE(trackingInterface->IsFrameRegistered(entity.GetId()));
  410. auto initialFrameId = trackingInterface->GetNamespacedFrameId(entity.GetId());
  411. ASSERT_FALSE(initialFrameId.empty());
  412. EXPECT_EQ(initialFrameId, "initial_namespace/dynamic_frame");
  413. // Change namespace configuration
  414. auto newConfig = frame->GetConfiguration();
  415. newConfig.m_namespaceConfiguration.m_customNamespace = "updated_namespace";
  416. entity.Deactivate();
  417. frame->SetConfiguration(newConfig);
  418. entity.Activate();
  419. // Check updated registration
  420. EXPECT_TRUE(trackingInterface->IsFrameRegistered(entity.GetId()));
  421. auto updatedFrameId = trackingInterface->GetNamespacedFrameId(entity.GetId());
  422. ASSERT_FALSE(updatedFrameId.empty());
  423. EXPECT_EQ(updatedFrameId, "updated_namespace/dynamic_frame");
  424. // Old frame ID should not exist anymore
  425. auto oldEntityLookup = trackingInterface->GetFrameEntityByNamespacedId("initial_namespace/dynamic_frame");
  426. EXPECT_FALSE(oldEntityLookup.IsValid());
  427. // New frame ID should work
  428. auto newEntityLookup = trackingInterface->GetFrameEntityByNamespacedId("updated_namespace/dynamic_frame");
  429. ASSERT_TRUE(newEntityLookup.IsValid());
  430. EXPECT_EQ(newEntityLookup, entity.GetId());
  431. }
  432. } // namespace UnitTest
  433. // required to support running integration tests with Qt
  434. AZTEST_EXPORT int AZ_UNIT_TEST_HOOK_NAME(int argc, char** argv)
  435. {
  436. ::testing::InitGoogleMock(&argc, argv);
  437. AzQtComponents::PrepareQtPaths();
  438. QApplication app(argc, argv);
  439. AZ::Test::printUnusedParametersWarning(argc, argv);
  440. AZ::Test::addTestEnvironments({ new UnitTest::ROS2FrameComponentTestEnvironment() });
  441. int result = RUN_ALL_TESTS();
  442. return result;
  443. }
  444. IMPLEMENT_TEST_EXECUTABLE_MAIN();