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

Moves the contents of the ros subdirectory to the new googlecartographer/cartographer_ros repo.

master
Damon Kohler 2016-08-03 12:46:59 +02:00
parent 166f2568bc
commit 188e5fb7bd
38 changed files with 0 additions and 3176 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

153
ros/google_cartographer/CMakeLists.txt
View File

@ -1,153 +0,0 @@
# Copyright 2016 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.
cmake_minimum_required(VERSION 2.8)
project(google_cartographer)
set(PACKAGE_DEPENDENCIES
eigen_conversions
geometry_msgs
google_cartographer_msgs
roscpp
rviz
sensor_msgs
tf2
tf2_eigen
)
set(CMAKE_CXX_FLAGS "-std=c++11")
find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES})
include_directories(${catkin_INCLUDE_DIRS})
catkin_package(
CATKIN_DEPENDS
message_runtime
${PACKAGE_DEPENDENCIES}
)
find_package(PCL REQUIRED COMPONENTS common io)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
find_package(Eigen3 REQUIRED)
include_directories(${EIGEN_INCLUDE_DIRS})
find_package(cartographer REQUIRED)
include_directories(${CARTOGRAPHER_INCLUDE_DIRS})
link_directories(${CARTOGRAPHER_LIBRARY_DIRS})
find_package(ZLIB REQUIRED)
include_directories(${ZLIB_INCLUDE_DIRS})
# TODO(pedrofernandez): CMake test rules are written differently in the core
# library. Change the way it's done here and reenable testing.
# enable_testing()
# find_package(GTest REQUIRED)
# include_directories(${GTEST_INCLUDE_DIRS})
set(CMAKE_AUTOMOC ON)
if(rviz_QT_VERSION VERSION_LESS "5")
message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}")
find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui)
include(${QT_USE_FILE})
else()
message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}")
find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets)
set(QT_LIBRARIES Qt5::Widgets)
endif()
add_definitions(-DQT_NO_KEYWORDS)
find_package(Boost REQUIRED
COMPONENTS
system
iostreams
)
add_definitions(${BOOST_DEFINITIONS})
include_directories(${Boost_INCLUDE_DIRS})
link_directories(${Boost_LIBRARY_DIRS})
add_executable(cartographer_node
src/cartographer_node_main.cc
src/node_constants.h
src/msg_conversion.h
src/msg_conversion.cc
src/time_conversion.h
src/time_conversion.cc
)
target_link_libraries(cartographer_node
${CARTOGRAPHER_LIBRARIES}
${catkin_LIBRARIES}
${PCL_LIBRARIES}
)
add_library(cartographer_rviz_submaps_visualization
src/drawable_submap.cc
src/drawable_submap.h
src/submaps_display.cc
src/submaps_display.h
src/trajectory.h
src/trajectory.cc
)
target_link_libraries(cartographer_rviz_submaps_visualization
${Boost_LIBRARIES}
${CARTOGRAPHER_LIBRARIES}
${catkin_LIBRARIES}
${QT_LIBRARIES}
${ZLIB_LIBRARIES}
)
add_executable(time_conversion_test
src/time_conversion_test.cc
src/time_conversion.h
src/time_conversion.cc
)
target_link_libraries(time_conversion_test
${GTEST_BOTH_LIBRARIES}
${CARTOGRAPHER_LIBRARIES}
${catkin_LIBRARIES}
)
add_test(time_conversion_test time_conversion_test)
install(DIRECTORY launch/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch/
)
install(DIRECTORY urdf/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf/
)
install(DIRECTORY configuration_files/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/configuration_files/
)
install(TARGETS
cartographer_rviz_submaps_visualization
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(FILES
plugin_description.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(DIRECTORY ogre_media/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ogre_media}
)

19
ros/google_cartographer/configuration_files/3d_mapping.lua
View File

@ -1,19 +0,0 @@
-- Copyright 2016 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 "backpack_3d.lua"
options.trajectory_builder.scans_per_accumulation = 2
return options

32
ros/google_cartographer/configuration_files/backpack_3d.lua
View File

@ -1,32 +0,0 @@
-- Copyright 2016 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 "trajectory_builder_3d.lua"
include "sparse_pose_graph.lua"
options = {
sparse_pose_graph = SPARSE_POSE_GRAPH,
trajectory_builder = TRAJECTORY_BUILDER_3D,
}
options.sparse_pose_graph.optimize_every_n_scans = 320
options.sparse_pose_graph.constraint_builder.sampling_ratio = 0.03
options.sparse_pose_graph.optimization_problem.ceres_solver_options.max_num_iterations = 10
-- Reuse the coarser 3D voxel filter to speed up the computation of loop closure
-- constraints.
options.sparse_pose_graph.constraint_builder.adaptive_voxel_filter = TRAJECTORY_BUILDER_3D.high_resolution_adaptive_voxel_filter
options.sparse_pose_graph.constraint_builder.min_score = 0.62
options.sparse_pose_graph.constraint_builder.log_matches = true
return options

26
ros/google_cartographer/configuration_files/turtlebot.lua
View File

@ -1,26 +0,0 @@
-- Copyright 2016 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 "trajectory_builder.lua"
include "sparse_pose_graph.lua"
options = {
sparse_pose_graph = SPARSE_POSE_GRAPH,
trajectory_builder = TRAJECTORY_BUILDER,
}
options.trajectory_builder.expect_imu_data = false
options.trajectory_builder.use_online_correlative_scan_matching = true
return options

100
ros/google_cartographer/launch/3d_mapping.launch
View File

@ -1,100 +0,0 @@
<!--
Copyright 2016 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.
-->
<launch>
<node pkg="nodelet" type="nodelet" name="velodyne_nodelet_manager"
args="manager" />
<node pkg="nodelet" type="nodelet" name="horizontal_driver_nodelet"
args="load velodyne_driver/DriverNodelet velodyne_nodelet_manager" >
<remap from="velodyne_packets" to="horizontal_velodyne_packets" />
<param name="model" value="VLP16"/>
<param name="pcap" value=""/>
<param name="read_once" value="false"/>
<param name="read_fast" value="false"/>
<param name="repeat_delay" value="0.0"/>
<param name="rpm" value="600.0"/>
<param name="port" value="2368" />
<param name="frame_id" value="horizontal_vlp16_link"/>
</node>
<node pkg="nodelet" type="nodelet" name="vertical_driver_nodelet"
args="load velodyne_driver/DriverNodelet velodyne_nodelet_manager" >
<remap from="velodyne_packets" to="vertical_velodyne_packets" />
<param name="model" value="VLP16"/>
<param name="pcap" value=""/>
<param name="read_once" value="false"/>
<param name="read_fast" value="false"/>
<param name="repeat_delay" value="0.0"/>
<param name="rpm" value="600.0"/>
<param name="port" value="2369" />
<param name="frame_id" value="vertical_vlp16_link"/>
</node>
<remap from="/imu/imu" to="/imu" />
<!-- TODO(hrapp): the IMU has a bug that makes it impossible to use arg for
setting the parameter. so we work around this by setting the param
directly -->
<param name="/imu/frameId" value="imu_link"/>
<include file="$(find imu_3dm_gx4)/launch/imu.launch">
<arg name="frame_id" value="imu_link"/>
</include>
<!-- VLP16 packets -> points -->
<arg name="calibration" default="$(find velodyne_pointcloud)/params/VLP16db.yaml"/>
<arg name="min_range" default="0.4" />
<arg name="max_range" default="130.0" />
<node pkg="nodelet" type="nodelet" name="cloud_nodelet_horizontal"
args="load velodyne_pointcloud/CloudNodelet velodyne_nodelet_manager">
<remap from="velodyne_packets" to="horizontal_velodyne_packets" />
<remap from="velodyne_points" to="horizontal_3d_laser" />
<param name="calibration" value="$(arg calibration)"/>
<param name="min_range" value="$(arg min_range)"/>
<param name="max_range" value="$(arg max_range)"/>
</node>
<node pkg="nodelet" type="nodelet" name="cloud_nodelet_vertical"
args="load velodyne_pointcloud/CloudNodelet velodyne_nodelet_manager">
<remap from="velodyne_packets" to="vertical_velodyne_packets" />
<remap from="velodyne_points" to="vertical_3d_laser" />
<param name="calibration" value="$(arg calibration)"/>
<param name="min_range" value="$(arg min_range)"/>
<param name="max_range" value="$(arg max_range)"/>
</node>
<param name="robot_description"
textfile="$(find google_cartographer)/urdf/backpack_3d.urdf" />
<node name="robot_state_publisher" pkg="robot_state_publisher"
type="robot_state_publisher" />
<node name="cartographer" pkg="google_cartographer"
type="cartographer_node" output="screen" >
<rosparam subst_value="true">
map_frame: "map"
tracking_frame: "base_link"
configuration_files_directories: [
"$(find google_cartographer)/configuration_files"
]
mapping_configuration_basename: "3d_mapping.lua"
imu_topic: "/imu"
laser_scan_3d_topics: ["/horizontal_3d_laser", "/vertical_3d_laser"]
laser_min_range_m: 0.
laser_max_range_m: 30.
laser_missing_echo_ray_length_m: 5.
</rosparam>
</node>
</launch>

73
ros/google_cartographer/launch/state.launch
View File

@ -1,73 +0,0 @@
<!--
Copyright 2016 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.
-->
<launch>
<param name="robot_description"
textfile="$(find google_cartographer)/urdf/backpack_3d.urdf" />
<node name="robot_state_publisher" pkg="robot_state_publisher"
type="robot_state_publisher" />
<node name="cartographer" pkg="google_cartographer"
type="cartographer_node" output="screen" >
<rosparam subst_value="true">
# This node publishes the transformation between the tracking and map
# frames.
map_frame: "map"
tracking_frame: "base_link"
# Lua configuration files are always searched in the cartographer
# installation directory. We also ship configuration that work well for
# the platforms that we used to collect our example data. You probably
# want to add your own configuration overwrites in your own node
# directories - you should add the path here as first entry then.
configuration_files_directories: [
"$(find google_cartographer)/configuration_files"
]
# Configuration file for SLAM. The settings in here are tweaked to the
# collection platform you are using.
mapping_configuration_basename: "backpack_3d.lua"
imu_topic: "/imu"
# Laser min and max range. Everything not in this range will not be used for mapping.
laser_min_range_m: 0.
laser_max_range_m: 30.
# Missing laser echoes will be inserted as free space at this distance.
laser_missing_echo_ray_length_m: 5.
# Only choose one in the next parameter block
# laser_scan_2d_topic: "/horizontal_2d_laser"
# multi_echo_laser_scan_2d_topic: "/horizontal_multiecho_2d_laser"
laser_scan_3d_topics: ["/horizontal_3d_laser", "/vertical_3d_laser"]
</rosparam>
</node>
<!-- TODO(hrapp): still useful for occasional testing, but delete eventually.
<node pkg="hector_mapping" type="hector_mapping" name="hector_height_mapping"
output="screen">
<param name="scan_topic" value="horizontal_2d_laser" />
<param name="pub_map_odom_transform" value="true"/>
<param name="map_frame" value="map" />
<param name="base_frame" value="base_link" />
<param name="odom_frame" value="base_link" />
<param name="map_size" value="4096" />
<param name="map_resolution" value="0.1" />
</node>
-->
</launch>

41
ros/google_cartographer/launch/turtlebot.launch
View File

@ -1,41 +0,0 @@
<!--
Copyright 2016 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.
-->
<launch>
<param name="robot_description"
textfile="$(find google_cartographer)/urdf/turtlebot.urdf" />
<node name="robot_state_publisher" pkg="robot_state_publisher"
type="robot_state_publisher" />
<node name="cartographer" pkg="google_cartographer"
type="cartographer_node" output="screen" >
<rosparam subst_value="true">
map_frame: "map"
tracking_frame: "base_link"
configuration_files_directories: [
"$(find google_cartographer)/configuration_files"
]
mapping_configuration_basename: "turtlebot.lua"
imu_topic: "/imu"
laser_scan_2d_topic: "/horizontal_2d_laser"
laser_min_range_m: 0.
laser_max_range_m: 30.
laser_missing_echo_ray_length_m: 5.
</rosparam>
</node>
</launch>

35
ros/google_cartographer/ogre_media/materials/glsl120/glsl120.program
View File

@ -1,35 +0,0 @@
/*
* Copyright 2016 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.
*/
fragment_program google_cartographer/glsl120/submap.frag glsl
{
source submap.frag
}
vertex_program google_cartographer/glsl120/submap.vert glsl
{
source submap.vert
}
fragment_program google_cartographer/glsl120/screen_blit.frag glsl
{
source screen_blit.frag
}
vertex_program google_cartographer/glsl120/screen_blit.vert glsl
{
source screen_blit.vert
}

29
ros/google_cartographer/ogre_media/materials/glsl120/screen_blit.frag
View File

@ -1,29 +0,0 @@
/*
* Copyright 2016 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.
*/
#version 120
varying vec2 out_texture_coordinate;
uniform sampler2D u_texture;
void main()
{
vec2 texture_color = texture2D(u_texture, out_texture_coordinate).rg;
float opacity = texture_color.g;
float value = texture_color.r;
gl_FragColor = vec4(value, value, value, opacity);
}

28
ros/google_cartographer/ogre_media/materials/glsl120/screen_blit.vert
View File

@ -1,28 +0,0 @@
/*
* Copyright 2016 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.
*/
#version 120
attribute vec4 vertex;
attribute vec4 uv0;
varying vec2 out_texture_coordinate;
void main()
{
out_texture_coordinate = vec2(uv0);
gl_Position = vertex;
}

31
ros/google_cartographer/ogre_media/materials/glsl120/submap.frag
View File

@ -1,31 +0,0 @@
/*
* Copyright 2016 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.
*/
#version 120
varying vec2 out_submap_texture_coordinate;
uniform sampler2D u_submap;
uniform float u_alpha;
void main()
{
vec2 texture_value = texture2D(u_submap, out_submap_texture_coordinate).rg;
float value = texture_value.r;
float alpha = texture_value.g;
float is_known = step(1e-3, alpha + value);
gl_FragColor = vec4(u_alpha * value, u_alpha * is_known, 0., u_alpha * alpha);
}

27
ros/google_cartographer/ogre_media/materials/glsl120/submap.vert
View File

@ -1,27 +0,0 @@
/*
* Copyright 2016 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.
*/
#version 120
attribute vec4 uv0;
varying vec2 out_submap_texture_coordinate;
void main()
{
out_submap_texture_coordinate = vec2(uv0);
gl_Position = ftransform();
}

31
ros/google_cartographer/ogre_media/materials/scripts/screen_blit.material
View File

@ -1,31 +0,0 @@
/*
* Copyright 2016 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.
*/
material google_cartographer/ScreenBlit
{
technique
{
pass
{
vertex_program_ref google_cartographer/glsl120/screen_blit.vert {}
fragment_program_ref google_cartographer/glsl120/screen_blit.frag
{
param_named u_texture int 0
}
}
}
}

32
ros/google_cartographer/ogre_media/materials/scripts/submap.material
View File

@ -1,32 +0,0 @@
/*
* Copyright 2016 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.
*/
material google_cartographer/Submap
{
technique
{
pass
{
vertex_program_ref google_cartographer/glsl120/submap.vert {}
fragment_program_ref google_cartographer/glsl120/submap.frag
{
param_named u_submap int 0
param_named u_alpha float 1.0
}
}
}
}

68
ros/google_cartographer/package.xml
View File

@ -1,68 +0,0 @@
<?xml version="1.0"?>
<!--
Copyright 2016 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.
-->
<package>
<name>google_cartographer</name>
<version>1.0.0</version>
<description>The google_cartographer package.</description>
<maintainer email="nobody@google.com">Google</maintainer>
<license>Apache 2.0</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>eigen_conversions</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>google_cartographer_msgs</build_depend>
<build_depend>libpcl-all-dev</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>pcl_conversions</build_depend>
<build_depend>qtbase5-dev</build_depend>
<build_depend>rosbag</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>roslib</build_depend>
<build_depend>rviz</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>tf2</build_depend>
<build_depend>tf2_eigen</build_depend>
<build_depend>visualization_msgs</build_depend>
<run_depend>eigen_conversions</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>google_cartographer_msgs</run_depend>
<run_depend>libpcl-all</run_depend>
<run_depend>libqt5-core</run_depend>
<run_depend>libqt5-gui</run_depend>
<run_depend>libqt5-widgets</run_depend>
<run_depend>message_runtime</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>pcl_conversions</run_depend>
<run_depend>rosbag</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>roslib</run_depend>
<run_depend>rviz</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>tf2_eigen</run_depend>
<run_depend>tf2</run_depend>
<run_depend>visualization_msgs</run_depend>
<export>
<rviz plugin="${prefix}/rviz_plugin_description.xml"/>
</export>
</package>

25
ros/google_cartographer/rviz_plugin_description.xml
View File

@ -1,25 +0,0 @@
<!--
Copyright 2016 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.
-->
<library path="lib/libcartographer_rviz_submaps_visualization">
<class name="Cartographer Submap visualization"
type="cartographer_ros::rviz::SubmapsDisplay"
base_class_type="rviz::Display">
<description>
Displays Google Cartographer's submaps as a unified map in RViz.
</description>
</class>
</library>

644
ros/google_cartographer/src/cartographer_node_main.cc
View File

@ -1,644 +0,0 @@
/*
* Copyright 2016 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 <cstring>
#include <queue>
#include <string>
#include <vector>
#include "Eigen/Core"
#include "cartographer/common/configuration_file_resolver.h"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/common/mutex.h"
#include "cartographer/common/port.h"
#include "cartographer/common/thread_pool.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/global_trajectory_builder_interface.h"
#include "cartographer/mapping/proto/submaps.pb.h"
#include "cartographer/mapping/sensor_collator.h"
#include "cartographer/mapping_2d/global_trajectory_builder.h"
#include "cartographer/mapping_2d/local_trajectory_builder.h"
#include "cartographer/mapping_2d/sparse_pose_graph.h"
#include "cartographer/mapping_3d/global_trajectory_builder.h"
#include "cartographer/mapping_3d/local_trajectory_builder.h"
#include "cartographer/mapping_3d/local_trajectory_builder_options.h"
#include "cartographer/mapping_3d/sparse_pose_graph.h"
#include "cartographer/sensor/laser.h"
#include "cartographer/sensor/proto/sensor.pb.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
#include "geometry_msgs/Transform.h"
#include "geometry_msgs/TransformStamped.h"
#include "glog/log_severity.h"
#include "glog/logging.h"
#include "google_cartographer_msgs/SubmapEntry.h"
#include "google_cartographer_msgs/SubmapList.h"
#include "google_cartographer_msgs/SubmapQuery.h"
#include "google_cartographer_msgs/TrajectorySubmapList.h"
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"
#include "pcl_conversions/pcl_conversions.h"
#include "ros/ros.h"
#include "ros/serialization.h"
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/LaserScan.h"
#include "sensor_msgs/MultiEchoLaserScan.h"
#include "sensor_msgs/PointCloud2.h"
#include "tf2_eigen/tf2_eigen.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_ros/transform_listener.h"
#include "msg_conversion.h"
#include "node_constants.h"
#include "time_conversion.h"
namespace cartographer_ros {
namespace {
using ::cartographer::transform::Rigid3d;
namespace proto = ::cartographer::sensor::proto;
// TODO(hrapp): Support multi trajectory mapping.
constexpr int64 kTrajectoryId = 0;
constexpr int kSubscriberQueueSize = 150;
constexpr int kSubmapPublishPeriodInUts = 300 * 10000ll; // 300 milliseconds
constexpr int kPosePublishPeriodInUts = 5 * 10000ll; // 5 milliseconds
Rigid3d ToRidig3d(const geometry_msgs::TransformStamped& transform) {
return Rigid3d(Eigen::Vector3d(transform.transform.translation.x,
transform.transform.translation.y,
transform.transform.translation.z),
Eigen::Quaterniond(transform.transform.rotation.w,
transform.transform.rotation.x,
transform.transform.rotation.y,
transform.transform.rotation.z));
}
// TODO(hrapp): move to msg_conversion.cc.
geometry_msgs::Transform ToGeometryMsgTransform(const Rigid3d& rigid) {
geometry_msgs::Transform transform;
transform.translation.x = rigid.translation().x();
transform.translation.y = rigid.translation().y();
transform.translation.z = rigid.translation().z();
transform.rotation.w = rigid.rotation().w();
transform.rotation.x = rigid.rotation().x();
transform.rotation.y = rigid.rotation().y();
transform.rotation.z = rigid.rotation().z();
return transform;
}
geometry_msgs::Pose ToGeometryMsgPose(const Rigid3d& rigid) {
geometry_msgs::Pose pose;
pose.position.x = rigid.translation().x();
pose.position.y = rigid.translation().y();
pose.position.z = rigid.translation().z();
pose.orientation.w = rigid.rotation().w();
pose.orientation.x = rigid.rotation().x();
pose.orientation.y = rigid.rotation().y();
pose.orientation.z = rigid.rotation().z();
return pose;
}
// This type is a logical union, i.e. only one proto is actually filled in. It
// is only used for time ordering sensor data before passing it on.
enum class SensorType { kImu, kLaserScan, kLaserFan3D };
struct SensorData {
SensorData(const string& init_frame_id, proto::Imu init_imu)
: type(SensorType::kImu), frame_id(init_frame_id), imu(init_imu) {}
SensorData(const string& init_frame_id, proto::LaserScan init_laser_scan)
: type(SensorType::kLaserScan),
frame_id(init_frame_id),
laser_scan(init_laser_scan) {}
SensorData(const string& init_frame_id, proto::LaserFan3D init_laser_fan_3d)
: type(SensorType::kLaserFan3D),
frame_id(init_frame_id),
laser_fan_3d(init_laser_fan_3d) {}
SensorType type;
string frame_id;
proto::Imu imu;
proto::LaserScan laser_scan;
proto::LaserFan3D laser_fan_3d;
};
// Node that listens to all the sensor data that we are interested in and wires
// it up to the SLAM.
class Node {
public:
Node();
Node(const Node&) = delete;
Node& operator=(const Node&) = delete;
void SpinForever();
void Initialize();
private:
void HandleSensorData(int64 timestamp,
std::unique_ptr<SensorData> sensor_data);
void ImuMessageCallback(const sensor_msgs::Imu::ConstPtr& msg);
void LaserScanMessageCallback(const sensor_msgs::LaserScan::ConstPtr& msg);
void MultiEchoLaserScanCallback(
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg);
void PointCloud2MessageCallback(
const string& topic, const sensor_msgs::PointCloud2::ConstPtr& msg);
void AddImu(int64 timestamp, const string& frame_id, const proto::Imu& imu);
void AddHorizontalLaserFan(int64 timestamp, const string& frame_id,
const proto::LaserScan& laser_scan);
void AddLaserFan3D(int64 timestamp, const string& frame_id,
const proto::LaserFan3D& laser_fan_3d);
template <typename T>
const T GetParamOrDie(const string& name);
// Returns true if a transform for 'frame_id' to 'tracking_frame_' exists at
// 'time'.
bool CanTransform(ros::Time time, const string& frame_id);
Rigid3d LookupToTrackingTransformOrDie(ros::Time time,
const string& frame_id);
bool HandleSubmapQuery(
::google_cartographer_msgs::SubmapQuery::Request& request,
::google_cartographer_msgs::SubmapQuery::Response& response);
void PublishSubmapList(int64 timestamp);
void PublishPose(int64 timestamp);
// TODO(hrapp): Pull out the common functionality between this and MapWriter
// into an open sourcable MapWriter.
::cartographer::mapping::SensorCollator<SensorData> sensor_collator_;
ros::NodeHandle node_handle_;
ros::Subscriber imu_subscriber_;
ros::Subscriber laser_2d_subscriber_;
std::vector<ros::Subscriber> laser_3d_subscribers_;
string tracking_frame_;
string map_frame_;
double laser_min_range_m_;
double laser_max_range_m_;
double laser_missing_echo_ray_length_m_;
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_;
tf2_ros::TransformBroadcaster tf_broadcaster_;
::cartographer::common::ThreadPool thread_pool_;
int64 last_pose_publish_timestamp_;
::cartographer::common::Mutex mutex_;
std::unique_ptr<::cartographer::mapping::GlobalTrajectoryBuilderInterface>
trajectory_builder_ GUARDED_BY(mutex_);
std::deque<::cartographer::mapping::TrajectoryNode::ConstantData>
constant_node_data_ GUARDED_BY(mutex_);
std::unique_ptr<::cartographer::mapping::SparsePoseGraph> sparse_pose_graph_;
::ros::Publisher submap_list_publisher_;
int64 last_submap_list_publish_timestamp_;
::ros::ServiceServer submap_query_server_;
};
Node::Node()
: node_handle_("~"),
tf_buffer_(ros::Duration(1000)),
tf_(tf_buffer_),
thread_pool_(10),
last_submap_list_publish_timestamp_(0),
last_pose_publish_timestamp_(0) {}
bool Node::CanTransform(ros::Time time, const string& frame_id) {
return tf_buffer_.canTransform(tracking_frame_, frame_id, time);
}
Rigid3d Node::LookupToTrackingTransformOrDie(ros::Time time,
const string& frame_id) {
geometry_msgs::TransformStamped stamped_transform;
try {
stamped_transform = tf_buffer_.lookupTransform(tracking_frame_, frame_id,
time, ros::Duration(1.));
} catch (tf2::TransformException& ex) {
LOG(FATAL) << "Timed out while waiting for transform: " << frame_id
<< " -> " << tracking_frame_ << ": " << ex.what();
}
return ToRidig3d(stamped_transform);
}
void Node::ImuMessageCallback(const sensor_msgs::Imu::ConstPtr& msg) {
auto sensor_data = ::cartographer::common::make_unique<SensorData>(
msg->header.frame_id, ToCartographer(*msg));
sensor_collator_.AddSensorData(
kTrajectoryId,
::cartographer::common::ToUniversal(FromRos(msg->header.stamp)),
imu_subscriber_.getTopic(), std::move(sensor_data));
}
void Node::AddImu(const int64 timestamp, const string& frame_id,
const proto::Imu& imu) {
const ::cartographer::common::Time time =
::cartographer::common::FromUniversal(timestamp);
if (!CanTransform(ToRos(time), frame_id)) {
LOG(WARNING) << "Cannot transform to " << frame_id;
return;
}
const Rigid3d sensor_to_tracking =
LookupToTrackingTransformOrDie(ToRos(time), frame_id);
CHECK(sensor_to_tracking.translation().norm() < 1e-5)
<< "The IMU is not colocated with the tracking frame. This makes it hard "
"and inprecise to transform its linear accelaration into the "
"tracking_frame and will decrease the quality of the SLAM.";
trajectory_builder_->AddImuData(
time, sensor_to_tracking.rotation() *
::cartographer::transform::ToEigen(imu.linear_acceleration()),
sensor_to_tracking.rotation() *
::cartographer::transform::ToEigen(imu.angular_velocity()));
}
void Node::LaserScanMessageCallback(
const sensor_msgs::LaserScan::ConstPtr& msg) {
auto sensor_data = ::cartographer::common::make_unique<SensorData>(
msg->header.frame_id, ToCartographer(*msg));
sensor_collator_.AddSensorData(
kTrajectoryId,
::cartographer::common::ToUniversal(FromRos(msg->header.stamp)),
laser_2d_subscriber_.getTopic(), std::move(sensor_data));
}
void Node::AddHorizontalLaserFan(const int64 timestamp, const string& frame_id,
const proto::LaserScan& laser_scan) {
const ::cartographer::common::Time time =
::cartographer::common::FromUniversal(timestamp);
if (!CanTransform(ToRos(time), frame_id)) {
LOG(WARNING) << "Cannot transform to " << frame_id;
return;
}
const Rigid3d sensor_to_tracking =
LookupToTrackingTransformOrDie(ToRos(time), frame_id);
// TODO(hrapp): Make things configurable? Through Lua? Through ROS params?
const ::cartographer::sensor::LaserFan laser_fan =
::cartographer::sensor::ToLaserFan(laser_scan, laser_min_range_m_,
laser_max_range_m_,
laser_missing_echo_ray_length_m_);
const auto laser_fan_3d = ::cartographer::sensor::TransformLaserFan3D(
::cartographer::sensor::ToLaserFan3D(laser_fan),
sensor_to_tracking.cast<float>());
trajectory_builder_->AddHorizontalLaserFan(time, laser_fan_3d);
}
void Node::MultiEchoLaserScanCallback(
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
// TODO(hrapp): Do something useful.
LOG(INFO) << "LaserScan message: " << msg->header.stamp;
}
void Node::PointCloud2MessageCallback(
const string& topic, const sensor_msgs::PointCloud2::ConstPtr& msg) {
pcl::PointCloud<pcl::PointXYZ> pcl_points;
pcl::fromROSMsg(*msg, pcl_points);
auto sensor_data = ::cartographer::common::make_unique<SensorData>(
msg->header.frame_id, ToCartographer(pcl_points));
sensor_collator_.AddSensorData(
kTrajectoryId,
::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), topic,
std::move(sensor_data));
}
void Node::AddLaserFan3D(const int64 timestamp, const string& frame_id,
const proto::LaserFan3D& laser_fan_3d) {
const ::cartographer::common::Time time =
::cartographer::common::FromUniversal(timestamp);
if (!CanTransform(ToRos(time), frame_id)) {
LOG(WARNING) << "Cannot transform to " << frame_id;
return;
}
const Rigid3d sensor_to_tracking =
LookupToTrackingTransformOrDie(ToRos(time), frame_id);
trajectory_builder_->AddLaserFan3D(
time, ::cartographer::sensor::TransformLaserFan3D(
::cartographer::sensor::FromProto(laser_fan_3d),
sensor_to_tracking.cast<float>()));
}
template <typename T>
const T Node::GetParamOrDie(const string& name) {
CHECK(node_handle_.hasParam(name)) << "Required parameter '" << name
<< "' is unset.";
T value;
node_handle_.getParam(name, value);
return value;
}
void Node::Initialize() {
tracking_frame_ = GetParamOrDie<string>("tracking_frame");
map_frame_ = GetParamOrDie<string>("map_frame");
laser_min_range_m_ = GetParamOrDie<double>("laser_min_range_m");
laser_max_range_m_ = GetParamOrDie<double>("laser_max_range_m");
laser_missing_echo_ray_length_m_ =
GetParamOrDie<double>("laser_missing_echo_ray_length_m");
// Subscribe to the IMU.
const string imu_topic = GetParamOrDie<string>("imu_topic");
imu_subscriber_ = node_handle_.subscribe(imu_topic, kSubscriberQueueSize,
&Node::ImuMessageCallback, this);
std::unordered_set<string> expected_sensor_identifiers;
expected_sensor_identifiers.insert(imu_topic);
// Subscribe to exactly one laser.
const bool has_laser_scan_2d = node_handle_.hasParam("laser_scan_2d_topic");
const bool has_multi_echo_laser_scan_2d =
node_handle_.hasParam("multi_echo_laser_scan_2d_topic");
const bool has_laser_scan_3d = node_handle_.hasParam("laser_scan_3d_topics");
CHECK(has_laser_scan_2d + has_multi_echo_laser_scan_2d + has_laser_scan_3d ==
1)
<< "Parameters 'laser_scan_2d_topic', 'multi_echo_laser_scan_2d_topic' "
"and 'laser_scan_3d_topics' are mutually exclusive, but one is "
"required.";
if (has_laser_scan_2d) {
const string topic = GetParamOrDie<string>("laser_scan_2d_topic");
laser_2d_subscriber_ = node_handle_.subscribe(
topic, kSubscriberQueueSize, &Node::LaserScanMessageCallback, this);
expected_sensor_identifiers.insert(topic);
}
if (has_multi_echo_laser_scan_2d) {
const string topic =
GetParamOrDie<string>("multi_echo_laser_scan_2d_topic");
laser_2d_subscriber_ = node_handle_.subscribe(
topic, kSubscriberQueueSize, &Node::MultiEchoLaserScanCallback, this);
expected_sensor_identifiers.insert(topic);
}
auto file_resolver = ::cartographer::common::make_unique<
::cartographer::common::ConfigurationFileResolver>(
GetParamOrDie<std::vector<string>>("configuration_files_directories"));
const string code = file_resolver->GetFileContentOrDie(
GetParamOrDie<string>("mapping_configuration_basename"));
::cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
code, std::move(file_resolver), nullptr);
if (has_laser_scan_2d || has_multi_echo_laser_scan_2d) {
auto sparse_pose_graph_2d = ::cartographer::common::make_unique<
::cartographer::mapping_2d::SparsePoseGraph>(
::cartographer::mapping::CreateSparsePoseGraphOptions(
lua_parameter_dictionary.GetDictionary("sparse_pose_graph").get()),
&thread_pool_, &constant_node_data_);
trajectory_builder_ = ::cartographer::common::make_unique<
::cartographer::mapping_2d::GlobalTrajectoryBuilder>(
::cartographer::mapping_2d::CreateLocalTrajectoryBuilderOptions(
lua_parameter_dictionary.GetDictionary("trajectory_builder").get()),
sparse_pose_graph_2d.get());
sparse_pose_graph_ = std::move(sparse_pose_graph_2d);
}
if (has_laser_scan_3d) {
const auto topics =
GetParamOrDie<std::vector<string>>("laser_scan_3d_topics");
for (const auto& topic : topics) {
laser_3d_subscribers_.push_back(node_handle_.subscribe(
topic, kSubscriberQueueSize,
boost::function<void(const sensor_msgs::PointCloud2::ConstPtr&)>(
[this, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) {
PointCloud2MessageCallback(topic, msg);
})));
expected_sensor_identifiers.insert(topic);
}
auto sparse_pose_graph_3d = ::cartographer::common::make_unique<
::cartographer::mapping_3d::SparsePoseGraph>(
::cartographer::mapping::CreateSparsePoseGraphOptions(
lua_parameter_dictionary.GetDictionary("sparse_pose_graph").get()),
&thread_pool_, &constant_node_data_);
trajectory_builder_ = ::cartographer::common::make_unique<
::cartographer::mapping_3d::GlobalTrajectoryBuilder>(
::cartographer::mapping_3d::CreateLocalTrajectoryBuilderOptions(
lua_parameter_dictionary.GetDictionary("trajectory_builder").get()),
sparse_pose_graph_3d.get());
sparse_pose_graph_ = std::move(sparse_pose_graph_3d);
}
CHECK(sparse_pose_graph_ != nullptr);
// TODO(hrapp): Add odometry subscribers here.
sensor_collator_.AddTrajectory(
kTrajectoryId, expected_sensor_identifiers,
[this](const int64 timestamp, std::unique_ptr<SensorData> sensor_data) {
HandleSensorData(timestamp, std::move(sensor_data));
});
submap_list_publisher_ =
node_handle_.advertise<::google_cartographer_msgs::SubmapList>(
kSubmapListTopic, 10);
submap_query_server_ = node_handle_.advertiseService(
kSubmapQueryServiceName, &Node::HandleSubmapQuery, this);
}
bool Node::HandleSubmapQuery(
::google_cartographer_msgs::SubmapQuery::Request& request,
::google_cartographer_msgs::SubmapQuery::Response& response) {
if (request.trajectory_id != 0) {
return false;
}
::cartographer::common::MutexLocker lock(&mutex_);
// TODO(hrapp): return error messages and extract common code from MapBuilder.
::cartographer::mapping::Submaps* submaps = trajectory_builder_->submaps();
if (request.submap_id < 0 || request.submap_id >= submaps->size()) {
return false;
}
::cartographer::mapping::proto::SubmapQuery::Response response_proto;
response_proto.set_submap_id(request.submap_id);
response_proto.set_submap_version(
submaps->Get(request.submap_id)->end_laser_fan_index);
const std::vector<::cartographer::transform::Rigid3d> submap_transforms =
sparse_pose_graph_->GetSubmapTransforms(*submaps);
submaps->SubmapToProto(request.submap_id,
sparse_pose_graph_->GetTrajectoryNodes(),
submap_transforms[request.submap_id], &response_proto);
response.submap_id = response_proto.submap_id();
response.submap_version = response_proto.submap_version();
response.cells.insert(response.cells.begin(), response_proto.cells().begin(),
response_proto.cells().end());
response.width = response_proto.width();
response.height = response_proto.height();
response.resolution = response_proto.resolution();
auto pose = ::cartographer::transform::ToRigid3(response_proto.slice_pose());
response.slice_pose.position.x =
response_proto.slice_pose().translation().x();
response.slice_pose.position.y =
response_proto.slice_pose().translation().y();
response.slice_pose.position.z =
response_proto.slice_pose().translation().z();
response.slice_pose.orientation.x =
response_proto.slice_pose().rotation().x();
response.slice_pose.orientation.y =
response_proto.slice_pose().rotation().y();
response.slice_pose.orientation.z =
response_proto.slice_pose().rotation().z();
response.slice_pose.orientation.w =
response_proto.slice_pose().rotation().w();
return true;
}
void Node::PublishSubmapList(int64 timestamp) {
::cartographer::common::MutexLocker lock(&mutex_);
const ::cartographer::mapping::Submaps* submaps =
trajectory_builder_->submaps();
const std::vector<::cartographer::transform::Rigid3d> submap_transforms =
sparse_pose_graph_->GetSubmapTransforms(*submaps);
CHECK_EQ(submap_transforms.size(), submaps->size());
::google_cartographer_msgs::TrajectorySubmapList ros_trajectory;
for (int i = 0; i != submaps->size(); ++i) {
::google_cartographer_msgs::SubmapEntry ros_submap;
ros_submap.submap_version = submaps->Get(i)->end_laser_fan_index;
ros_submap.pose = ToGeometryMsgPose(submap_transforms[i]);
ros_trajectory.submap.push_back(ros_submap);
}
::google_cartographer_msgs::SubmapList ros_submap_list;
ros_submap_list.trajectory.push_back(ros_trajectory);
submap_list_publisher_.publish(ros_submap_list);
last_submap_list_publish_timestamp_ = timestamp;
}
void Node::PublishPose(int64 timestamp) {
::cartographer::common::MutexLocker lock(&mutex_);
const ::cartographer::mapping::Submaps* submaps =
trajectory_builder_->submaps();
const ::cartographer::transform::Rigid3d odometry_to_map =
sparse_pose_graph_->GetOdometryToMapTransform(*submaps);
const auto& pose_estimate = trajectory_builder_->pose_estimate();
const ::cartographer::transform::Rigid3d pose =
odometry_to_map * pose_estimate.pose;
const ::cartographer::common::Time time =
::cartographer::common::FromUniversal(timestamp);
geometry_msgs::TransformStamped stamped_transform;
stamped_transform.header.stamp = ToRos(time);
stamped_transform.header.frame_id = map_frame_;
stamped_transform.child_frame_id = tracking_frame_;
stamped_transform.transform = ToGeometryMsgTransform(pose);
tf_broadcaster_.sendTransform(stamped_transform);
last_pose_publish_timestamp_ = timestamp;
}
void Node::HandleSensorData(const int64 timestamp,
std::unique_ptr<SensorData> sensor_data) {
if (last_submap_list_publish_timestamp_ + kSubmapPublishPeriodInUts <
timestamp) {
PublishSubmapList(timestamp);
}
if (last_pose_publish_timestamp_ + kPosePublishPeriodInUts < timestamp) {
PublishPose(timestamp);
}
switch (sensor_data->type) {
case SensorType::kImu:
AddImu(timestamp, sensor_data->frame_id, sensor_data->imu);
return;
case SensorType::kLaserScan:
AddHorizontalLaserFan(timestamp, sensor_data->frame_id,
sensor_data->laser_scan);
return;
case SensorType::kLaserFan3D:
AddLaserFan3D(timestamp, sensor_data->frame_id,
sensor_data->laser_fan_3d);
return;
}
LOG(FATAL);
}
void Node::SpinForever() { ros::spin(); }
void Run() {
Node node;
node.Initialize();
node.SpinForever();
}
const char* GetBasename(const char* filepath) {
const char* base = strrchr(filepath, '/');
return base ? (base + 1) : filepath;
}
// Makes Google logging use ROS logging for output while an instance of this
// class exists.
class ScopedRosLogSink : public google::LogSink {
public:
ScopedRosLogSink() : will_die_(false) { AddLogSink(this); }
~ScopedRosLogSink() override { RemoveLogSink(this); }
void send(google::LogSeverity severity, const char* filename,
const char* base_filename, int line, const struct ::tm* tm_time,
const char* message, size_t message_len) override {
const std::string message_string = google::LogSink::ToString(
severity, GetBasename(filename), line, tm_time, message, message_len);
switch (severity) {
case google::GLOG_INFO:
ROS_INFO_STREAM(message_string);
break;
case google::GLOG_WARNING:
ROS_WARN_STREAM(message_string);
break;
case google::GLOG_ERROR:
ROS_ERROR_STREAM(message_string);
break;
case google::GLOG_FATAL:
ROS_FATAL_STREAM(message_string);
will_die_ = true;
break;
}
}
void WaitTillSent() override {
if (will_die_) {
// Arbirarily give ROS some time to actually publish our message.
std::this_thread::sleep_for(
::cartographer::common::FromMilliseconds(1000));
}
}
private:
bool will_die_;
};
} // namespace
} // namespace cartographer_ros
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
ros::init(argc, argv, "cartographer_node");
ros::start();
::cartographer_ros::ScopedRosLogSink ros_log_sink;
::cartographer_ros::Run();
ros::shutdown();
return 0;
}

269
ros/google_cartographer/src/drawable_submap.cc
View File

@ -1,269 +0,0 @@
/*
* Copyright 2016 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 "drawable_submap.h"
#include <OgreGpuProgramParams.h>
#include <OgreImage.h>
#include <cartographer/common/port.h>
#include <eigen_conversions/eigen_msg.h>
#include <google_cartographer_msgs/SubmapQuery.h>
#include <ros/ros.h>
#include <rviz/display_context.h>
#include <rviz/frame_manager.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <chrono>
#include <future>
#include <sstream>
#include <string>
namespace cartographer_ros {
namespace rviz {
namespace {
constexpr std::chrono::milliseconds kMinQueryDelayInMs(250);
constexpr char kMapFrame[] = "/map";
constexpr char kSubmapTexturePrefix[] = "SubmapTexture";
constexpr char kManualObjectPrefix[] = "ManualObjectSubmap";
constexpr char kSubmapMaterialPrefix[] = "SubmapMaterial";
constexpr char kSubmapSourceMaterialName[] = "google_cartographer/Submap";
// Distance before which the submap will be shown at full opacity, and distance
// over which the submap will then fade out.
constexpr double kFadeOutStartDistanceInMeters = 1.;
constexpr double kFadeOutDistanceInMeters = 2.;
constexpr float kAlphaUpdateThreshold = 0.2f;
std::string GetSubmapIdentifier(const int trajectory_id, const int submap_id) {
return std::to_string(trajectory_id) + "-" + std::to_string(submap_id);
}
} // namespace
DrawableSubmap::DrawableSubmap(const int submap_id, const int trajectory_id,
::rviz::FrameManager* const frame_manager,
Ogre::SceneManager* const scene_manager)
: frame_manager_(frame_manager),
scene_node_(scene_manager->getRootSceneNode()->createChildSceneNode()),
manual_object_(scene_manager->createManualObject(
kManualObjectPrefix + GetSubmapIdentifier(trajectory_id, submap_id))),
submap_id_(submap_id),
trajectory_id_(trajectory_id),
last_query_timestamp_(0),
query_in_progress_(false),
texture_count_(0),
current_alpha_(0) {
material_ = Ogre::MaterialManager::getSingleton().getByName(
kSubmapSourceMaterialName);
material_ = material_->clone(kSubmapMaterialPrefix +
GetSubmapIdentifier(trajectory_id_, submap_id));
material_->setReceiveShadows(false);
material_->getTechnique(0)->setLightingEnabled(false);
material_->setDepthWriteEnabled(false);
scene_node_->attachObject(manual_object_);
connect(this, SIGNAL(RequestSucceeded()), this, SLOT(OnRequestSuccess()));
}
DrawableSubmap::~DrawableSubmap() {
Ogre::MaterialManager::getSingleton().remove(material_->getHandle());
if (!texture_.isNull()) {
Ogre::TextureManager::getSingleton().remove(texture_->getHandle());
texture_.setNull();
}
}
bool DrawableSubmap::Update(
const ::google_cartographer_msgs::SubmapEntry& metadata,
ros::ServiceClient* const client) {
::cartographer::common::MutexLocker locker(&mutex_);
tf::poseMsgToEigen(metadata.pose, submap_pose_);
const bool newer_version_available = version_ < metadata.submap_version;
const std::chrono::milliseconds now =
std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::system_clock::now().time_since_epoch());
const bool recently_queried =
last_query_timestamp_ + kMinQueryDelayInMs > now;
if (!newer_version_available || recently_queried || query_in_progress_) {
return false;
}
query_in_progress_ = true;
last_query_timestamp_ = now;
last_query_slice_height_ = metadata.pose.position.z;
QuerySubmap(submap_id_, trajectory_id_, client);
return true;
}
void DrawableSubmap::QuerySubmap(const int submap_id, const int trajectory_id,
ros::ServiceClient* const client) {
rpc_request_future_ = std::async(
std::launch::async, [this, submap_id, trajectory_id, client]() {
::google_cartographer_msgs::SubmapQuery srv;
srv.request.submap_id = submap_id;
srv.request.trajectory_id = trajectory_id;
if (client->call(srv)) {
response_.reset(new ::google_cartographer_msgs::SubmapQuery::Response(
srv.response));
Q_EMIT RequestSucceeded();
} else {
OnRequestFailure();
}
});
}
void DrawableSubmap::OnRequestSuccess() {
::cartographer::common::MutexLocker locker(&mutex_);
version_ = response_->submap_version;
resolution_ = response_->resolution;
width_ = response_->width;
height_ = response_->height;
slice_height_ = last_query_slice_height_;
std::string compressed_cells(response_->cells.begin(),
response_->cells.end());
cells_.clear();
::cartographer::common::FastGunzipString(compressed_cells, &cells_);
Eigen::Affine3d slice_pose;
tf::poseMsgToEigen(response_->slice_pose, slice_pose);
tf::poseEigenToMsg(submap_pose_ * slice_pose, transformed_pose_);
response_.reset();
query_in_progress_ = false;
UpdateSceneNode();
}
void DrawableSubmap::OnRequestFailure() {
::cartographer::common::MutexLocker locker(&mutex_);
query_in_progress_ = false;
}
bool DrawableSubmap::QueryInProgress() {
::cartographer::common::MutexLocker locker(&mutex_);
return query_in_progress_;
}
void DrawableSubmap::UpdateSceneNode() {
// The call to Ogre's loadRawData below does not work with an RG texture,
// therefore we create an RGB one whose blue channel is always 0.
std::vector<char> rgb;
for (int i = 0; i < height_; ++i) {
for (int j = 0; j < width_; ++j) {
auto r = cells_[(i * width_ + j) * 2];
auto g = cells_[(i * width_ + j) * 2 + 1];
rgb.push_back(r);
rgb.push_back(g);
rgb.push_back(0.);
}
}
manual_object_->clear();
const float metric_width = resolution_ * width_;
const float metric_height = resolution_ * height_;
manual_object_->begin(material_->getName(),
Ogre::RenderOperation::OT_TRIANGLE_LIST);
{
{
// Bottom left
manual_object_->position(-metric_height, 0.0f, 0.0f);
manual_object_->textureCoord(0.0f, 1.0f);
manual_object_->normal(0.0f, 0.0f, 1.0f);
// Bottom right
manual_object_->position(-metric_height, -metric_width, 0.0f);
manual_object_->textureCoord(1.0f, 1.0f);
manual_object_->normal(0.0f, 0.0f, 1.0f);
// Top left
manual_object_->position(0.0f, 0.0f, 0.0f);
manual_object_->textureCoord(0.0f, 0.0f);
manual_object_->normal(0.0f, 0.0f, 1.0f);
// Top left
manual_object_->position(0.0f, 0.0f, 0.0f);
manual_object_->textureCoord(0.0f, 0.0f);
manual_object_->normal(0.0f, 0.0f, 1.0f);
// Bottom right
manual_object_->position(-metric_height, -metric_width, 0.0f);
manual_object_->textureCoord(1.0f, 1.0f);
manual_object_->normal(0.0f, 0.0f, 1.0f);
// Top right
manual_object_->position(0.0f, -metric_width, 0.0f);
manual_object_->textureCoord(1.0f, 0.0f);
manual_object_->normal(0.0f, 0.0f, 1.0f);
}
}
manual_object_->end();
Ogre::DataStreamPtr pixel_stream;
pixel_stream.bind(new Ogre::MemoryDataStream(rgb.data(), rgb.size()));
if (!texture_.isNull()) {
Ogre::TextureManager::getSingleton().remove(texture_->getHandle());
texture_.setNull();
}
const std::string texture_name =
kSubmapTexturePrefix + GetSubmapIdentifier(trajectory_id_, submap_id_) +
std::to_string(texture_count_);
texture_ = Ogre::TextureManager::getSingleton().loadRawData(
texture_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
pixel_stream, width_, height_, Ogre::PF_BYTE_RGB, Ogre::TEX_TYPE_2D, 0);
++texture_count_;
Ogre::Pass* const pass = material_->getTechnique(0)->getPass(0);
pass->setSceneBlending(Ogre::SBF_ONE, Ogre::SBF_ONE_MINUS_SOURCE_ALPHA);
Ogre::TextureUnitState* const texture_unit =
pass->getNumTextureUnitStates() > 0 ? pass->getTextureUnitState(0)
: pass->createTextureUnitState();
texture_unit->setTextureName(texture_->getName());
texture_unit->setTextureFiltering(Ogre::TFO_NONE);
}
void DrawableSubmap::Transform(const ros::Time& ros_time) {
Ogre::Vector3 position;
Ogre::Quaternion orientation;
frame_manager_->transform(kMapFrame, ros_time, transformed_pose_, position,
orientation);
scene_node_->setPosition(position);
scene_node_->setOrientation(orientation);
}
void DrawableSubmap::SetAlpha(const double current_tracking_z) {
const double distance_z = std::abs(slice_height_ - current_tracking_z);
const double fade_distance =
std::max(distance_z - kFadeOutStartDistanceInMeters, 0.);
const float alpha =
(float)std::max(0., 1. - fade_distance / kFadeOutDistanceInMeters);
const Ogre::GpuProgramParametersSharedPtr parameters =
material_->getTechnique(0)->getPass(0)->getFragmentProgramParameters();
parameters->setNamedConstant("u_alpha", UpdateAlpha(alpha));
}
float DrawableSubmap::UpdateAlpha(const float target_alpha) {
if (std::abs(target_alpha - current_alpha_) > kAlphaUpdateThreshold ||
target_alpha == 0.f || target_alpha == 1.f) {
current_alpha_ = target_alpha;
}
return current_alpha_;
}
} // namespace rviz
} // namespace cartographer_ros

117
ros/google_cartographer/src/drawable_submap.h
View File

@ -1,117 +0,0 @@
/*
* Copyright 2016 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_GOOGLE_CARTOGRAPHER_SRC_DRAWABLE_SUBMAP_H_
#define CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_DRAWABLE_SUBMAP_H_
#include <OgreManualObject.h>
#include <OgreMaterial.h>
#include <OgreSceneManager.h>
#include <OgreSceneNode.h>
#include <OgreTexture.h>
#include <cartographer/common/mutex.h>
#include <google_cartographer_msgs/SubmapEntry.h>
#include <google_cartographer_msgs/SubmapQuery.h>
#include <ros/ros.h>
#include <rviz/display_context.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <future>
namespace cartographer_ros {
namespace rviz {
// Contains all the information needed to render a submap onto the final
// texture representing the whole map.
class DrawableSubmap : public QObject {
Q_OBJECT
public:
// Each submap is identified by a 'trajectory_id' plus a 'submap_id'. The
// 'frame_manager' is needed to transform the scene node before rendering
// onto the offscreen texture. 'scene_manager' is the Ogre scene manager which
// contains all submaps.
DrawableSubmap(int submap_id, int trajectory_id,
::rviz::FrameManager* frame_manager,
Ogre::SceneManager* scene_manager);
~DrawableSubmap() override;
DrawableSubmap(const DrawableSubmap&) = delete;
DrawableSubmap& operator=(const DrawableSubmap&) = delete;
// 'submap_entry' contains metadata which is used to find out whether this
// 'DrawableSubmap' should update itself. If an update is needed, it will send
// an RPC using 'client' to request the new data for the submap.
bool Update(const ::google_cartographer_msgs::SubmapEntry& submap_entry,
ros::ServiceClient* client);
// Returns whether an RPC is in progress.
bool QueryInProgress();
// Transforms the scene node for this submap before being rendered onto a
// texture.
void Transform(const ros::Time& ros_time);
// Sets the alpha of the submap taking into account its slice height and the
// 'current_tracking_z'.
void SetAlpha(double current_tracking_z);
Q_SIGNALS:
// RPC request succeeded.
void RequestSucceeded();
private Q_SLOTS:
// Callback when an rpc request succeeded.
void OnRequestSuccess();
private:
void QuerySubmap(int submap_id, int trajectory_id,
ros::ServiceClient* client);
void OnRequestFailure();
void UpdateSceneNode();
float UpdateAlpha(float target_alpha);
const int submap_id_;
const int trajectory_id_;
::cartographer::common::Mutex mutex_;
::rviz::FrameManager* frame_manager_;
Ogre::SceneNode* const scene_node_;
Ogre::ManualObject* manual_object_;
Ogre::TexturePtr texture_;
Ogre::MaterialPtr material_;
Eigen::Affine3d submap_pose_ GUARDED_BY(mutex_);
geometry_msgs::Pose transformed_pose_ GUARDED_BY(mutex_);
std::chrono::milliseconds last_query_timestamp_ GUARDED_BY(mutex_);
bool query_in_progress_ GUARDED_BY(mutex_);
float resolution_ GUARDED_BY(mutex_);
int width_ GUARDED_BY(mutex_);
int height_ GUARDED_BY(mutex_);
int version_ GUARDED_BY(mutex_);
double slice_height_ GUARDED_BY(mutex_);
double last_query_slice_height_ GUARDED_BY(mutex_);
std::future<void> rpc_request_future_;
std::string cells_ GUARDED_BY(mutex_);
std::unique_ptr<::google_cartographer_msgs::SubmapQuery::Response> response_
GUARDED_BY(mutex_);
int texture_count_;
float current_alpha_;
};
} // namespace rviz
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_DRAWABLE_SUBMAP_H_

261
ros/google_cartographer/src/msg_conversion.cc
View File

@ -1,261 +0,0 @@
/*
* Copyright 2016 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 "msg_conversion.h"
#include "cartographer/common/port.h"
#include "cartographer/common/time.h"
#include "cartographer/sensor/proto/sensor.pb.h"
#include "cartographer/transform/proto/transform.pb.h"
#include "cartographer/transform/transform.h"
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/Quaternion.h"
#include "geometry_msgs/Transform.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Vector3.h"
#include "glog/logging.h"
#include "ros/ros.h"
#include "ros/serialization.h"
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/LaserScan.h"
#include "sensor_msgs/MultiEchoLaserScan.h"
#include "sensor_msgs/PointCloud2.h"
#include "time_conversion.h"
namespace cartographer_ros {
namespace {
// The ros::sensor_msgs::PointCloud2 binary data contains 4 floats for each
// point. The last one must be this value or RViz is not showing the point cloud
// properly.
constexpr float kPointCloudComponentFourMagic = 1.;
void ToMessage(const ::cartographer::transform::proto::Vector3d& proto,
geometry_msgs::Vector3* vector) {
vector->x = proto.x();
vector->y = proto.y();
vector->z = proto.z();
}
void ToMessage(const ::cartographer::transform::proto::Quaterniond& proto,
geometry_msgs::Quaternion* quaternion) {
quaternion->w = proto.w();
quaternion->x = proto.x();
quaternion->y = proto.y();
quaternion->z = proto.z();
}
} // namespace
sensor_msgs::MultiEchoLaserScan ToMultiEchoLaserScanMessage(
const int64 timestamp, const string& frame_id,
const ::cartographer::sensor::proto::LaserScan& laser_scan) {
sensor_msgs::MultiEchoLaserScan msg;
msg.header.stamp = ToRos(::cartographer::common::FromUniversal(timestamp));
msg.header.frame_id = frame_id;
msg.angle_min = laser_scan.angle_min();
msg.angle_max = laser_scan.angle_max();
msg.angle_increment = laser_scan.angle_increment();
msg.time_increment = laser_scan.time_increment();
msg.scan_time = laser_scan.scan_time();
msg.range_min = laser_scan.range_min();
msg.range_max = laser_scan.range_max();
for (const auto& echoes : laser_scan.range()) {
msg.ranges.emplace_back();
std::copy(echoes.value().begin(), echoes.value().end(),
std::back_inserter(msg.ranges.back().echoes));
}
for (const auto& echoes : laser_scan.intensity()) {
msg.intensities.emplace_back();
std::copy(echoes.value().begin(), echoes.value().end(),
std::back_inserter(msg.intensities.back().echoes));
}
return msg;
}
sensor_msgs::Imu ToImuMessage(const int64 timestamp, const string& frame_id,
const ::cartographer::sensor::proto::Imu& imu) {
sensor_msgs::Imu message;
message.header.stamp =
ToRos(::cartographer::common::FromUniversal(timestamp));
message.header.frame_id = frame_id;
ToMessage(imu.orientation(), &message.orientation);
message.orientation_covariance = {{0., 0., 0., 0., 0., 0., 0., 0., 0.}};
ToMessage(imu.angular_velocity(), &message.angular_velocity);
message.angular_velocity_covariance = {{0., 0., 0., 0., 0., 0., 0., 0., 0.}};
ToMessage(imu.linear_acceleration(), &message.linear_acceleration);
message.linear_acceleration_covariance = {
{0., 0., 0., 0., 0., 0., 0., 0., 0.}};
return message;
}
sensor_msgs::LaserScan ToLaserScan(
const int64 timestamp, const string& frame_id,
const ::cartographer::sensor::proto::LaserScan& laser_scan) {
sensor_msgs::LaserScan msg;
msg.header.stamp = ToRos(::cartographer::common::FromUniversal(timestamp));
msg.header.frame_id = frame_id;
msg.angle_min = laser_scan.angle_min();
msg.angle_max = laser_scan.angle_max();
msg.angle_increment = laser_scan.angle_increment();
msg.time_increment = laser_scan.time_increment();
msg.scan_time = laser_scan.scan_time();
msg.range_min = laser_scan.range_min();
msg.range_max = laser_scan.range_max();
for (const auto& echoes : laser_scan.range()) {
if (echoes.value_size() > 0) {
msg.ranges.push_back(echoes.value(0));
} else {
msg.ranges.push_back(0.);
}
}
bool has_intensities = false;
for (const auto& echoes : laser_scan.intensity()) {
if (echoes.value_size() > 0) {
msg.intensities.push_back(echoes.value(0));
has_intensities = true;
} else {
msg.intensities.push_back(0);
}
}
if (!has_intensities) {
msg.intensities.clear();
}
return msg;
}
sensor_msgs::PointCloud2 ToPointCloud2Message(
const int64 timestamp, const string& frame_id,
const ::cartographer::sensor::proto::LaserFan3D& laser_scan_3d) {
CHECK(::cartographer::transform::ToEigen(laser_scan_3d.origin()).norm() == 0)
<< "Trying to convert a laser_scan_3d that is not at the origin.";
sensor_msgs::PointCloud2 msg;
msg.header.stamp = ToRos(::cartographer::common::FromUniversal(timestamp));
msg.header.frame_id = frame_id;
const auto& point_cloud = laser_scan_3d.point_cloud();
CHECK_EQ(point_cloud.x_size(), point_cloud.y_size());
CHECK_EQ(point_cloud.x_size(), point_cloud.z_size());
const auto num_points = point_cloud.x_size();
msg.height = 1;
msg.width = num_points;
msg.fields.resize(3);
msg.fields[0].name = "x";
msg.fields[0].offset = 0;
msg.fields[0].datatype = 7;
msg.fields[0].count = 1;
msg.fields[1].name = "y";
msg.fields[1].offset = 4;
msg.fields[1].datatype = 7;
msg.fields[1].count = 1;
msg.fields[2].name = "z";
msg.fields[2].offset = 8;
msg.fields[2].datatype = 7;
msg.fields[2].count = 1;
msg.is_bigendian = false;
msg.point_step = 16;
msg.row_step = 16 * msg.width;
msg.is_dense = true;
msg.data.resize(16 * num_points);
::ros::serialization::OStream stream(msg.data.data(), msg.data.size());
for (int i = 0; i < num_points; ++i) {
stream.next(point_cloud.x(i));
stream.next(point_cloud.y(i));
stream.next(point_cloud.z(i));
stream.next(kPointCloudComponentFourMagic);
}
return msg;
}
::cartographer::sensor::proto::Imu ToCartographer(const sensor_msgs::Imu& msg) {
::cartographer::sensor::proto::Imu proto;
if (msg.orientation_covariance[0] != -1) {
auto* orientation = proto.mutable_orientation();
orientation->set_x(msg.orientation.x);
orientation->set_y(msg.orientation.y);
orientation->set_z(msg.orientation.z);
orientation->set_w(msg.orientation.w);
}
if (msg.angular_velocity_covariance[0] != -1) {
auto* angular_velocity = proto.mutable_angular_velocity();
angular_velocity->set_x(msg.angular_velocity.x);
angular_velocity->set_y(msg.angular_velocity.y);
angular_velocity->set_z(msg.angular_velocity.z);
}
if (msg.linear_acceleration_covariance[0] != -1) {
auto* linear_acceleration = proto.mutable_linear_acceleration();
linear_acceleration->set_x(msg.linear_acceleration.x);
linear_acceleration->set_y(msg.linear_acceleration.y);
linear_acceleration->set_z(msg.linear_acceleration.z);
}
return proto;
}
::cartographer::sensor::proto::LaserScan ToCartographer(
const sensor_msgs::LaserScan& msg) {
::cartographer::sensor::proto::LaserScan proto;
proto.set_angle_min(msg.angle_min);
proto.set_angle_max(msg.angle_max);
proto.set_angle_increment(msg.angle_increment);
proto.set_time_increment(msg.time_increment);
proto.set_scan_time(msg.scan_time);
proto.set_range_min(msg.range_min);
proto.set_range_max(msg.range_max);
for (const auto& range : msg.ranges) {
proto.add_range()->mutable_value()->Add(range);
}
for (const auto& intensity : msg.intensities) {
proto.add_intensity()->mutable_value()->Add(intensity);
}
return proto;
}
::cartographer::sensor::proto::LaserFan3D ToCartographer(
const pcl::PointCloud<pcl::PointXYZ>& pcl_points) {
::cartographer::sensor::proto::LaserFan3D proto;
auto* origin = proto.mutable_origin();
origin->set_x(0.);
origin->set_y(0.);
origin->set_z(0.);
auto* point_cloud = proto.mutable_point_cloud();
for (const auto& point : pcl_points) {
point_cloud->add_x(point.x);
point_cloud->add_y(point.y);
point_cloud->add_z(point.z);
}
return proto;
}
} // namespace cartographer_ros

57
ros/google_cartographer/src/msg_conversion.h
View File

@ -1,57 +0,0 @@
/*
* Copyright 2016 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_GOOGLE_CARTOGRAPHER_SRC_MSG_CONVERSION_H_
#define CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_MSG_CONVERSION_H_
#include "cartographer/common/port.h"
#include "cartographer/sensor/proto/sensor.pb.h"
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/LaserScan.h"
#include "sensor_msgs/MultiEchoLaserScan.h"
#include "sensor_msgs/PointCloud2.h"
namespace cartographer_ros {
// Only uses first echo of each beam.
sensor_msgs::LaserScan ToLaserScan(
int64 timestamp, const string& frame_id,
const ::cartographer::sensor::proto::LaserScan& laser_scan);
sensor_msgs::MultiEchoLaserScan ToMultiEchoLaserScanMessage(
int64 timestamp, const string& frame_id,
const ::cartographer::sensor::proto::LaserScan& laser_scan);
sensor_msgs::Imu ToImuMessage(int64 timestamp, const string& frame_id,
const ::cartographer::sensor::proto::Imu& imu);
sensor_msgs::PointCloud2 ToPointCloud2Message(
int64 timestamp, const string& frame_id,
const ::cartographer::sensor::proto::LaserFan3D& laser_scan_3d);
::cartographer::sensor::proto::Imu ToCartographer(const sensor_msgs::Imu& msg);
::cartographer::sensor::proto::LaserScan ToCartographer(
const sensor_msgs::LaserScan& msg);
::cartographer::sensor::proto::LaserFan3D ToCartographer(
const pcl::PointCloud<pcl::PointXYZ>& pcl_points);
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_MSG_CONVERSION_H_

31
ros/google_cartographer/src/node_constants.h
View File

@ -1,31 +0,0 @@
/*
* Copyright 2016 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_GOOGLE_CARTOGRAPHER_SRC_NODE_CONSTANTS_H_
#define CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_NODE_CONSTANTS_H_
namespace cartographer_ros {
// The topic that the node will subscrbe under.
constexpr char kSubmapListTopic[] = "submap_list";
// The service we serve in the Node and query in the RViz plugin for submap
// which are used for visualization.
constexpr char kSubmapQueryServiceName[] = "submap_query";
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_NODE_CONSTANTS_H_

331
ros/google_cartographer/src/submaps_display.cc
View File

@ -1,331 +0,0 @@
/*
* Copyright 2016 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 "submaps_display.h"
#include <OgreColourValue.h>
#include <OgreHardwarePixelBuffer.h>
#include <OgreManualObject.h>
#include <OgreMaterialManager.h>
#include <OgreOverlay.h>
#include <OgreOverlayContainer.h>
#include <OgreOverlayManager.h>
#include <OgreRenderTexture.h>
#include <OgreResourceGroupManager.h>
#include <OgreRoot.h>
#include <OgreSceneManager.h>
#include <OgreSceneNode.h>
#include <OgreSharedPtr.h>
#include <OgreTechnique.h>
#include <OgreTexture.h>
#include <OgreTextureManager.h>
#include <OgreViewport.h>
#include <cartographer/common/mutex.h>
#include <geometry_msgs/TransformStamped.h>
#include <google_cartographer_msgs/SubmapList.h>
#include <google_cartographer_msgs/SubmapQuery.h>
#include <pluginlib/class_list_macros.h>
#include <ros/package.h>
#include <ros/ros.h>
#include <rviz/display_context.h>
#include <rviz/frame_manager.h>
#include <rviz/properties/ros_topic_property.h>
#include <rviz/properties/string_property.h>
namespace cartographer_ros {
namespace rviz {
namespace {
constexpr int kMaxOnGoingRequests = 6;
constexpr char kMaterialsDirectory[] = "/ogre_media/materials";
constexpr char kGlsl120Directory[] = "/glsl120";
constexpr char kScriptsDirectory[] = "/scripts";
constexpr char kScreenBlitMaterialName[] = "ScreenBlitMaterial";
constexpr char kScreenBlitSourceMaterialName[] =
"google_cartographer/ScreenBlit";
constexpr char kSubmapsRttPrefix[] = "SubmapsRtt";
constexpr char kMapTextureName[] = "MapTexture";
constexpr char kMapOverlayName[] = "MapOverlay";
constexpr char kSubmapsSceneCameraName[] = "SubmapsSceneCamera";
constexpr char kSubmapTexturesGroup[] = "SubmapTexturesGroup";
constexpr char kDefaultMapFrame[] = "map";
constexpr char kDefaultTrackingFrame[] = "base_link";
} // namespace
SubmapsDisplay::SubmapsDisplay()
: Display(),
rtt_count_(0),
scene_manager_listener_([this]() { UpdateMapTexture(); }),
tf_listener_(tf_buffer_) {
connect(this, SIGNAL(SubmapListUpdated()), this, SLOT(RequestNewSubmaps()));
topic_property_ = new ::rviz::RosTopicProperty(
"Topic", "",
QString::fromStdString(ros::message_traits::datatype<
::google_cartographer_msgs::SubmapList>()),
"google_cartographer_msgs::SubmapList topic to subscribe to.", this,
SLOT(UpdateTopic()));
submap_query_service_property_ = new ::rviz::StringProperty(
"Submap query service", "", "Submap query service to connect to.", this,
SLOT(UpdateSubmapQueryServiceName()));
map_frame_property_ = new ::rviz::StringProperty(
"Map frame", kDefaultMapFrame, "Map frame, used for fading out submaps.",
this);
tracking_frame_property_ = new ::rviz::StringProperty(
"Tracking frame", kDefaultTrackingFrame,
"Tracking frame, used for fading out submaps.", this);
client_ =
update_nh_.serviceClient<::google_cartographer_msgs::SubmapQuery>("");
const std::string package_path = ::ros::package::getPath(ROS_PACKAGE_NAME);
Ogre::ResourceGroupManager::getSingleton().addResourceLocation(
package_path + kMaterialsDirectory, "FileSystem", ROS_PACKAGE_NAME);
Ogre::ResourceGroupManager::getSingleton().addResourceLocation(
package_path + kMaterialsDirectory + kGlsl120Directory, "FileSystem",
ROS_PACKAGE_NAME);
Ogre::ResourceGroupManager::getSingleton().addResourceLocation(
package_path + kMaterialsDirectory + kScriptsDirectory, "FileSystem",
ROS_PACKAGE_NAME);
Ogre::ResourceGroupManager::getSingleton().initialiseAllResourceGroups();
}
SubmapsDisplay::~SubmapsDisplay() {
Unsubscribe();
client_.shutdown();
Clear();
}
void SubmapsDisplay::onInitialize() {
submaps_scene_manager_ =
Ogre::Root::getSingletonPtr()->createSceneManager(Ogre::ST_GENERIC);
submaps_scene_camera_ =
submaps_scene_manager_->createCamera(kSubmapsSceneCameraName);
submap_scene_material_ = Ogre::MaterialManager::getSingleton().create(
kMapTextureName, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
screen_blit_material_ = Ogre::MaterialManager::getSingleton().getByName(
kScreenBlitSourceMaterialName);
screen_blit_material_ = screen_blit_material_->clone(kScreenBlitMaterialName);
screen_blit_material_->setReceiveShadows(false);
screen_blit_material_->getTechnique(0)->setLightingEnabled(false);
screen_blit_material_->setDepthWriteEnabled(false);
Ogre::OverlayManager& overlay_manager = Ogre::OverlayManager::getSingleton();
overlay_ = overlay_manager.create(kMapOverlayName);
panel_ = static_cast<Ogre::OverlayContainer*>(
overlay_manager.createOverlayElement("Panel", "PanelName"));
overlay_->add2D(panel_);
panel_->setPosition(0.0, 0.0);
panel_->setDimensions(1., 1.);
panel_->setMaterialName(kScreenBlitMaterialName);
Ogre::ResourceGroupManager::getSingleton().createResourceGroup(
kSubmapTexturesGroup);
scene_manager_->addListener(&scene_manager_listener_);
UpdateTopic();
}
void SubmapsDisplay::UpdateTopic() {
Unsubscribe();
Clear();
Subscribe();
}
void SubmapsDisplay::UpdateSubmapQueryServiceName() {
Unsubscribe();
Clear();
client_.shutdown();
client_ = update_nh_.serviceClient<::google_cartographer_msgs::SubmapQuery>(
submap_query_service_property_->getStdString());
Subscribe();
}
void SubmapsDisplay::reset() {
Display::reset();
Clear();
UpdateTopic();
}
void SubmapsDisplay::onEnable() { Subscribe(); }
void SubmapsDisplay::onDisable() {
Unsubscribe();
Clear();
}
void SubmapsDisplay::Subscribe() {
if (!isEnabled()) {
return;
}
if (!topic_property_->getTopic().isEmpty()) {
try {
submap_list_subscriber_ =
update_nh_.subscribe(topic_property_->getTopicStd(), 1,
&SubmapsDisplay::IncomingSubmapList, this,
ros::TransportHints().reliable());
setStatus(::rviz::StatusProperty::Ok, "Topic", "OK");
} catch (ros::Exception& e) {
setStatus(::rviz::StatusProperty::Error, "Topic",
QString("Error subscribing: ") + e.what());
}
}
}
void SubmapsDisplay::Unsubscribe() { submap_list_subscriber_.shutdown(); }
void SubmapsDisplay::IncomingSubmapList(
const ::google_cartographer_msgs::SubmapList::ConstPtr& msg) {
submap_list_ = *msg;
Q_EMIT SubmapListUpdated();
}
void SubmapsDisplay::Clear() {
::cartographer::common::MutexLocker locker(&mutex_);
submaps_scene_manager_->clearScene();
if (!rttTexture_.isNull()) {
rttTexture_->unload();
rttTexture_.setNull();
}
Ogre::ResourceGroupManager::getSingleton().clearResourceGroup(
kSubmapTexturesGroup);
trajectories_.clear();
overlay_->hide();
}
void SubmapsDisplay::RequestNewSubmaps() {
::cartographer::common::MutexLocker locker(&mutex_);
for (int trajectory_id = 0; trajectory_id < submap_list_.trajectory.size();
++trajectory_id) {
if (trajectory_id >= trajectories_.size()) {
trajectories_.emplace_back(new Trajectory);
}
const std::vector<::google_cartographer_msgs::SubmapEntry>& submap_entries =
submap_list_.trajectory[trajectory_id].submap;
if (submap_entries.empty()) {
return;
}
for (int submap_id = trajectories_[trajectory_id]->Size();
submap_id < submap_entries.size(); ++submap_id) {
trajectories_[trajectory_id]->Add(submap_id, trajectory_id,
context_->getFrameManager(),
submaps_scene_manager_);
}
}
int num_ongoing_requests = 0;
for (const auto& trajectory : trajectories_) {
for (int i = 0; i < trajectory->Size(); ++i) {
if (trajectory->Get(i).QueryInProgress()) {
++num_ongoing_requests;
if (num_ongoing_requests == kMaxOnGoingRequests) {
return;
}
}
}
}
for (int trajectory_id = 0; trajectory_id < submap_list_.trajectory.size();
++trajectory_id) {
const std::vector<::google_cartographer_msgs::SubmapEntry>& submap_entries =
submap_list_.trajectory[trajectory_id].submap;
for (int submap_id = submap_entries.size() - 1; submap_id >= 0;
--submap_id) {
if (trajectories_[trajectory_id]->Get(submap_id).Update(
submap_entries[submap_id], &client_)) {
++num_ongoing_requests;
if (num_ongoing_requests == kMaxOnGoingRequests) {
return;
}
}
}
}
}
void SubmapsDisplay::UpdateMapTexture() {
if (trajectories_.empty()) {
return;
}
const int width = scene_manager_->getCurrentViewport()->getActualWidth();
const int height = scene_manager_->getCurrentViewport()->getActualHeight();
if (!rttTexture_.isNull()) {
rttTexture_->unload();
rttTexture_.setNull();
}
// If the rtt texture is freed every time UpdateMapTexture() is called, the
// code slows down a lot. Therefore, we assign them to a group and free them
// every 100th texture.
if (rtt_count_ % 100 == 0) {
Ogre::ResourceGroupManager::getSingleton().clearResourceGroup(
kSubmapTexturesGroup);
}
rttTexture_ =
Ogre::Root::getSingletonPtr()->getTextureManager()->createManual(
kSubmapsRttPrefix + std::to_string(rtt_count_), kSubmapTexturesGroup,
Ogre::TEX_TYPE_2D, width, height, 0, Ogre::PF_RG8,
Ogre::TU_RENDERTARGET);
rtt_count_++;
Ogre::Pass* rtt_pass = submap_scene_material_->getTechnique(0)->getPass(0);
Ogre::TextureUnitState* const rtt_tex_unit =
rtt_pass->getNumTextureUnitStates() > 0
? rtt_pass->getTextureUnitState(0)
: rtt_pass->createTextureUnitState();
rtt_tex_unit->setTexture(rttTexture_);
Ogre::RenderTexture* const renderTexture =
rttTexture_->getBuffer()->getRenderTarget();
renderTexture->addViewport(submaps_scene_camera_)
->setBackgroundColour(Ogre::ColourValue(0.5f, 0.f, 0.f));
{
::cartographer::common::MutexLocker locker(&mutex_);
// TODO(pedrofernandez): Add support for more than one trajectory.
for (int i = 0; i < trajectories_.front()->Size(); ++i) {
trajectories_.front()->Get(i).Transform(ros::Time());
try {
const ::geometry_msgs::TransformStamped transform_stamped =
tf_buffer_.lookupTransform(map_frame_property_->getStdString(),
tracking_frame_property_->getStdString(),
ros::Time(0) /* latest */);
trajectories_.front()->Get(i).SetAlpha(
transform_stamped.transform.translation.z);
} catch (tf2::TransformException& e) {
ROS_WARN("Could not compute submap fading: %s", e.what());
}
}
}
Ogre::Camera* const actual_camera =
scene_manager_->getCurrentViewport()->getCamera();
submaps_scene_camera_->synchroniseBaseSettingsWith(actual_camera);
submaps_scene_camera_->setCustomProjectionMatrix(
true, actual_camera->getProjectionMatrix());
renderTexture->update();
Ogre::Pass* const pass = screen_blit_material_->getTechnique(0)->getPass(0);
pass->setSceneBlending(Ogre::SBF_SOURCE_ALPHA,
Ogre::SBF_ONE_MINUS_SOURCE_ALPHA);
Ogre::TextureUnitState* const tex_unit = pass->getNumTextureUnitStates() > 0
? pass->getTextureUnitState(0)
: pass->createTextureUnitState();
tex_unit->setTextureName(rttTexture_->getName());
tex_unit->setTextureFiltering(Ogre::TFO_NONE);
overlay_->show();
}
} // namespace rviz
} // namespace cartographer_ros
PLUGINLIB_EXPORT_CLASS(cartographer_ros::rviz::SubmapsDisplay, ::rviz::Display)

