Visual plugin for changing the color of some visual elements using ROS messages. This plugin accepts the following SDF parameters: More...
#include <light_buoy_plugin.hh>
Public Member Functions | |
LightBuoyPlugin () | |
void | Load (gazebo::rendering::VisualPtr _parent, sdf::ElementPtr _sdf) |
Private Types | |
using | Colors_t = std::pair< std_msgs::ColorRGBA, std::string > |
using | Pattern_t = std::array< uint8_t, 5 > |
Private Member Functions | |
void | ChangePattern (const std_msgs::Empty::ConstPtr &_msg) |
ROS callback for generating a new color pattern. More... | |
void | ChangePatternTo (gazebo::ConstLightBuoyColorsPtr &_msg) |
Gazebo callback for changing light to a specific color pattern. More... | |
void | InitializeAllPatterns () |
Initialize all color sequences. More... | |
bool | ParseSDF (sdf::ElementPtr _sdf) |
Parse all SDF parameters. More... | |
void | Update () |
Display the next color in the sequence, or start over if at the end. More... | |
Static Private Member Functions | |
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. More... | |
static uint8_t | IndexFromColor (const std::string &_color) |
Return the index of the color from its string. More... | |
Private Attributes | |
std::vector< Pattern_t > | allPatterns |
All color sequences. More... | |
size_t | allPatternsIdx = 0u |
The index pointing to one of the potential color sequences. More... | |
ros::Subscriber | changePatternSub |
Subscriber to generate and display a new color sequence. More... | |
gazebo::transport::SubscriberPtr | colorSub |
std::string | gzColorsTopic |
gazebo topic. More... | |
gazebo::transport::NodePtr | gzNode |
std::mutex | mutex |
Locks state and pattern member variables. More... | |
gazebo::common::Time | nextUpdateTime |
Next time where the plugin should be updated. More... | |
ros::NodeHandle | nh |
ROS Node handle. More... | |
std::string | ns |
ROS namespace. More... | |
Pattern_t | pattern |
The color pattern. More... | |
std::string | rosShuffleTopic |
ROS topic. More... | |
gazebo::rendering::ScenePtr | scene |
Pointer to the scene node. More... | |
bool | shuffleEnabled = true |
Whether shuffle is enabled via a ROS topic or not. More... | |
uint8_t | state = 0u |
Track current index in pattern. More... | |
gazebo::event::ConnectionPtr | updateConnection |
Connects to rendering update event. More... | |
std::vector< std::string > | visualNames |
Collection of visual names. More... | |
std::vector< gazebo::rendering::VisualPtr > | visuals |
Pointer to the visual elements to modify. More... | |
Static Private Attributes | |
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. More... | |
Visual plugin for changing the color of some visual elements using ROS messages. This plugin accepts the following SDF parameters:
<color_1>: The first color of the sequence (RED, GREEN, BLUE, YELLOW). <color_2>: The second color of the sequence (RED, GREEN, BLUE, YELLOW). <color_3>: The third color of the sequence (RED, GREEN, BLUE, YELLOW). <shuffle>: True if the topic for shuffling the sequence is enabled. <robot_namespace>: The ROS namespace for this node. If not present, the model name without any "::"" will be used. E.g.: The plugin under a visual named "model1::my_submodel::link::visual" will use "model1" as namespace unless a value is specified. <ros_shuffle_topic>: The ROS topic used to request color changes. <gz_colors_topic>: The gazebo topic used to request specific color changes. <visuals>: The collection of visuals that change in color. It accepts N elements of <visual> elements.
Here's an example: <plugin name="light_buoy_plugin" filename="liblight_buoy_plugin.so"> <color_1>RED</color_1> <color_2>GREEN</color_2> <color_3>BLUE</color_3> <visuals> <visual>robotx_light_buoy::base_link::panel_1</visual> <visual>robotx_light_buoy::base_link::panel_2</visual> <visual>robotx_light_buoy::base_link::panel_3</visual> </visuals> <shuffle>true</shuffle> <robot_namespace>vrx</robot_namespace> <ros_shuffle_topic>light_buoy/shuffle</ros_shuffle_topic> </plugin>
Definition at line 73 of file light_buoy_plugin.hh.
|
private |
Definition at line 118 of file light_buoy_plugin.hh.
|
private |
Definition at line 123 of file light_buoy_plugin.hh.
LightBuoyPlugin::LightBuoyPlugin | ( | ) |
Definition at line 80 of file light_buoy_plugin.cc.
|
private |
ROS callback for generating a new color pattern.
[in] | _msg | Not used. |
Definition at line 271 of file light_buoy_plugin.cc.
|
private |
Gazebo callback for changing light to a specific color pattern.
[in] | _msg | New color sequence. |
Definition at line 86 of file light_buoy_plugin.cc.
|
staticprivate |
Creates a std_msgs::ColorRGBA message from 4 doubles.
[in] | _r | Red. |
[in] | _g | Green. |
[in] | _b | Blue. |
[in] | _a | Alpha. |
Definition at line 30 of file light_buoy_plugin.cc.
|
staticprivate |
Return the index of the color from its string.
[in] | _color | The color |
Definition at line 42 of file light_buoy_plugin.cc.
|
private |
Initialize all color sequences.
Definition at line 57 of file light_buoy_plugin.cc.
void LightBuoyPlugin::Load | ( | gazebo::rendering::VisualPtr | _parent, |
sdf::ElementPtr | _sdf | ||
) |
Definition at line 104 of file light_buoy_plugin.cc.
|
private |
Parse all SDF parameters.
[in] | _sdf | SDF elements. |
Definition at line 143 of file light_buoy_plugin.cc.
|
private |
Display the next color in the sequence, or start over if at the end.
Definition at line 227 of file light_buoy_plugin.cc.
|
private |
All color sequences.
Definition at line 130 of file light_buoy_plugin.hh.
|
private |
The index pointing to one of the potential color sequences.
Definition at line 133 of file light_buoy_plugin.hh.
|
private |
Subscriber to generate and display a new color sequence.
Definition at line 145 of file light_buoy_plugin.hh.
|
private |
Definition at line 154 of file light_buoy_plugin.hh.
|
private |
gazebo topic.
Definition at line 169 of file light_buoy_plugin.hh.
|
private |
Definition at line 151 of file light_buoy_plugin.hh.
|
staticprivate |
List of the color options (red, green, blue, yellow and no color) with their string name for logging.
Definition at line 127 of file light_buoy_plugin.hh.
|
private |
Locks state and pattern member variables.
Definition at line 181 of file light_buoy_plugin.hh.
|
private |
Next time where the plugin should be updated.
Definition at line 178 of file light_buoy_plugin.hh.
|
private |
ROS Node handle.
Definition at line 148 of file light_buoy_plugin.hh.
|
private |
ROS namespace.
Definition at line 163 of file light_buoy_plugin.hh.
|
private |
The color pattern.
Definition at line 157 of file light_buoy_plugin.hh.
|
private |
ROS topic.
Definition at line 166 of file light_buoy_plugin.hh.
|
private |
Pointer to the scene node.
Definition at line 172 of file light_buoy_plugin.hh.
|
private |
Whether shuffle is enabled via a ROS topic or not.
Definition at line 142 of file light_buoy_plugin.hh.
|
private |
Track current index in pattern.
Definition at line 160 of file light_buoy_plugin.hh.
|
private |
Connects to rendering update event.
Definition at line 175 of file light_buoy_plugin.hh.
|
private |
Collection of visual names.
Definition at line 136 of file light_buoy_plugin.hh.
|
private |
Pointer to the visual elements to modify.
Definition at line 139 of file light_buoy_plugin.hh.