18 #include <gazebo/rendering/Scene.hh> 19 #include <ignition/math/Rand.hh> 31 const double _g,
const double _b,
const double _a)
33 static std_msgs::ColorRGBA color;
47 if (_color == color.second)
53 return std::numeric_limits<uint8_t>::max();
60 GZ_ASSERT(_parent !=
nullptr,
"Received NULL model pointer");
62 this->
scene = _parent->GetScene();
63 GZ_ASSERT(this->
scene !=
nullptr,
"NULL scene");
66 if (!this->
scene->EnableVisualizations())
97 for (
auto colorIndex : {
"color_1",
"color_2",
"color_3"})
99 if (!_sdf->HasElement(colorIndex))
105 auto color = _sdf->GetElement(colorIndex)->Get<std::string>();
108 if (color !=
"RED" && color !=
"GREEN" &&
109 color !=
"BLUE" && color !=
"YELLOW")
111 ROS_ERROR(
"Invalid color [%s]", color.c_str());
122 if (!_sdf->HasElement(
"visuals"))
128 auto visualsElem = _sdf->GetElement(
"visuals");
129 if (!visualsElem->HasElement(
"visual"))
135 auto visualElem = visualsElem->GetElement(
"visual");
138 std::string visualName = visualElem->Get<std::string>();
140 visualElem = visualElem->GetNextElement();
144 if (_sdf->HasElement(
"shuffle"))
149 if (!_sdf->HasElement(
"topic"))
153 this->
topic = _sdf->GetElement(
"topic")->Get<std::string>();
157 if (_sdf->HasElement(
"robot_namespace"))
158 this->
ns = _sdf->GetElement(
"robot_namespace")->Get<std::string>();
171 auto visualPtr = this->
scene->GetVisual(visualName);
173 this->
visuals.push_back(visualPtr);
175 ROS_ERROR(
"Unable to find [%s] visual", visualName.c_str());
179 if (this->
timer.GetElapsed() < gazebo::common::Time(1.0))
186 std::lock_guard<std::mutex> lock(this->
mutex);
193 gazebo::common::Color gazeboColor(color.r, color.g, color.b, color.a);
196 for (
auto visual : this->
visuals)
198 visual->SetAmbient(gazeboColor);
199 visual->SetDiffuse(gazeboColor);
208 std_srvs::Trigger::Response &_res)
211 _res.message =
"New pattern: " + _res.message;
226 for (
size_t i = 0; i < 3; ++i)
227 newPattern[i] = ignition::math::Rand::IntUniform(0, 3);
229 while (newPattern[0] == newPattern[1] || newPattern[1] == newPattern[2])
230 newPattern[1] = ignition::math::Rand::IntUniform(0, 3);
231 }
while (newPattern == this->
pattern);
233 std::lock_guard<std::mutex> lock(this->
mutex);
239 for (
size_t i = 0; i < 3; ++i)
240 _message += this->
kColors[newPattern[i]].second[0];
242 ROS_INFO_NAMED(
"LightBuoyPlugin",
"Pattern is %s", _message.c_str());
static std_msgs::ColorRGBA CreateColor(const double _r, const double _g, const double _b, const double _a)
Creates a std_msgs::ColorRGBA message from 4 doubles.
std::vector< gazebo::rendering::VisualPtr > visuals
Pointer to the visual elements to modify.
#define ROS_INFO_NAMED(name,...)
void Load(gazebo::rendering::VisualPtr _parent, sdf::ElementPtr _sdf)
bool shuffleEnabled
Whether shuffle is enabled via a ROS topic or not.
Visual plugin for changing the color of some visual elements using ROS messages. This plugin accepts ...
ROSCPP_DECL bool isInitialized()
std::vector< std::string > visualNames
Collection of visual names.
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
bool ParseSDF(sdf::ElementPtr _sdf)
Parse all SDF parameters.
gazebo::common::Timer timer
Timer used to switch colors every second.
ros::NodeHandle nh
ROS Node handle.
static uint8_t IndexFromColor(const std::string &_color)
Return the index of the color from its string.
std::string ns
ROS namespace.
std::pair< std_msgs::ColorRGBA, std::string > Colors_t
void Update()
Display the next color in the sequence, or start over if at the end.
static const std::array< Colors_t, 5 > kColors
List of the color options (red, green, blue, yellow and no color) with their string name for logging...
Pattern_t pattern
The color pattern.
gazebo::rendering::ScenePtr scene
Pointer to the scene node.
std::string topic
ROS topic.
std::mutex mutex
Locks state and pattern member variables.
std::array< uint8_t, 4 > Pattern_t
GZ_REGISTER_VISUAL_PLUGIN(GazeboRosVideo)
bool ChangePattern(std_srvs::Trigger::Request &_req, std_srvs::Trigger::Response &_res)
Callback for change pattern service, calls other changePattern internaly.
uint8_t state
Track current index in pattern.
gazebo::event::ConnectionPtr updateConnection
Connects to rendering update event.
ros::ServiceServer changePatternServer
Service to generate and display a new color sequence.