diff --git a/cartographer_ros/cartographer_ros/color.cc b/cartographer_ros/cartographer_ros/color.cc deleted file mode 100644 index 1a33394..0000000 --- a/cartographer_ros/cartographer_ros/color.cc +++ /dev/null @@ -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 - -#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 diff --git a/cartographer_ros/cartographer_ros/color.h b/cartographer_ros/cartographer_ros/color.h deleted file mode 100644 index 7024b7d..0000000 --- a/cartographer_ros/cartographer_ros/color.h +++ /dev/null @@ -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 - -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_ diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index 0fe326a..a7c7f6f 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/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 + - constraint.submap_id.trajectory_id + 25); + color_constraint = ToMessage(cartographer::io::GetColor( + 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( diff --git a/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc b/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc index f2fa815..261b627 100644 --- a/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc +++ b/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; - -UniqueCairoSurfacePtr MakeUniqueCairoSurfacePtr(cairo_surface_t* surface) { - return UniqueCairoSurfacePtr(surface, cairo_surface_destroy); -} - -// std::unique_ptr for Cairo contexts. -using UniqueCairoPtr = std::unique_ptr; - -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 cairo_data; @@ -197,8 +183,8 @@ void Node::HandleSubmapList( (observed << 8) | 0); } - submap_state.surface = - MakeUniqueCairoSurfacePtr(cairo_image_surface_create_for_data( + submap_state.surface = ::cartographer::io::MakeUniqueCairoSurfacePtr( + cairo_image_surface_create_for_data( reinterpret_cast(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());