From 6c85257be8dd65f1fb50e394fae12ef12f7eeaff Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= <juraj.orsulic@fer.hr>
Date: Wed, 14 Jun 2017 16:25:53 +0200
Subject: [PATCH] Return ColorRGBA directly from GetColor(). (#377)

---
 cartographer_ros/cartographer_ros/color.cc    | 29 ++++++++++++-------
 cartographer_ros/cartographer_ros/color.h     | 15 ++++------
 .../cartographer_ros/map_builder_bridge.cc    |  6 +---
 3 files changed, 25 insertions(+), 25 deletions(-)

diff --git a/cartographer_ros/cartographer_ros/color.cc b/cartographer_ros/cartographer_ros/color.cc
index 0f767da..1a33394 100644
--- a/cartographer_ros/cartographer_ros/color.cc
+++ b/cartographer_ros/cartographer_ros/color.cc
@@ -28,9 +28,18 @@ constexpr float kInitialHue = 0.69f;
 constexpr float kSaturation = 0.85f;
 constexpr float kValue = 0.77f;
 constexpr float kGoldenRatioConjugate = (std::sqrt(5.f) - 1.f) / 2.f;
+constexpr float kAlpha = 1.f;
 
-::cartographer_ros::ColorRgb HsvToRgb(const float h, const float s,
-                                      const float v) {
+::std_msgs::ColorRGBA CreateRgba(const float r, const float g, const float b) {
+  ::std_msgs::ColorRGBA result;
+  result.r = r;
+  result.g = g;
+  result.b = b;
+  result.a = kAlpha;
+  return result;
+}
+
+::std_msgs::ColorRGBA HsvToRgb(const float h, const float s, const float v) {
   const float h_6 = (h == 1.f) ? 0.f : 6 * h;
   const int h_i = std::floor(h_6);
   const float f = h_6 - h_i;
@@ -40,25 +49,25 @@ constexpr float kGoldenRatioConjugate = (std::sqrt(5.f) - 1.f) / 2.f;
   const float t = v * (1.f - (1.f - f) * s);
 
   if (h_i == 0) {
-    return {v, t, p};
+    return CreateRgba(v, t, p);
   } else if (h_i == 1) {
-    return {q, v, p};
+    return CreateRgba(q, v, p);
   } else if (h_i == 2) {
-    return {p, v, t};
+    return CreateRgba(p, v, t);
   } else if (h_i == 3) {
-    return {p, q, v};
+    return CreateRgba(p, q, v);
   } else if (h_i == 4) {
-    return {t, p, v};
+    return CreateRgba(t, p, v);
   } else if (h_i == 5) {
-    return {v, p, q};
+    return CreateRgba(v, p, q);
   } else {
-    return {0.f, 0.f, 0.f};
+    return CreateRgba(0.f, 0.f, 0.f);
   }
 }
 
 }  // namespace
 
-ColorRgb GetColor(int id) {
+::std_msgs::ColorRGBA GetColor(int id) {
   CHECK_GE(id, 0);
   // Uniform color sampling using the golden ratio from
   // http://martin.ankerl.com/2009/12/09/how-to-create-random-colors-programmatically/
diff --git a/cartographer_ros/cartographer_ros/color.h b/cartographer_ros/cartographer_ros/color.h
index adc5516..7024b7d 100644
--- a/cartographer_ros/cartographer_ros/color.h
+++ b/cartographer_ros/cartographer_ros/color.h
@@ -17,18 +17,13 @@
 #ifndef CARTOGRAPHER_ROS_COLOR_MANAGER_H_
 #define CARTOGRAPHER_ROS_COLOR_MANAGER_H_
 
+#include <std_msgs/ColorRGBA.h>
+
 namespace cartographer_ros {
 
-struct ColorRgb {
-  // r, g, b are from [0,1]
-  float r;
-  float g;
-  float b;
-};
-
-// A function for on-demand generation of a colour palette, with every two
-// direct successors having large contrast. Initial hue is from [0, 1>.
-ColorRgb GetColor(int id);
+// A function for on-demand generation of a color palette, with every two
+// direct successors having large contrast.
+::std_msgs::ColorRGBA GetColor(int id);
 
 }  // namespace cartographer_ros
 
diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc
index da033b6..23856ec 100644
--- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc
+++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc
@@ -217,11 +217,7 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodesList() {
     marker.type = visualization_msgs::Marker::LINE_STRIP;
     marker.header.stamp = ::ros::Time::now();
     marker.header.frame_id = node_options_.map_frame;
-    const ColorRgb trajectory_color = GetColor(trajectory_id);
-    marker.color.r = trajectory_color.r;
-    marker.color.g = trajectory_color.g;
-    marker.color.b = trajectory_color.b;
-    marker.color.a = 1.0;
+    marker.color = GetColor(trajectory_id);
     marker.scale.x = kTrajectoryLineStripMarkerScale;
     marker.pose.orientation.w = 1.0;
     for (const auto& node : single_trajectory_nodes) {