12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Track googlecartographer/cartographer#420. (#439)

master
Holger Rapp 2017-07-24 14:00:42 +02:00 committed by GitHub
parent 99da2f20d8
commit 3dceac9a46
4 changed files with 31 additions and 137 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

78
cartographer_ros/cartographer_ros/color.cc
View File

@ -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

30
cartographer_ros/cartographer_ros/color.h
View File

@ -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_

24
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -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(

36
cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc
View File

@ -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 =
MakeUniqueCairoSurfacePtr(cairo_image_surface_create_for_data(
submap_state.surface = ::cartographer::io::MakeUniqueCairoSurfacePtr(
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());