From fe335f9cd47e7f7b3ff35159babe28d9fd55670d Mon Sep 17 00:00:00 2001
From: William Lew <WilliamMilesLew@gmail.com>
Date: Tue, 28 Dec 2021 14:13:08 -0800
Subject: [PATCH] Light Commands via topic (#1222)

Signed-off-by: William Lew <WilliamMilesLew@gmail.com>
---
 src/systems/user_commands/UserCommands.cc | 23 +++++++++++++++++++++++
 test/integration/user_commands.cc         | 19 +++++++++++++++++++
 2 files changed, 42 insertions(+)

diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc
index 6993461c23..ab32210375 100644
--- a/src/systems/user_commands/UserCommands.cc
+++ b/src/systems/user_commands/UserCommands.cc
@@ -443,6 +443,10 @@ class ignition::gazebo::systems::UserCommandsPrivate
   /// \return True if successful.
   public: bool LightService(const msgs::Light &_req, msgs::Boolean &_res);
 
+  /// \brief Callback for light subscription
+  /// \param[in] _msg Light message
+  public: void OnCmdLight(const msgs::Light &_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.
@@ -602,6 +606,10 @@ void UserCommands::Configure(const Entity &_entity,
   ignmsg << "Light configuration service on [" << lightService << "]"
     << std::endl;
 
+  std::string lightTopic{"/world/" + validWorldName + "/light_config"};
+  this->dataPtr->node.Subscribe(lightTopic, &UserCommandsPrivate::OnCmdLight,
+                                this->dataPtr.get());
+
   // Physics service
   std::string physicsService{"/world/" + validWorldName + "/set_physics"};
   this->dataPtr->node.Advertise(physicsService,
@@ -754,6 +762,21 @@ bool UserCommandsPrivate::LightService(const msgs::Light &_req,
   return true;
 }
 
+//////////////////////////////////////////////////
+void UserCommandsPrivate::OnCmdLight(const msgs::Light &_msg)
+{
+  auto msg = _msg.New();
+  msg->CopyFrom(_msg);
+  auto cmd = std::make_unique<LightCommand>(msg, this->iface);
+
+  // Push to pending
+  {
+    std::lock_guard<std::mutex> lock(this->pendingMutex);
+    this->pendingCmds.push_back(std::move(cmd));
+  }
+}
+
+
 //////////////////////////////////////////////////
 bool UserCommandsPrivate::PoseService(const msgs::Pose &_req,
     msgs::Boolean &_res)
diff --git a/test/integration/user_commands.cc b/test/integration/user_commands.cc
index 47cccb8354..20cdaa22f3 100644
--- a/test/integration/user_commands.cc
+++ b/test/integration/user_commands.cc
@@ -919,6 +919,25 @@ TEST_F(UserCommandsTest, Light)
   EXPECT_NEAR(1.5, spotLightComp->Data().SpotInnerAngle().Radian(), 0.1);
   EXPECT_NEAR(0.3, spotLightComp->Data().SpotOuterAngle().Radian(), 0.1);
   EXPECT_NEAR(0.9, spotLightComp->Data().SpotFalloff(), 0.1);
+
+  // Test light_config topic
+  const std::string lightTopic = "/world/lights_command/light_config";
+
+  msgs::Light lightMsg;
+  lightMsg.set_name("spot");
+  ignition::msgs::Set(lightMsg.mutable_diffuse(),
+    ignition::math::Color(1.0f, 1.0f, 1.0f, 1.0f));
+
+  // Publish light config
+  auto pub = node.Advertise<msgs::Light>(lightTopic);
+  pub.Publish(lightMsg);
+
+  server.Run(true, 100, false);
+  // Sleep for a small duration to allow Run thread to start
+  IGN_SLEEP_MS(10);
+
+  EXPECT_EQ(math::Color(1.0f, 1.0f, 1.0f, 1.0f),
+    spotLightComp->Data().Diffuse());
 }
 
 /////////////////////////////////////////////////