light_buoy_plugin.cc
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2019 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 
18 #include <gazebo/rendering/Scene.hh>
19 #include <ignition/math/Rand.hh>
21 
22 const std::array<LightBuoyPlugin::Colors_t, 5> LightBuoyPlugin::kColors
23  = {LightBuoyPlugin::Colors_t(CreateColor(1.0, 0.0, 0.0, 1.0), "RED"),
24  LightBuoyPlugin::Colors_t(CreateColor(0.0, 1.0, 0.0, 1.0), "GREEN"),
25  LightBuoyPlugin::Colors_t(CreateColor(0.0, 0.0, 1.0, 1.0), "BLUE"),
26  LightBuoyPlugin::Colors_t(CreateColor(1.0, 1.0, 0.0, 1.0), "YELLOW"),
27  LightBuoyPlugin::Colors_t(CreateColor(0.0, 0.0, 0.0, 1.0), "OFF")};
28 
30 std_msgs::ColorRGBA LightBuoyPlugin::CreateColor(const double _r,
31  const double _g, const double _b, const double _a)
32 {
33  static std_msgs::ColorRGBA color;
34  color.r = _r;
35  color.g = _g;
36  color.b = _b;
37  color.a = _a;
38  return color;
39 }
40 
42 uint8_t LightBuoyPlugin::IndexFromColor(const std::string &_color)
43 {
44  uint8_t index = 0u;
45  for (auto color : kColors)
46  {
47  if (_color == color.second)
48  return index;
49 
50  ++index;
51  }
52 
53  return std::numeric_limits<uint8_t>::max();
54 }
55 
57 void LightBuoyPlugin::Load(gazebo::rendering::VisualPtr _parent,
58  sdf::ElementPtr _sdf)
59 {
60  GZ_ASSERT(_parent != nullptr, "Received NULL model pointer");
61 
62  this->scene = _parent->GetScene();
63  GZ_ASSERT(this->scene != nullptr, "NULL scene");
64 
65  // This is important to disable the visual plugin running inside the GUI.
66  if (!this->scene->EnableVisualizations())
67  return;
68 
69  if (!this->ParseSDF(_sdf))
70  return;
71 
72  // Quit if ros plugin was not loaded
73  if (!ros::isInitialized())
74  {
75  ROS_ERROR("ROS was not initialized.");
76  return;
77  }
78 
79  if (this->shuffleEnabled)
80  {
81  this->nh = ros::NodeHandle(this->ns);
84  }
85 
86  this->timer.Start();
87 
88  this->updateConnection = gazebo::event::Events::ConnectPreRender(
89  std::bind(&LightBuoyPlugin::Update, this));
90 }
91 
93 bool LightBuoyPlugin::ParseSDF(sdf::ElementPtr _sdf)
94 {
95  // Required: Sequence of colors.
96  uint8_t i = 0u;
97  for (auto colorIndex : {"color_1", "color_2", "color_3"})
98  {
99  if (!_sdf->HasElement(colorIndex))
100  {
101  ROS_ERROR("<%s> missing", colorIndex);
102  return false;
103  }
104 
105  auto color = _sdf->GetElement(colorIndex)->Get<std::string>();
106 
107  // Sanity check: color should be red, green, blue or yellow.
108  if (color != "RED" && color != "GREEN" &&
109  color != "BLUE" && color != "YELLOW")
110  {
111  ROS_ERROR("Invalid color [%s]", color.c_str());
112  return false;
113  }
114 
115  this->pattern[i++] = IndexFromColor(color);
116  }
117 
118  // The last color of the pattern is always black.
119  this->pattern[3] = IndexFromColor("OFF");
120 
121  // Required: visuals.
122  if (!_sdf->HasElement("visuals"))
123  {
124  ROS_ERROR("<visuals> missing");
125  return false;
126  }
127 
128  auto visualsElem = _sdf->GetElement("visuals");
129  if (!visualsElem->HasElement("visual"))
130  {
131  ROS_ERROR("<visual> missing");
132  return false;
133  }
134 
135  auto visualElem = visualsElem->GetElement("visual");
136  while (visualElem)
137  {
138  std::string visualName = visualElem->Get<std::string>();
139  this->visualNames.push_back(visualName);
140  visualElem = visualElem->GetNextElement();
141  }
142 
143  // Optional: Is shuffle enabled?
144  if (_sdf->HasElement("shuffle"))
145  {
146  this->shuffleEnabled = _sdf->GetElement("shuffle")->Get<bool>();
147 
148  // Required if shuffle enabled: ROS topic.
149  if (!_sdf->HasElement("topic"))
150  {
151  ROS_ERROR("<topic> missing");
152  }
153  this->topic = _sdf->GetElement("topic")->Get<std::string>();
154  }
155 
156  // Optional: ROS namespace.
157  if (_sdf->HasElement("robot_namespace"))
158  this->ns = _sdf->GetElement("robot_namespace")->Get<std::string>();
159 
160  return true;
161 }
162 
165 {
166  // Get the visuals if needed.
167  if (this->visuals.empty())
168  {
169  for (auto visualName : this->visualNames)
170  {
171  auto visualPtr = this->scene->GetVisual(visualName);
172  if (visualPtr)
173  this->visuals.push_back(visualPtr);
174  else
175  ROS_ERROR("Unable to find [%s] visual", visualName.c_str());
176  }
177  }
178 
179  if (this->timer.GetElapsed() < gazebo::common::Time(1.0))
180  return;
181 
182  // Restart the timer.
183  this->timer.Reset();
184  this->timer.Start();
185 
186  std::lock_guard<std::mutex> lock(this->mutex);
187 
188  // Start over if at end of pattern
189  if (this->state > 3)
190  this->state = 0;
191 
192  auto color = this->kColors[this->pattern[this->state]].first;
193  gazebo::common::Color gazeboColor(color.r, color.g, color.b, color.a);
194 
195  // Update the visuals.
196  for (auto visual : this->visuals)
197  {
198  visual->SetAmbient(gazeboColor);
199  visual->SetDiffuse(gazeboColor);
200  }
201 
202  // Increment index for next timer callback
203  ++this->state;
204 }
205 
207 bool LightBuoyPlugin::ChangePattern(std_srvs::Trigger::Request &_req,
208  std_srvs::Trigger::Response &_res)
209 {
210  this->ChangePattern(_res.message);
211  _res.message = "New pattern: " + _res.message;
212  _res.success = true;
213  return _res.success;
214 }
215 
217 void LightBuoyPlugin::ChangePattern(std::string &_message)
218 {
219  Pattern_t newPattern;
220  // Last phase in pattern is always off
221  newPattern[3] = IndexFromColor("OFF");;
222 
223  // Loop until random pattern is different from current one
224  do {
225  // Generate random sequence of 3 colors among RED, GREEN, BLUE, and YELLOW
226  for (size_t i = 0; i < 3; ++i)
227  newPattern[i] = ignition::math::Rand::IntUniform(0, 3);
228  // Ensure there is no consecutive repeats
229  while (newPattern[0] == newPattern[1] || newPattern[1] == newPattern[2])
230  newPattern[1] = ignition::math::Rand::IntUniform(0, 3);
231  } while (newPattern == this->pattern);
232 
233  std::lock_guard<std::mutex> lock(this->mutex);
234  // Copy newly generated pattern to pattern
235  this->pattern = newPattern;
236  // Start in OFF state so pattern restarts at beginning
237  this->state = 3;
238  // Generate string representing pattern, ex: "RGB"
239  for (size_t i = 0; i < 3; ++i)
240  _message += this->kColors[newPattern[i]].second[0];
241  // Log the new pattern
242  ROS_INFO_NAMED("LightBuoyPlugin", "Pattern is %s", _message.c_str());
243 }
244 
245 // Register plugin with gazebo
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.
#define ROS_ERROR(...)
ros::ServiceServer changePatternServer
Service to generate and display a new color sequence.


vmrc_gazebo
Author(s): Brian Bingham , Carlos Aguero
autogenerated on Sun Feb 17 2019 03:14:16