diff --git a/ros/google_cartographer/CMakeLists.txt b/ros/google_cartographer/CMakeLists.txt deleted file mode 100644 index 77d8088..0000000 --- a/ros/google_cartographer/CMakeLists.txt +++ /dev/null @@ -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} -) diff --git a/ros/google_cartographer/configuration_files/3d_mapping.lua b/ros/google_cartographer/configuration_files/3d_mapping.lua deleted file mode 100644 index 1d42b17..0000000 --- a/ros/google_cartographer/configuration_files/3d_mapping.lua +++ /dev/null @@ -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 diff --git a/ros/google_cartographer/configuration_files/backpack_3d.lua b/ros/google_cartographer/configuration_files/backpack_3d.lua deleted file mode 100644 index 43a4a4c..0000000 --- a/ros/google_cartographer/configuration_files/backpack_3d.lua +++ /dev/null @@ -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 diff --git a/ros/google_cartographer/configuration_files/turtlebot.lua b/ros/google_cartographer/configuration_files/turtlebot.lua deleted file mode 100644 index 2f1ff65..0000000 --- a/ros/google_cartographer/configuration_files/turtlebot.lua +++ /dev/null @@ -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 diff --git a/ros/google_cartographer/launch/3d_mapping.launch b/ros/google_cartographer/launch/3d_mapping.launch deleted file mode 100644 index 2671e2b..0000000 --- a/ros/google_cartographer/launch/3d_mapping.launch +++ /dev/null @@ -1,100 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 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. - - - - diff --git a/ros/google_cartographer/launch/state.launch b/ros/google_cartographer/launch/state.launch deleted file mode 100644 index c07f760..0000000 --- a/ros/google_cartographer/launch/state.launch +++ /dev/null @@ -1,73 +0,0 @@ - - - - - - - - - - # 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"] - - - - - diff --git a/ros/google_cartographer/launch/turtlebot.launch b/ros/google_cartographer/launch/turtlebot.launch deleted file mode 100644 index 75c9576..0000000 --- a/ros/google_cartographer/launch/turtlebot.launch +++ /dev/null @@ -1,41 +0,0 @@ - - - - - - - - - - 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. - - - - diff --git a/ros/google_cartographer/ogre_media/materials/glsl120/glsl120.program b/ros/google_cartographer/ogre_media/materials/glsl120/glsl120.program deleted file mode 100644 index 045f381..0000000 --- a/ros/google_cartographer/ogre_media/materials/glsl120/glsl120.program +++ /dev/null @@ -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 -} diff --git a/ros/google_cartographer/ogre_media/materials/glsl120/screen_blit.frag b/ros/google_cartographer/ogre_media/materials/glsl120/screen_blit.frag deleted file mode 100644 index 195d813..0000000 --- a/ros/google_cartographer/ogre_media/materials/glsl120/screen_blit.frag +++ /dev/null @@ -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); -} diff --git a/ros/google_cartographer/ogre_media/materials/glsl120/screen_blit.vert b/ros/google_cartographer/ogre_media/materials/glsl120/screen_blit.vert deleted file mode 100644 index 74d1bba..0000000 --- a/ros/google_cartographer/ogre_media/materials/glsl120/screen_blit.vert +++ /dev/null @@ -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; -} diff --git a/ros/google_cartographer/ogre_media/materials/glsl120/submap.frag b/ros/google_cartographer/ogre_media/materials/glsl120/submap.frag deleted file mode 100644 index 1998f53..0000000 --- a/ros/google_cartographer/ogre_media/materials/glsl120/submap.frag +++ /dev/null @@ -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); -} diff --git a/ros/google_cartographer/ogre_media/materials/glsl120/submap.vert b/ros/google_cartographer/ogre_media/materials/glsl120/submap.vert deleted file mode 100644 index 864bfa1..0000000 --- a/ros/google_cartographer/ogre_media/materials/glsl120/submap.vert +++ /dev/null @@ -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(); -} diff --git a/ros/google_cartographer/ogre_media/materials/scripts/screen_blit.material b/ros/google_cartographer/ogre_media/materials/scripts/screen_blit.material deleted file mode 100644 index cd850fd..0000000 --- a/ros/google_cartographer/ogre_media/materials/scripts/screen_blit.material +++ /dev/null @@ -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 - } - } - } -} diff --git a/ros/google_cartographer/ogre_media/materials/scripts/submap.material b/ros/google_cartographer/ogre_media/materials/scripts/submap.material deleted file mode 100644 index 1304156..0000000 --- a/ros/google_cartographer/ogre_media/materials/scripts/submap.material +++ /dev/null @@ -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 - } - } - } -} diff --git a/ros/google_cartographer/package.xml b/ros/google_cartographer/package.xml deleted file mode 100644 index bf3d0b9..0000000 --- a/ros/google_cartographer/package.xml +++ /dev/null @@ -1,68 +0,0 @@ - - - - - google_cartographer - 1.0.0 - The google_cartographer package. - Google - Apache 2.0 - - catkin - - eigen_conversions - geometry_msgs - google_cartographer_msgs - libpcl-all-dev - message_generation - nav_msgs - pcl_conversions - qtbase5-dev - rosbag - roscpp - roslib - rviz - sensor_msgs - std_msgs - tf2 - tf2_eigen - visualization_msgs - - eigen_conversions - geometry_msgs - google_cartographer_msgs - libpcl-all - libqt5-core - libqt5-gui - libqt5-widgets - message_runtime - nav_msgs - pcl_conversions - rosbag - roscpp - roslib - rviz - sensor_msgs - std_msgs - tf2_eigen - tf2 - visualization_msgs - - - - - diff --git a/ros/google_cartographer/rviz_plugin_description.xml b/ros/google_cartographer/rviz_plugin_description.xml deleted file mode 100644 index b990bd7..0000000 --- a/ros/google_cartographer/rviz_plugin_description.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - Displays Google Cartographer's submaps as a unified map in RViz. - - - diff --git a/ros/google_cartographer/src/cartographer_node_main.cc b/ros/google_cartographer/src/cartographer_node_main.cc deleted file mode 100644 index 5dc619a..0000000 --- a/ros/google_cartographer/src/cartographer_node_main.cc +++ /dev/null @@ -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 -#include -#include -#include - -#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 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 - 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 sensor_collator_; - ros::NodeHandle node_handle_; - ros::Subscriber imu_subscriber_; - ros::Subscriber laser_2d_subscriber_; - std::vector 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( - 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( - 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()); - 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_points; - pcl::fromROSMsg(*msg, pcl_points); - - auto sensor_data = ::cartographer::common::make_unique( - 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())); -} - -template -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("tracking_frame"); - map_frame_ = GetParamOrDie("map_frame"); - laser_min_range_m_ = GetParamOrDie("laser_min_range_m"); - laser_max_range_m_ = GetParamOrDie("laser_max_range_m"); - laser_missing_echo_ray_length_m_ = - GetParamOrDie("laser_missing_echo_ray_length_m"); - - // Subscribe to the IMU. - const string imu_topic = GetParamOrDie("imu_topic"); - imu_subscriber_ = node_handle_.subscribe(imu_topic, kSubscriberQueueSize, - &Node::ImuMessageCallback, this); - std::unordered_set 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("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("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>("configuration_files_directories")); - const string code = file_resolver->GetFileContentOrDie( - GetParamOrDie("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>("laser_scan_3d_topics"); - for (const auto& topic : topics) { - laser_3d_subscribers_.push_back(node_handle_.subscribe( - topic, kSubscriberQueueSize, - boost::function( - [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 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 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; -} diff --git a/ros/google_cartographer/src/drawable_submap.cc b/ros/google_cartographer/src/drawable_submap.cc deleted file mode 100644 index 5e2a035..0000000 --- a/ros/google_cartographer/src/drawable_submap.cc +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -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::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 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 diff --git a/ros/google_cartographer/src/drawable_submap.h b/ros/google_cartographer/src/drawable_submap.h deleted file mode 100644 index cbdd53f..0000000 --- a/ros/google_cartographer/src/drawable_submap.h +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -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 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_ diff --git a/ros/google_cartographer/src/msg_conversion.cc b/ros/google_cartographer/src/msg_conversion.cc deleted file mode 100644 index 31a18e4..0000000 --- a/ros/google_cartographer/src/msg_conversion.cc +++ /dev/null @@ -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_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 diff --git a/ros/google_cartographer/src/msg_conversion.h b/ros/google_cartographer/src/msg_conversion.h deleted file mode 100644 index f553d04..0000000 --- a/ros/google_cartographer/src/msg_conversion.h +++ /dev/null @@ -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_points); - -} // namespace cartographer_ros - -#endif // CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_MSG_CONVERSION_H_ diff --git a/ros/google_cartographer/src/node_constants.h b/ros/google_cartographer/src/node_constants.h deleted file mode 100644 index 84c3330..0000000 --- a/ros/google_cartographer/src/node_constants.h +++ /dev/null @@ -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_ diff --git a/ros/google_cartographer/src/submaps_display.cc b/ros/google_cartographer/src/submaps_display.cc deleted file mode 100644 index 10e24f4..0000000 --- a/ros/google_cartographer/src/submaps_display.cc +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -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( - 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) diff --git a/ros/google_cartographer/src/submaps_display.h b/ros/google_cartographer/src/submaps_display.h deleted file mode 100644 index f3e85e6..0000000 --- a/ros/google_cartographer/src/submaps_display.h +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#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 callback) - : callback_(callback) {} - void preUpdateSceneGraph(Ogre::SceneManager* source, Ogre::Camera* camera) { - callback_(); - } - - private: - std::function 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> 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_ diff --git a/ros/google_cartographer/src/time_conversion.cc b/ros/google_cartographer/src/time_conversion.cc deleted file mode 100644 index 7dfda18..0000000 --- a/ros/google_cartographer/src/time_conversion.cc +++ /dev/null @@ -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 diff --git a/ros/google_cartographer/src/time_conversion.h b/ros/google_cartographer/src/time_conversion.h deleted file mode 100644 index 863d871..0000000 --- a/ros/google_cartographer/src/time_conversion.h +++ /dev/null @@ -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_ diff --git a/ros/google_cartographer/src/time_conversion_test.cc b/ros/google_cartographer/src/time_conversion_test.cc deleted file mode 100644 index 693a67b..0000000 --- a/ros/google_cartographer/src/time_conversion_test.cc +++ /dev/null @@ -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 - -#include "cartographer/common/time.h" -#include "gtest/gtest.h" -#include "ros/ros.h" - -namespace cartographer_ros { - -namespace { - -TEST(TimeConversion, testToRos) { - std::vector 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 diff --git a/ros/google_cartographer/src/trajectory.cc b/ros/google_cartographer/src/trajectory.cc deleted file mode 100644 index 995a605..0000000 --- a/ros/google_cartographer/src/trajectory.cc +++ /dev/null @@ -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 -#include -#include -#include - -#include -#include -#include - -#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 guard(mutex_); - drawable_submaps_.push_back( - ::cartographer::common::make_unique( - submap_id, trajectory_id, frame_manager, scene_manager)); -} - -DrawableSubmap& Trajectory::Get(const int index) { - std::lock_guard guard(mutex_); - return *drawable_submaps_[index]; -} - -int Trajectory::Size() { return drawable_submaps_.size(); } - -} // namespace rviz -} // namespace cartographer_ros diff --git a/ros/google_cartographer/src/trajectory.h b/ros/google_cartographer/src/trajectory.h deleted file mode 100644 index 12ff234..0000000 --- a/ros/google_cartographer/src/trajectory.h +++ /dev/null @@ -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 -#include -#include - -#include -#include -#include - -#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> drawable_submaps_; -}; - -} // namespace rviz -} // namespace cartographer_ros - -#endif // CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_TRAJECTORY_H_ diff --git a/ros/google_cartographer/urdf/backpack_2d.urdf b/ros/google_cartographer/urdf/backpack_2d.urdf deleted file mode 100644 index e72be50..0000000 --- a/ros/google_cartographer/urdf/backpack_2d.urdf +++ /dev/null @@ -1,85 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ros/google_cartographer/urdf/backpack_3d.urdf b/ros/google_cartographer/urdf/backpack_3d.urdf deleted file mode 100644 index 89f32ef..0000000 --- a/ros/google_cartographer/urdf/backpack_3d.urdf +++ /dev/null @@ -1,85 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ros/google_cartographer/urdf/turtlebot.urdf b/ros/google_cartographer/urdf/turtlebot.urdf deleted file mode 100644 index f97dcb7..0000000 --- a/ros/google_cartographer/urdf/turtlebot.urdf +++ /dev/null @@ -1,53 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ros/google_cartographer_msgs/CMakeLists.txt b/ros/google_cartographer_msgs/CMakeLists.txt deleted file mode 100644 index 9551707..0000000 --- a/ros/google_cartographer_msgs/CMakeLists.txt +++ /dev/null @@ -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() diff --git a/ros/google_cartographer_msgs/msg/SubmapEntry.msg b/ros/google_cartographer_msgs/msg/SubmapEntry.msg deleted file mode 100644 index 605de24..0000000 --- a/ros/google_cartographer_msgs/msg/SubmapEntry.msg +++ /dev/null @@ -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 diff --git a/ros/google_cartographer_msgs/msg/SubmapList.msg b/ros/google_cartographer_msgs/msg/SubmapList.msg deleted file mode 100644 index dbc7b08..0000000 --- a/ros/google_cartographer_msgs/msg/SubmapList.msg +++ /dev/null @@ -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 diff --git a/ros/google_cartographer_msgs/msg/TrajectorySubmapList.msg b/ros/google_cartographer_msgs/msg/TrajectorySubmapList.msg deleted file mode 100644 index 678a886..0000000 --- a/ros/google_cartographer_msgs/msg/TrajectorySubmapList.msg +++ /dev/null @@ -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 diff --git a/ros/google_cartographer_msgs/package.xml b/ros/google_cartographer_msgs/package.xml deleted file mode 100644 index a6d2cad..0000000 --- a/ros/google_cartographer_msgs/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - - google_cartographer_msgs - 1.0.0 - - The google_cartographer_msgs package. - - Google - Apache 2.0 - - catkin - - message_generation - diff --git a/ros/google_cartographer_msgs/srv/SubmapQuery.srv b/ros/google_cartographer_msgs/srv/SubmapQuery.srv deleted file mode 100644 index 4463260..0000000 --- a/ros/google_cartographer_msgs/srv/SubmapQuery.srv +++ /dev/null @@ -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