Track googlecartographer/cartographer#420. (#439)

cartographer_ros/cartographer_ros/color.cc

@@ -1,78 +0,0 @@
-/*
- * Copyright 2017 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#include "cartographer_ros/color.h"
-
-#include <cmath>
-
-#include "glog/logging.h"
-
-namespace cartographer_ros {
-
-namespace {
-
-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;
-
-::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;
-
-  const float p = v * (1.f - s);
-  const float q = v * (1.f - f * s);
-  const float t = v * (1.f - (1.f - f) * s);
-
-  if (h_i == 0) {
-    return CreateRgba(v, t, p);
-  } else if (h_i == 1) {
-    return CreateRgba(q, v, p);
-  } else if (h_i == 2) {
-    return CreateRgba(p, v, t);
-  } else if (h_i == 3) {
-    return CreateRgba(p, q, v);
-  } else if (h_i == 4) {
-    return CreateRgba(t, p, v);
-  } else if (h_i == 5) {
-    return CreateRgba(v, p, q);
-  } else {
-    return CreateRgba(0.f, 0.f, 0.f);
-  }
-}
-
-}  // namespace
-
-::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/
-  const float hue = std::fmod(kInitialHue + kGoldenRatioConjugate * id, 1.f);
-  return HsvToRgb(hue, kSaturation, kValue);
-}
-
-}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/color.h

@@ -1,30 +0,0 @@
-/*
- * Copyright 2017 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#ifndef CARTOGRAPHER_ROS_COLOR_MANAGER_H_
-#define CARTOGRAPHER_ROS_COLOR_MANAGER_H_
-
-#include <std_msgs/ColorRGBA.h>
-
-namespace cartographer_ros {
-
-// 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
-
-#endif  // CARTOGRAPHER_ROS_COLOR_MANAGER_H_

cartographer_ros/cartographer_ros/map_builder_bridge.cc

@@ -16,15 +16,28 @@
 
 #include "cartographer_ros/map_builder_bridge.h"
 
+#include "cartographer/io/color.h"
 #include "cartographer/io/proto_stream.h"
-#include "cartographer_ros/color.h"
 #include "cartographer_ros/msg_conversion.h"
 
 namespace cartographer_ros {
 
+namespace {
+
 constexpr double kTrajectoryLineStripMarkerScale = 0.07;
 constexpr double kConstraintMarkerScale = 0.025;
 
+::std_msgs::ColorRGBA ToMessage(const cartographer::io::Color& color) {
+  ::std_msgs::ColorRGBA result;
+  result.r = color[0] / 255.;
+  result.g = color[1] / 255.;
+  result.b = color[2] / 255.;
+  result.a = 1.f;
+  return result;
+}
+
+}  // namespace
+
 MapBuilderBridge::MapBuilderBridge(const NodeOptions& node_options,
                                    tf2_ros::Buffer* const tf_buffer)
     : node_options_(node_options),
@@ -166,7 +179,7 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() {
     marker.type = visualization_msgs::Marker::LINE_STRIP;
     marker.header.stamp = ::ros::Time::now();
     marker.header.frame_id = node_options_.map_frame;
-    marker.color = GetColor(trajectory_id);
+    marker.color = ToMessage(cartographer::io::GetColor(trajectory_id));
     marker.scale.x = kTrajectoryLineStripMarkerScale;
     marker.pose.orientation.w = 1.0;
     marker.pose.position.z = 0.05;
@@ -242,8 +255,9 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() {
       // Color mapping for submaps of various trajectories - add trajectory id
       // to ensure different starting colors. Also add a fixed offset of 25
       // to avoid having identical colors as trajectories.
-      color_constraint = GetColor(constraint.submap_id.submap_index +
+      color_constraint = ToMessage(cartographer::io::GetColor(
-                                  constraint.submap_id.trajectory_id + 25);
+          constraint.submap_id.submap_index +
+          constraint.submap_id.trajectory_id + 25));
       color_residual.a = 1.0;
       color_residual.r = 1.0;
     } else {
@@ -274,7 +288,7 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() {
                                 .pose;
     // Similar to GetTrajectoryNodeList(), we can skip multiplying with
     // node.constant_data->tracking_to_pose.
-    const ::cartographer::transform::Rigid3d constraint_pose =
+    const cartographer::transform::Rigid3d constraint_pose =
         submap_pose * constraint.pose.zbar_ij;
 
     constraint_marker->points.push_back(

cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc

@@ -23,6 +23,7 @@
 #include "cairo/cairo.h"
 #include "cartographer/common/mutex.h"
 #include "cartographer/common/port.h"
+#include "cartographer/io/image.h"
 #include "cartographer/mapping/id.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "cartographer_ros/msg_conversion.h"
@@ -45,28 +46,13 @@ using ::cartographer::mapping::SubmapId;
 
 constexpr cairo_format_t kCairoFormat = CAIRO_FORMAT_ARGB32;
 
-// std::unique_ptr for Cairo surfaces. The surface is destroyed when the
-// std::unique_ptr is reset or destroyed.
-using UniqueCairoSurfacePtr =
-    std::unique_ptr<cairo_surface_t, void (*)(cairo_surface_t*)>;
-
-UniqueCairoSurfacePtr MakeUniqueCairoSurfacePtr(cairo_surface_t* surface) {
-  return UniqueCairoSurfacePtr(surface, cairo_surface_destroy);
-}
-
-// std::unique_ptr for Cairo contexts.
-using UniqueCairoPtr = std::unique_ptr<cairo_t, void (*)(cairo_t*)>;
-
-UniqueCairoPtr MakeUniqueCairoPtr(cairo_t* surface) {
-  return UniqueCairoPtr(surface, cairo_destroy);
-}
-
 Eigen::Affine3d ToEigen(const ::cartographer::transform::Rigid3d& rigid3) {
   return Eigen::Translation3d(rigid3.translation()) * rigid3.rotation();
 }
 
 struct SubmapState {
-  SubmapState() : surface(MakeUniqueCairoSurfacePtr(nullptr)) {}
+  SubmapState()
+      : surface(::cartographer::io::MakeUniqueCairoSurfacePtr(nullptr)) {}
 
   // Texture data.
   int width;
@@ -74,7 +60,7 @@ struct SubmapState {
   int version;
   double resolution;
   ::cartographer::transform::Rigid3d slice_pose;
-  UniqueCairoSurfacePtr surface;
+  ::cartographer::io::UniqueCairoSurfacePtr surface;
   // Pixel data used by 'surface'. Must outlive 'surface'.
   std::vector<uint32_t> cairo_data;
 
@@ -197,8 +183,8 @@ void Node::HandleSubmapList(
                                         (observed << 8) | 0);
     }
 
-    submap_state.surface =
+    submap_state.surface = ::cartographer::io::MakeUniqueCairoSurfacePtr(
-        MakeUniqueCairoSurfacePtr(cairo_image_surface_create_for_data(
+        cairo_image_surface_create_for_data(
             reinterpret_cast<unsigned char*>(submap_state.cairo_data.data()),
             kCairoFormat, submap_state.width, submap_state.height,
             expected_stride));
@@ -217,9 +203,10 @@ void Node::DrawAndPublish(const string& frame_id, const ros::Time& time) {
 
   Eigen::AlignedBox2f bounding_box;
   {
-    auto surface = MakeUniqueCairoSurfacePtr(
+    auto surface = ::cartographer::io::MakeUniqueCairoSurfacePtr(
         cairo_image_surface_create(kCairoFormat, 1, 1));
-    auto cr = MakeUniqueCairoPtr(cairo_create(surface.get()));
+    auto cr =
+        ::cartographer::io::MakeUniqueCairoPtr(cairo_create(surface.get()));
     const auto update_bounding_box = [&bounding_box, &cr](double x, double y) {
       cairo_user_to_device(cr.get(), &x, &y);
       bounding_box.extend(Eigen::Vector2f(x, y));
@@ -243,9 +230,10 @@ void Node::DrawAndPublish(const string& frame_id, const ros::Time& time) {
                               -bounding_box.min().y() + kPaddingPixel);
 
   {
-    auto surface = MakeUniqueCairoSurfacePtr(
+    auto surface = ::cartographer::io::MakeUniqueCairoSurfacePtr(
         cairo_image_surface_create(kCairoFormat, size.x(), size.y()));
-    auto cr = MakeUniqueCairoPtr(cairo_create(surface.get()));
+    auto cr =
+        ::cartographer::io::MakeUniqueCairoPtr(cairo_create(surface.get()));
     cairo_set_source_rgba(cr.get(), 0.5, 0.0, 0.0, 1.);
     cairo_paint(cr.get());
     cairo_translate(cr.get(), origin.x(), origin.y());