Skip to content

Commit

Permalink
Change an entities visual material color by topic. (gazebosim#2286)
Browse files Browse the repository at this point in the history 
This change allows for a simpler mapping from `MaterialColor` msg in gz to the much more cumbersome Visual msg. By doing this and adding a topic listener it allows users to rapidly change the colors of a entities visual material color components, including from the [ROS side using the gz bridge](gazebosim/ros_gz#486). This is especially useful for simulating status lighting.


---------

Signed-off-by: Benjamin Perseghetti <[email protected]>
Co-authored-by: Alejandro Hernández Cordero <[email protected]>
Co-authored-by: Addisu Z. Taddese <[email protected]>
  • Loading branch information
@bperseghetti @ahcorde @azeey
3 people authored Jan 25, 2024
1 parent a267a68 commit 08b5cfe
Show file tree
Hide file tree
Showing 3 changed files with 323 additions and 35 deletions.
175 changes: 140 additions & 35 deletions src/systems/user_commands/UserCommands.cc
View file
Original file line number Diff line number Diff line change
@@ -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.
Expand Down Expand Up @@ -33,6 +35,7 @@
#include <gz/msgs/entity_factory.pb.h>
#include <gz/msgs/entity_factory_v.pb.h>
#include <gz/msgs/light.pb.h>
#include <gz/msgs/material_color.pb.h>
#include <gz/msgs/physics.pb.h>
#include <gz/msgs/pose.pb.h>
#include <gz/msgs/pose_v.pb.h>
Expand All @@ -45,6 +48,7 @@
#include <unordered_set>
#include <vector>

#include <gz/math/Color.hh>
#include <gz/math/SphericalCoordinates.hh>
#include <gz/msgs/Utility.hh>

Expand Down Expand Up @@ -326,6 +330,12 @@ class VisualCommand : public UserCommandBase
public: VisualCommand(msgs::Visual *_msg,
std::shared_ptr<UserCommandsInterface> &_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<UserCommandsInterface> &_iface);

// Documentation inherited
public: bool Execute() final;

Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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<VisualCommand>(msg, this->iface);
// Push to pending
{
std::lock_guard<std::mutex> lock(this->pendingMutex);
this->pendingCmds.push_back(std::move(cmd));
}

return;
}

//////////////////////////////////////////////////
bool UserCommandsPrivate::WheelSlipService(
const msgs::WheelSlipParametersCmd &_req,
Expand Down Expand Up @@ -1720,57 +1754,128 @@ VisualCommand::VisualCommand(msgs::Visual *_msg,
{
}

//////////////////////////////////////////////////
VisualCommand::VisualCommand(msgs::MaterialColor *_msg,
std::shared_ptr<UserCommandsInterface> &_iface)
: UserCommandBase(_msg, _iface)
{
}

//////////////////////////////////////////////////
bool VisualCommand::Execute()
{
auto visualMsg = dynamic_cast<const msgs::Visual *>(this->msg);
if (nullptr == visualMsg)
auto materialColorMsg = dynamic_cast<const msgs::MaterialColor *>(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<components::VisualCmd>(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<components::VisualCmd>(visualEntity);
if (!visualCmdComp)
{
this->iface->ecm->CreateComponent(
visualEntity, components::VisualCmd(*visualMsg));
auto visualCmdComp =
this->iface->ecm->Component<components::VisualCmd>(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;
}
Expand Down
107 changes: 107 additions & 0 deletions test/integration/user_commands.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
#include <gz/msgs/entity.pb.h>
#include <gz/msgs/entity_factory.pb.h>
#include <gz/msgs/light.pb.h>
#include <gz/msgs/material_color.pb.h>
#include <gz/msgs/physics.pb.h>
#include <gz/msgs/pose.pb.h>
#include <gz/msgs/pose_v.pb.h>
Expand Down Expand Up @@ -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<components::Material>(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<msgs::MaterialColor>(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<components::Material>(sphereVisualEntity0);
auto updatedVisual1 =
ecm->Component<components::Material>(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))
{
Expand Down
Loading

0 comments on commit 08b5cfe

Please sign in to comment.