127
ros/google_cartographer/src/submaps_display.h
View File

@ -1,127 +0,0 @@
/*
* Copyright 2016 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_GOOGLE_CARTOGRAPHER_SRC_SUBMAPS_DISPLAY_H_
#define CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_SUBMAPS_DISPLAY_H_
#include <OgreMaterial.h>
#include <OgreOverlay.h>
#include <OgreOverlayContainer.h>
#include <OgreSceneManager.h>
#include <OgreSharedPtr.h>
#include <OgreTexture.h>
#include <OgreVector3.h>
#include <cartographer/common/mutex.h>
#include <cartographer/common/port.h>
#include <google_cartographer_msgs/SubmapList.h>
#include <nav_msgs/MapMetaData.h>
#include <ros/time.h>
#include <rviz/display.h>
#include <rviz/properties/ros_topic_property.h>
#include <tf/tfMessage.h>
#include <tf2_ros/transform_listener.h>
#include <memory>
#include <vector>
#include "drawable_submap.h"
#include "trajectory.h"
namespace cartographer_ros {
namespace rviz {
// RViz plugin used for displaying maps which are represented by a collection of
// submaps.
//
// We keep a separate Ogre scene to the one provided by rviz and in it we place
// every submap as a SceneNode. We show an X-ray view of the map which is
// achieved by shipping textures for every submap containing pre-multiplied
// alpha and grayscale values, these are then alpha blended together onto an
// offscreen texture. This offscreen texture is then screen blit onto the screen
// as a grayscale image.
class SubmapsDisplay : public ::rviz::Display {
Q_OBJECT
public:
SubmapsDisplay();
~SubmapsDisplay() override;
SubmapsDisplay(const SubmapsDisplay&) = delete;
SubmapsDisplay& operator=(const SubmapsDisplay&) = delete;
// Called by RViz on initialization of the plugin.
void onInitialize() override;
// Called to tell the display to clear its state.
void reset() override;
Q_SIGNALS:
void SubmapListUpdated();
private Q_SLOTS:
void RequestNewSubmaps();
void UpdateTopic();
void UpdateSubmapQueryServiceName();
private:
class SceneManagerListener : public Ogre::SceneManager::Listener {
public:
SceneManagerListener(std::function<void()> callback)
: callback_(callback) {}
void preUpdateSceneGraph(Ogre::SceneManager* source, Ogre::Camera* camera) {
callback_();
}
private:
std::function<void()> callback_;
};
void onEnable() override;
void onDisable() override;
void Subscribe();
void Unsubscribe();
void UpdateMapTexture();
void IncomingSubmapList(
const ::google_cartographer_msgs::SubmapList::ConstPtr& msg);
// Clears the current map.
void Clear();
void UpdateCurrentTrackingZ(const tf::tfMessage::ConstPtr& msg);
int rtt_count_;
SceneManagerListener scene_manager_listener_;
::google_cartographer_msgs::SubmapList submap_list_;
ros::Subscriber submap_list_subscriber_;
::tf2_ros::Buffer tf_buffer_;
::tf2_ros::TransformListener tf_listener_;
ros::ServiceClient client_;
::rviz::RosTopicProperty* topic_property_;
::rviz::StringProperty* submap_query_service_property_;
::rviz::StringProperty* map_frame_property_;
::rviz::StringProperty* tracking_frame_property_;
std::vector<std::unique_ptr<Trajectory>> trajectories_ GUARDED_BY(mutex_);
Ogre::SceneManager* submaps_scene_manager_;
Ogre::Camera* submaps_scene_camera_;
Ogre::MaterialPtr submap_scene_material_;
Ogre::MaterialPtr screen_blit_material_;
Ogre::Overlay* overlay_;
Ogre::OverlayContainer* panel_;
Ogre::TexturePtr rttTexture_;
::cartographer::common::Mutex mutex_;
};
} // namespace rviz
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_SUBMAPS_DISPLAY_H_

43
ros/google_cartographer/src/time_conversion.cc
View File

@ -1,43 +0,0 @@
/*
* Copyright 2016 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 "time_conversion.h"
#include "cartographer/common/time.h"
#include "ros/ros.h"
namespace cartographer_ros {
::ros::Time ToRos(::cartographer::common::Time time) {
int64 uts_timestamp = ::cartographer::common::ToUniversal(time);
int64 ns_since_unix_epoch =
(uts_timestamp - kUtsEpochOffsetFromUnixEpochInSeconds * 10000000ll) *
100ll;
::ros::Time ros_time;
ros_time.fromNSec(ns_since_unix_epoch);
return ros_time;
}
// TODO(pedrofernandez): Write test.
::cartographer::common::Time FromRos(const ::ros::Time& time) {
// The epoch of the ICU Universal Time Scale is "0001-01-01 00:00:00.0 +0000",
// exactly 719162 days before the Unix epoch.
return ::cartographer::common::FromUniversal(
(time.sec + kUtsEpochOffsetFromUnixEpochInSeconds) * 10000000ll +
(time.nsec + 50) / 100); // + 50 to get the rounding correct.
}
} // namespace cartographer_ros

34
ros/google_cartographer/src/time_conversion.h
View File

@ -1,34 +0,0 @@
/*
* Copyright 2016 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_GOOGLE_CARTOGRAPHER_SRC_TIME_CONVERSION_H_
#define CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_TIME_CONVERSION_H_
#include "cartographer/common/time.h"
#include "ros/ros.h"
namespace cartographer_ros {
constexpr int64 kUtsEpochOffsetFromUnixEpochInSeconds =
(719162ll * 24ll * 60ll * 60ll);
::ros::Time ToRos(::cartographer::common::Time time);
::cartographer::common::Time FromRos(const ::ros::Time& time);
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_TIME_CONVERSION_H_

44
ros/google_cartographer/src/time_conversion_test.cc
View File

@ -1,44 +0,0 @@
/*
* Copyright 2016 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 "time_conversion.h"
#include <chrono>
#include "cartographer/common/time.h"
#include "gtest/gtest.h"
#include "ros/ros.h"
namespace cartographer_ros {
namespace {
TEST(TimeConversion, testToRos) {
std::vector<int64> values = {0, 1469091375, 1466481821, 1462101382,
1468238899};
for (int64 seconds_since_epoch : values) {
::ros::Time ros_now;
ros_now.fromSec(seconds_since_epoch);
::cartographer::common::Time cartographer_now(
::cartographer::common::FromSeconds(
seconds_since_epoch + kUtsEpochOffsetFromUnixEpochInSeconds));
EXPECT_EQ(cartographer_now, ::cartographer_ros::FromRos(ros_now));
EXPECT_EQ(ros_now, ::cartographer_ros::ToRos(cartographer_now));
}
}
} // namespace
} // namespace cartographer_ros

50
ros/google_cartographer/src/trajectory.cc
View File

@ -1,50 +0,0 @@
/*
* Copyright 2016 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 "trajectory.h"
#include <OgreSceneManager.h>
#include <OgreSceneNode.h>
#include <cartographer/common/make_unique.h>
#include <rviz/frame_manager.h>
#include <memory>
#include <mutex>
#include <vector>
#include "drawable_submap.h"
namespace cartographer_ros {
namespace rviz {
void Trajectory::Add(const int submap_id, const int trajectory_id,
::rviz::FrameManager* const frame_manager,
Ogre::SceneManager* const scene_manager) {
std::lock_guard<std::mutex> guard(mutex_);
drawable_submaps_.push_back(
::cartographer::common::make_unique<DrawableSubmap>(
submap_id, trajectory_id, frame_manager, scene_manager));
}
DrawableSubmap& Trajectory::Get(const int index) {
std::lock_guard<std::mutex> guard(mutex_);
return *drawable_submaps_[index];
}
int Trajectory::Size() { return drawable_submaps_.size(); }
} // namespace rviz
} // namespace cartographer_ros

58
ros/google_cartographer/src/trajectory.h
View File

@ -1,58 +0,0 @@
/*
* Copyright 2016 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_GOOGLE_CARTOGRAPHER_SRC_TRAJECTORY_H_
#define CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_TRAJECTORY_H_
#include <OgreSceneManager.h>
#include <OgreSceneNode.h>
#include <rviz/frame_manager.h>
#include <memory>
#include <mutex>
#include <vector>
#include "drawable_submap.h"
namespace cartographer_ros {
namespace rviz {
// Contains a list of drawable submaps.
class Trajectory {
public:
Trajectory() = default;
Trajectory(const Trajectory&) = delete;
Trajectory& operator=(const Trajectory&) = delete;
// Creates a new DrawableSubmap and stores it as part of this trajectory.
void Add(int submap_id, int trajectory_id,
::rviz::FrameManager* frame_manager,
Ogre::SceneManager* scene_manager);
// Gets the submap at 'index' and returns a non-const reference.
DrawableSubmap& Get(int index);
// Returns the number of DrawableSubmaps this trajectory contains.
int Size();
private:
std::mutex mutex_;
std::vector<std::unique_ptr<DrawableSubmap>> drawable_submaps_;
};
} // namespace rviz
} // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_TRAJECTORY_H_

85
ros/google_cartographer/urdf/backpack_2d.urdf
View File

@ -1,85 +0,0 @@
<!--
Copyright 2016 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.
-->
<robot name="google_cartographer">
<material name="orange">
<color rgba="1.0 0.5 0.2 1"/>
</material>
<material name="gray">
<color rgba="0.2 0.2 0.2 1"/>
</material>
<material name="green">
<color rgba="0.2 0.4 0.2 1"/>
</material>
<link name="/imu_link">
<visual>
<origin xyz="0.01 0.01 -0.01"/>
<geometry>
<box size="0.055 0.055 0.02"/>
</geometry>
<material name="orange"/>
</visual>
</link>
<link name="/horizontal_laser_link">
<visual>
<origin xyz="0.0 0.0 -0.01"/>
<geometry>
<cylinder length="0.09" radius="0.03"/>
</geometry>
<material name="gray"/>
</visual>
</link>
<link name="/vertical_laser_link">
<visual>
<origin xyz="0.0 0.0 -0.01"/>
<geometry>
<cylinder length="0.09" radius="0.03"/>
</geometry>
<material name="gray"/>
</visual>
</link>
<link name="/base_footprint">
<visual>
<origin xyz="0.18 0.0 0.2"/>
<geometry>
<box size="0.03 0.4 0.8"/>
</geometry>
<material name="green"/>
</visual>
</link>
<joint name="base_link_joint" type="fixed">
<child link="/imu_link"/>
<parent link="/base_footprint"/>
<origin xyz="0 0 0" rpy="0 0 3.1416"/>
</joint>
<joint name="horizontal_laser_link_joint" type="fixed">
<parent link="/imu_link"/>
<child link="/horizontal_laser_link"/>
<origin xyz="0.1251 0.0937 0.05262"/>
</joint>
<joint name="vertical_laser_link_joint" type="fixed">
<parent link="/imu_link"/>
<child link="/vertical_laser_link"/>
<origin rpy="0.0 -1.57 3.14" xyz="0.1251 0.0937 0.17772"/>
</joint>
</robot>

85
ros/google_cartographer/urdf/backpack_3d.urdf
View File

@ -1,85 +0,0 @@
<!--
Copyright 2016 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.
-->
<robot name="google_cartographer">
<material name="orange">
<color rgba="1.0 0.5 0.2 1"/>
</material>
<material name="gray">
<color rgba="0.2 0.2 0.2 1"/>
</material>
<material name="green">
<color rgba="0.2 0.4 0.2 1"/>
</material>
<link name="/imu_link">
<visual>
<origin xyz="0.01 0.01 -0.01"/>
<geometry>
<box size="0.055 0.055 0.02"/>
</geometry>
<material name="orange"/>
</visual>
</link>
<link name="/horizontal_vlp16_link">
<visual>
<origin xyz="0.0 0.0 -0.01"/>
<geometry>
<cylinder length="0.09" radius="0.03"/>
</geometry>
<material name="gray"/>
</visual>
</link>
<link name="/vertical_vlp16_link">
<visual>
<origin xyz="0.0 0.0 -0.01"/>
<geometry>
<cylinder length="0.09" radius="0.03"/>
</geometry>
<material name="gray"/>
</visual>
</link>
<link name="/base_link">
<visual>
<origin xyz="0.18 0.0 0.2"/>
<geometry>
<box size="0.03 0.4 0.8"/>
</geometry>
<material name="green"/>
</visual>
</link>
<joint name="base_link_joint" type="fixed">
<parent link="/base_link"/>
<child link="/imu_link"/>
<origin xyz="0 0 0" rpy="0 0 3.1416"/>
</joint>
<joint name="horizontal_vlp16_link_joint" type="fixed">
<parent link="/base_link"/>
<child link="/horizontal_vlp16_link"/>
<origin xyz="0.01 0. 0.19" rpy="0. -0.1745 3.1416" />
</joint>
<joint name="vertical_vlp16_link_joint" type="fixed">
<parent link="/base_link"/>
<child link="/vertical_vlp16_link"/>
<origin xyz="0.19 0. 0.04" rpy="0. 1.3963 0."/>
</joint>
</robot>

53
ros/google_cartographer/urdf/turtlebot.urdf
View File

@ -1,53 +0,0 @@
<!--
Copyright 2016 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.
-->
<robot name="google_cartographer">
<material name="orange">
<color rgba="1.0 0.5 0.2 1"/>
</material>
<material name="gray">
<color rgba="0.2 0.2 0.2 1"/>
</material>
<material name="green">
<color rgba="0.2 0.4 0.2 1"/>
</material>
<link name="/horizontal_laser_link">
<visual>
<origin xyz="0.0 0.0 0.0"/>
<geometry>
<cylinder length="0.09" radius="0.03"/>
</geometry>
<material name="gray"/>
</visual>
</link>
<link name="/base_link">
<visual>
<origin xyz="0. 0. 0."/>
<geometry>
<box size="0.03 0.4 0.8"/>
</geometry>
<material name="green"/>
</visual>
</link>
<joint name="horizontal_laser_link_joint" type="fixed">
<parent link="/base_link"/>
<child link="/horizontal_laser_link"/>
<origin xyz="0. 0. 0.6"/>
</joint>
</robot>

37
ros/google_cartographer_msgs/CMakeLists.txt
View File

@ -1,37 +0,0 @@
# Copyright 2016 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.
cmake_minimum_required(VERSION 2.8.3)
project(google_cartographer_msgs)
set(CMAKE_CXX_FLAGS "-std=c++0x ${CMAKE_CXX_FLAGS}")
find_package(catkin REQUIRED COMPONENTS geometry_msgs message_generation)
add_message_files(
FILES
SubmapList.msg
TrajectorySubmapList.msg
SubmapEntry.msg
)
add_service_files(
FILES
SubmapQuery.srv
)
generate_messages(
DEPENDENCIES
geometry_msgs
)
catkin_package()

16
ros/google_cartographer_msgs/msg/SubmapEntry.msg
View File

@ -1,16 +0,0 @@
# Copyright 2016 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.
uint32 submap_version
geometry_msgs/Pose pose

15
ros/google_cartographer_msgs/msg/SubmapList.msg
View File

@ -1,15 +0,0 @@
# Copyright 2016 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.
TrajectorySubmapList[] trajectory

15
ros/google_cartographer_msgs/msg/TrajectorySubmapList.msg
View File

@ -1,15 +0,0 @@
# Copyright 2016 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.
SubmapEntry[] submap

29
ros/google_cartographer_msgs/package.xml
View File

@ -1,29 +0,0 @@
<!--
Copyright 2016 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.
-->
<package>
<name>google_cartographer_msgs</name>
<version>1.0.0</version>
<description>
The google_cartographer_msgs package.
</description>
<maintainer email="nobody@google.com">Google</maintainer>
<license>Apache 2.0</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>message_generation</build_depend>
</package>

25
ros/google_cartographer_msgs/srv/SubmapQuery.srv
View File

@ -1,25 +0,0 @@
# Copyright 2016 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.
int32 submap_id
int32 trajectory_id
---
int32 submap_id
int32 submap_version
uint8[] cells
int32 width
int32 height
float64 resolution
geometry_msgs/Pose slice_pose
string error_message