diff --git a/.gitignore b/.gitignore index fd13d2802..8eba26eae 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,2 @@ snap_ws/* +.vscode \ No newline at end of file diff --git a/CMake/FindCholmod.cmake b/CMake/FindCHOLMOD.cmake similarity index 100% rename from CMake/FindCholmod.cmake rename to CMake/FindCHOLMOD.cmake diff --git a/CMake/FindCSparse.cmake b/CMake/FindCSparse.cmake index 0fd1ca4d3..053c4e76d 100644 --- a/CMake/FindCSparse.cmake +++ b/CMake/FindCSparse.cmake @@ -21,5 +21,5 @@ FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse ) include(FindPackageHandleStandardArgs) -find_package_handle_standard_args(CSPARSE DEFAULT_MSG +find_package_handle_standard_args(CSparse DEFAULT_MSG CSPARSE_INCLUDE_DIR CSPARSE_LIBRARY) \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 80f9eef25..3dbda8a9e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,6 +11,8 @@ set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/CMake/") list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/lib/karto_sdk/cmake) find_package(ament_cmake REQUIRED) +find_package(maidbot_msg_utils REQUIRED) +find_package(maidbot_std_srvs REQUIRED) find_package(message_filters REQUIRED) find_package(nav_msgs REQUIRED) find_package(rclcpp REQUIRED) @@ -20,18 +22,11 @@ find_package(tf2_ros REQUIRED) find_package(visualization_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(tf2_geometry_msgs REQUIRED) -find_package(tf2_sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(rosidl_default_generators REQUIRED) -find_package(nav2_map_server REQUIRED) -find_package(rviz_common REQUIRED) -find_package(rviz_default_plugins REQUIRED) -find_package(rviz_ogre_vendor REQUIRED) -find_package(rviz_rendering REQUIRED) -find_package(interactive_markers REQUIRED) -find_package(Qt5 REQUIRED COMPONENTS Core Gui Widgets Test Concurrent) +# find_package(nav2_map_server REQUIRED) #karto_sdk lib set(BUILD_SHARED_LIBS ON) @@ -41,6 +36,8 @@ set(CMAKE_INCLUDE_CURRENT_DIR ON) set(dependencies rclcpp + maidbot_msg_utils + maidbot_std_srvs message_filters nav_msgs sensor_msgs @@ -48,35 +45,28 @@ set(dependencies tf2_ros visualization_msgs pluginlib - nav2_map_server tf2_geometry_msgs - tf2_sensor_msgs std_msgs std_srvs builtin_interfaces - rviz_common - rviz_default_plugins - rviz_ogre_vendor - rviz_rendering - interactive_markers - Qt5 + # nav2_map_server ) set(libraries toolbox_common - SlamToolboxPlugin ceres_solver_plugin async_slam_toolbox sync_slam_toolbox localization_slam_toolbox lifelong_slam_toolbox + map_and_localization_slam_toolbox ) find_package(PkgConfig REQUIRED) find_package(Eigen3 REQUIRED) +find_package(CHOLMOD REQUIRED) find_package(CSparse REQUIRED) find_package(G2O REQUIRED) -find_package(Cholmod REQUIRED) find_package(LAPACK REQUIRED) find_package(Ceres REQUIRED COMPONENTS SuiteSparse) @@ -84,7 +74,8 @@ set(CMAKE_INCLUDE_CURRENT_DIR ON) add_definitions(-DQT_NO_KEYWORDS) find_package(Boost REQUIRED system serialization filesystem thread) -include_directories(include ${EIGEN3_INCLUDE_DIRS} +include_directories(include lib/karto_sdk/include + ${EIGEN3_INCLUDE_DIRS} ${CHOLMOD_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} ${TBB_INCLUDE_DIRS} @@ -94,7 +85,6 @@ include_directories(include ${EIGEN3_INCLUDE_DIRS} add_definitions(${EIGEN3_DEFINITIONS}) rosidl_generate_interfaces(${PROJECT_NAME} - srvs/Pause.srv srvs/ClearQueue.srv srvs/ToggleInteractive.srv srvs/Clear.srv @@ -107,24 +97,6 @@ rosidl_generate_interfaces(${PROJECT_NAME} DEPENDENCIES builtin_interfaces geometry_msgs std_msgs nav_msgs visualization_msgs ) -#### rviz Plugin -qt5_wrap_cpp(MOC_FILES rviz_plugin/slam_toolbox_rviz_plugin.hpp) -add_library(SlamToolboxPlugin SHARED - rviz_plugin/slam_toolbox_rviz_plugin.cpp - ${MOC_FILES}) -ament_target_dependencies(SlamToolboxPlugin - ${dependencies} -) -target_include_directories(SlamToolboxPlugin PUBLIC - ${Qt5Widgets_INCLUDE_DIRS} - ${OGRE_INCLUDE_DIRS} -) -target_link_libraries(SlamToolboxPlugin ${QT_LIBRARIES} rviz_common::rviz_common) -rosidl_target_interfaces(SlamToolboxPlugin ${PROJECT_NAME} "rosidl_typesupport_cpp") -target_compile_definitions(SlamToolboxPlugin PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") -target_compile_definitions(SlamToolboxPlugin PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDING_LIBRARY") -pluginlib_export_plugin_description_file(rviz_common rviz_plugins.xml) - #### Ceres Plugin add_library(ceres_solver_plugin solvers/ceres_solver.cpp) ament_target_dependencies(ceres_solver_plugin ${dependencies}) @@ -133,7 +105,8 @@ target_link_libraries(ceres_solver_plugin ${CERES_LIBRARIES} ${TBB_LIBRARIES} kartoSlamToolbox ) -rosidl_target_interfaces(ceres_solver_plugin ${PROJECT_NAME} "rosidl_typesupport_cpp") +rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") +target_link_libraries(ceres_solver_plugin "${cpp_typesupport_target}") pluginlib_export_plugin_description_file(slam_toolbox solver_plugins.xml) #### Tool lib for mapping @@ -142,7 +115,8 @@ ament_target_dependencies(toolbox_common ${dependencies} ) target_link_libraries(toolbox_common kartoSlamToolbox ${Boost_LIBRARIES}) -rosidl_target_interfaces(toolbox_common ${PROJECT_NAME} "rosidl_typesupport_cpp") +rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") +target_link_libraries(toolbox_common "${cpp_typesupport_target}") #### Mapping executibles add_library(async_slam_toolbox src/slam_toolbox_async.cpp) @@ -165,9 +139,10 @@ target_link_libraries(lifelong_slam_toolbox toolbox_common kartoSlamToolbox ${Bo add_executable(lifelong_slam_toolbox_node src/experimental/slam_toolbox_lifelong_node.cpp) target_link_libraries(lifelong_slam_toolbox_node lifelong_slam_toolbox) -#### Merging maps tool -add_executable(merge_maps_kinematic src/merge_maps_kinematic.cpp) -target_link_libraries(merge_maps_kinematic kartoSlamToolbox toolbox_common) +add_library(map_and_localization_slam_toolbox src/experimental/slam_toolbox_map_and_localization.cpp) +target_link_libraries(map_and_localization_slam_toolbox localization_slam_toolbox toolbox_common kartoSlamToolbox ${Boost_LIBRARIES}) +add_executable(map_and_localization_slam_toolbox_node src/experimental/slam_toolbox_map_and_localization_node.cpp) +target_link_libraries(map_and_localization_slam_toolbox_node map_and_localization_slam_toolbox) #### testing #if(BUILD_TESTING) @@ -184,21 +159,13 @@ install(TARGETS async_slam_toolbox_node sync_slam_toolbox_node localization_slam_toolbox_node lifelong_slam_toolbox_node - merge_maps_kinematic + map_and_localization_slam_toolbox_node ${libraries} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME} ) -install(TARGETS SlamToolboxPlugin - EXPORT SlamToolboxPlugin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include -) - install(DIRECTORY include/ DESTINATION include ) @@ -211,12 +178,11 @@ install(DIRECTORY config DESTINATION share/${PROJECT_NAME} ) -install(FILES solver_plugins.xml rviz_plugins.xml +install(FILES solver_plugins.xml DESTINATION share ) ament_export_include_directories(include) ament_export_libraries(${libraries}) ament_export_dependencies(${dependencies}) -ament_export_targets(SlamToolboxPlugin HAS_LIBRARY_TARGET) ament_package() diff --git a/README.md b/README.md index 785b2b2c3..1efdf9841 100644 --- a/README.md +++ b/README.md @@ -24,7 +24,7 @@ You can find this work [here](https://joss.theoj.org/papers/10.21105/joss.02783) # Introduction -Slam Toolbox is a set of tools and capabilities for 2D SLAM built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101) while at [Simbe Robotics](https://www.simberobotics.com/), maintained whil at Samsung Research, and largely in his free time. +Slam Toolbox is a set of tools and capabilities for 2D SLAM built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101) while at [Simbe Robotics](https://www.simberobotics.com/), maintained while at Samsung Research, and largely in his free time. This project contains the ability to do most everything any other available SLAM library, both free and paid, and more. This includes: - Ordinary point-and-shoot 2D SLAM mobile robotics folks expect (start, map, save pgm file) with some nice built in utilities like saving maps @@ -43,7 +43,7 @@ For running on live production robots, I recommend using the snap: slam-toolbox, This package has been benchmarked mapping building at 5x+ realtime up to about 30,000 sqft and 3x realtime up to about 60,000 sqft. with the largest area (I'm aware of) used was a 200,000 sq.ft. building in synchronous mode (e.i. processing all scans, regardless of lag), and *much* larger spaces in asynchronous mode. -The video below was collected at [Circuit Launch](https://www.circuitlaunch.com/) in Oakland, California. Thanks to [Silicon Valley Robotics](https://svrobo.org/) & Circuit Launch for being a testbed for some of this work. This data is currently available upon request, but its going to be included in a larger open-source dataset down the line. +The video below was collected at [Circuit Launch](https://www.circuitlaunch.com/) in Oakland, California. Thanks to [Silicon Valley Robotics](https://svrobo.org/) & Circuit Launch for being a testbed for some of this work. ![map_image](/images/circuit_launch.gif?raw=true "Map Image") @@ -174,8 +174,10 @@ The following are the services/topics that are exposed for use. See the rviz plu ## Published topics -| map | `nav_msgs/OccupancyGrid` | occupancy grid representation of the pose-graph at `map_update_interval` frequency | +| Topic | Type | Description | |-----|----|----| +| map | `nav_msgs/OccupancyGrid` | occupancy grid representation of the pose-graph at `map_update_interval` frequency | +| pose | `geometry_msgs/PoseWithCovarianceStamped` | pose of the base_frame in the configured map_frame along with the covariance calculated from the scan match | ## Exposed Services @@ -236,19 +238,25 @@ The following settings and options are exposed to you. My default configuration `enable_interactive_mode` - Whether or not to allow for interactive mode to be enabled. Interactive mode will retain a cache of laser scans mapped to their ID for visualization in interactive mode. As a result the memory for the process will increase. This is manually disabled in localization and lifelong modes since they would increase the memory utilization over time. Valid for either mapping or continued mapping modes. +`position_covariance_scale` - Amount to scale position covariance when publishing pose from scan match. This can be used to tune the influence of the pose position in a downstream localization filter. The covariance represents the uncertainty of the measurement, so scaling up the covariance will result in the pose position having less influence on downstream filters. Default: 1.0 + +`yaw_covariance_scale` - Amount to scale yaw covariance when publishing pose from scan match. See description of position_covariance_scale. Default: 1.0 + `resolution` - Resolution of the 2D occupancy map to generate `max_laser_range` - Maximum laser range to use for 2D occupancy map rastering `minimum_time_interval` - The minimum duration of time between scans to be processed in synchronous mode +`maximum_match_interval` - Max interval in seconds between scan matches in async mode. Note: Scan matches triggered by this condition won't update the map or scan buffer. Default: -1 (infinity). + `transform_timeout` - TF timeout for looking up transforms `tf_buffer_duration` - Duration to store TF messages for lookup. Set high if running offline at multiple times speed in synchronous mode. `stack_size_to_use` - The number of bytes to reset the stack size to, to enable serialization/deserialization of files. A liberal default is 40000000, but less is fine. -`minimum_travel_distance` - Minimum distance of travel before processing a new scan +`minimum_travel_distance` - Minimum distance of travel before processing a new scan into the map, or adding a scan to the queue for processing in sync or localization modes. ## Matcher Params @@ -256,7 +264,7 @@ The following settings and options are exposed to you. My default configuration `use_scan_barycenter` - Whether to use the barycenter or scan pose -`minimum_travel_heading` - Minimum changing in heading to justify an update +`minimum_travel_heading` - Minimum changing in heading before processing a new scan into the map `scan_buffer_size` - The number of scans to buffer into a chain, also used as the number of scans in the circular buffer of localization mode diff --git a/config/mapper_params_localization.yaml b/config/mapper_params_localization.yaml index 22b9c99e3..371395e1b 100644 --- a/config/mapper_params_localization.yaml +++ b/config/mapper_params_localization.yaml @@ -36,18 +36,18 @@ slam_toolbox: minimum_travel_heading: 0.5 scan_buffer_size: 3 scan_buffer_maximum_scan_distance: 10.0 - link_match_minimum_response_fine: 0.1 + link_match_minimum_response_fine: 0.1 link_scan_maximum_distance: 1.5 - do_loop_closing: true + do_loop_closing: true loop_match_minimum_chain_size: 3 - loop_match_maximum_variance_coarse: 3.0 - loop_match_minimum_response_coarse: 0.35 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 loop_match_minimum_response_fine: 0.45 # Correlation Parameters - Correlation Parameters correlation_search_space_dimension: 0.5 correlation_search_space_resolution: 0.01 - correlation_search_space_smear_deviation: 0.1 + correlation_search_space_smear_deviation: 0.1 # Correlation Parameters - Loop Closure Parameters loop_search_space_dimension: 8.0 @@ -56,12 +56,12 @@ slam_toolbox: loop_search_maximum_distance: 3.0 # Scan Matcher Parameters - distance_variance_penalty: 0.5 - angle_variance_penalty: 1.0 + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 - fine_search_angle_offset: 0.00349 - coarse_search_angle_offset: 0.349 - coarse_angle_resolution: 0.0349 + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 minimum_angle_penalty: 0.9 minimum_distance_penalty: 0.5 use_response_expansion: true diff --git a/config/mapper_params_offline.yaml b/config/mapper_params_offline.yaml index c700dbb1c..421776c96 100644 --- a/config/mapper_params_offline.yaml +++ b/config/mapper_params_offline.yaml @@ -25,7 +25,6 @@ slam_toolbox: transform_timeout: 0.2 tf_buffer_duration: 14400. stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps - enable_interactive_mode: true # General Parameters use_scan_matching: true diff --git a/config/mapper_params_online_async.yaml b/config/mapper_params_online_async.yaml index 752053663..749b973b1 100644 --- a/config/mapper_params_online_async.yaml +++ b/config/mapper_params_online_async.yaml @@ -33,7 +33,6 @@ slam_toolbox: transform_timeout: 0.2 tf_buffer_duration: 30. stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps - enable_interactive_mode: true # General Parameters use_scan_matching: true diff --git a/config/mapper_params_online_sync.yaml b/config/mapper_params_online_sync.yaml index a48c04860..c28a08010 100644 --- a/config/mapper_params_online_sync.yaml +++ b/config/mapper_params_online_sync.yaml @@ -33,7 +33,6 @@ slam_toolbox: transform_timeout: 0.2 tf_buffer_duration: 30. stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps - enable_interactive_mode: true # General Parameters use_scan_matching: true diff --git a/include/slam_toolbox/experimental/slam_toolbox_map_and_localization.hpp b/include/slam_toolbox/experimental/slam_toolbox_map_and_localization.hpp new file mode 100644 index 000000000..0acd08951 --- /dev/null +++ b/include/slam_toolbox/experimental/slam_toolbox_map_and_localization.hpp @@ -0,0 +1,66 @@ +/* + * slam_toolbox + * Copyright (c) 2022, Steve Macenski + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +#ifndef SLAM_TOOLBOX__SLAM_TOOLBOX_MAP_AND_LOCALIZATION_HPP_ +#define SLAM_TOOLBOX__SLAM_TOOLBOX_MAP_AND_LOCALIZATION_HPP_ + +#include +#include "slam_toolbox/slam_toolbox_localization.hpp" +#include "std_srvs/srv/set_bool.hpp" + +namespace slam_toolbox +{ + +class MapAndLocalizationSlamToolbox : public LocalizationSlamToolbox +{ +public: + explicit MapAndLocalizationSlamToolbox(rclcpp::NodeOptions options); + virtual ~MapAndLocalizationSlamToolbox() {} + void loadPoseGraphByParams() override; + void configure() override; + +protected: + void laserCallback( + sensor_msgs::msg::LaserScan::ConstSharedPtr scan) override; + void localizePoseCallback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) override; + bool serializePoseGraphCallback( + const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr resp) override; + bool deserializePoseGraphCallback( + const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr resp) override; + bool setLocalizationModeCallback( + const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr resp); + LocalizedRangeScan * addScan( + LaserRangeFinder * laser, + const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan, + Pose2 & pose) override; + void toggleMode(bool enable_localization); + + std::shared_ptr> ssSetLocalizationMode_; + std::shared_ptr> + map_and_localization_pose_sub_; + +}; + +} // namespace slam_toolbox + +#endif // SLAM_TOOLBOX__SLAM_TOOLBOX_MAP_AND_LOCALIZATION_HPP_ diff --git a/include/slam_toolbox/get_pose_helper.hpp b/include/slam_toolbox/get_pose_helper.hpp index 4ddcfe665..46e049e2d 100644 --- a/include/slam_toolbox/get_pose_helper.hpp +++ b/include/slam_toolbox/get_pose_helper.hpp @@ -19,10 +19,11 @@ #ifndef SLAM_TOOLBOX__GET_POSE_HELPER_HPP_ #define SLAM_TOOLBOX__GET_POSE_HELPER_HPP_ -#include #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "karto_sdk/Mapper.h" #include "slam_toolbox/toolbox_types.hpp" -#include "../lib/karto_sdk/include/karto_sdk/Mapper.h" +#include +#include namespace pose_utils { @@ -31,39 +32,67 @@ namespace pose_utils class GetPoseHelper { public: - GetPoseHelper( - tf2_ros::Buffer * tf, - const std::string & base_frame, - const std::string & odom_frame) - : tf_(tf), base_frame_(base_frame), odom_frame_(odom_frame) + GetPoseHelper(tf2_ros::Buffer* tf, const std::string& base_frame, const std::string& odom_frame) + : tf_(tf), base_frame_(base_frame), odom_frame_(odom_frame) { } - bool getOdomPose(karto::Pose2 & karto_pose, const rclcpp::Time & t) + bool getOdomPose(karto::Pose2& karto_pose, const rclcpp::Time& t) { geometry_msgs::msg::TransformStamped base_ident, odom_pose; - base_ident.header.stamp = t; - base_ident.header.frame_id = base_frame_; + base_ident.header.stamp = t; + base_ident.header.frame_id = base_frame_; base_ident.transform.rotation.w = 1.0; - try { + try + { odom_pose = tf_->transform(base_ident, odom_frame_); - } catch (tf2::TransformException & e) { + } + catch (tf2::TransformException& e) + { return false; } const double yaw = tf2::getYaw(odom_pose.transform.rotation); - karto_pose = karto::Pose2(odom_pose.transform.translation.x, - odom_pose.transform.translation.y, yaw); + karto_pose = + karto::Pose2(odom_pose.transform.translation.x, odom_pose.transform.translation.y, yaw); + + return true; + } + + bool getMapPose(karto::Pose2& karto_pose, const rclcpp::Time& t, + const tf2::Transform& odom_to_map) + { + geometry_msgs::msg::TransformStamped base_ident, odom_pose, map_pose; + base_ident.header.stamp = t; + base_ident.header.frame_id = base_frame_; + base_ident.transform.rotation.w = 1.0; + + try + { + odom_pose = tf_->transform(base_ident, odom_frame_); + } + catch (tf2::TransformException& e) + { + return false; + } + + geometry_msgs::msg::TransformStamped odom_to_map_msg; + odom_to_map_msg.transform = tf2::toMsg(odom_to_map); + tf2::doTransform(odom_pose, map_pose, odom_to_map_msg); + + const double yaw = tf2::getYaw(map_pose.transform.rotation); + karto_pose = + karto::Pose2(map_pose.transform.translation.x, map_pose.transform.translation.y, yaw); return true; } private: - tf2_ros::Buffer * tf_; + tf2_ros::Buffer* tf_; std::string base_frame_, odom_frame_; }; -} // namespace pose_utils +} // namespace pose_utils -#endif // SLAM_TOOLBOX__GET_POSE_HELPER_HPP_ +#endif // SLAM_TOOLBOX__GET_POSE_HELPER_HPP_ diff --git a/include/slam_toolbox/laser_utils.hpp b/include/slam_toolbox/laser_utils.hpp index 56c2e1041..dfef8244f 100644 --- a/include/slam_toolbox/laser_utils.hpp +++ b/include/slam_toolbox/laser_utils.hpp @@ -100,6 +100,7 @@ class ScanHolder ~ScanHolder(); sensor_msgs::msg::LaserScan getCorrectedScan(const int & id); void addScan(const sensor_msgs::msg::LaserScan scan); + void clearScans(); private: std::unique_ptr> current_scans_; diff --git a/include/slam_toolbox/loop_closure_assistant.hpp b/include/slam_toolbox/loop_closure_assistant.hpp index 86e141c2f..05020e314 100644 --- a/include/slam_toolbox/loop_closure_assistant.hpp +++ b/include/slam_toolbox/loop_closure_assistant.hpp @@ -26,10 +26,9 @@ #include #include "tf2_ros/transform_broadcaster.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2/utils.h" #include "rclcpp/rclcpp.hpp" -#include "interactive_markers/interactive_marker_server.hpp" -#include "interactive_markers/menu_handler.hpp" #include "slam_toolbox/toolbox_types.hpp" #include "slam_toolbox/laser_utils.hpp" @@ -49,23 +48,10 @@ class LoopClosureAssistant ProcessType & processor_type); void clearMovedNodes(); - void processInteractiveFeedback( - const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr feedback); void publishGraph(); + void setMapper(karto::Mapper * mapper); private: - bool manualLoopClosureCallback( - const std::shared_ptr request_header, - const std::shared_ptr req, - std::shared_ptr resp); - bool clearChangesCallback( - const std::shared_ptr request_header, - const std::shared_ptr req, - std::shared_ptr resp); - bool interactiveModeCallback( - const std::shared_ptr request_header, - const std::shared_ptr req, - std::shared_ptr resp); void moveNode(const int& id, const Eigen::Vector3d& pose); void addMovedNodes(const int& id, Eigen::Vector3d vec); @@ -74,16 +60,10 @@ class LoopClosureAssistant laser_utils::ScanHolder * scan_holder_; rclcpp::Publisher::SharedPtr marker_publisher_; rclcpp::Publisher::SharedPtr scan_publisher_; - rclcpp::Service::SharedPtr ssClear_manual_; - rclcpp::Service::SharedPtr ssLoopClosure_; - rclcpp::Service::SharedPtr ssInteractive_; boost::mutex moved_nodes_mutex_; std::map moved_nodes_; karto::Mapper * mapper_; karto::ScanSolver * solver_; - std::unique_ptr interactive_server_; - boost::mutex interactive_mutex_; - bool interactive_mode_, enable_interactive_mode_; rclcpp::Node::SharedPtr node_; std::string map_frame_; PausedState & state_; diff --git a/include/slam_toolbox/merge_maps_kinematic.hpp b/include/slam_toolbox/merge_maps_kinematic.hpp deleted file mode 100644 index 12737c03e..000000000 --- a/include/slam_toolbox/merge_maps_kinematic.hpp +++ /dev/null @@ -1,108 +0,0 @@ -/* - * Author - * Copyright (c) 2018, Simbe Robotics, Inc. - * - * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE - * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY - * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS - * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. - * - * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO - * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS - * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND - * CONDITIONS. - * - */ - -/* Author: Steven Macenski */ - -#ifndef SLAM_TOOLBOX__MERGE_MAPS_KINEMATIC_HPP_ -#define SLAM_TOOLBOX__MERGE_MAPS_KINEMATIC_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "interactive_markers/interactive_marker_server.hpp" -#include "interactive_markers/menu_handler.hpp" -#include "tf2_ros/transform_broadcaster.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/message_filter.h" -#include "tf2/LinearMath/Matrix3x3.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" -#include "tf2/utils.h" - -#include "../lib/karto_sdk/include/karto_sdk/Mapper.h" -#include "slam_toolbox/toolbox_types.hpp" -#include "slam_toolbox/toolbox_msgs.hpp" -#include "slam_toolbox/laser_utils.hpp" -#include "slam_toolbox/visualization_utils.hpp" - -using namespace toolbox_types; // NOLINT -using namespace karto; // NOLINT - -class MergeMapsKinematic : public rclcpp::Node -{ - typedef std::vector::iterator LocalizedRangeScansVecIt; - typedef karto::LocalizedRangeScanVector::iterator LocalizedRangeScansIt; - -public: - MergeMapsKinematic(); - ~MergeMapsKinematic(); - // setup - void configure(); - -private: - // callback - bool mergeMapCallback( - const std::shared_ptr request_header, - const std::shared_ptr req, - std::shared_ptr resp); - bool addSubmapCallback( - const std::shared_ptr request_header, - const std::shared_ptr req, - std::shared_ptr resp); - void processInteractiveFeedback( - visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr feedback); - void kartoToROSOccupancyGrid( - const karto::LocalizedRangeScanVector & scans, - nav_msgs::srv::GetMap::Response & map); - void transformScan(LocalizedRangeScansIt iter, tf2::Transform & submap_correction); - - // apply transformation to correct pose - karto::Pose2 applyCorrection(const karto::Pose2 & pose, const tf2::Transform & submap_correction); - karto::Vector2 applyCorrection( - const karto::Vector2 & pose, - const tf2::Transform & submap_correction); - - // ROS-y-ness - std::vector>> sstmS_; - std::vector>> sstS_; - std::shared_ptr> ssMap_; - std::shared_ptr> ssSubmap_; - - // karto bookkeeping - std::map lasers_; - std::vector> dataset_vec_; - - // TF - std::unique_ptr tfB_; - - // visualization - std::unique_ptr interactive_server_; - - // state - std::map submap_locations_; - std::vector scans_vec_; - std::map submap_marker_transform_; - double resolution_; - int num_submaps_; -}; - -#endif // SLAM_TOOLBOX__MERGE_MAPS_KINEMATIC_HPP_ diff --git a/include/slam_toolbox/serialization.hpp b/include/slam_toolbox/serialization.hpp index 7d1aaaae4..bcdd426a9 100644 --- a/include/slam_toolbox/serialization.hpp +++ b/include/slam_toolbox/serialization.hpp @@ -23,7 +23,7 @@ #include #include #include "rclcpp/rclcpp.hpp" -#include "../lib/karto_sdk/include/karto_sdk/Mapper.h" +#include "karto_sdk/Mapper.h" namespace serialization { @@ -34,7 +34,7 @@ inline bool fileExists(const std::string & name) return stat(name.c_str(), &buffer) == 0; } -inline void write( +inline bool write( const std::string & filename, karto::Mapper & mapper, karto::Dataset & dataset, @@ -43,9 +43,11 @@ inline void write( try { mapper.SaveToFile(filename + std::string(".posegraph")); dataset.SaveToFile(filename + std::string(".data")); + return true; } catch (boost::archive::archive_exception e) { RCLCPP_ERROR(node->get_logger(), "Failed to write file: Exception %s", e.what()); + return false; } } diff --git a/include/slam_toolbox/slam_mapper.hpp b/include/slam_toolbox/slam_mapper.hpp index 869a632ef..a902cab5d 100644 --- a/include/slam_toolbox/slam_mapper.hpp +++ b/include/slam_toolbox/slam_mapper.hpp @@ -20,7 +20,9 @@ #define SLAM_TOOLBOX__SLAM_MAPPER_HPP_ #include +#include "geometry_msgs/msg/quaternion.hpp" #include "rclcpp/rclcpp.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2/utils.h" #include "slam_toolbox/toolbox_types.hpp" diff --git a/include/slam_toolbox/slam_toolbox_common.hpp b/include/slam_toolbox/slam_toolbox_common.hpp index 9274ba545..035f029a3 100644 --- a/include/slam_toolbox/slam_toolbox_common.hpp +++ b/include/slam_toolbox/slam_toolbox_common.hpp @@ -19,68 +19,73 @@ #ifndef SLAM_TOOLBOX__SLAM_TOOLBOX_COMMON_HPP_ #define SLAM_TOOLBOX__SLAM_TOOLBOX_COMMON_HPP_ -#include #include -#include -#include -#include -#include #include -#include #include +#include +#include +#include +#include +#include +#include -#include "rclcpp/rclcpp.hpp" #include "message_filters/subscriber.h" -#include "tf2_ros/transform_broadcaster.h" -#include "tf2_ros/transform_listener.h" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/bool.hpp" +#include "std_srvs/srv/set_bool.hpp" +#include "std_srvs/srv/trigger.hpp" +#include "tf2/LinearMath/Matrix3x3.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2_ros/create_timer_ros.h" #include "tf2_ros/message_filter.h" -#include "tf2/LinearMath/Matrix3x3.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" -#include "tf2_sensor_msgs/tf2_sensor_msgs.h" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_listener.h" #include "pluginlib/class_loader.hpp" -#include "slam_toolbox/toolbox_types.hpp" -#include "slam_toolbox/slam_mapper.hpp" -#include "slam_toolbox/snap_utils.hpp" -#include "slam_toolbox/laser_utils.hpp" +#include "maidbot_std_srvs/srv/get_compressed_map.hpp" + #include "slam_toolbox/get_pose_helper.hpp" -#include "slam_toolbox/map_saver.hpp" +#include "slam_toolbox/laser_utils.hpp" #include "slam_toolbox/loop_closure_assistant.hpp" +#include "slam_toolbox/map_saver.hpp" +#include "slam_toolbox/slam_mapper.hpp" +#include "slam_toolbox/snap_utils.hpp" +#include "slam_toolbox/toolbox_types.hpp" namespace slam_toolbox { // dirty, dirty cheat I love -using namespace ::toolbox_types; // NOLINT -using namespace ::karto; // NOLINT +using namespace ::toolbox_types; // NOLINT +using namespace ::karto; // NOLINT class SlamToolbox : public rclcpp::Node { public: explicit SlamToolbox(rclcpp::NodeOptions); SlamToolbox(); - ~SlamToolbox(); - void configure(); + virtual ~SlamToolbox(); + virtual void configure(); virtual void loadPoseGraphByParams(); protected: // threads void publishVisualizations(); - void publishTransformLoop(const double & transform_publish_period); + void publishTransformLoop(const double& transform_publish_period); // setup void setParams(); void setSolver(); void setROSInterfaces(); + void resetSlam(); // callbacks virtual void laserCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr scan) = 0; - bool mapCallback( - const std::shared_ptr request_header, - const std::shared_ptr req, - std::shared_ptr res); + void setInitialPoseCallback(geometry_msgs::msg::Pose2D::SharedPtr initial_pose); + bool mapCallback(const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr res); virtual bool serializePoseGraphCallback( const std::shared_ptr request_header, const std::shared_ptr req, @@ -89,62 +94,71 @@ class SlamToolbox : public rclcpp::Node const std::shared_ptr request_header, const std::shared_ptr req, std::shared_ptr resp); + void startSlamCallback(const std::shared_ptr req, + std::shared_ptr resp); + void stopSlamCallback(const std::shared_ptr req, + std::shared_ptr resp); // Loaders - void loadSerializedPoseGraph(std::unique_ptr &, std::unique_ptr &); + void loadSerializedPoseGraph(std::unique_ptr&, std::unique_ptr&); // functional bits - karto::LaserRangeFinder * getLaser(const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan); - virtual karto::LocalizedRangeScan * addScan( - karto::LaserRangeFinder * laser, const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan, - karto::Pose2 & karto_pose); - karto::LocalizedRangeScan * addScan(karto::LaserRangeFinder * laser, PosedScan & scanWPose); + karto::LaserRangeFinder* getLaser(const sensor_msgs::msg::LaserScan::ConstSharedPtr& scan); + virtual karto::LocalizedRangeScan* addScan( + karto::LaserRangeFinder* laser, const sensor_msgs::msg::LaserScan::ConstSharedPtr& scan, + karto::Pose2& karto_pose); + karto::LocalizedRangeScan* addScan(karto::LaserRangeFinder* laser, PosedScan& scanWPose); bool updateMap(); - tf2::Stamped setTransformFromPoses( - const karto::Pose2 & pose, - const karto::Pose2 & karto_pose, const rclcpp::Time & t, - const bool & update_reprocessing_transform); - karto::LocalizedRangeScan * getLocalizedRangeScan( - karto::LaserRangeFinder * laser, - const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan, - karto::Pose2 & karto_pose); - bool shouldStartWithPoseGraph( - std::string & filename, geometry_msgs::msg::Pose2D & pose, - bool & start_at_dock); - bool shouldProcessScan( - const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan, - const karto::Pose2 & pose); + bool waitForTransform(const std::string& scan_frame, const rclcpp::Time& stamp); + tf2::Stamped setTransformFromPoses(const karto::Pose2& pose, + const karto::Pose2& karto_pose, + const rclcpp::Time& t, + const bool& update_reprocessing_transform); + karto::LocalizedRangeScan* getLocalizedRangeScan( + karto::LaserRangeFinder* laser, const sensor_msgs::msg::LaserScan::ConstSharedPtr& scan, + karto::Pose2& karto_pose); + bool shouldStartWithPoseGraph(std::string& filename, geometry_msgs::msg::Pose2D& pose, + bool& start_at_dock); + bool shouldProcessScan(const sensor_msgs::msg::LaserScan::ConstSharedPtr& scan, + const karto::Pose2& pose); + void publishPose(const Pose2& pose, const Matrix3& cov, const rclcpp::Time& t); + void toggleScanProcessing(); // pausing bits - bool isPaused(const PausedApplication & app); - bool pauseNewMeasurementsCallback( - const std::shared_ptr request_header, - const std::shared_ptr req, - std::shared_ptr resp); + bool isPaused(const PausedApplication& app); + bool pauseNewMeasurementsCallback(const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr resp); // ROS-y-ness std::unique_ptr tf_; std::unique_ptr tfL_; std::unique_ptr tfB_; - std::unique_ptr> scan_filter_sub_; - std::unique_ptr> scan_filter_; + rclcpp::Subscription::SharedPtr scan_sub_; std::shared_ptr> sst_; std::shared_ptr> sstm_; - std::shared_ptr> ssMap_; - std::shared_ptr> ssPauseMeasurements_; + std::shared_ptr> pose_pub_; + std::shared_ptr> status_pub_; + std::shared_ptr> ssMap_; + std::shared_ptr> ssPauseMeasurements_; std::shared_ptr> ssSerialize_; std::shared_ptr> ssDesserialize_; + std::shared_ptr> ssStart_, ssStop_; // Storage for ROS parameters std::string odom_frame_, map_frame_, base_frame_, map_name_, scan_topic_; rclcpp::Duration transform_timeout_, minimum_time_interval_; - rclcpp::Time scan_timestamped; + rclcpp::Duration maximum_match_interval_; + std_msgs::msg::Header scan_header; int throttle_scans_; double resolution_; - bool first_measurement_, enable_interactive_mode_; + double position_covariance_scale_; + double yaw_covariance_scale_; + bool first_measurement_, paused_at_startup_; // Book keeping + bool slam_running_; std::unique_ptr smapper_; std::unique_ptr dataset_; std::map lasers_; @@ -171,6 +185,6 @@ class SlamToolbox : public rclcpp::Node std::shared_ptr solver_; }; -} // namespace slam_toolbox +} // namespace slam_toolbox -#endif // SLAM_TOOLBOX__SLAM_TOOLBOX_COMMON_HPP_ +#endif // SLAM_TOOLBOX__SLAM_TOOLBOX_COMMON_HPP_ diff --git a/include/slam_toolbox/slam_toolbox_localization.hpp b/include/slam_toolbox/slam_toolbox_localization.hpp index 36452a430..d956a1746 100644 --- a/include/slam_toolbox/slam_toolbox_localization.hpp +++ b/include/slam_toolbox/slam_toolbox_localization.hpp @@ -30,29 +30,29 @@ class LocalizationSlamToolbox : public SlamToolbox { public: explicit LocalizationSlamToolbox(rclcpp::NodeOptions options); - ~LocalizationSlamToolbox() {} + virtual ~LocalizationSlamToolbox() {} virtual void loadPoseGraphByParams(); protected: - void laserCallback( + virtual void laserCallback( sensor_msgs::msg::LaserScan::ConstSharedPtr scan) override; - void localizePoseCallback( + virtual void localizePoseCallback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); bool clearLocalizationBuffer( const std::shared_ptr request_header, const std::shared_ptr req, std::shared_ptr resp); - bool serializePoseGraphCallback( + virtual bool serializePoseGraphCallback( const std::shared_ptr request_header, const std::shared_ptr req, std::shared_ptr resp) override; - bool deserializePoseGraphCallback( + virtual bool deserializePoseGraphCallback( const std::shared_ptr request_header, const std::shared_ptr req, std::shared_ptr resp) override; - LocalizedRangeScan * addScan( + virtual LocalizedRangeScan * addScan( LaserRangeFinder * laser, const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan, Pose2 & pose) override; diff --git a/include/slam_toolbox/toolbox_msgs.hpp b/include/slam_toolbox/toolbox_msgs.hpp index 1300962d4..0797f8fb2 100644 --- a/include/slam_toolbox/toolbox_msgs.hpp +++ b/include/slam_toolbox/toolbox_msgs.hpp @@ -29,7 +29,6 @@ #include "visualization_msgs/msg/interactive_marker_control.hpp" #include "visualization_msgs/msg/interactive_marker_feedback.hpp" -#include "slam_toolbox/srv/pause.hpp" #include "slam_toolbox/srv/clear_queue.hpp" #include "slam_toolbox/srv/toggle_interactive.hpp" #include "slam_toolbox/srv/clear.hpp" diff --git a/include/slam_toolbox/toolbox_types.hpp b/include/slam_toolbox/toolbox_types.hpp index f9e6b20c4..dcf6a7273 100644 --- a/include/slam_toolbox/toolbox_types.hpp +++ b/include/slam_toolbox/toolbox_types.hpp @@ -24,11 +24,10 @@ #include #include "tf2_ros/buffer.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2/transform_datatypes.h" -// god... getting this to work in ROS2 was a real pain -#include "../lib/karto_sdk/include/karto_sdk/Mapper.h" +#include "karto_sdk/Mapper.h" #include "slam_toolbox/toolbox_msgs.hpp" // compute linear index for given map coords diff --git a/lib/karto_sdk/CMakeLists.txt b/lib/karto_sdk/CMakeLists.txt index be14006e4..6502466cd 100644 --- a/lib/karto_sdk/CMakeLists.txt +++ b/lib/karto_sdk/CMakeLists.txt @@ -12,14 +12,13 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(Eigen3 REQUIRED) find_package(Boost REQUIRED system serialization filesystem thread) -find_package(TBB REQUIRED) +find_package(TBB REQUIRED NO_CMAKE_PACKAGE_REGISTRY) set(dependencies rclcpp ) -include_directories(include ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} +include_directories(include ${EIGEN3_INCLUDE_DIRS} ${Boost_INCLUDE_DIR} ${BOOST_INCLUDE_DIRS} ${TBB_INCLUDE_DIRS} @@ -27,10 +26,10 @@ include_directories(include ${catkin_INCLUDE_DIRS} add_definitions(${EIGEN3_DEFINITIONS}) -include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${TBB_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) +include_directories(include ${EIGEN3_INCLUDE_DIRS} ${TBB_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) add_library(kartoSlamToolbox SHARED src/Karto.cpp src/Mapper.cpp) ament_target_dependencies(kartoSlamToolbox ${dependencies}) -target_link_libraries(kartoSlamToolbox ${Boost_LIBRARIES} ${TBB_LIBRARIES}) +target_link_libraries(kartoSlamToolbox ${Boost_LIBRARIES} TBB::tbb) install(DIRECTORY include/ DESTINATION include/ diff --git a/lib/karto_sdk/include/karto_sdk/Karto.h b/lib/karto_sdk/include/karto_sdk/Karto.h index c03f67578..5cc9c837a 100644 --- a/lib/karto_sdk/include/karto_sdk/Karto.h +++ b/lib/karto_sdk/include/karto_sdk/Karto.h @@ -5606,6 +5606,16 @@ class LocalizedRangeScan : public LaserRangeScan m_BoundingBox = bbox; } + inline void SetCovariance(const Matrix3& covariance) + { + m_Covariance = covariance; + } + + inline const Matrix3 GetCovariance() const + { + return m_Covariance; + } + /** * Get point readings in local coordinates */ @@ -5734,6 +5744,17 @@ class LocalizedRangeScan : public LaserRangeScan */ Pose2 m_CorrectedPose; + /** + * Covariance of corrected pose. + * + * | var-x cov-xy 0 | + * | | + * | cov-xy var-y 0 | + * | | + * | 0 0 var-yaw | + */ + Matrix3 m_Covariance; + protected: /** * Average of all the point readings @@ -6557,6 +6578,7 @@ class Dataset } } + m_SensorNameLookup.clear(); m_Lasers.clear(); m_Data.clear(); @@ -6578,16 +6600,16 @@ class Dataset template void serialize(Archive & ar, const unsigned int version) { - std::cout << "**Serializing Dataset**\n"; - std::cout << "Dataset <- m_SensorNameLookup\n"; + // std::cout << "**Serializing Dataset**\n"; + // std::cout << "Dataset <- m_SensorNameLookup\n"; ar & BOOST_SERIALIZATION_NVP(m_SensorNameLookup); - std::cout << "Dataset <- m_Data\n"; + // std::cout << "Dataset <- m_Data\n"; ar & BOOST_SERIALIZATION_NVP(m_Data); - std::cout << "Dataset <- m_Lasers\n"; + // std::cout << "Dataset <- m_Lasers\n"; ar & BOOST_SERIALIZATION_NVP(m_Lasers); - std::cout << "Dataset <- m_pDatasetInfo\n"; + // std::cout << "Dataset <- m_pDatasetInfo\n"; ar & BOOST_SERIALIZATION_NVP(m_pDatasetInfo); - std::cout << "**Finished serializing Dataset**\n"; + // std::cout << "**Finished serializing Dataset**\n"; } }; // Dataset BOOST_SERIALIZATION_ASSUME_ABSTRACT(Dataset) diff --git a/lib/karto_sdk/include/karto_sdk/Mapper.h b/lib/karto_sdk/include/karto_sdk/Mapper.h index 6f5687c0d..7090c627c 100644 --- a/lib/karto_sdk/include/karto_sdk/Mapper.h +++ b/lib/karto_sdk/include/karto_sdk/Mapper.h @@ -21,13 +21,13 @@ #include #include #include -#include +#include #include #include #include #include -#include "tbb/parallel_do.h" +#include "tbb/parallel_for_each.h" #include "tbb/parallel_for.h" #include "tbb/blocked_range.h" @@ -689,9 +689,9 @@ class Graph template void serialize(Archive & ar, const unsigned int version) { - std::cout << "Graph <- m_Edges; "; + // std::cout << "Graph <- m_Edges; "; ar & BOOST_SERIALIZATION_NVP(m_Edges); - std::cout << "Graph <- m_Vertices\n"; + // std::cout << "Graph <- m_Vertices\n"; ar & BOOST_SERIALIZATION_NVP(m_Vertices); } }; // Graph @@ -910,6 +910,19 @@ class KARTO_EXPORT MapperGraph : public Graph const Name & rSensorName, kt_int32u & rStartNum); + /** + * Finds the shortest distance through the graph from the source to the + * target vertex (assumes each node to node traversal is one unit of distance). + * @param source vertex to start at + * @param target vertex to stop at + * @param vertex_map map from vertex id's to vertex objects + * @return minimum distance from source to target, INT_MAX if they aren't connected + */ + int FindShortestDistanceBetweenVertices( + karto::Vertex* source, + karto::Vertex* target, + const std::map*>& vertex_map); + private: /** * Mapper of this graph @@ -933,13 +946,13 @@ class KARTO_EXPORT MapperGraph : public Graph template void serialize(Archive & ar, const unsigned int version) { - std::cout << "MapperGraph <- Graph; "; + // std::cout << "MapperGraph <- Graph; "; ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Graph); - std::cout << "MapperGraph <- m_pMapper; "; + // std::cout << "MapperGraph <- m_pMapper; "; ar & BOOST_SERIALIZATION_NVP(m_pMapper); - std::cout << "MapperGraph <- m_pLoopScanMatcher; "; + // std::cout << "MapperGraph <- m_pLoopScanMatcher; "; ar & BOOST_SERIALIZATION_NVP(m_pLoopScanMatcher); - std::cout << "MapperGraph <- m_pTraversal\n"; + // std::cout << "MapperGraph <- m_pTraversal\n"; ar & BOOST_SERIALIZATION_NVP(m_pTraversal); } }; // MapperGraph @@ -1057,6 +1070,16 @@ class ScanSolver std::cout << "GetNodeOrientation method not implemented for this solver type." << std::endl; } + virtual void SetNodeConstant(const int & unique_id) + { + std::cout << "SetNodeConstant method not implemented for this solver type." << std::endl; + } + + virtual void SetNodeVariable(const int & unique_id) + { + std::cout << "SetNodeVariable method not implemented for this solver type." << std::endl; + } + friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) @@ -1483,6 +1506,7 @@ class KARTO_EXPORT ScanMatcher m_pCorrelationGrid(NULL), m_pSearchSpaceProbs(NULL), m_pGridLookup(NULL), + m_pPoseResponse(NULL), m_doPenalize(false) { } @@ -1519,13 +1543,26 @@ class KARTO_EXPORT ScanMatcher ar & BOOST_SERIALIZATION_NVP(m_nAngles); ar & BOOST_SERIALIZATION_NVP(m_searchAngleResolution); ar & BOOST_SERIALIZATION_NVP(m_doPenalize); + + // Note - m_pPoseResponse is generally only ever defined within the + // execution of ScanMatcher::CorrelateScan and used as a temporary + // accumulator for multithreaded matching results. It would normally + // not make sense to serialize, but we don't want to break compatibility + // with previously serialized data. Gen some dummy data that we free + // immediately after so that we don't alloc here and leak. kt_int32u poseResponseSize = static_cast(m_xPoses.size() * m_yPoses.size() * m_nAngles); - if (Archive::is_loading::value) { - m_pPoseResponse = new std::pair[poseResponseSize]; - } + + // We could check first if m_pPoseResponse == nullptr for good measure, but + // based on the codepaths it should always be freed and set to null outside of + // any execution of ScanMatcher::CorrelateScan, so go ahead and alloc here. + m_pPoseResponse = new std::pair[poseResponseSize]; ar & boost::serialization::make_array>(m_pPoseResponse, poseResponseSize); + + // Aaaand now, clean up the dummy data + delete[] m_pPoseResponse; + m_pPoseResponse = nullptr; } }; // ScanMatcher @@ -1922,7 +1959,7 @@ struct LocalizationScanVertex Vertex * vertex; }; -typedef std::queue LocalizationScanVertices; +typedef std::deque LocalizationScanVertices; class KARTO_EXPORT Mapper : public Module { @@ -1980,9 +2017,15 @@ class KARTO_EXPORT Mapper : public Module * is that of the range device originating the scan. Note that the mapper will set corrected pose * information in the scan object. * + * @param force_match_only Flag to force a scan match regardless of distance or angle traveled and to only return + * the resulting pose instead of adding the scan to the map. + * * @return true if the scan was added successfully, false otherwise */ - virtual kt_bool Process(LocalizedRangeScan * pScan); + virtual kt_bool Process( + LocalizedRangeScan * pScan, + Matrix3 * covariance = nullptr, + bool force_match_only = false); /** * Process an Object @@ -1990,14 +2033,20 @@ class KARTO_EXPORT Mapper : public Module virtual kt_bool Process(Object * pObject); // processors - kt_bool ProcessAtDock(LocalizedRangeScan * pScan); - kt_bool ProcessAgainstNode(LocalizedRangeScan * pScan, const int & nodeId); - kt_bool ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool addScanToLocalizationBuffer = false); - kt_bool ProcessLocalization(LocalizedRangeScan * pScan); + kt_bool ProcessAtDock(LocalizedRangeScan * pScan, Matrix3 * covariance = nullptr); + kt_bool ProcessAgainstNode(LocalizedRangeScan * pScan, const int & nodeId, Matrix3 * covariance = nullptr); + kt_bool ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool addScanToLocalizationBuffer = false, Matrix3 * covariance = nullptr); + kt_bool ProcessLocalization(LocalizedRangeScan * pScan, Matrix3 * covariance = nullptr, bool force_match_only = false); kt_bool RemoveNodeFromGraph(Vertex *); void AddScanToLocalizationBuffer(LocalizedRangeScan * pScan, Vertex * scan_vertex); void ClearLocalizationBuffer(); + /** + * Checks if removing the given vertex would disconnect the localization graph from the mapping graph. + * @param localization_vertex + */ + bool cutsLocalizationAndMappingGraphs(const LocalizationScanVertex & localization_vertex); + /** * Returns all processed scans added to the mapper. * NOTE: The returned scans have their corrected pose updated. @@ -2072,6 +2121,11 @@ class KARTO_EXPORT Mapper : public Module m_pGraph->CorrectPoses(); } + const LocalizationScanVertices& GetLocalizationVertices() + { + return m_LocalizationScanVertices; + } + protected: void InitializeParameters(); @@ -2342,16 +2396,16 @@ class KARTO_EXPORT Mapper : public Module template void serialize(Archive & ar, const unsigned int version) { - std::cout << "Mapper <- Module\n"; + // std::cout << "Mapper <- Module\n"; ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Module); ar & BOOST_SERIALIZATION_NVP(m_Initialized); - std::cout << "Mapper <- m_pSequentialScanMatcher\n"; + // std::cout << "Mapper <- m_pSequentialScanMatcher\n"; ar & BOOST_SERIALIZATION_NVP(m_pSequentialScanMatcher); - std::cout << "Mapper <- m_pGraph\n"; + // std::cout << "Mapper <- m_pGraph\n"; ar & BOOST_SERIALIZATION_NVP(m_pGraph); - std::cout << "Mapper <- m_pMapperSensorManager\n"; + // std::cout << "Mapper <- m_pMapperSensorManager\n"; ar & BOOST_SERIALIZATION_NVP(m_pMapperSensorManager); - std::cout << "Mapper <- m_Listeners\n"; + // std::cout << "Mapper <- m_Listeners\n"; ar & BOOST_SERIALIZATION_NVP(m_Listeners); ar & BOOST_SERIALIZATION_NVP(m_pUseScanMatching); ar & BOOST_SERIALIZATION_NVP(m_pUseScanBarycenter); @@ -2382,7 +2436,7 @@ class KARTO_EXPORT Mapper : public Module ar & BOOST_SERIALIZATION_NVP(m_pMinimumAnglePenalty); ar & BOOST_SERIALIZATION_NVP(m_pMinimumDistancePenalty); ar & BOOST_SERIALIZATION_NVP(m_pUseResponseExpansion); - std::cout << "**Finished serializing Mapper**\n"; + // std::cout << "**Finished serializing Mapper**\n"; } public: diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index e42d07604..b6a9729da 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -15,23 +15,22 @@ * along with this program. If not, see . */ - -#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 "karto_sdk/Mapper.h" @@ -49,9 +48,9 @@ namespace karto // enable this for verbose debug information // #define KARTO_DEBUG - #define MAX_VARIANCE 500.0 - #define DISTANCE_PENALTY_GAIN 0.2 - #define ANGLE_PENALTY_GAIN 0.2 +#define MAX_VARIANCE 500.0 +#define DISTANCE_PENALTY_GAIN 0.2 +#define ANGLE_PENALTY_GAIN 0.2 //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -67,14 +66,14 @@ class ScanManager * Default constructor */ ScanManager(kt_int32u runningBufferMaximumSize, kt_double runningBufferMaximumDistance) - : m_pLastScan(NULL), - m_RunningBufferMaximumSize(runningBufferMaximumSize), - m_RunningBufferMaximumDistance(runningBufferMaximumDistance), - m_NextStateId(0) + : m_pLastScan(NULL), m_RunningBufferMaximumSize(runningBufferMaximumSize), + m_RunningBufferMaximumDistance(runningBufferMaximumDistance), m_NextStateId(0) { } - ScanManager() {} + ScanManager() + { + } /** * Destructor @@ -89,7 +88,7 @@ class ScanManager * Adds scan to vector of processed scans tagging scan with given unique id * @param pScan */ - inline void AddScan(LocalizedRangeScan * pScan, kt_int32s uniqueId) + inline void AddScan(LocalizedRangeScan* pScan, kt_int32s uniqueId) { // assign state id to scan pScan->SetStateId(m_NextStateId); @@ -107,7 +106,7 @@ class ScanManager * @param deviceId * @return last localized range scan */ - inline LocalizedRangeScan * GetLastScan() + inline LocalizedRangeScan* GetLastScan() { return m_pLastScan; } @@ -125,7 +124,7 @@ class ScanManager * Sets the last scan * @param pScan */ - void SetLastScan(LocalizedRangeScan * pScan) + void SetLastScan(LocalizedRangeScan* pScan) { m_pLastScan = pScan; } @@ -134,7 +133,7 @@ class ScanManager * Gets scans * @return scans */ - inline LocalizedRangeScanMap & GetScans() + inline LocalizedRangeScanMap& GetScans() { return m_Scans; } @@ -143,7 +142,7 @@ class ScanManager * Gets running scans * @return running scans */ - inline LocalizedRangeScanVector & GetRunningScans() + inline LocalizedRangeScanVector& GetRunningScans() { return m_RunningScans; } @@ -152,7 +151,7 @@ class ScanManager * Gets running scan buffer size * @return running scan buffer size */ - inline kt_int32u & GetRunningScanBufferSize() + inline kt_int32u& GetRunningScanBufferSize() { return m_RunningBufferMaximumSize; } @@ -161,7 +160,7 @@ class ScanManager * Sets running scan buffer size * @param rScanBufferSize */ - void SetRunningScanBufferSize(const kt_int32u & rScanBufferSize) + void SetRunningScanBufferSize(const kt_int32u& rScanBufferSize) { m_RunningBufferMaximumSize = rScanBufferSize; } @@ -170,7 +169,7 @@ class ScanManager * Sets running scan buffer maximum distance * @param rScanBufferMaxDistance */ - void SetRunningScanBufferMaximumDistance(const kt_int32u & rScanBufferMaxDistance) + void SetRunningScanBufferMaximumDistance(const kt_int32u& rScanBufferMaxDistance) { m_RunningBufferMaximumDistance = rScanBufferMaxDistance; } @@ -179,26 +178,26 @@ class ScanManager * Adds scan to vector of running scans * @param pScan */ - void AddRunningScan(LocalizedRangeScan * pScan) + void AddRunningScan(LocalizedRangeScan* pScan) { m_RunningScans.push_back(pScan); // vector has at least one element (first line of this function), so this is valid Pose2 frontScanPose = m_RunningScans.front()->GetSensorPose(); - Pose2 backScanPose = m_RunningScans.back()->GetSensorPose(); + Pose2 backScanPose = m_RunningScans.back()->GetSensorPose(); // cap vector size and remove all scans from front of vector that are too far from end of vector - kt_double squaredDistance = frontScanPose.GetPosition().SquaredDistance( - backScanPose.GetPosition()); + kt_double squaredDistance = + frontScanPose.GetPosition().SquaredDistance(backScanPose.GetPosition()); while (m_RunningScans.size() > m_RunningBufferMaximumSize || - squaredDistance > math::Square(m_RunningBufferMaximumDistance) - KT_TOLERANCE) + squaredDistance > math::Square(m_RunningBufferMaximumDistance) - KT_TOLERANCE) { // remove front of running scans m_RunningScans.erase(m_RunningScans.begin()); // recompute stats of running scans - frontScanPose = m_RunningScans.front()->GetSensorPose(); - backScanPose = m_RunningScans.back()->GetSensorPose(); + frontScanPose = m_RunningScans.front()->GetSensorPose(); + backScanPose = m_RunningScans.back()->GetSensorPose(); squaredDistance = frontScanPose.GetPosition().SquaredDistance(backScanPose.GetPosition()); } } @@ -207,13 +206,16 @@ class ScanManager * Finds and replaces a scan from m_scans with NULL * @param pScan */ - void RemoveScan(LocalizedRangeScan * pScan) + void RemoveScan(LocalizedRangeScan* pScan) { LocalizedRangeScanMap::iterator it = m_Scans.find(pScan->GetStateId()); - if (it != m_Scans.end()) { + if (it != m_Scans.end()) + { it->second = NULL; m_Scans.erase(it); - } else { + } + else + { std::cout << "Remove Scan: Failed to find scan in m_Scans" << std::endl; } } @@ -237,55 +239,58 @@ class ScanManager private: friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) + template + void serialize(Archive& ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(m_Scans); - ar & BOOST_SERIALIZATION_NVP(m_RunningScans); - ar & BOOST_SERIALIZATION_NVP(m_pLastScan); - ar & BOOST_SERIALIZATION_NVP(m_RunningBufferMaximumSize); - ar & BOOST_SERIALIZATION_NVP(m_RunningBufferMaximumDistance); - ar & BOOST_SERIALIZATION_NVP(m_NextStateId); + ar& BOOST_SERIALIZATION_NVP(m_Scans); + ar& BOOST_SERIALIZATION_NVP(m_RunningScans); + ar& BOOST_SERIALIZATION_NVP(m_pLastScan); + ar& BOOST_SERIALIZATION_NVP(m_RunningBufferMaximumSize); + ar& BOOST_SERIALIZATION_NVP(m_RunningBufferMaximumDistance); + ar& BOOST_SERIALIZATION_NVP(m_NextStateId); } private: LocalizedRangeScanMap m_Scans; LocalizedRangeScanVector m_RunningScans; - LocalizedRangeScan * m_pLastScan; + LocalizedRangeScan* m_pLastScan; kt_int32u m_NextStateId; kt_int32u m_RunningBufferMaximumSize; kt_double m_RunningBufferMaximumDistance; -}; // ScanManager +}; // ScanManager //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// -void MapperSensorManager::RegisterSensor(const Name & rSensorName) +void MapperSensorManager::RegisterSensor(const Name& rSensorName) { - if (GetScanManager(rSensorName) == NULL) { - m_ScanManagers[rSensorName] = new ScanManager( - m_RunningBufferMaximumSize, - m_RunningBufferMaximumDistance); + if (GetScanManager(rSensorName) == NULL) + { + m_ScanManagers[rSensorName] = + new ScanManager(m_RunningBufferMaximumSize, m_RunningBufferMaximumDistance); } } - /** * Gets scan from given device with given ID * @param rSensorName * @param scanNum * @return localized range scan */ -LocalizedRangeScan * MapperSensorManager::GetScan(const Name & rSensorName, kt_int32s scanIndex) +LocalizedRangeScan* MapperSensorManager::GetScan(const Name& rSensorName, kt_int32s scanIndex) { - ScanManager * pScanManager = GetScanManager(rSensorName); - if (pScanManager != NULL) { + ScanManager* pScanManager = GetScanManager(rSensorName); + if (pScanManager != NULL) + { LocalizedRangeScanMap::iterator it = pScanManager->GetScans().find(scanIndex); - if (it != pScanManager->GetScans().end()) { + if (it != pScanManager->GetScans().end()) + { return it->second; - } else { + } + else + { return nullptr; } } @@ -299,7 +304,7 @@ LocalizedRangeScan * MapperSensorManager::GetScan(const Name & rSensorName, kt_i * @param pLaserRangeFinder * @return last localized range scan of device */ -inline LocalizedRangeScan * MapperSensorManager::GetLastScan(const Name & rSensorName) +inline LocalizedRangeScan* MapperSensorManager::GetLastScan(const Name& rSensorName) { RegisterSensor(rSensorName); @@ -310,7 +315,7 @@ inline LocalizedRangeScan * MapperSensorManager::GetLastScan(const Name & rSenso * Sets the last scan of device of given scan * @param pScan */ -void MapperSensorManager::SetLastScan(LocalizedRangeScan * pScan) +void MapperSensorManager::SetLastScan(LocalizedRangeScan* pScan) { GetScanManager(pScan)->SetLastScan(pScan); } @@ -337,7 +342,7 @@ void MapperSensorManager::ClearLastScan(const Name& name) * Adds scan to scan vector of device that recorded scan * @param pScan */ -void MapperSensorManager::AddScan(LocalizedRangeScan * pScan) +void MapperSensorManager::AddScan(LocalizedRangeScan* pScan) { GetScanManager(pScan)->AddScan(pScan, m_NextScanId); m_Scans.insert({m_NextScanId, pScan}); @@ -348,7 +353,7 @@ void MapperSensorManager::AddScan(LocalizedRangeScan * pScan) * Adds scan to running scans of device that recorded scan * @param pScan */ -inline void MapperSensorManager::AddRunningScan(LocalizedRangeScan * pScan) +inline void MapperSensorManager::AddRunningScan(LocalizedRangeScan* pScan) { GetScanManager(pScan)->AddRunningScan(pScan); } @@ -357,15 +362,18 @@ inline void MapperSensorManager::AddRunningScan(LocalizedRangeScan * pScan) * Finds and replaces a scan from m_Scans with NULL * @param pScan */ -void MapperSensorManager::RemoveScan(LocalizedRangeScan * pScan) +void MapperSensorManager::RemoveScan(LocalizedRangeScan* pScan) { GetScanManager(pScan)->RemoveScan(pScan); LocalizedRangeScanMap::iterator it = m_Scans.find(pScan->GetUniqueId()); - if (it != m_Scans.end()) { + if (it != m_Scans.end()) + { it->second = NULL; m_Scans.erase(it); - } else { + } + else + { std::cout << "RemoveScan: Failed to find scan in m_Scans" << std::endl; } } @@ -375,7 +383,7 @@ void MapperSensorManager::RemoveScan(LocalizedRangeScan * pScan) * @param rSensorName * @return scans of device */ -inline LocalizedRangeScanMap & MapperSensorManager::GetScans(const Name & rSensorName) +inline LocalizedRangeScanMap& MapperSensorManager::GetScans(const Name& rSensorName) { return GetScanManager(rSensorName)->GetScans(); } @@ -385,17 +393,17 @@ inline LocalizedRangeScanMap & MapperSensorManager::GetScans(const Name & rSenso * @param rSensorName * @return running scans of device */ -inline LocalizedRangeScanVector & MapperSensorManager::GetRunningScans(const Name & rSensorName) +inline LocalizedRangeScanVector& MapperSensorManager::GetRunningScans(const Name& rSensorName) { return GetScanManager(rSensorName)->GetRunningScans(); } -void MapperSensorManager::ClearRunningScans(const Name & rSensorName) +void MapperSensorManager::ClearRunningScans(const Name& rSensorName) { GetScanManager(rSensorName)->ClearRunningScans(); } -inline kt_int32u MapperSensorManager::GetRunningScanBufferSize(const Name & rSensorName) +inline kt_int32u MapperSensorManager::GetRunningScanBufferSize(const Name& rSensorName) { return GetScanManager(rSensorName)->GetRunningScanBufferSize(); } @@ -405,7 +413,8 @@ void MapperSensorManager::SetRunningScanBufferSize(kt_int32u rScanBufferSize) m_RunningBufferMaximumSize = rScanBufferSize; std::vector names = GetSensorNames(); - for (uint i = 0; i != names.size(); i++) { + for (uint i = 0; i != names.size(); i++) + { GetScanManager(names[i])->SetRunningScanBufferSize(rScanBufferSize); } } @@ -415,7 +424,8 @@ void MapperSensorManager::SetRunningScanBufferMaximumDistance(kt_double rScanBuf m_RunningBufferMaximumDistance = rScanBufferMaxDistance; std::vector names = GetSensorNames(); - for (uint i = 0; i != names.size(); i++) { + for (uint i = 0; i != names.size(); i++) + { GetScanManager(names[i])->SetRunningScanBufferMaximumDistance(rScanBufferMaxDistance); } } @@ -430,10 +440,11 @@ LocalizedRangeScanVector MapperSensorManager::GetAllScans() forEach(ScanManagerMap, &m_ScanManagers) { - LocalizedRangeScanMap & rScans = iter->second->GetScans(); + LocalizedRangeScanMap& rScans = iter->second->GetScans(); LocalizedRangeScanMap::iterator it; - for (it = rScans.begin(); it != rScans.end(); ++it) { + for (it = rScans.begin(); it != rScans.end(); ++it) + { scans.push_back(it->second); } } @@ -446,7 +457,7 @@ LocalizedRangeScanVector MapperSensorManager::GetAllScans() */ void MapperSensorManager::Clear() { -// SensorManager::Clear(); + // SensorManager::Clear(); forEach(ScanManagerMap, &m_ScanManagers) { @@ -463,32 +474,38 @@ void MapperSensorManager::Clear() ScanMatcher::~ScanMatcher() { - if (m_pCorrelationGrid) { + if (m_pCorrelationGrid) + { delete m_pCorrelationGrid; } - if (m_pSearchSpaceProbs) { + if (m_pSearchSpaceProbs) + { delete m_pSearchSpaceProbs; } - if (m_pGridLookup) { + if (m_pGridLookup) + { delete m_pGridLookup; } } -ScanMatcher * ScanMatcher::Create( - Mapper * pMapper, kt_double searchSize, kt_double resolution, - kt_double smearDeviation, kt_double rangeThreshold) +ScanMatcher* ScanMatcher::Create(Mapper* pMapper, kt_double searchSize, kt_double resolution, + kt_double smearDeviation, kt_double rangeThreshold) { // invalid parameters - if (resolution <= 0) { + if (resolution <= 0) + { return NULL; } - if (searchSize <= 0) { + if (searchSize <= 0) + { return NULL; } - if (smearDeviation < 0) { + if (smearDeviation < 0) + { return NULL; } - if (rangeThreshold <= 0) { + if (rangeThreshold <= 0) + { return NULL; } @@ -506,17 +523,17 @@ ScanMatcher * ScanMatcher::Create( // create correlation grid assert(gridSize % 2 == 1); - CorrelationGrid * pCorrelationGrid = CorrelationGrid::CreateGrid(gridSize, gridSize, resolution, - smearDeviation); + CorrelationGrid* pCorrelationGrid = + CorrelationGrid::CreateGrid(gridSize, gridSize, resolution, smearDeviation); // create search space probabilities - Grid * pSearchSpaceProbs = Grid::CreateGrid(searchSpaceSideSize, - searchSpaceSideSize, resolution); + Grid* pSearchSpaceProbs = + Grid::CreateGrid(searchSpaceSideSize, searchSpaceSideSize, resolution); - ScanMatcher * pScanMatcher = new ScanMatcher(pMapper); - pScanMatcher->m_pCorrelationGrid = pCorrelationGrid; + ScanMatcher* pScanMatcher = new ScanMatcher(pMapper); + pScanMatcher->m_pCorrelationGrid = pCorrelationGrid; pScanMatcher->m_pSearchSpaceProbs = pSearchSpaceProbs; - pScanMatcher->m_pGridLookup = new GridIndexLookup(pCorrelationGrid); + pScanMatcher->m_pGridLookup = new GridIndexLookup(pCorrelationGrid); return pScanMatcher; } @@ -528,13 +545,13 @@ ScanMatcher * ScanMatcher::Create( * @param rMean output parameter of mean (best pose) of match * @param rCovariance output parameter of covariance of match * @param doPenalize whether to penalize matches further from the search center - * @param doRefineMatch whether to do finer-grained matching if coarse match is good (default is true) + * @param doRefineMatch whether to do finer-grained matching if coarse match is good (default is + * true) * @return strength of response */ -template -kt_double ScanMatcher::MatchScan( - LocalizedRangeScan * pScan, const T & rBaseScans, Pose2 & rMean, - Matrix3 & rCovariance, kt_bool doPenalize, kt_bool doRefineMatch) +template +kt_double ScanMatcher::MatchScan(LocalizedRangeScan* pScan, const T& rBaseScans, Pose2& rMean, + Matrix3& rCovariance, kt_bool doPenalize, kt_bool doRefineMatch) { /////////////////////////////////////// // set scan pose to be center of grid @@ -544,14 +561,14 @@ kt_double ScanMatcher::MatchScan( // scan has no readings; cannot do scan matching // best guess of pose is based off of adjusted odometer reading - if (pScan->GetNumberOfRangeReadings() == 0) { + if (pScan->GetNumberOfRangeReadings() == 0) + { rMean = scanPose; // maximum covariance - rCovariance(0, 0) = MAX_VARIANCE; // XX - rCovariance(1, 1) = MAX_VARIANCE; // YY - rCovariance(2, 2) = - 4 * math::Square(m_pMapper->m_pCoarseAngleResolution->GetValue()); // TH*TH + rCovariance(0, 0) = MAX_VARIANCE; // XX + rCovariance(1, 1) = MAX_VARIANCE; // YY + rCovariance(2, 2) = 4 * math::Square(m_pMapper->m_pCoarseAngleResolution->GetValue()); // TH*TH return 0.0; } @@ -563,7 +580,7 @@ kt_double ScanMatcher::MatchScan( Vector2 offset; offset.SetX(scanPose.GetX() - (0.5 * (roi.GetWidth() - 1) * m_pCorrelationGrid->GetResolution())); offset.SetY(scanPose.GetY() - - (0.5 * (roi.GetHeight() - 1) * m_pCorrelationGrid->GetResolution())); + (0.5 * (roi.GetHeight() - 1) * m_pCorrelationGrid->GetResolution())); // 4. set offset m_pCorrelationGrid->GetCoordinateConverter()->SetOffset(offset); @@ -575,70 +592,74 @@ kt_double ScanMatcher::MatchScan( // compute how far to search in each direction Vector2 searchDimensions(m_pSearchSpaceProbs->GetWidth(), - m_pSearchSpaceProbs->GetHeight()); - Vector2 coarseSearchOffset(0.5 * (searchDimensions.GetX() - 1) * - m_pCorrelationGrid->GetResolution(), + m_pSearchSpaceProbs->GetHeight()); + Vector2 coarseSearchOffset( + 0.5 * (searchDimensions.GetX() - 1) * m_pCorrelationGrid->GetResolution(), 0.5 * (searchDimensions.GetY() - 1) * m_pCorrelationGrid->GetResolution()); // a coarse search only checks half the cells in each dimension Vector2 coarseSearchResolution(2 * m_pCorrelationGrid->GetResolution(), - 2 * m_pCorrelationGrid->GetResolution()); + 2 * m_pCorrelationGrid->GetResolution()); // actual scan-matching - kt_double bestResponse = CorrelateScan(pScan, scanPose, coarseSearchOffset, - coarseSearchResolution, - m_pMapper->m_pCoarseSearchAngleOffset->GetValue(), - m_pMapper->m_pCoarseAngleResolution->GetValue(), - doPenalize, rMean, rCovariance, false); - - if (m_pMapper->m_pUseResponseExpansion->GetValue() == true) { - if (math::DoubleEqual(bestResponse, 0.0)) { + kt_double bestResponse = CorrelateScan( + pScan, scanPose, coarseSearchOffset, coarseSearchResolution, + m_pMapper->m_pCoarseSearchAngleOffset->GetValue(), + m_pMapper->m_pCoarseAngleResolution->GetValue(), doPenalize, rMean, rCovariance, false); + + if (m_pMapper->m_pUseResponseExpansion->GetValue() == true) + { + if (math::DoubleEqual(bestResponse, 0.0)) + { #ifdef KARTO_DEBUG std::cout << "Mapper Info: Expanding response search space!" << std::endl; #endif // try and increase search angle offset with 20 degrees and do another match kt_double newSearchAngleOffset = m_pMapper->m_pCoarseSearchAngleOffset->GetValue(); - for (kt_int32u i = 0; i < 3; i++) { + for (kt_int32u i = 0; i < 3; i++) + { newSearchAngleOffset += math::DegreesToRadians(20); - bestResponse = CorrelateScan(pScan, scanPose, coarseSearchOffset, coarseSearchResolution, - newSearchAngleOffset, m_pMapper->m_pCoarseAngleResolution->GetValue(), - doPenalize, rMean, rCovariance, false); + bestResponse = CorrelateScan( + pScan, scanPose, coarseSearchOffset, coarseSearchResolution, newSearchAngleOffset, + m_pMapper->m_pCoarseAngleResolution->GetValue(), doPenalize, rMean, rCovariance, false); - if (math::DoubleEqual(bestResponse, 0.0) == false) { + if (math::DoubleEqual(bestResponse, 0.0) == false) + { break; } } #ifdef KARTO_DEBUG - if (math::DoubleEqual(bestResponse, 0.0)) { + if (math::DoubleEqual(bestResponse, 0.0)) + { std::cout << "Mapper Warning: Unable to calculate response!" << std::endl; } #endif } } - if (doRefineMatch) { + if (doRefineMatch) + { Vector2 fineSearchOffset(coarseSearchResolution * 0.5); Vector2 fineSearchResolution(m_pCorrelationGrid->GetResolution(), - m_pCorrelationGrid->GetResolution()); + m_pCorrelationGrid->GetResolution()); bestResponse = CorrelateScan(pScan, rMean, fineSearchOffset, fineSearchResolution, - 0.5 * m_pMapper->m_pCoarseAngleResolution->GetValue(), - m_pMapper->m_pFineSearchAngleOffset->GetValue(), - doPenalize, rMean, rCovariance, true); + 0.5 * m_pMapper->m_pCoarseAngleResolution->GetValue(), + m_pMapper->m_pFineSearchAngleOffset->GetValue(), doPenalize, rMean, + rCovariance, true); } #ifdef KARTO_DEBUG - std::cout << " BEST POSE = " << rMean << " BEST RESPONSE = " << bestResponse << - ", VARIANCE = " << - rCovariance(0, 0) << ", " << rCovariance(1, 1) << std::endl; + std::cout << " BEST POSE = " << rMean << " BEST RESPONSE = " << bestResponse + << ", VARIANCE = " << rCovariance(0, 0) << ", " << rCovariance(1, 1) << std::endl; #endif assert(math::InRange(rMean.GetHeading(), -KT_PI, KT_PI)); return bestResponse; } -void ScanMatcher::operator()(const kt_double & y) const +void ScanMatcher::operator()(const kt_double& y) const { kt_int32u poseResponseCounter; kt_int32u x_pose; @@ -647,48 +668,49 @@ void ScanMatcher::operator()(const kt_double & y) const const kt_int32u size_x = m_xPoses.size(); kt_double newPositionY = m_rSearchCenter.GetY() + y; - kt_double squareY = math::Square(y); + kt_double squareY = math::Square(y); for (std::vector::const_iterator xIter = m_xPoses.begin(); xIter != m_xPoses.end(); - ++xIter) + ++xIter) { - x_pose = std::distance(m_xPoses.begin(), xIter); - kt_double x = *xIter; + x_pose = std::distance(m_xPoses.begin(), xIter); + kt_double x = *xIter; kt_double newPositionX = m_rSearchCenter.GetX() + x; - kt_double squareX = math::Square(x); + kt_double squareX = math::Square(x); Vector2 gridPoint = m_pCorrelationGrid->WorldToGrid(Vector2(newPositionX, newPositionY)); kt_int32s gridIndex = m_pCorrelationGrid->GridIndex(gridPoint); assert(gridIndex >= 0); - kt_double angle = 0.0; + kt_double angle = 0.0; kt_double startAngle = m_rSearchCenter.GetHeading() - m_searchAngleOffset; - for (kt_int32u angleIndex = 0; angleIndex < m_nAngles; angleIndex++) { + for (kt_int32u angleIndex = 0; angleIndex < m_nAngles; angleIndex++) + { angle = startAngle + angleIndex * m_searchAngleResolution; kt_double response = GetResponse(angleIndex, gridIndex); - if (m_doPenalize && (math::DoubleEqual(response, 0.0) == false)) { + if (m_doPenalize && (math::DoubleEqual(response, 0.0) == false)) + { // simple model (approximate Gaussian) to take odometry into account kt_double squaredDistance = squareX + squareY; - kt_double distancePenalty = 1.0 - (DISTANCE_PENALTY_GAIN * - squaredDistance / m_pMapper->m_pDistanceVariancePenalty->GetValue()); - distancePenalty = math::Maximum(distancePenalty, - m_pMapper->m_pMinimumDistancePenalty->GetValue()); + kt_double distancePenalty = 1.0 - (DISTANCE_PENALTY_GAIN * squaredDistance / + m_pMapper->m_pDistanceVariancePenalty->GetValue()); + distancePenalty = + math::Maximum(distancePenalty, m_pMapper->m_pMinimumDistancePenalty->GetValue()); kt_double squaredAngleDistance = math::Square(angle - m_rSearchCenter.GetHeading()); - kt_double anglePenalty = 1.0 - (ANGLE_PENALTY_GAIN * - squaredAngleDistance / m_pMapper->m_pAngleVariancePenalty->GetValue()); + kt_double anglePenalty = 1.0 - (ANGLE_PENALTY_GAIN * squaredAngleDistance / + m_pMapper->m_pAngleVariancePenalty->GetValue()); anglePenalty = math::Maximum(anglePenalty, m_pMapper->m_pMinimumAnglePenalty->GetValue()); response *= (distancePenalty * anglePenalty); } // store response and pose - poseResponseCounter = (y_pose * size_x + x_pose) * (m_nAngles) + angleIndex; - m_pPoseResponse[poseResponseCounter] = - std::pair(response, Pose2(newPositionX, newPositionY, - math::NormalizeAngle(angle))); + poseResponseCounter = (y_pose * size_x + x_pose) * (m_nAngles) + angleIndex; + m_pPoseResponse[poseResponseCounter] = std::pair( + response, Pose2(newPositionX, newPositionY, math::NormalizeAngle(angle))); } } } @@ -709,21 +731,22 @@ void ScanMatcher::operator()(const kt_double & y) const * @param doingFineMatch whether to do a finer search after coarse search * @return strength of response */ -kt_double ScanMatcher::CorrelateScan( - LocalizedRangeScan * pScan, const Pose2 & rSearchCenter, - const Vector2 & rSearchSpaceOffset, - const Vector2 & rSearchSpaceResolution, - kt_double searchAngleOffset, kt_double searchAngleResolution, - kt_bool doPenalize, Pose2 & rMean, Matrix3 & rCovariance, kt_bool doingFineMatch) +kt_double ScanMatcher::CorrelateScan(LocalizedRangeScan* pScan, const Pose2& rSearchCenter, + const Vector2& rSearchSpaceOffset, + const Vector2& rSearchSpaceResolution, + kt_double searchAngleOffset, kt_double searchAngleResolution, + kt_bool doPenalize, Pose2& rMean, Matrix3& rCovariance, + kt_bool doingFineMatch) { assert(searchAngleResolution != 0.0); // setup lookup arrays - m_pGridLookup->ComputeOffsets(pScan, - rSearchCenter.GetHeading(), searchAngleOffset, searchAngleResolution); + m_pGridLookup->ComputeOffsets(pScan, rSearchCenter.GetHeading(), searchAngleOffset, + searchAngleResolution); // only initialize probability grid if computing positional covariance (during coarse match) - if (!doingFineMatch) { + if (!doingFineMatch) + { m_pSearchSpaceProbs->Clear(); // position search grid - finds lower left corner of search grid @@ -734,19 +757,21 @@ kt_double ScanMatcher::CorrelateScan( // calculate position arrays m_xPoses.clear(); - kt_int32u nX = static_cast(math::Round(rSearchSpaceOffset.GetX() * - 2.0 / rSearchSpaceResolution.GetX()) + 1); + kt_int32u nX = static_cast( + math::Round(rSearchSpaceOffset.GetX() * 2.0 / rSearchSpaceResolution.GetX()) + 1); kt_double startX = -rSearchSpaceOffset.GetX(); - for (kt_int32u xIndex = 0; xIndex < nX; xIndex++) { + for (kt_int32u xIndex = 0; xIndex < nX; xIndex++) + { m_xPoses.push_back(startX + xIndex * rSearchSpaceResolution.GetX()); } assert(math::DoubleEqual(m_xPoses.back(), -startX)); m_yPoses.clear(); - kt_int32u nY = static_cast(math::Round(rSearchSpaceOffset.GetY() * - 2.0 / rSearchSpaceResolution.GetY()) + 1); + kt_int32u nY = static_cast( + math::Round(rSearchSpaceOffset.GetY() * 2.0 / rSearchSpaceResolution.GetY()) + 1); kt_double startY = -rSearchSpaceOffset.GetY(); - for (kt_int32u yIndex = 0; yIndex < nY; yIndex++) { + for (kt_int32u yIndex = 0; yIndex < nY; yIndex++) + { m_yPoses.push_back(startY + yIndex * rSearchSpaceResolution.GetY()); } assert(math::DoubleEqual(m_yPoses.back(), -startY)); @@ -760,39 +785,51 @@ kt_double ScanMatcher::CorrelateScan( // allocate array m_pPoseResponse = new std::pair[poseResponseSize]; - Vector2 startGridPoint = - m_pCorrelationGrid->WorldToGrid(Vector2(rSearchCenter.GetX() + - startX, rSearchCenter.GetY() + startY)); + Vector2 startGridPoint = m_pCorrelationGrid->WorldToGrid( + Vector2(rSearchCenter.GetX() + startX, rSearchCenter.GetY() + startY)); // this isn't good but its the fastest way to iterate. Should clean up later. - m_rSearchCenter = rSearchCenter; - m_searchAngleOffset = searchAngleOffset; - m_nAngles = nAngles; + m_rSearchCenter = rSearchCenter; + m_searchAngleOffset = searchAngleOffset; + m_nAngles = nAngles; m_searchAngleResolution = searchAngleResolution; - m_doPenalize = doPenalize; - tbb::parallel_do(m_yPoses, (*this) ); + m_doPenalize = doPenalize; + + // SCAN MATCHING HERE. + for (const double y : m_yPoses) + { + (*this)(y); + } + // NOTE: NOT parallelizing scan matching b/c it didn't seem to improve CPU resources + // tbb::parallel_for_each(m_yPoses, (*this)); // find value of best response (in [0; 1]) kt_double bestResponse = -1; - for (kt_int32u i = 0; i < poseResponseSize; i++) { + for (kt_int32u i = 0; i < poseResponseSize; i++) + { bestResponse = math::Maximum(bestResponse, m_pPoseResponse[i].first); // will compute positional covariance, save best relative probability for each cell - if (!doingFineMatch) { - const Pose2 & rPose = m_pPoseResponse[i].second; + if (!doingFineMatch) + { + const Pose2& rPose = m_pPoseResponse[i].second; Vector2 grid = m_pSearchSpaceProbs->WorldToGrid(rPose.GetPosition()); - kt_double * ptr; + kt_double* ptr; - try { - ptr = (kt_double *)(m_pSearchSpaceProbs->GetDataPointer(grid)); // NOLINT - } catch (...) { + try + { + ptr = (kt_double*)(m_pSearchSpaceProbs->GetDataPointer(grid)); // NOLINT + } + catch (...) + { throw std::runtime_error("Mapper FATAL ERROR - " - "unable to get pointer in probability search!"); + "unable to get pointer in probability search!"); } - if (ptr == NULL) { + if (ptr == NULL) + { throw std::runtime_error("Mapper FATAL ERROR - " - "Index out of range in probability search!"); + "Index out of range in probability search!"); } *ptr = math::Maximum(m_pPoseResponse[i].first, *ptr); @@ -801,11 +838,13 @@ kt_double ScanMatcher::CorrelateScan( // average all poses with same highest response Vector2 averagePosition; - kt_double thetaX = 0.0; - kt_double thetaY = 0.0; + kt_double thetaX = 0.0; + kt_double thetaY = 0.0; kt_int32s averagePoseCount = 0; - for (kt_int32u i = 0; i < poseResponseSize; i++) { - if (math::DoubleEqual(m_pPoseResponse[i].first, bestResponse)) { + for (kt_int32u i = 0; i < poseResponseSize; i++) + { + if (math::DoubleEqual(m_pPoseResponse[i].first, bestResponse)) + { averagePosition += m_pPoseResponse[i].second.GetPosition(); kt_double heading = m_pPoseResponse[i].second.GetHeading(); @@ -817,31 +856,38 @@ kt_double ScanMatcher::CorrelateScan( } Pose2 averagePose; - if (averagePoseCount > 0) { + if (averagePoseCount > 0) + { averagePosition /= averagePoseCount; thetaX /= averagePoseCount; thetaY /= averagePoseCount; averagePose = Pose2(averagePosition, atan2(thetaY, thetaX)); - } else { + } + else + { throw std::runtime_error("Mapper FATAL ERROR - Unable to find best position"); } // delete pose response array delete[] m_pPoseResponse; + m_pPoseResponse = nullptr; #ifdef KARTO_DEBUG std::cout << "bestPose: " << averagePose << std::endl; std::cout << "bestResponse: " << bestResponse << std::endl; #endif - if (!doingFineMatch) { + if (!doingFineMatch) + { ComputePositionalCovariance(averagePose, bestResponse, rSearchCenter, rSearchSpaceOffset, - rSearchSpaceResolution, searchAngleResolution, rCovariance); - } else { - ComputeAngularCovariance(averagePose, bestResponse, rSearchCenter, - searchAngleOffset, searchAngleResolution, rCovariance); + rSearchSpaceResolution, searchAngleResolution, rCovariance); + } + else + { + ComputeAngularCovariance(averagePose, bestResponse, rSearchCenter, searchAngleOffset, + searchAngleResolution, rCovariance); } rMean = averagePose; @@ -850,7 +896,8 @@ kt_double ScanMatcher::CorrelateScan( std::cout << "bestPose: " << averagePose << std::endl; #endif - if (bestResponse > 1.0) { + if (bestResponse > 1.0) + { bestResponse = 1.0; } @@ -870,21 +917,21 @@ kt_double ScanMatcher::CorrelateScan( * @param searchAngleResolution * @param rCovariance */ -void ScanMatcher::ComputePositionalCovariance( - const Pose2 & rBestPose, kt_double bestResponse, - const Pose2 & rSearchCenter, - const Vector2 & rSearchSpaceOffset, - const Vector2 & rSearchSpaceResolution, - kt_double searchAngleResolution, Matrix3 & rCovariance) +void ScanMatcher::ComputePositionalCovariance(const Pose2& rBestPose, kt_double bestResponse, + const Pose2& rSearchCenter, + const Vector2& rSearchSpaceOffset, + const Vector2& rSearchSpaceResolution, + kt_double searchAngleResolution, Matrix3& rCovariance) { // reset covariance to identity matrix rCovariance.SetToIdentity(); // if best response is vary small return max variance - if (bestResponse < KT_TOLERANCE) { - rCovariance(0, 0) = MAX_VARIANCE; // XX - rCovariance(1, 1) = MAX_VARIANCE; // YY - rCovariance(2, 2) = 4 * math::Square(searchAngleResolution); // TH*TH + if (bestResponse < KT_TOLERANCE) + { + rCovariance(0, 0) = MAX_VARIANCE; // XX + rCovariance(1, 1) = MAX_VARIANCE; // YY + rCovariance(2, 2) = 4 * math::Square(searchAngleResolution); // TH*TH return; } @@ -892,7 +939,7 @@ void ScanMatcher::ComputePositionalCovariance( kt_double accumulatedVarianceXX = 0; kt_double accumulatedVarianceXY = 0; kt_double accumulatedVarianceYY = 0; - kt_double norm = 0; + kt_double norm = 0; kt_double dx = rBestPose.GetX() - rSearchCenter.GetX(); kt_double dy = rBestPose.GetY() - rSearchCenter.GetY(); @@ -910,19 +957,21 @@ void ScanMatcher::ComputePositionalCovariance( kt_double startY = -offsetY; assert(math::DoubleEqual(startY + (nY - 1) * rSearchSpaceResolution.GetY(), -startY)); - for (kt_int32u yIndex = 0; yIndex < nY; yIndex++) { + for (kt_int32u yIndex = 0; yIndex < nY; yIndex++) + { kt_double y = startY + yIndex * rSearchSpaceResolution.GetY(); - for (kt_int32u xIndex = 0; xIndex < nX; xIndex++) { + for (kt_int32u xIndex = 0; xIndex < nX; xIndex++) + { kt_double x = startX + xIndex * rSearchSpaceResolution.GetX(); - Vector2 gridPoint = - m_pSearchSpaceProbs->WorldToGrid(Vector2(rSearchCenter.GetX() + x, - rSearchCenter.GetY() + y)); + Vector2 gridPoint = m_pSearchSpaceProbs->WorldToGrid( + Vector2(rSearchCenter.GetX() + x, rSearchCenter.GetY() + y)); kt_double response = *(m_pSearchSpaceProbs->GetDataPointer(gridPoint)); // response is not a low response - if (response >= (bestResponse - 0.1)) { + if (response >= (bestResponse - 0.1)) + { norm += response; accumulatedVarianceXX += (math::Square(x - dx) * response); accumulatedVarianceXY += ((x - dx) * (y - dy) * response); @@ -931,35 +980,38 @@ void ScanMatcher::ComputePositionalCovariance( } } - if (norm > KT_TOLERANCE) { - kt_double varianceXX = accumulatedVarianceXX / norm; - kt_double varianceXY = accumulatedVarianceXY / norm; - kt_double varianceYY = accumulatedVarianceYY / norm; + if (norm > KT_TOLERANCE) + { + kt_double varianceXX = accumulatedVarianceXX / norm; + kt_double varianceXY = accumulatedVarianceXY / norm; + kt_double varianceYY = accumulatedVarianceYY / norm; kt_double varianceTHTH = 4 * math::Square(searchAngleResolution); // lower-bound variances so that they are not too small; // ensures that links are not too tight kt_double minVarianceXX = 0.1 * math::Square(rSearchSpaceResolution.GetX()); kt_double minVarianceYY = 0.1 * math::Square(rSearchSpaceResolution.GetY()); - varianceXX = math::Maximum(varianceXX, minVarianceXX); - varianceYY = math::Maximum(varianceYY, minVarianceYY); + varianceXX = math::Maximum(varianceXX, minVarianceXX); + varianceYY = math::Maximum(varianceYY, minVarianceYY); // increase variance for poorer responses kt_double multiplier = 1.0 / bestResponse; - rCovariance(0, 0) = varianceXX * multiplier; - rCovariance(0, 1) = varianceXY * multiplier; - rCovariance(1, 0) = varianceXY * multiplier; - rCovariance(1, 1) = varianceYY * multiplier; - rCovariance(2, 2) = varianceTHTH; // this value will be set in ComputeAngularCovariance + rCovariance(0, 0) = varianceXX * multiplier; + rCovariance(0, 1) = varianceXY * multiplier; + rCovariance(1, 0) = varianceXY * multiplier; + rCovariance(1, 1) = varianceYY * multiplier; + rCovariance(2, 2) = varianceTHTH; // this value will be set in ComputeAngularCovariance } // if values are 0, set to MAX_VARIANCE // values might be 0 if points are too sparse and thus don't hit other points - if (math::DoubleEqual(rCovariance(0, 0), 0.0)) { + if (math::DoubleEqual(rCovariance(0, 0), 0.0)) + { rCovariance(0, 0) = MAX_VARIANCE; } - if (math::DoubleEqual(rCovariance(1, 1), 0.0)) { + if (math::DoubleEqual(rCovariance(1, 1), 0.0)) + { rCovariance(1, 1) = MAX_VARIANCE; } } @@ -973,50 +1025,52 @@ void ScanMatcher::ComputePositionalCovariance( * @param searchAngleResolution * @param rCovariance */ -void ScanMatcher::ComputeAngularCovariance( - const Pose2 & rBestPose, - kt_double bestResponse, - const Pose2 & rSearchCenter, - kt_double searchAngleOffset, - kt_double searchAngleResolution, - Matrix3 & rCovariance) +void ScanMatcher::ComputeAngularCovariance(const Pose2& rBestPose, kt_double bestResponse, + const Pose2& rSearchCenter, kt_double searchAngleOffset, + kt_double searchAngleResolution, Matrix3& rCovariance) { // NOTE: do not reset covariance matrix // normalize angle difference - kt_double bestAngle = math::NormalizeAngleDifference( - rBestPose.GetHeading(), rSearchCenter.GetHeading()); + kt_double bestAngle = + math::NormalizeAngleDifference(rBestPose.GetHeading(), rSearchCenter.GetHeading()); Vector2 gridPoint = m_pCorrelationGrid->WorldToGrid(rBestPose.GetPosition()); - kt_int32s gridIndex = m_pCorrelationGrid->GridIndex(gridPoint); + kt_int32s gridIndex = m_pCorrelationGrid->GridIndex(gridPoint); kt_int32u nAngles = static_cast(math::Round(searchAngleOffset * 2 / searchAngleResolution) + 1); - kt_double angle = 0.0; + kt_double angle = 0.0; kt_double startAngle = rSearchCenter.GetHeading() - searchAngleOffset; - kt_double norm = 0.0; + kt_double norm = 0.0; kt_double accumulatedVarianceThTh = 0.0; - for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++) { - angle = startAngle + angleIndex * searchAngleResolution; + for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++) + { + angle = startAngle + angleIndex * searchAngleResolution; kt_double response = GetResponse(angleIndex, gridIndex); // response is not a low response - if (response >= (bestResponse - 0.1)) { + if (response >= (bestResponse - 0.1)) + { norm += response; accumulatedVarianceThTh += (math::Square(angle - bestAngle) * response); } } assert(math::DoubleEqual(angle, rSearchCenter.GetHeading() + searchAngleOffset)); - if (norm > KT_TOLERANCE) { - if (accumulatedVarianceThTh < KT_TOLERANCE) { + if (norm > KT_TOLERANCE) + { + if (accumulatedVarianceThTh < KT_TOLERANCE) + { accumulatedVarianceThTh = math::Square(searchAngleResolution); } accumulatedVarianceThTh /= norm; - } else { + } + else + { accumulatedVarianceThTh = 1000 * math::Square(searchAngleResolution); } @@ -1028,14 +1082,15 @@ void ScanMatcher::ComputeAngularCovariance( * @param rScans scans whose points will mark cells in grid as being occupied * @param viewPoint do not add points that belong to scans "opposite" the view point */ -void ScanMatcher::AddScans(const LocalizedRangeScanVector & rScans, Vector2 viewPoint) +void ScanMatcher::AddScans(const LocalizedRangeScanVector& rScans, Vector2 viewPoint) { m_pCorrelationGrid->Clear(); // add all scans to grid const_forEach(LocalizedRangeScanVector, &rScans) { - if (*iter == NULL) { + if (*iter == NULL) + { continue; } @@ -1048,14 +1103,15 @@ void ScanMatcher::AddScans(const LocalizedRangeScanVector & rScans, Vector2 viewPoint) +void ScanMatcher::AddScans(const LocalizedRangeScanMap& rScans, Vector2 viewPoint) { m_pCorrelationGrid->Clear(); // add all scans to grid const_forEach(LocalizedRangeScanMap, &rScans) { - if (iter->second == NULL) { + if (iter->second == NULL) + { continue; } @@ -1069,9 +1125,8 @@ void ScanMatcher::AddScans(const LocalizedRangeScanMap & rScans, Vector2 & rViewPoint, - kt_bool doSmear) +void ScanMatcher::AddScan(LocalizedRangeScan* pScan, const Vector2& rViewPoint, + kt_bool doSmear) { PointVectorDouble validPoints = FindValidPoints(pScan, rViewPoint); @@ -1080,7 +1135,7 @@ void ScanMatcher::AddScan( { Vector2 gridPoint = m_pCorrelationGrid->WorldToGrid(*iter); if (!math::IsUpTo(gridPoint.GetX(), m_pCorrelationGrid->GetROI().GetWidth()) || - !math::IsUpTo(gridPoint.GetY(), m_pCorrelationGrid->GetROI().GetHeight())) + !math::IsUpTo(gridPoint.GetY(), m_pCorrelationGrid->GetROI().GetHeight())) { // point not in grid continue; @@ -1089,7 +1144,8 @@ void ScanMatcher::AddScan( int gridIndex = m_pCorrelationGrid->GridIndex(gridPoint); // set grid cell as occupied - if (m_pCorrelationGrid->GetDataPointer()[gridIndex] == GridStates_Occupied) { + if (m_pCorrelationGrid->GetDataPointer()[gridIndex] == GridStates_Occupied) + { // value already set continue; } @@ -1097,7 +1153,8 @@ void ScanMatcher::AddScan( m_pCorrelationGrid->GetDataPointer()[gridIndex] = GridStates_Occupied; // smear grid - if (doSmear == true) { + if (doSmear == true) + { m_pCorrelationGrid->SmearPoint(gridPoint); } } @@ -1109,14 +1166,13 @@ void ScanMatcher::AddScan( * @param rViewPoint * @return points on the same side */ -PointVectorDouble ScanMatcher::FindValidPoints( - LocalizedRangeScan * pScan, - const Vector2 & rViewPoint) const +PointVectorDouble ScanMatcher::FindValidPoints(LocalizedRangeScan* pScan, + const Vector2& rViewPoint) const { - const PointVectorDouble & rPointReadings = pScan->GetPointReadings(); + const PointVectorDouble& rPointReadings = pScan->GetPointReadings(); // points must be at least 10 cm away when making comparisons of inside/outside of viewpoint - const kt_double minSquareDistance = math::Square(0.1); // in m^2 + const kt_double minSquareDistance = math::Square(0.1); // in m^2 // this iterator lags from the main iterator adding points only when the points are on // the same side as the viewpoint @@ -1129,30 +1185,36 @@ PointVectorDouble ScanMatcher::FindValidPoints( { Vector2 currentPoint = *iter; - if (firstTime && !std::isnan(currentPoint.GetX()) && !std::isnan(currentPoint.GetY())) { + if (firstTime && !std::isnan(currentPoint.GetX()) && !std::isnan(currentPoint.GetY())) + { firstPoint = currentPoint; - firstTime = false; + firstTime = false; } Vector2 delta = firstPoint - currentPoint; - if (delta.SquaredLength() > minSquareDistance) { + if (delta.SquaredLength() > minSquareDistance) + { // This compute the Determinant (viewPoint FirstPoint, viewPoint currentPoint) // Which computes the direction of rotation, if the rotation is counterclock // wise then we are looking at data we should keep. If it's negative rotation // we should not included in in the matching // have enough distance, check viewpoint - double a = rViewPoint.GetY() - firstPoint.GetY(); - double b = firstPoint.GetX() - rViewPoint.GetX(); - double c = firstPoint.GetY() * rViewPoint.GetX() - firstPoint.GetX() * rViewPoint.GetY(); + double a = rViewPoint.GetY() - firstPoint.GetY(); + double b = firstPoint.GetX() - rViewPoint.GetX(); + double c = firstPoint.GetY() * rViewPoint.GetX() - firstPoint.GetX() * rViewPoint.GetY(); double ss = currentPoint.GetX() * a + currentPoint.GetY() * b + c; // reset beginning point firstPoint = currentPoint; - if (ss < 0.0) { // wrong side, skip and keep going + if (ss < 0.0) + { // wrong side, skip and keep going trailingPointIter = iter; - } else { - for (; trailingPointIter != iter; ++trailingPointIter) { + } + else + { + for (; trailingPointIter != iter; ++trailingPointIter) + { validPoints.push_back(*trailingPointIter); } } @@ -1173,24 +1235,26 @@ kt_double ScanMatcher::GetResponse(kt_int32u angleIndex, kt_int32s gridPositionI kt_double response = 0.0; // add up value for each point - kt_int8u * pByte = m_pCorrelationGrid->GetDataPointer() + gridPositionIndex; + kt_int8u* pByte = m_pCorrelationGrid->GetDataPointer() + gridPositionIndex; - const LookupArray * pOffsets = m_pGridLookup->GetLookupArray(angleIndex); + const LookupArray* pOffsets = m_pGridLookup->GetLookupArray(angleIndex); assert(pOffsets != NULL); // get number of points in offset list kt_int32u nPoints = pOffsets->GetSize(); - if (nPoints == 0) { + if (nPoints == 0) + { return response; } // calculate response - kt_int32s * pAngleIndexPointer = pOffsets->GetArrayPointer(); - for (kt_int32u i = 0; i < nPoints; i++) { + kt_int32s* pAngleIndexPointer = pOffsets->GetArrayPointer(); + for (kt_int32u i = 0; i < nPoints; i++) + { // ignore points that fall off the grid kt_int32s pointGridIndex = gridPositionIndex + pAngleIndexPointer[i]; - if (!math::IsUpTo(pointGridIndex, - m_pCorrelationGrid->GetDataSize()) || pAngleIndexPointer[i] == INVALID_SCAN) + if (!math::IsUpTo(pointGridIndex, m_pCorrelationGrid->GetDataSize()) || + pAngleIndexPointer[i] == INVALID_SCAN) { continue; } @@ -1206,12 +1270,11 @@ kt_double ScanMatcher::GetResponse(kt_int32u angleIndex, kt_int32s gridPositionI return response; } - //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// -template +template class BreadthFirstTraversal : public GraphTraversal { public: @@ -1221,8 +1284,7 @@ class BreadthFirstTraversal : public GraphTraversal BreadthFirstTraversal() { } - explicit BreadthFirstTraversal(Graph * pGraph) - : GraphTraversal(pGraph) + explicit BreadthFirstTraversal(Graph* pGraph) : GraphTraversal(pGraph) { } @@ -1240,12 +1302,12 @@ class BreadthFirstTraversal : public GraphTraversal * @param pVisitor * @return visited vertice scans */ - virtual std::vector TraverseForScans(Vertex * pStartVertex, Visitor * pVisitor) + virtual std::vector TraverseForScans(Vertex* pStartVertex, Visitor* pVisitor) { - std::vector *> validVertices = TraverseForVertices(pStartVertex, pVisitor); + std::vector*> validVertices = TraverseForVertices(pStartVertex, pVisitor); - std::vector objects; - forEach(typename std::vector *>, &validVertices) + std::vector objects; + forEach(typename std::vector*>, &validVertices) { objects.push_back((*iter)->GetObject()); } @@ -1259,32 +1321,33 @@ class BreadthFirstTraversal : public GraphTraversal * @param pVisitor * @return visited vertices */ - virtual std::vector *> TraverseForVertices( - Vertex * pStartVertex, - Visitor * pVisitor) + virtual std::vector*> TraverseForVertices(Vertex* pStartVertex, Visitor* pVisitor) { - std::queue *> toVisit; - std::set *> seenVertices; - std::vector *> validVertices; + std::queue*> toVisit; + std::set*> seenVertices; + std::vector*> validVertices; toVisit.push(pStartVertex); seenVertices.insert(pStartVertex); - do { - Vertex * pNext = toVisit.front(); + do + { + Vertex* pNext = toVisit.front(); toVisit.pop(); - if (pNext != NULL && pVisitor->Visit(pNext)) { + if (pNext != NULL && pVisitor->Visit(pNext)) + { // vertex is valid, explore neighbors validVertices.push_back(pNext); - std::vector *> adjacentVertices = pNext->GetAdjacentVertices(); - forEach(typename std::vector *>, &adjacentVertices) + std::vector*> adjacentVertices = pNext->GetAdjacentVertices(); + forEach(typename std::vector*>, &adjacentVertices) { - Vertex * pAdjacent = *iter; + Vertex* pAdjacent = *iter; // adjacent vertex has not yet been seen, add to queue for processing - if (seenVertices.find(pAdjacent) == seenVertices.end()) { + if (seenVertices.find(pAdjacent) == seenVertices.end()) + { toVisit.push(pAdjacent); seenVertices.insert(pAdjacent); } @@ -1296,12 +1359,12 @@ class BreadthFirstTraversal : public GraphTraversal } friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) + template + void serialize(Archive& ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GraphTraversal); + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(GraphTraversal); } -}; // class BreadthFirstTraversal +}; // class BreadthFirstTraversal //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -1310,21 +1373,23 @@ class BreadthFirstTraversal : public GraphTraversal class NearScanVisitor : public Visitor { public: - NearScanVisitor(LocalizedRangeScan * pScan, kt_double maxDistance, kt_bool useScanBarycenter) - : m_MaxDistanceSquared(math::Square(maxDistance)), - m_UseScanBarycenter(useScanBarycenter) + NearScanVisitor(LocalizedRangeScan* pScan, kt_double maxDistance, kt_bool useScanBarycenter) + : m_MaxDistanceSquared(math::Square(maxDistance)), m_UseScanBarycenter(useScanBarycenter) { m_CenterPose = pScan->GetReferencePose(m_UseScanBarycenter); } - virtual kt_bool Visit(Vertex * pVertex) + virtual kt_bool Visit(Vertex* pVertex) { - try { - LocalizedRangeScan * pScan = pVertex->GetObject(); - Pose2 pose = pScan->GetReferencePose(m_UseScanBarycenter); + try + { + LocalizedRangeScan* pScan = pVertex->GetObject(); + Pose2 pose = pScan->GetReferencePose(m_UseScanBarycenter); kt_double squaredDistance = pose.GetPosition().SquaredDistance(m_CenterPose.GetPosition()); return squaredDistance <= m_MaxDistanceSquared - KT_TOLERANCE; - } catch (...) { + } + catch (...) + { // relocalization vertex elements missing std::cout << "Unable to visit valid vertex elements!" << std::endl; return false; @@ -1336,15 +1401,15 @@ class NearScanVisitor : public Visitor kt_double m_MaxDistanceSquared; kt_bool m_UseScanBarycenter; friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) + template + void serialize(Archive& ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Visitor); - ar & BOOST_SERIALIZATION_NVP(m_CenterPose); - ar & BOOST_SERIALIZATION_NVP(m_MaxDistanceSquared); - ar & BOOST_SERIALIZATION_NVP(m_UseScanBarycenter); + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Visitor); + ar& BOOST_SERIALIZATION_NVP(m_CenterPose); + ar& BOOST_SERIALIZATION_NVP(m_MaxDistanceSquared); + ar& BOOST_SERIALIZATION_NVP(m_UseScanBarycenter); } -}; // NearScanVisitor +}; // NearScanVisitor //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -1354,15 +1419,14 @@ class NearPoseVisitor : public Visitor { public: NearPoseVisitor(Pose2 refPose, kt_double maxDistance, kt_bool useScanBarycenter) - : m_MaxDistanceSquared(math::Square(maxDistance)), - m_UseScanBarycenter(useScanBarycenter) + : m_MaxDistanceSquared(math::Square(maxDistance)), m_UseScanBarycenter(useScanBarycenter) { m_CenterPose = refPose; } - virtual kt_bool Visit(Vertex * pVertex) + virtual kt_bool Visit(Vertex* pVertex) { - LocalizedRangeScan * pScan = pVertex->GetObject(); + LocalizedRangeScan* pScan = pVertex->GetObject(); Pose2 pose = pScan->GetReferencePose(m_UseScanBarycenter); @@ -1375,28 +1439,26 @@ class NearPoseVisitor : public Visitor kt_double m_MaxDistanceSquared; kt_bool m_UseScanBarycenter; friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) + template + void serialize(Archive& ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Visitor); - ar & BOOST_SERIALIZATION_NVP(m_CenterPose); - ar & BOOST_SERIALIZATION_NVP(m_MaxDistanceSquared); - ar & BOOST_SERIALIZATION_NVP(m_UseScanBarycenter); + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Visitor); + ar& BOOST_SERIALIZATION_NVP(m_CenterPose); + ar& BOOST_SERIALIZATION_NVP(m_MaxDistanceSquared); + ar& BOOST_SERIALIZATION_NVP(m_UseScanBarycenter); } -}; // NearPoseVisitor +}; // NearPoseVisitor //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// - -MapperGraph::MapperGraph(Mapper * pMapper, kt_double rangeThreshold) -: m_pMapper(pMapper) +MapperGraph::MapperGraph(Mapper* pMapper, kt_double rangeThreshold) : m_pMapper(pMapper) { - m_pLoopScanMatcher = ScanMatcher::Create(pMapper, - m_pMapper->m_pLoopSearchSpaceDimension->GetValue(), - m_pMapper->m_pLoopSearchSpaceResolution->GetValue(), - m_pMapper->m_pLoopSearchSpaceSmearDeviation->GetValue(), rangeThreshold); + m_pLoopScanMatcher = + ScanMatcher::Create(pMapper, m_pMapper->m_pLoopSearchSpaceDimension->GetValue(), + m_pMapper->m_pLoopSearchSpaceResolution->GetValue(), + m_pMapper->m_pLoopSearchSpaceSmearDeviation->GetValue(), rangeThreshold); assert(m_pLoopScanMatcher); m_pTraversal = new BreadthFirstTraversal(this); @@ -1404,24 +1466,28 @@ MapperGraph::MapperGraph(Mapper * pMapper, kt_double rangeThreshold) MapperGraph::~MapperGraph() { - if (m_pLoopScanMatcher) { + if (m_pLoopScanMatcher) + { delete m_pLoopScanMatcher; m_pLoopScanMatcher = NULL; } - if (m_pTraversal) { + if (m_pTraversal) + { delete m_pTraversal; m_pTraversal = NULL; } } -Vertex * MapperGraph::AddVertex(LocalizedRangeScan * pScan) +Vertex* MapperGraph::AddVertex(LocalizedRangeScan* pScan) { assert(pScan); - if (pScan != NULL) { - Vertex * pVertex = new Vertex(pScan); + if (pScan != NULL) + { + Vertex* pVertex = new Vertex(pScan); Graph::AddVertex(pScan->GetSensorName(), pVertex); - if (m_pMapper->m_pScanOptimizer != NULL) { + if (m_pMapper->m_pScanOptimizer != NULL) + { m_pMapper->m_pScanOptimizer->AddNode(pVertex); } return pVertex; @@ -1430,18 +1496,20 @@ Vertex * MapperGraph::AddVertex(LocalizedRangeScan * pScan) return nullptr; } -void MapperGraph::AddEdges(LocalizedRangeScan * pScan, const Matrix3 & rCovariance) +void MapperGraph::AddEdges(LocalizedRangeScan* pScan, const Matrix3& rCovariance) { - MapperSensorManager * pSensorManager = m_pMapper->m_pMapperSensorManager; + MapperSensorManager* pSensorManager = m_pMapper->m_pMapperSensorManager; const Name rSensorName = pScan->GetSensorName(); // link to previous scan kt_int32s previousScanNum = pScan->GetStateId() - 1; - if (pSensorManager->GetLastScan(rSensorName) != NULL) { + if (pSensorManager->GetLastScan(rSensorName) != NULL) + { assert(previousScanNum >= 0); - LocalizedRangeScan * pPrevScan = pSensorManager->GetScan(rSensorName, previousScanNum); - if (!pPrevScan) { + LocalizedRangeScan* pPrevScan = pSensorManager->GetScan(rSensorName, previousScanNum); + if (!pPrevScan) + { return; } LinkScans(pPrevScan, pScan, pScan->GetSensorPose(), rCovariance); @@ -1451,17 +1519,18 @@ void MapperGraph::AddEdges(LocalizedRangeScan * pScan, const Matrix3 & rCovarian std::vector covariances; // first scan (link to first scan of other robots) - if (pSensorManager->GetLastScan(rSensorName) == NULL) { + if (pSensorManager->GetLastScan(rSensorName) == NULL) + { assert(pSensorManager->GetScans(rSensorName).size() == 1); std::vector deviceNames = pSensorManager->GetSensorNames(); forEach(std::vector, &deviceNames) { - const Name & rCandidateSensorName = *iter; + const Name& rCandidateSensorName = *iter; // skip if candidate device is the same or other device has no scans if ((rCandidateSensorName == rSensorName) || - (pSensorManager->GetScans(rCandidateSensorName).empty())) + (pSensorManager->GetScans(rCandidateSensorName).empty())) { continue; } @@ -1469,18 +1538,19 @@ void MapperGraph::AddEdges(LocalizedRangeScan * pScan, const Matrix3 & rCovarian Pose2 bestPose; Matrix3 covariance; kt_double response = m_pMapper->m_pSequentialScanMatcher->MatchScan( - pScan, - pSensorManager->GetScans(rCandidateSensorName), - bestPose, covariance); + pScan, pSensorManager->GetScans(rCandidateSensorName), bestPose, covariance); LinkScans(pScan, pSensorManager->GetScan(rCandidateSensorName, 0), bestPose, covariance); // only add to means and covariances if response was high "enough" - if (response > m_pMapper->m_pLinkMatchMinimumResponseFine->GetValue()) { + if (response > m_pMapper->m_pLinkMatchMinimumResponseFine->GetValue()) + { means.push_back(bestPose); covariances.push_back(covariance); } } - } else { + } + else + { // link to running scans Pose2 scanPose = pScan->GetSensorPose(); means.push_back(scanPose); @@ -1491,12 +1561,13 @@ void MapperGraph::AddEdges(LocalizedRangeScan * pScan, const Matrix3 & rCovarian // link to other near chains (chains that include new scan are invalid) LinkNearChains(pScan, means, covariances); - if (!means.empty()) { + if (!means.empty()) + { pScan->SetSensorPose(ComputeWeightedMean(means, covariances)); } } -kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSensorName) +kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan* pScan, const Name& rSensorName) { kt_bool loopClosed = false; @@ -1504,43 +1575,45 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens LocalizedRangeScanVector candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex); - while (!candidateChain.empty()) { + while (!candidateChain.empty()) + { Pose2 bestPose; Matrix3 covariance; - kt_double coarseResponse = m_pLoopScanMatcher->MatchScan(pScan, candidateChain, - bestPose, covariance, false, false); + kt_double coarseResponse = + m_pLoopScanMatcher->MatchScan(pScan, candidateChain, bestPose, covariance, false, false); std::stringstream stream; - stream << "COARSE RESPONSE: " << coarseResponse << - " (> " << m_pMapper->m_pLoopMatchMinimumResponseCoarse->GetValue() << ")" << - std::endl; - stream << " var: " << covariance(0, 0) << ", " << covariance(1, 1) << - " (< " << m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue() << ")"; + stream << "COARSE RESPONSE: " << coarseResponse << " (> " + << m_pMapper->m_pLoopMatchMinimumResponseCoarse->GetValue() << ")" << std::endl; + stream << " var: " << covariance(0, 0) << ", " << covariance(1, 1) << " (< " + << m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue() << ")"; m_pMapper->FireLoopClosureCheck(stream.str()); if ((coarseResponse > m_pMapper->m_pLoopMatchMinimumResponseCoarse->GetValue()) && - (covariance(0, 0) < m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue()) && - (covariance(1, 1) < m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue())) + (covariance(0, 0) < m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue()) && + (covariance(1, 1) < m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue())) { LocalizedRangeScan tmpScan(pScan->GetSensorName(), pScan->GetRangeReadingsVector()); tmpScan.SetUniqueId(pScan->GetUniqueId()); tmpScan.SetTime(pScan->GetTime()); tmpScan.SetStateId(pScan->GetStateId()); tmpScan.SetCorrectedPose(pScan->GetCorrectedPose()); - tmpScan.SetSensorPose(bestPose); // This also updates OdometricPose. - kt_double fineResponse = m_pMapper->m_pSequentialScanMatcher->MatchScan(&tmpScan, - candidateChain, - bestPose, covariance, false); + tmpScan.SetSensorPose(bestPose); // This also updates OdometricPose. + kt_double fineResponse = m_pMapper->m_pSequentialScanMatcher->MatchScan( + &tmpScan, candidateChain, bestPose, covariance, false); std::stringstream stream1; - stream1 << "FINE RESPONSE: " << fineResponse << " (>" << - m_pMapper->m_pLoopMatchMinimumResponseFine->GetValue() << ")" << std::endl; + stream1 << "FINE RESPONSE: " << fineResponse << " (>" + << m_pMapper->m_pLoopMatchMinimumResponseFine->GetValue() << ")" << std::endl; m_pMapper->FireLoopClosureCheck(stream1.str()); - if (fineResponse < m_pMapper->m_pLoopMatchMinimumResponseFine->GetValue()) { + if (fineResponse < m_pMapper->m_pLoopMatchMinimumResponseFine->GetValue()) + { m_pMapper->FireLoopClosureCheck("REJECTED!"); - } else { + } + else + { m_pMapper->FireBeginLoopClosure("Closing loop..."); pScan->SetSensorPose(bestPose); @@ -1559,99 +1632,163 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens return loopClosed; } -LocalizedRangeScan * MapperGraph::GetClosestScanToPose( - const LocalizedRangeScanVector & rScans, - const Pose2 & rPose) const +LocalizedRangeScan* MapperGraph::GetClosestScanToPose(const LocalizedRangeScanVector& rScans, + const Pose2& rPose) const { - LocalizedRangeScan * pClosestScan = NULL; - kt_double bestSquaredDistance = DBL_MAX; + LocalizedRangeScan* pClosestScan = NULL; + kt_double bestSquaredDistance = DBL_MAX; const_forEach(LocalizedRangeScanVector, &rScans) { Pose2 scanPose = (*iter)->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue()); kt_double squaredDistance = rPose.GetPosition().SquaredDistance(scanPose.GetPosition()); - if (squaredDistance < bestSquaredDistance) { + if (squaredDistance < bestSquaredDistance) + { bestSquaredDistance = squaredDistance; - pClosestScan = *iter; + pClosestScan = *iter; } } return pClosestScan; } -Edge * MapperGraph::AddEdge( - LocalizedRangeScan * pSourceScan, - LocalizedRangeScan * pTargetScan, kt_bool & rIsNewEdge) +Edge* MapperGraph::AddEdge(LocalizedRangeScan* pSourceScan, + LocalizedRangeScan* pTargetScan, kt_bool& rIsNewEdge) { - std::map *>::iterator v1 = m_Vertices[pSourceScan->GetSensorName()].find( - pSourceScan->GetStateId()); - std::map *>::iterator v2 = m_Vertices[pTargetScan->GetSensorName()].find( - pTargetScan->GetStateId()); + std::map*>::iterator v1 = + m_Vertices[pSourceScan->GetSensorName()].find(pSourceScan->GetStateId()); + std::map*>::iterator v2 = + m_Vertices[pTargetScan->GetSensorName()].find(pTargetScan->GetStateId()); if (v1 == m_Vertices[pSourceScan->GetSensorName()].end() || - v2 == m_Vertices[pSourceScan->GetSensorName()].end()) + v2 == m_Vertices[pSourceScan->GetSensorName()].end()) { std::cout << "AddEdge: At least one vertex is invalid." << std::endl; return NULL; } // see if edge already exists - const_forEach(std::vector *>, &(v1->second->GetEdges())) + const_forEach(std::vector*>, &(v1->second->GetEdges())) { - Edge * pEdge = *iter; + Edge* pEdge = *iter; - if (pEdge->GetTarget() == v2->second) { + if (pEdge->GetTarget() == v2->second) + { rIsNewEdge = false; return pEdge; } } - Edge * pEdge = new Edge(v1->second, v2->second); + Edge* pEdge = new Edge(v1->second, v2->second); Graph::AddEdge(pEdge); rIsNewEdge = true; return pEdge; } -void MapperGraph::LinkScans( - LocalizedRangeScan * pFromScan, LocalizedRangeScan * pToScan, - const Pose2 & rMean, const Matrix3 & rCovariance) +int MapperGraph::FindShortestDistanceBetweenVertices( + karto::Vertex* source, + karto::Vertex* target, + const std::map*>& vertex_map) { - kt_bool isNewEdge = true; - Edge * pEdge = AddEdge(pFromScan, pToScan, isNewEdge); + // straight-forward Dijkstra's algorithm: + // https://www.geeksforgeeks.org/dijkstras-shortest-path-algorithm-greedy-algo-7/ + const int V = vertex_map.size(); + const int source_id = source->GetObject()->GetUniqueId(); + const int target_id = target->GetObject()->GetUniqueId(); + if (std::abs(source_id - target_id) <= 1) + { + return 0; + } - if (pEdge == NULL) { + // Using maps instead of vectors since we don't seem to be guaranteed that the vertex ids + // are nicely ordered. + std::unordered_map dist; // distance from each vertex to the source + std::set unvisited; // set of vertices we need to visit + for (const auto& pair : vertex_map) + { + dist[pair.first] = INT_MAX; + unvisited.insert(pair.first); + } + + // Distance of source vertex from itself is always 0 + dist[source_id] = 0; + + // Find shortest path for all vertices + for (int count = 0; count < V - 1; count++) + { + // Pick the minimum distance vertex from the set of + // vertices not yet visited. + int min_dist = INT_MAX, min_id = -1; + for (const int vertex_id : unvisited) + { + if (dist[vertex_id] <= min_dist) + { + min_dist = dist[vertex_id], min_id = vertex_id; + } + } + + // Remove the picked vertex + unvisited.erase(min_id); + + // Update dist value of the adjacent vertices of the + // picked vertex. + for (const auto& adj_vertex : vertex_map.at(min_id)->GetAdjacentVertices()) + { + const int adj_vertex_id = adj_vertex->GetObject()->GetUniqueId(); + // vertex hasn't been visited, has a valid distance to the source, and + // going to that vertex has a shorter distance. + if (unvisited.count(adj_vertex_id) > 0 && dist[min_id] != INT_MAX && + dist[min_id] + 1 < dist[adj_vertex_id]) + { + dist[adj_vertex_id] = dist[min_id] + 1; + } + } + } + return dist[target_id]; +} + +void MapperGraph::LinkScans(LocalizedRangeScan* pFromScan, LocalizedRangeScan* pToScan, + const Pose2& rMean, const Matrix3& rCovariance) +{ + kt_bool isNewEdge = true; + Edge* pEdge = AddEdge(pFromScan, pToScan, isNewEdge); + + if (pEdge == NULL) + { return; } // only attach link information if the edge is new - if (isNewEdge == true) { - pEdge->SetLabel(new LinkInfo(pFromScan->GetCorrectedPose(), pToScan->GetCorrectedAt(rMean), rCovariance)); - if (m_pMapper->m_pScanOptimizer != NULL) { + if (isNewEdge == true) + { + pEdge->SetLabel( + new LinkInfo(pFromScan->GetCorrectedPose(), pToScan->GetCorrectedAt(rMean), rCovariance)); + if (m_pMapper->m_pScanOptimizer != NULL) + { m_pMapper->m_pScanOptimizer->AddConstraint(pEdge); } } } -void MapperGraph::LinkNearChains( - LocalizedRangeScan * pScan, Pose2Vector & rMeans, - std::vector & rCovariances) +void MapperGraph::LinkNearChains(LocalizedRangeScan* pScan, Pose2Vector& rMeans, + std::vector& rCovariances) { const std::vector nearChains = FindNearChains(pScan); const_forEach(std::vector, &nearChains) { - if (iter->size() < m_pMapper->m_pLoopMatchMinimumChainSize->GetValue()) { + if (iter->size() < m_pMapper->m_pLoopMatchMinimumChainSize->GetValue()) + { continue; } Pose2 mean; Matrix3 covariance; // match scan against "near" chain - kt_double response = m_pMapper->m_pSequentialScanMatcher->MatchScan(pScan, *iter, mean, - covariance, false); - if (response > m_pMapper->m_pLinkMatchMinimumResponseFine->GetValue() - KT_TOLERANCE) { + kt_double response = + m_pMapper->m_pSequentialScanMatcher->MatchScan(pScan, *iter, mean, covariance, false); + if (response > m_pMapper->m_pLinkMatchMinimumResponseFine->GetValue() - KT_TOLERANCE) + { rMeans.push_back(mean); rCovariances.push_back(covariance); LinkChainToScan(*iter, pScan, mean, covariance); @@ -1659,13 +1796,12 @@ void MapperGraph::LinkNearChains( } } -void MapperGraph::LinkChainToScan( - const LocalizedRangeScanVector & rChain, LocalizedRangeScan * pScan, - const Pose2 & rMean, const Matrix3 & rCovariance) +void MapperGraph::LinkChainToScan(const LocalizedRangeScanVector& rChain, LocalizedRangeScan* pScan, + const Pose2& rMean, const Matrix3& rCovariance) { Pose2 pose = pScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue()); - LocalizedRangeScan * pClosestScan = GetClosestScanToPose(rChain, pose); + LocalizedRangeScan* pClosestScan = GetClosestScanToPose(rChain, pose); assert(pClosestScan != NULL); Pose2 closestScanPose = @@ -1673,13 +1809,13 @@ void MapperGraph::LinkChainToScan( kt_double squaredDistance = pose.GetPosition().SquaredDistance(closestScanPose.GetPosition()); if (squaredDistance < - math::Square(m_pMapper->m_pLinkScanMaximumDistance->GetValue()) + KT_TOLERANCE) + math::Square(m_pMapper->m_pLinkScanMaximumDistance->GetValue()) + KT_TOLERANCE) { LinkScans(pClosestScan, pScan, rMean, rCovariance); } } -std::vector MapperGraph::FindNearChains(LocalizedRangeScan * pScan) +std::vector MapperGraph::FindNearChains(LocalizedRangeScan* pScan) { std::vector nearChains; @@ -1688,18 +1824,20 @@ std::vector MapperGraph::FindNearChains(LocalizedRange // to keep track of which scans have been added to a chain LocalizedRangeScanVector processed; - const LocalizedRangeScanVector nearLinkedScans = FindNearLinkedScans(pScan, - m_pMapper->m_pLinkScanMaximumDistance->GetValue()); + const LocalizedRangeScanVector nearLinkedScans = + FindNearLinkedScans(pScan, m_pMapper->m_pLinkScanMaximumDistance->GetValue()); const_forEach(LocalizedRangeScanVector, &nearLinkedScans) { - LocalizedRangeScan * pNearScan = *iter; + LocalizedRangeScan* pNearScan = *iter; - if (pNearScan == pScan) { + if (pNearScan == pScan) + { continue; } // scan has already been processed, skip - if (find(processed.begin(), processed.end(), pNearScan) != processed.end()) { + if (find(processed.begin(), processed.end(), pNearScan) != processed.end()) + { continue; } @@ -1707,37 +1845,40 @@ std::vector MapperGraph::FindNearChains(LocalizedRange // build up chain kt_bool isValidChain = true; - std::list chain; + std::list chain; // add scans before current scan being processed for (kt_int32s candidateScanNum = pNearScan->GetStateId() - 1; candidateScanNum >= 0; - candidateScanNum--) + candidateScanNum--) { - LocalizedRangeScan * pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan( - pNearScan->GetSensorName(), - candidateScanNum); + LocalizedRangeScan* pCandidateScan = + m_pMapper->m_pMapperSensorManager->GetScan(pNearScan->GetSensorName(), candidateScanNum); // chain is invalid--contains scan being added - if (pCandidateScan == pScan) { + if (pCandidateScan == pScan) + { isValidChain = false; } // probably removed in localization mode - if (pCandidateScan == NULL) { + if (pCandidateScan == NULL) + { continue; } - Pose2 candidatePose = pCandidateScan->GetReferencePose( - m_pMapper->m_pUseScanBarycenter->GetValue()); + Pose2 candidatePose = + pCandidateScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue()); kt_double squaredDistance = scanPose.GetPosition().SquaredDistance(candidatePose.GetPosition()); if (squaredDistance < - math::Square(m_pMapper->m_pLinkScanMaximumDistance->GetValue()) + KT_TOLERANCE) + math::Square(m_pMapper->m_pLinkScanMaximumDistance->GetValue()) + KT_TOLERANCE) { chain.push_front(pCandidateScan); processed.push_back(pCandidateScan); - } else { + } + else + { break; } } @@ -1745,41 +1886,44 @@ std::vector MapperGraph::FindNearChains(LocalizedRange chain.push_back(pNearScan); // add scans after current scan being processed - kt_int32u end = - static_cast(m_pMapper->m_pMapperSensorManager->GetScans( - pNearScan->GetSensorName()).size()); + kt_int32u end = static_cast( + m_pMapper->m_pMapperSensorManager->GetScans(pNearScan->GetSensorName()).size()); for (kt_int32u candidateScanNum = pNearScan->GetStateId() + 1; candidateScanNum < end; - candidateScanNum++) + candidateScanNum++) { - LocalizedRangeScan * pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan( - pNearScan->GetSensorName(), - candidateScanNum); + LocalizedRangeScan* pCandidateScan = + m_pMapper->m_pMapperSensorManager->GetScan(pNearScan->GetSensorName(), candidateScanNum); - if (pCandidateScan == pScan) { + if (pCandidateScan == pScan) + { isValidChain = false; } // probably removed in localization mode - if (pCandidateScan == NULL) { + if (pCandidateScan == NULL) + { continue; } - Pose2 candidatePose = pCandidateScan->GetReferencePose( - m_pMapper->m_pUseScanBarycenter->GetValue()); + Pose2 candidatePose = + pCandidateScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue()); kt_double squaredDistance = scanPose.GetPosition().SquaredDistance(candidatePose.GetPosition()); if (squaredDistance < - math::Square(m_pMapper->m_pLinkScanMaximumDistance->GetValue()) + KT_TOLERANCE) + math::Square(m_pMapper->m_pLinkScanMaximumDistance->GetValue()) + KT_TOLERANCE) { chain.push_back(pCandidateScan); processed.push_back(pCandidateScan); - } else { + } + else + { break; } } - if (isValidChain) { + if (isValidChain) + { // change list to vector LocalizedRangeScanVector tempChain; std::copy(chain.begin(), chain.end(), std::inserter(tempChain, tempChain.begin())); @@ -1791,40 +1935,37 @@ std::vector MapperGraph::FindNearChains(LocalizedRange return nearChains; } -LocalizedRangeScanVector MapperGraph::FindNearLinkedScans( - LocalizedRangeScan * pScan, - kt_double maxDistance) +LocalizedRangeScanVector MapperGraph::FindNearLinkedScans(LocalizedRangeScan* pScan, + kt_double maxDistance) { - NearScanVisitor * pVisitor = new NearScanVisitor(pScan, maxDistance, - m_pMapper->m_pUseScanBarycenter->GetValue()); - LocalizedRangeScanVector nearLinkedScans = m_pTraversal->TraverseForScans(GetVertex( - pScan), pVisitor); + NearScanVisitor* pVisitor = + new NearScanVisitor(pScan, maxDistance, m_pMapper->m_pUseScanBarycenter->GetValue()); + LocalizedRangeScanVector nearLinkedScans = + m_pTraversal->TraverseForScans(GetVertex(pScan), pVisitor); delete pVisitor; return nearLinkedScans; } -std::vector *> MapperGraph::FindNearLinkedVertices( - LocalizedRangeScan * pScan, kt_double maxDistance) +std::vector*> MapperGraph::FindNearLinkedVertices( + LocalizedRangeScan* pScan, kt_double maxDistance) { - NearScanVisitor * pVisitor = new NearScanVisitor(pScan, maxDistance, - m_pMapper->m_pUseScanBarycenter->GetValue()); - std::vector *> nearLinkedVertices = - m_pTraversal->TraverseForVertices(GetVertex( - pScan), pVisitor); + NearScanVisitor* pVisitor = + new NearScanVisitor(pScan, maxDistance, m_pMapper->m_pUseScanBarycenter->GetValue()); + std::vector*> nearLinkedVertices = + m_pTraversal->TraverseForVertices(GetVertex(pScan), pVisitor); delete pVisitor; return nearLinkedVertices; } -LocalizedRangeScanVector MapperGraph::FindNearByScans( - Name name, const Pose2 refPose, - kt_double maxDistance) +LocalizedRangeScanVector MapperGraph::FindNearByScans(Name name, const Pose2 refPose, + kt_double maxDistance) { - NearPoseVisitor * pVisitor = new NearPoseVisitor(refPose, maxDistance, - m_pMapper->m_pUseScanBarycenter->GetValue()); + NearPoseVisitor* pVisitor = + new NearPoseVisitor(refPose, maxDistance, m_pMapper->m_pUseScanBarycenter->GetValue()); - Vertex * closestVertex = FindNearByScan(name, refPose); + Vertex* closestVertex = FindNearByScan(name, refPose); LocalizedRangeScanVector nearLinkedScans = m_pTraversal->TraverseForScans(closestVertex, pVisitor); @@ -1833,31 +1974,32 @@ LocalizedRangeScanVector MapperGraph::FindNearByScans( return nearLinkedScans; } -std::vector *> MapperGraph::FindNearByVertices( - Name name, - const Pose2 refPose, - kt_double maxDistance) +std::vector*> MapperGraph::FindNearByVertices(Name name, + const Pose2 refPose, + kt_double maxDistance) { - VertexMap vertexMap = GetVertices(); - std::map *> & vertices = vertexMap[name]; + VertexMap vertexMap = GetVertices(); + std::map*>& vertices = vertexMap[name]; - std::vector *> vertices_to_search; - std::map *>::iterator it; - for (it = vertices.begin(); it != vertices.end(); ++it) { - if (it->second) { + std::vector*> vertices_to_search; + std::map*>::iterator it; + for (it = vertices.begin(); it != vertices.end(); ++it) + { + if (it->second) + { vertices_to_search.push_back(it->second); } } const size_t dim = 2; - typedef VertexVectorPoseNanoFlannAdaptor *>> P2KD; + typedef VertexVectorPoseNanoFlannAdaptor*>> P2KD; const P2KD p2kd(vertices_to_search); - typedef nanoflann::KDTreeSingleIndexAdaptor, P2KD, - dim> my_kd_tree_t; + typedef nanoflann::KDTreeSingleIndexAdaptor, P2KD, dim> + my_kd_tree_t; - my_kd_tree_t index(dim, p2kd, nanoflann::KDTreeSingleIndexAdaptorParams(10) ); + my_kd_tree_t index(dim, p2kd, nanoflann::KDTreeSingleIndexAdaptorParams(10)); index.buildIndex(); std::vector> ret_matches; @@ -1865,37 +2007,40 @@ std::vector *> MapperGraph::FindNearByVertices( nanoflann::SearchParams params; const size_t num_results = index.radiusSearch(&query_pt[0], maxDistance, ret_matches, params); - std::vector *> rtn_vertices; + std::vector*> rtn_vertices; rtn_vertices.reserve(ret_matches.size()); - for (uint i = 0; i != ret_matches.size(); i++) { + for (uint i = 0; i != ret_matches.size(); i++) + { rtn_vertices.push_back(vertices_to_search[ret_matches[i].first]); } return rtn_vertices; } -Vertex * MapperGraph::FindNearByScan(Name name, const Pose2 refPose) +Vertex* MapperGraph::FindNearByScan(Name name, const Pose2 refPose) { - VertexMap vertexMap = GetVertices(); - std::map *> & vertices = vertexMap[name]; + VertexMap vertexMap = GetVertices(); + std::map*>& vertices = vertexMap[name]; - std::vector *> vertices_to_search; - std::map *>::iterator it; - for (it = vertices.begin(); it != vertices.end(); ++it) { - if (it->second) { + std::vector*> vertices_to_search; + std::map*>::iterator it; + for (it = vertices.begin(); it != vertices.end(); ++it) + { + if (it->second) + { vertices_to_search.push_back(it->second); } } size_t num_results = 1; - const size_t dim = 2; + const size_t dim = 2; - typedef VertexVectorPoseNanoFlannAdaptor *>> P2KD; + typedef VertexVectorPoseNanoFlannAdaptor*>> P2KD; const P2KD p2kd(vertices_to_search); - typedef nanoflann::KDTreeSingleIndexAdaptor, P2KD, - dim> my_kd_tree_t; + typedef nanoflann::KDTreeSingleIndexAdaptor, P2KD, dim> + my_kd_tree_t; - my_kd_tree_t index(dim, p2kd, nanoflann::KDTreeSingleIndexAdaptorParams(10) ); + my_kd_tree_t index(dim, p2kd, nanoflann::KDTreeSingleIndexAdaptorParams(10)); index.buildIndex(); std::vector ret_index(num_results); @@ -1903,16 +2048,18 @@ Vertex * MapperGraph::FindNearByScan(Name name, const Pose2 const double query_pt[2] = {refPose.GetX(), refPose.GetY()}; num_results = index.knnSearch(&query_pt[0], num_results, &ret_index[0], &out_dist_sqr[0]); - if (num_results > 0) { + if (num_results > 0) + { return vertices_to_search[ret_index[0]]; - } else { + } + else + { return NULL; } } -Pose2 MapperGraph::ComputeWeightedMean( - const Pose2Vector & rMeans, - const std::vector & rCovariances) const +Pose2 MapperGraph::ComputeWeightedMean(const Pose2Vector& rMeans, + const std::vector& rCovariances) const { assert(rMeans.size() == rCovariances.size()); @@ -1938,7 +2085,7 @@ Pose2 MapperGraph::ComputeWeightedMean( Pose2Vector::const_iterator meansIter = rMeans.begin(); const_forEach(std::vector, &inverses) { - Pose2 pose = *meansIter; + Pose2 pose = *meansIter; kt_double angle = pose.GetHeading(); thetaX += cos(angle); thetaY += sin(angle); @@ -1956,12 +2103,11 @@ Pose2 MapperGraph::ComputeWeightedMean( return accumulatedPose; } -LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure( - LocalizedRangeScan * pScan, - const Name & rSensorName, - kt_int32u & rStartNum) +LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure(LocalizedRangeScan* pScan, + const Name& rSensorName, + kt_int32u& rStartNum) { - LocalizedRangeScanVector chain; // return value + LocalizedRangeScanVector chain; // return value Pose2 pose = pScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue()); @@ -1972,34 +2118,43 @@ LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure( kt_int32u nScans = static_cast(m_pMapper->m_pMapperSensorManager->GetScans(rSensorName).size()); - for (; rStartNum < nScans; rStartNum++) { - LocalizedRangeScan * pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan(rSensorName, - rStartNum); + for (; rStartNum < nScans; rStartNum++) + { + LocalizedRangeScan* pCandidateScan = + m_pMapper->m_pMapperSensorManager->GetScan(rSensorName, rStartNum); - if (pCandidateScan == NULL) { + if (pCandidateScan == NULL) + { continue; } - Pose2 candidateScanPose = pCandidateScan->GetReferencePose( - m_pMapper->m_pUseScanBarycenter->GetValue()); + Pose2 candidateScanPose = + pCandidateScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue()); kt_double squaredDistance = candidateScanPose.GetPosition().SquaredDistance(pose.GetPosition()); if (squaredDistance < - math::Square(m_pMapper->m_pLoopSearchMaximumDistance->GetValue()) + KT_TOLERANCE) + math::Square(m_pMapper->m_pLoopSearchMaximumDistance->GetValue()) + KT_TOLERANCE) { // a linked scan cannot be in the chain - if (find(nearLinkedScans.begin(), nearLinkedScans.end(), - pCandidateScan) != nearLinkedScans.end()) + if (find(nearLinkedScans.begin(), nearLinkedScans.end(), pCandidateScan) != + nearLinkedScans.end()) { chain.clear(); - } else { + } + else + { chain.push_back(pCandidateScan); } - } else { + } + else + { // return chain if it is long "enough" - if (chain.size() >= m_pMapper->m_pLoopMatchMinimumChainSize->GetValue()) { + if (chain.size() >= m_pMapper->m_pLoopMatchMinimumChainSize->GetValue()) + { return chain; - } else { + } + else + { chain.clear(); } } @@ -2010,33 +2165,61 @@ LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure( void MapperGraph::CorrectPoses() { + // tracking for how much the graph changed + double totalCorrectionX = 0.0; + double totalCorrectionY = 0.0; + double totalCorrectionHeading = 0.0; + std::size_t numberOfScans = 0.0; + // optimize scans! - ScanSolver * pSolver = m_pMapper->m_pScanOptimizer; - if (pSolver != NULL) { + ScanSolver* pSolver = m_pMapper->m_pScanOptimizer; + if (pSolver != NULL) + { pSolver->Compute(); const_forEach(ScanSolver::IdPoseVector, &pSolver->GetCorrections()) { - LocalizedRangeScan * scan = m_pMapper->m_pMapperSensorManager->GetScan(iter->first); - if (scan == NULL) { + LocalizedRangeScan* scan = m_pMapper->m_pMapperSensorManager->GetScan(iter->first); + if (scan == NULL) + { continue; } + auto old_pose = scan->GetCorrectedPose(); + totalCorrectionX += std::abs(iter->second.GetX() - old_pose.GetX()); + totalCorrectionY += std::abs(iter->second.GetY() - old_pose.GetY()); + totalCorrectionHeading += + std::abs(math::NormalizeAngleDifference(iter->second.GetHeading(), old_pose.GetHeading())); + numberOfScans++; + scan->SetCorrectedPoseAndUpdate(iter->second); } pSolver->Clear(); } + if (numberOfScans > 0) + { + double averageHeadingCorrection = totalCorrectionHeading / numberOfScans; + if (totalCorrectionX > 0.6 || totalCorrectionY > 0.6 || averageHeadingCorrection > 0.3) + { + std::cout << "MapperGraph::CorrectPoses - Loop closure resulted in large pose change" + << std::endl; + std::cout << "Graph size: " << numberOfScans << std::endl; + std::cout << "Total Pose Correction: \nX: " << totalCorrectionX << "\nY:" << totalCorrectionY + << "\nHeading: " << totalCorrectionHeading << std::endl; + } + } } void MapperGraph::UpdateLoopScanMatcher(kt_double rangeThreshold) { - if (m_pLoopScanMatcher) { + if (m_pLoopScanMatcher) + { delete m_pLoopScanMatcher; } - m_pLoopScanMatcher = ScanMatcher::Create(m_pMapper, - m_pMapper->m_pLoopSearchSpaceDimension->GetValue(), - m_pMapper->m_pLoopSearchSpaceResolution->GetValue(), - m_pMapper->m_pLoopSearchSpaceSmearDeviation->GetValue(), rangeThreshold); + m_pLoopScanMatcher = + ScanMatcher::Create(m_pMapper, m_pMapper->m_pLoopSearchSpaceDimension->GetValue(), + m_pMapper->m_pLoopSearchSpaceResolution->GetValue(), + m_pMapper->m_pLoopSearchSpaceSmearDeviation->GetValue(), rangeThreshold); assert(m_pLoopScanMatcher); } @@ -2048,13 +2231,8 @@ void MapperGraph::UpdateLoopScanMatcher(kt_double rangeThreshold) * Default constructor */ Mapper::Mapper() -: Module("Mapper"), - m_Initialized(false), - m_Deserialized(false), - m_pSequentialScanMatcher(NULL), - m_pMapperSensorManager(NULL), - m_pGraph(NULL), - m_pScanOptimizer(NULL) + : Module("Mapper"), m_Initialized(false), m_Deserialized(false), m_pSequentialScanMatcher(NULL), + m_pMapperSensorManager(NULL), m_pGraph(NULL), m_pScanOptimizer(NULL) { InitializeParameters(); } @@ -2062,14 +2240,9 @@ Mapper::Mapper() /** * Default constructor */ -Mapper::Mapper(const std::string & rName) -: Module(rName), - m_Initialized(false), - m_Deserialized(false), - m_pSequentialScanMatcher(NULL), - m_pMapperSensorManager(NULL), - m_pGraph(NULL), - m_pScanOptimizer(NULL) +Mapper::Mapper(const std::string& rName) + : Module(rName), m_Initialized(false), m_Deserialized(false), m_pSequentialScanMatcher(NULL), + m_pMapperSensorManager(NULL), m_pGraph(NULL), m_pScanOptimizer(NULL) { InitializeParameters(); } @@ -2086,45 +2259,44 @@ Mapper::~Mapper() void Mapper::InitializeParameters() { - m_pUseScanMatching = new Parameter( - "UseScanMatching", - "When set to true, the mapper will use a scan matching algorithm. " - "In most real-world situations this should be set to true so that the " - "mapper algorithm can correct for noise and errors in odometry and " - "scan data. In some simulator environments where the simulated scan " - "and odometry data are very accurate, the scan matching algorithm can " - "produce worse results. In those cases set this to false to improve " - "results.", - true, - GetParameterManager()); - - m_pUseScanBarycenter = new Parameter( - "UseScanBarycenter", - "Use the barycenter of scan endpoints to define distances between " - "scans.", - true, GetParameterManager()); - - m_pMinimumTimeInterval = new Parameter( - "MinimumTimeInterval", - "Sets the minimum time between scans. If a new scan's time stamp is " - "longer than MinimumTimeInterval from the previously processed scan, " - "the mapper will use the data from the new scan. Otherwise, it will " - "discard the new scan if it also does not meet the minimum travel " - "distance and heading requirements. For performance reasons, it is " - "generally it is a good idea to only process scans if a reasonable " - "amount of time has passed. This parameter is particularly useful " - "when there is a need to process scans while the robot is stationary.", - 3600, GetParameterManager()); - - m_pMinimumTravelDistance = new Parameter( - "MinimumTravelDistance", - "Sets the minimum travel between scans. If a new scan's position is " - "more than minimumTravelDistance from the previous scan, the mapper " - "will use the data from the new scan. Otherwise, it will discard the " - "new scan if it also does not meet the minimum change in heading " - "requirement. For performance reasons, generally it is a good idea to " - "only process scans if the robot has moved a reasonable amount.", - 0.2, GetParameterManager()); + m_pUseScanMatching = + new Parameter("UseScanMatching", + "When set to true, the mapper will use a scan matching algorithm. " + "In most real-world situations this should be set to true so that the " + "mapper algorithm can correct for noise and errors in odometry and " + "scan data. In some simulator environments where the simulated scan " + "and odometry data are very accurate, the scan matching algorithm can " + "produce worse results. In those cases set this to false to improve " + "results.", + true, GetParameterManager()); + + m_pUseScanBarycenter = + new Parameter("UseScanBarycenter", + "Use the barycenter of scan endpoints to define distances between " + "scans.", + true, GetParameterManager()); + + m_pMinimumTimeInterval = + new Parameter("MinimumTimeInterval", + "Sets the minimum time between scans. If a new scan's time stamp is " + "longer than MinimumTimeInterval from the previously processed scan, " + "the mapper will use the data from the new scan. Otherwise, it will " + "discard the new scan if it also does not meet the minimum travel " + "distance and heading requirements. For performance reasons, it is " + "generally it is a good idea to only process scans if a reasonable " + "amount of time has passed. This parameter is particularly useful " + "when there is a need to process scans while the robot is stationary.", + 3600, GetParameterManager()); + + m_pMinimumTravelDistance = + new Parameter("MinimumTravelDistance", + "Sets the minimum travel between scans. If a new scan's position is " + "more than minimumTravelDistance from the previous scan, the mapper " + "will use the data from the new scan. Otherwise, it will discard the " + "new scan if it also does not meet the minimum change in heading " + "requirement. For performance reasons, generally it is a good idea to " + "only process scans if the robot has moved a reasonable amount.", + 0.2, GetParameterManager()); m_pMinimumTravelHeading = new Parameter( "MinimumTravelHeading", @@ -2153,42 +2325,40 @@ void Mapper::InitializeParameters() "first and last scans in the scan chain stored for matching.", 20.0, GetParameterManager()); - m_pLinkMatchMinimumResponseFine = new Parameter( - "LinkMatchMinimumResponseFine", - "Scans are linked only if the correlation response value is greater " - "than this value.", - 0.8, GetParameterManager()); - - m_pLinkScanMaximumDistance = new Parameter( - "LinkScanMaximumDistance", - "Maximum distance between linked scans. Scans that are farther apart " - "will not be linked regardless of the correlation response value.", - 10.0, GetParameterManager()); - - m_pLoopSearchMaximumDistance = new Parameter( - "LoopSearchMaximumDistance", - "Scans less than this distance from the current position will be " - "considered for a match in loop closure.", - 4.0, GetParameterManager()); - - m_pDoLoopClosing = new Parameter( - "DoLoopClosing", - "Enable/disable loop closure.", - true, GetParameterManager()); - - m_pLoopMatchMinimumChainSize = new Parameter( - "LoopMatchMinimumChainSize", - "When the loop closure detection finds a candidate it must be part of " - "a large set of linked scans. If the chain of scans is less than this " - "value we do not attempt to close the loop.", - 10, GetParameterManager()); - - m_pLoopMatchMaximumVarianceCoarse = new Parameter( - "LoopMatchMaximumVarianceCoarse", - "The co-variance values for a possible loop closure have to be less " - "than this value to consider a viable solution. This applies to the " - "coarse search.", - math::Square(0.4), GetParameterManager()); + m_pLinkMatchMinimumResponseFine = + new Parameter("LinkMatchMinimumResponseFine", + "Scans are linked only if the correlation response value is greater " + "than this value.", + 0.8, GetParameterManager()); + + m_pLinkScanMaximumDistance = + new Parameter("LinkScanMaximumDistance", + "Maximum distance between linked scans. Scans that are farther apart " + "will not be linked regardless of the correlation response value.", + 10.0, GetParameterManager()); + + m_pLoopSearchMaximumDistance = + new Parameter("LoopSearchMaximumDistance", + "Scans less than this distance from the current position will be " + "considered for a match in loop closure.", + 4.0, GetParameterManager()); + + m_pDoLoopClosing = new Parameter("DoLoopClosing", "Enable/disable loop closure.", true, + GetParameterManager()); + + m_pLoopMatchMinimumChainSize = + new Parameter("LoopMatchMinimumChainSize", + "When the loop closure detection finds a candidate it must be part of " + "a large set of linked scans. If the chain of scans is less than this " + "value we do not attempt to close the loop.", + 10, GetParameterManager()); + + m_pLoopMatchMaximumVarianceCoarse = + new Parameter("LoopMatchMaximumVarianceCoarse", + "The co-variance values for a possible loop closure have to be less " + "than this value to consider a viable solution. This applies to the " + "coarse search.", + math::Square(0.4), GetParameterManager()); m_pLoopMatchMinimumResponseCoarse = new Parameter( "LoopMatchMinimumResponseCoarse", @@ -2214,63 +2384,55 @@ void Mapper::InitializeParameters() m_pCorrelationSearchSpaceResolution = new Parameter( "CorrelationSearchSpaceResolution", - "The resolution (size of a grid cell) of the correlation grid.", - 0.01, GetParameterManager()); - - m_pCorrelationSearchSpaceSmearDeviation = new Parameter( - "CorrelationSearchSpaceSmearDeviation", - "The point readings are smeared by this value in X and Y to create a " - "smoother response.", - 0.03, GetParameterManager()); + "The resolution (size of a grid cell) of the correlation grid.", 0.01, GetParameterManager()); + m_pCorrelationSearchSpaceSmearDeviation = + new Parameter("CorrelationSearchSpaceSmearDeviation", + "The point readings are smeared by this value in X and Y to create a " + "smoother response.", + 0.03, GetParameterManager()); ////////////////////////////////////////////////////////////////////////////// // CorrelationParameters loopCorrelationParameters; m_pLoopSearchSpaceDimension = new Parameter( - "LoopSearchSpaceDimension", - "The size of the search grid used by the matcher.", - 8.0, GetParameterManager()); + "LoopSearchSpaceDimension", "The size of the search grid used by the matcher.", 8.0, + GetParameterManager()); m_pLoopSearchSpaceResolution = new Parameter( - "LoopSearchSpaceResolution", - "The resolution (size of a grid cell) of the correlation grid.", + "LoopSearchSpaceResolution", "The resolution (size of a grid cell) of the correlation grid.", 0.05, GetParameterManager()); - m_pLoopSearchSpaceSmearDeviation = new Parameter( - "LoopSearchSpaceSmearDeviation", - "The point readings are smeared by this value in X and Y to create a " - "smoother response.", - 0.03, GetParameterManager()); + m_pLoopSearchSpaceSmearDeviation = + new Parameter("LoopSearchSpaceSmearDeviation", + "The point readings are smeared by this value in X and Y to create a " + "smoother response.", + 0.03, GetParameterManager()); ////////////////////////////////////////////////////////////////////////////// // ScanMatcherParameters; - m_pDistanceVariancePenalty = new Parameter( - "DistanceVariancePenalty", - "Variance of penalty for deviating from odometry when scan-matching. " - "The penalty is a multiplier (less than 1.0) is a function of the " - "delta of the scan position being tested and the odometric pose.", - math::Square(0.3), GetParameterManager()); + m_pDistanceVariancePenalty = + new Parameter("DistanceVariancePenalty", + "Variance of penalty for deviating from odometry when scan-matching. " + "The penalty is a multiplier (less than 1.0) is a function of the " + "delta of the scan position being tested and the odometric pose.", + math::Square(0.3), GetParameterManager()); - m_pAngleVariancePenalty = new Parameter( - "AngleVariancePenalty", - "See DistanceVariancePenalty.", - math::Square(math::DegreesToRadians(20)), GetParameterManager()); + m_pAngleVariancePenalty = + new Parameter("AngleVariancePenalty", "See DistanceVariancePenalty.", + math::Square(math::DegreesToRadians(20)), GetParameterManager()); m_pFineSearchAngleOffset = new Parameter( - "FineSearchAngleOffset", - "The range of angles to search during a fine search.", + "FineSearchAngleOffset", "The range of angles to search during a fine search.", math::DegreesToRadians(0.2), GetParameterManager()); m_pCoarseSearchAngleOffset = new Parameter( - "CoarseSearchAngleOffset", - "The range of angles to search during a coarse search.", + "CoarseSearchAngleOffset", "The range of angles to search during a coarse search.", math::DegreesToRadians(20), GetParameterManager()); m_pCoarseAngleResolution = new Parameter( - "CoarseAngleResolution", - "Resolution of angles to search during a coarse search.", + "CoarseAngleResolution", "Resolution of angles to search during a coarse search.", math::DegreesToRadians(2), GetParameterManager()); m_pMinimumAnglePenalty = new Parameter( @@ -2279,17 +2441,17 @@ void Mapper::InitializeParameters() "too small.", 0.9, GetParameterManager()); - m_pMinimumDistancePenalty = new Parameter( - "MinimumDistancePenalty", - "Minimum value of the distance penalty multiplier so scores do not " - "become too small.", - 0.5, GetParameterManager()); + m_pMinimumDistancePenalty = + new Parameter("MinimumDistancePenalty", + "Minimum value of the distance penalty multiplier so scores do not " + "become too small.", + 0.5, GetParameterManager()); - m_pUseResponseExpansion = new Parameter( - "UseResponseExpansion", - "Whether to increase the search space if no good matches are initially " - "found.", - false, GetParameterManager()); + m_pUseResponseExpansion = + new Parameter("UseResponseExpansion", + "Whether to increase the search space if no good matches are initially " + "found.", + false, GetParameterManager()); } /* Adding in getters and setters here for easy parameter access */ @@ -2539,7 +2701,6 @@ void Mapper::setParamCorrelationSearchSpaceSmearDeviation(double d) m_pCorrelationSearchSpaceSmearDeviation->SetValue((kt_double)d); } - // Correlation Parameters - Loop Closure Parameters void Mapper::setParamLoopSearchSpaceDimension(double d) { @@ -2556,7 +2717,6 @@ void Mapper::setParamLoopSearchSpaceSmearDeviation(double d) m_pLoopSearchSpaceSmearDeviation->SetValue((kt_double)d); } - // Scan Matcher Parameters void Mapper::setParamDistanceVariancePenalty(double d) { @@ -2598,32 +2758,36 @@ void Mapper::setParamUseResponseExpansion(bool b) m_pUseResponseExpansion->SetValue((kt_bool)b); } - void Mapper::Initialize(kt_double rangeThreshold) { - if (m_Initialized) { + if (m_Initialized) + { return; } // create sequential scan and loop matcher, update if deserialized - if (m_pSequentialScanMatcher) { + if (m_pSequentialScanMatcher) + { delete m_pSequentialScanMatcher; } - m_pSequentialScanMatcher = ScanMatcher::Create(this, - m_pCorrelationSearchSpaceDimension->GetValue(), - m_pCorrelationSearchSpaceResolution->GetValue(), - m_pCorrelationSearchSpaceSmearDeviation->GetValue(), - rangeThreshold); + m_pSequentialScanMatcher = + ScanMatcher::Create(this, m_pCorrelationSearchSpaceDimension->GetValue(), + m_pCorrelationSearchSpaceResolution->GetValue(), + m_pCorrelationSearchSpaceSmearDeviation->GetValue(), rangeThreshold); assert(m_pSequentialScanMatcher); - if (m_Deserialized) { + if (m_Deserialized) + { m_pMapperSensorManager->SetRunningScanBufferSize(m_pScanBufferSize->GetValue()); - m_pMapperSensorManager->SetRunningScanBufferMaximumDistance(m_pScanBufferMaximumScanDistance->GetValue()); + m_pMapperSensorManager->SetRunningScanBufferMaximumDistance( + m_pScanBufferMaximumScanDistance->GetValue()); m_pGraph->UpdateLoopScanMatcher(rangeThreshold); - } else { + } + else + { m_pMapperSensorManager = new MapperSensorManager(m_pScanBufferSize->GetValue(), - m_pScanBufferMaximumScanDistance->GetValue()); + m_pScanBufferMaximumScanDistance->GetValue()); m_pGraph = new MapperGraph(this, rangeThreshold); } @@ -2631,7 +2795,7 @@ void Mapper::Initialize(kt_double rangeThreshold) m_Initialized = true; } -void Mapper::SaveToFile(const std::string & filename) +void Mapper::SaveToFile(const std::string& filename) { printf("Save To File %s \n", filename.c_str()); std::ofstream ofs(filename.c_str()); @@ -2639,104 +2803,118 @@ void Mapper::SaveToFile(const std::string & filename) oa << BOOST_SERIALIZATION_NVP(*this); } -void Mapper::LoadFromFile(const std::string & filename) +void Mapper::LoadFromFile(const std::string& filename) { printf("Load From File %s \n", filename.c_str()); std::ifstream ifs(filename.c_str()); boost::archive::binary_iarchive ia(ifs, boost::archive::no_codecvt); ia >> BOOST_SERIALIZATION_NVP(*this); m_Deserialized = true; - m_Initialized = false; + m_Initialized = false; } void Mapper::Reset() { - if (m_pSequentialScanMatcher) { + if (m_pSequentialScanMatcher) + { delete m_pSequentialScanMatcher; m_pSequentialScanMatcher = NULL; } - if (m_pGraph) { + if (m_pGraph) + { delete m_pGraph; m_pGraph = NULL; } - if (m_pMapperSensorManager) { + if (m_pMapperSensorManager) + { delete m_pMapperSensorManager; m_pMapperSensorManager = NULL; } - m_Initialized = false; + m_Initialized = false; m_Deserialized = false; - while (!m_LocalizationScanVertices.empty()) { - m_LocalizationScanVertices.pop(); - } + m_LocalizationScanVertices.clear(); } -kt_bool Mapper::Process(Object * /*pObject*/) // NOLINT +kt_bool Mapper::Process(Object* /*pObject*/) // NOLINT { return true; } -kt_bool Mapper::Process(LocalizedRangeScan * pScan) +kt_bool Mapper::Process(LocalizedRangeScan* pScan, Matrix3* covariance, bool force_match_only) { - if (pScan != NULL) { - karto::LaserRangeFinder * pLaserRangeFinder = pScan->GetLaserRangeFinder(); + if (pScan != NULL) + { + karto::LaserRangeFinder* pLaserRangeFinder = pScan->GetLaserRangeFinder(); // validate scan - if (pLaserRangeFinder == NULL || pScan == NULL || pLaserRangeFinder->Validate(pScan) == false) { + if (pLaserRangeFinder == NULL || pScan == NULL || pLaserRangeFinder->Validate(pScan) == false) + { return false; } - if (m_Initialized == false) { + if (m_Initialized == false) + { // initialize mapper with range threshold from device Initialize(pLaserRangeFinder->GetRangeThreshold()); } // get last scan - LocalizedRangeScan * pLastScan = m_pMapperSensorManager->GetLastScan(pScan->GetSensorName()); + LocalizedRangeScan* pLastScan = m_pMapperSensorManager->GetLastScan(pScan->GetSensorName()); // update scans corrected pose based on last correction - if (pLastScan != NULL) { + if (pLastScan != NULL) + { Transform lastTransform(pLastScan->GetOdometricPose(), pLastScan->GetCorrectedPose()); pScan->SetCorrectedPose(lastTransform.TransformPose(pScan->GetOdometricPose())); } // test if scan is outside minimum boundary or if heading is larger then minimum heading - if (!HasMovedEnough(pScan, pLastScan)) { + if (!force_match_only && !HasMovedEnough(pScan, pLastScan)) + { return false; } - Matrix3 covariance; - covariance.SetToIdentity(); + Matrix3 cov; + cov.SetToIdentity(); // correct scan (if not first scan) - if (m_pUseScanMatching->GetValue() && pLastScan != NULL) { + if (m_pUseScanMatching->GetValue() && pLastScan != NULL) + { Pose2 bestPose; - m_pSequentialScanMatcher->MatchScan(pScan, - m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()), - bestPose, - covariance); + m_pSequentialScanMatcher->MatchScan( + pScan, m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()), bestPose, cov); pScan->SetSensorPose(bestPose); + if (covariance) + { + *covariance = cov; + } } - // add scan to buffer and assign id - m_pMapperSensorManager->AddScan(pScan); + if (!force_match_only) + { + // add scan to buffer and assign id + m_pMapperSensorManager->AddScan(pScan); - if (m_pUseScanMatching->GetValue()) { - // add to graph - m_pGraph->AddVertex(pScan); - m_pGraph->AddEdges(pScan, covariance); + if (m_pUseScanMatching->GetValue()) + { + // add to graph + m_pGraph->AddVertex(pScan); + m_pGraph->AddEdges(pScan, cov); - m_pMapperSensorManager->AddRunningScan(pScan); + m_pMapperSensorManager->AddRunningScan(pScan); - if (m_pDoLoopClosing->GetValue()) { - std::vector deviceNames = m_pMapperSensorManager->GetSensorNames(); - const_forEach(std::vector, &deviceNames) + if (m_pDoLoopClosing->GetValue()) { - m_pGraph->TryCloseLoop(pScan, *iter); + std::vector deviceNames = m_pMapperSensorManager->GetSensorNames(); + const_forEach(std::vector, &deviceNames) + { + m_pGraph->TryCloseLoop(pScan, *iter); + } } } - } - m_pMapperSensorManager->SetLastScan(pScan); + m_pMapperSensorManager->SetLastScan(pScan); + } return true; } @@ -2744,63 +2922,71 @@ kt_bool Mapper::Process(LocalizedRangeScan * pScan) return false; } -kt_bool Mapper::ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool addScanToLocalizationBuffer) +kt_bool Mapper::ProcessAgainstNodesNearBy(LocalizedRangeScan* pScan, + kt_bool addScanToLocalizationBuffer, Matrix3* covariance) { - if (pScan != NULL) { - karto::LaserRangeFinder * pLaserRangeFinder = pScan->GetLaserRangeFinder(); + if (pScan != NULL) + { + karto::LaserRangeFinder* pLaserRangeFinder = pScan->GetLaserRangeFinder(); // validate scan - if (pLaserRangeFinder == NULL || pScan == NULL || - pLaserRangeFinder->Validate(pScan) == false) + if (pLaserRangeFinder == NULL || pScan == NULL || pLaserRangeFinder->Validate(pScan) == false) { return false; } - if (m_Initialized == false) { + if (m_Initialized == false) + { // initialize mapper with range threshold from device Initialize(pLaserRangeFinder->GetRangeThreshold()); } - Vertex * closetVertex = m_pGraph->FindNearByScan( - pScan->GetSensorName(), pScan->GetOdometricPose()); - LocalizedRangeScan * pLastScan = NULL; - if (closetVertex) { + Vertex* closetVertex = + m_pGraph->FindNearByScan(pScan->GetSensorName(), pScan->GetOdometricPose()); + LocalizedRangeScan* pLastScan = NULL; + if (closetVertex) + { pLastScan = m_pMapperSensorManager->GetScan(pScan->GetSensorName(), - closetVertex->GetObject()->GetStateId()); + closetVertex->GetObject()->GetStateId()); m_pMapperSensorManager->ClearRunningScans(pScan->GetSensorName()); m_pMapperSensorManager->AddRunningScan(pLastScan); m_pMapperSensorManager->SetLastScan(pLastScan); } - Matrix3 covariance; - covariance.SetToIdentity(); + Matrix3 cov; + cov.SetToIdentity(); // correct scan (if not first scan) - if (m_pUseScanMatching->GetValue() && pLastScan != NULL) { + if (m_pUseScanMatching->GetValue() && pLastScan != NULL) + { Pose2 bestPose; - m_pSequentialScanMatcher->MatchScan(pScan, - m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()), - bestPose, - covariance); + m_pSequentialScanMatcher->MatchScan( + pScan, m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()), bestPose, cov); pScan->SetSensorPose(bestPose); } pScan->SetOdometricPose(pScan->GetCorrectedPose()); + if (covariance) + { + *covariance = cov; + } + // add scan to buffer and assign id m_pMapperSensorManager->AddScan(pScan); - Vertex * scan_vertex = NULL; - if (m_pUseScanMatching->GetValue()) { + Vertex* scan_vertex = NULL; + if (m_pUseScanMatching->GetValue()) + { // add to graph scan_vertex = m_pGraph->AddVertex(pScan); - m_pGraph->AddEdges(pScan, covariance); + m_pGraph->AddEdges(pScan, cov); m_pMapperSensorManager->AddRunningScan(pScan); - if (m_pDoLoopClosing->GetValue()) { - std::vector deviceNames = - m_pMapperSensorManager->GetSensorNames(); + if (m_pDoLoopClosing->GetValue()) + { + std::vector deviceNames = m_pMapperSensorManager->GetSensorNames(); const_forEach(std::vector, &deviceNames) { m_pGraph->TryCloseLoop(pScan, *iter); @@ -2810,7 +2996,8 @@ kt_bool Mapper::ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool ad m_pMapperSensorManager->SetLastScan(pScan); - if (addScanToLocalizationBuffer) { + if (addScanToLocalizationBuffer) + { AddScanToLocalizationBuffer(pScan, scan_vertex); } @@ -2820,93 +3007,114 @@ kt_bool Mapper::ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool ad return false; } -kt_bool Mapper::ProcessLocalization(LocalizedRangeScan * pScan) +kt_bool Mapper::ProcessLocalization(LocalizedRangeScan* pScan, Matrix3* covariance, + bool force_match_only) { - if (pScan == NULL) { + if (pScan == NULL) + { return false; } - karto::LaserRangeFinder * pLaserRangeFinder = pScan->GetLaserRangeFinder(); + karto::LaserRangeFinder* pLaserRangeFinder = pScan->GetLaserRangeFinder(); // validate scan - if (pLaserRangeFinder == NULL || pScan == NULL || - pLaserRangeFinder->Validate(pScan) == false) + if (pLaserRangeFinder == NULL || pScan == NULL || pLaserRangeFinder->Validate(pScan) == false) { return false; } - if (m_Initialized == false) { + if (m_Initialized == false) + { // initialize mapper with range threshold from device Initialize(pLaserRangeFinder->GetRangeThreshold()); } // get last scan - LocalizedRangeScan * pLastScan = m_pMapperSensorManager->GetLastScan( - pScan->GetSensorName()); + LocalizedRangeScan* pLastScan = m_pMapperSensorManager->GetLastScan(pScan->GetSensorName()); // update scans corrected pose based on last correction - if (pLastScan != NULL) { - Transform lastTransform(pLastScan->GetOdometricPose(), - pLastScan->GetCorrectedPose()); - pScan->SetCorrectedPose(lastTransform.TransformPose( - pScan->GetOdometricPose())); + if (pLastScan != NULL) + { + Transform lastTransform(pLastScan->GetOdometricPose(), pLastScan->GetCorrectedPose()); + pScan->SetCorrectedPose(lastTransform.TransformPose(pScan->GetOdometricPose())); } // test if scan is outside minimum boundary // or if heading is larger then minimum heading - if (!HasMovedEnough(pScan, pLastScan)) { + if (!force_match_only && !HasMovedEnough(pScan, pLastScan)) + { return false; } - Matrix3 covariance; - covariance.SetToIdentity(); + Matrix3 cov; + cov.SetToIdentity(); // correct scan (if not first scan) - if (m_pUseScanMatching->GetValue() && pLastScan != NULL) { + if (m_pUseScanMatching->GetValue() && pLastScan != NULL) + { Pose2 bestPose; - m_pSequentialScanMatcher->MatchScan(pScan, - m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()), - bestPose, - covariance); + m_pSequentialScanMatcher->MatchScan( + pScan, m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()), bestPose, cov); pScan->SetSensorPose(bestPose); + if (covariance) + { + *covariance = cov; + } } - // add scan to buffer and assign id - m_pMapperSensorManager->AddScan(pScan); + if (!force_match_only) + { + // add scan to buffer and assign id + m_pMapperSensorManager->AddScan(pScan); - Vertex * scan_vertex = NULL; - if (m_pUseScanMatching->GetValue()) { - // add to graph - scan_vertex = m_pGraph->AddVertex(pScan); - m_pGraph->AddEdges(pScan, covariance); + Vertex* scan_vertex = NULL; + if (m_pUseScanMatching->GetValue()) + { + // add to graph + scan_vertex = m_pGraph->AddVertex(pScan); + m_pGraph->AddEdges(pScan, cov); - m_pMapperSensorManager->AddRunningScan(pScan); + m_pMapperSensorManager->AddRunningScan(pScan); - if (m_pDoLoopClosing->GetValue()) { - std::vector deviceNames = m_pMapperSensorManager->GetSensorNames(); - const_forEach(std::vector, &deviceNames) + if (m_pDoLoopClosing->GetValue()) { - m_pGraph->TryCloseLoop(pScan, *iter); + std::vector deviceNames = m_pMapperSensorManager->GetSensorNames(); + const_forEach(std::vector, &deviceNames) + { + m_pGraph->TryCloseLoop(pScan, *iter); + } } } - } - m_pMapperSensorManager->SetLastScan(pScan); - AddScanToLocalizationBuffer(pScan, scan_vertex); + m_pMapperSensorManager->SetLastScan(pScan); + AddScanToLocalizationBuffer(pScan, scan_vertex); + // if a localization vertex is a bridge between the mapping and localization graphs, convert it to a mapping + // vertex + if(m_LocalizationScanVertices.size() > 3 && cutsLocalizationAndMappingGraphs(m_LocalizationScanVertices.front())){ + karto::LocalizationScanVertex& cut_vertex = m_LocalizationScanVertices.front(); + // remove from localization buffer + m_LocalizationScanVertices.pop_front(); + // TODO (john.dangelo@tailos.com): Is it really that easy to convert a node from a localizatoin to a mapping node? + // should we also freeze it in the scan solver? + } + } return true; } -void Mapper::AddScanToLocalizationBuffer(LocalizedRangeScan * pScan, Vertex * scan_vertex) +void Mapper::AddScanToLocalizationBuffer(LocalizedRangeScan* pScan, + Vertex* scan_vertex) { // generate the info to store and later decay, outside of dataset LocalizationScanVertex lsv; - lsv.scan = pScan; + lsv.scan = pScan; lsv.vertex = scan_vertex; - m_LocalizationScanVertices.push(lsv); + m_LocalizationScanVertices.push_back(lsv); - if (m_LocalizationScanVertices.size() > getParamScanBufferSize()) { - LocalizationScanVertex & oldLSV = m_LocalizationScanVertices.front(); + while (m_LocalizationScanVertices.size() > getParamScanBufferSize() && + !cutsLocalizationAndMappingGraphs(m_LocalizationScanVertices.front())) + { + LocalizationScanVertex& oldLSV = m_LocalizationScanVertices.front(); RemoveNodeFromGraph(oldLSV.vertex); // delete node and scans @@ -2914,15 +3122,74 @@ void Mapper::AddScanToLocalizationBuffer(LocalizedRangeScan * pScan, Vertex RemoveObject(); m_pMapperSensorManager->RemoveScan(oldLSV.scan); - if (oldLSV.scan) { + if (oldLSV.scan) + { delete oldLSV.scan; oldLSV.scan = NULL; } - m_LocalizationScanVertices.pop(); + m_LocalizationScanVertices.pop_front(); } } +bool Mapper::cutsLocalizationAndMappingGraphs(const LocalizationScanVertex& localization_vertex) +{ + // Tarjan’s algorithm would likely perform better if we expect the localization graph to get, + // really big. but for now I use a simpler, but naive, approach where I check the localization + // vertices to see if both of these conditions are true: + // 1. the given localization vertex connects to the mapping graph + // 2. no other localization vertex connects to the mapping graph + const int query_id = localization_vertex.vertex->GetObject()->GetUniqueId(); + + // figure out where the mapping vertices stop and localization vertices start + int first_localization_id = std::numeric_limits::max(); + if (!m_LocalizationScanVertices.empty()) + { + first_localization_id = m_LocalizationScanVertices.front().vertex->GetObject()->GetUniqueId(); + } + + // Make sure we were not given a mapping vertex... + if (query_id < first_localization_id) + { + std::cout << "isMapArticulationPoint: WARNING - user provided mapping vertex!" << std::endl; + return false; + } + // Check for Condition 1: the given localization vertex connects to the mapping graph + bool query_connects_to_map_graph = false; + for (const auto& adj_vertex : localization_vertex.vertex->GetAdjacentVertices()) + { + // is adj_vertex a mapping vertex? + if (adj_vertex != nullptr && adj_vertex->GetObject()->GetUniqueId() < first_localization_id) + { + query_connects_to_map_graph = true; + break; + } + } + if (!query_connects_to_map_graph) + { + return false; + } + + // Check for Condition 2: no other localization vertex connects to the mapping graph + for (const auto& vertex : m_LocalizationScanVertices) + { + const int id = vertex.vertex->GetObject()->GetUniqueId(); + if (id == query_id) + { + continue; + } + for (const auto& adj_vertex : vertex.vertex->GetAdjacentVertices()) + { + // is adj_vertex a mapping vertex? + if (adj_vertex != nullptr && adj_vertex->GetObject()->GetUniqueId() < first_localization_id) + { + return false; + } + } + } + return true; +} + void Mapper::ClearLocalizationBuffer() { while (!m_LocalizationScanVertices.empty()) @@ -2937,7 +3204,7 @@ void Mapper::ClearLocalizationBuffer() oldLSV.scan = NULL; } - m_LocalizationScanVertices.pop(); + m_LocalizationScanVertices.pop_front(); } std::vector names = m_pMapperSensorManager->GetSensorNames(); @@ -2950,41 +3217,43 @@ void Mapper::ClearLocalizationBuffer() return; } -kt_bool Mapper::RemoveNodeFromGraph(Vertex * vertex_to_remove) +kt_bool Mapper::RemoveNodeFromGraph(Vertex* vertex_to_remove) { // 1) delete edges in adjacent vertices, graph, and optimizer - std::vector *> adjVerts = - vertex_to_remove->GetAdjacentVertices(); - for (int i = 0; i != adjVerts.size(); i++) { - std::vector *> adjEdges = adjVerts[i]->GetEdges(); - bool found = false; - for (int j = 0; j != adjEdges.size(); j++) { + std::vector*> adjVerts = vertex_to_remove->GetAdjacentVertices(); + for (int i = 0; i != adjVerts.size(); i++) + { + std::vector*> adjEdges = adjVerts[i]->GetEdges(); + bool found = false; + for (int j = 0; j != adjEdges.size(); j++) + { if (adjEdges[j]->GetTarget() == vertex_to_remove || - adjEdges[j]->GetSource() == vertex_to_remove) + adjEdges[j]->GetSource() == vertex_to_remove) { adjVerts[i]->RemoveEdge(j); - m_pScanOptimizer->RemoveConstraint( - adjEdges[j]->GetSource()->GetObject()->GetUniqueId(), - adjEdges[j]->GetTarget()->GetObject()->GetUniqueId()); - std::vector *> edges = m_pGraph->GetEdges(); - std::vector *>::iterator edgeGraphIt = + m_pScanOptimizer->RemoveConstraint(adjEdges[j]->GetSource()->GetObject()->GetUniqueId(), + adjEdges[j]->GetTarget()->GetObject()->GetUniqueId()); + std::vector*> edges = m_pGraph->GetEdges(); + std::vector*>::iterator edgeGraphIt = std::find(edges.begin(), edges.end(), adjEdges[j]); - if (edgeGraphIt == edges.end()) { + if (edgeGraphIt == edges.end()) + { std::cout << "Edge not found in graph to remove!" << std::endl; continue; } int posEdge = edgeGraphIt - edges.begin(); - m_pGraph->RemoveEdge(posEdge); // remove from graph - delete *edgeGraphIt; // free hat! + m_pGraph->RemoveEdge(posEdge); // remove from graph + delete *edgeGraphIt; // free hat! *edgeGraphIt = NULL; - found = true; + found = true; } } - if (!found) { - std::cout << "Failed to find any edge in adj. vertex" << - " with a matching vertex to current!" << std::endl; + if (!found) + { + std::cout << "Failed to find any edge in adj. vertex" + << " with a matching vertex to current!" << std::endl; } } @@ -2992,16 +3261,18 @@ kt_bool Mapper::RemoveNodeFromGraph(Vertex * vertex_to_remov m_pScanOptimizer->RemoveNode(vertex_to_remove->GetObject()->GetUniqueId()); // 3) delete from vertex map - std::map *>> - vertexMap = m_pGraph->GetVertices(); - std::map *> graphVertices = + std::map*>> vertexMap = m_pGraph->GetVertices(); + std::map*> graphVertices = vertexMap[vertex_to_remove->GetObject()->GetSensorName()]; - std::map *>::iterator - vertexGraphIt = graphVertices.find(vertex_to_remove->GetObject()->GetStateId()); - if (vertexGraphIt != graphVertices.end()) { + std::map*>::iterator vertexGraphIt = + graphVertices.find(vertex_to_remove->GetObject()->GetStateId()); + if (vertexGraphIt != graphVertices.end()) + { m_pGraph->RemoveVertex(vertex_to_remove->GetObject()->GetSensorName(), - vertexGraphIt->second->GetObject()->GetStateId()); - } else { + vertexGraphIt->second->GetObject()->GetStateId()); + } + else + { std::cout << "Vertex not found in graph to remove!" << std::endl; return false; } @@ -3009,21 +3280,21 @@ kt_bool Mapper::RemoveNodeFromGraph(Vertex * vertex_to_remov return true; } -kt_bool Mapper::ProcessAgainstNode( - LocalizedRangeScan * pScan, - const int & nodeId) +kt_bool Mapper::ProcessAgainstNode(LocalizedRangeScan* pScan, const int& nodeId, + Matrix3* covariance) { - if (pScan != NULL) { - karto::LaserRangeFinder * pLaserRangeFinder = pScan->GetLaserRangeFinder(); + if (pScan != NULL) + { + karto::LaserRangeFinder* pLaserRangeFinder = pScan->GetLaserRangeFinder(); // validate scan - if (pLaserRangeFinder == NULL || pScan == NULL || - pLaserRangeFinder->Validate(pScan) == false) + if (pLaserRangeFinder == NULL || pScan == NULL || pLaserRangeFinder->Validate(pScan) == false) { return false; } - if (m_Initialized == false) { + if (m_Initialized == false) + { // initialize mapper with range threshold from device Initialize(pLaserRangeFinder->GetRangeThreshold()); } @@ -3031,40 +3302,43 @@ kt_bool Mapper::ProcessAgainstNode( // If we're matching against a node from an older mapping session // lets get the first scan as the last scan and populate running scans // with the first few from that run as well. - LocalizedRangeScan * pLastScan = - m_pMapperSensorManager->GetScan(pScan->GetSensorName(), nodeId); + LocalizedRangeScan* pLastScan = m_pMapperSensorManager->GetScan(pScan->GetSensorName(), nodeId); m_pMapperSensorManager->ClearRunningScans(pScan->GetSensorName()); m_pMapperSensorManager->AddRunningScan(pLastScan); m_pMapperSensorManager->SetLastScan(pLastScan); - Matrix3 covariance; - covariance.SetToIdentity(); + Matrix3 cov; + cov.SetToIdentity(); // correct scan (if not first scan) - if (m_pUseScanMatching->GetValue() && pLastScan != NULL) { + if (m_pUseScanMatching->GetValue() && pLastScan != NULL) + { Pose2 bestPose; - m_pSequentialScanMatcher->MatchScan(pScan, - m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()), - bestPose, - covariance); + m_pSequentialScanMatcher->MatchScan( + pScan, m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()), bestPose, cov); pScan->SetSensorPose(bestPose); } pScan->SetOdometricPose(pScan->GetCorrectedPose()); + if (covariance) + { + *covariance = cov; + } // add scan to buffer and assign id m_pMapperSensorManager->AddScan(pScan); - if (m_pUseScanMatching->GetValue()) { + if (m_pUseScanMatching->GetValue()) + { // add to graph m_pGraph->AddVertex(pScan); - m_pGraph->AddEdges(pScan, covariance); + m_pGraph->AddEdges(pScan, cov); m_pMapperSensorManager->AddRunningScan(pScan); - if (m_pDoLoopClosing->GetValue()) { - std::vector deviceNames = - m_pMapperSensorManager->GetSensorNames(); + if (m_pDoLoopClosing->GetValue()) + { + std::vector deviceNames = m_pMapperSensorManager->GetSensorNames(); const_forEach(std::vector, &deviceNames) { m_pGraph->TryCloseLoop(pScan, *iter); @@ -3080,10 +3354,10 @@ kt_bool Mapper::ProcessAgainstNode( return false; } -kt_bool Mapper::ProcessAtDock(LocalizedRangeScan * pScan) +kt_bool Mapper::ProcessAtDock(LocalizedRangeScan* pScan, Matrix3* covariance) { // Special case of processing against node where node is the starting point - return ProcessAgainstNode(pScan, 0); + return ProcessAgainstNode(pScan, 0, covariance); } /** @@ -3092,33 +3366,37 @@ kt_bool Mapper::ProcessAtDock(LocalizedRangeScan * pScan) * @param pLastScan * @return true if the scans are sufficiently far */ -kt_bool Mapper::HasMovedEnough(LocalizedRangeScan * pScan, LocalizedRangeScan * pLastScan) const +kt_bool Mapper::HasMovedEnough(LocalizedRangeScan* pScan, LocalizedRangeScan* pLastScan) const { // test if first scan - if (pLastScan == NULL) { + if (pLastScan == NULL) + { return true; } // test if enough time has passed kt_double timeInterval = pScan->GetTime() - pLastScan->GetTime(); - if (timeInterval >= m_pMinimumTimeInterval->GetValue()) { + if (timeInterval >= m_pMinimumTimeInterval->GetValue()) + { return true; } Pose2 lastScannerPose = pLastScan->GetSensorAt(pLastScan->GetOdometricPose()); - Pose2 scannerPose = pScan->GetSensorAt(pScan->GetOdometricPose()); + Pose2 scannerPose = pScan->GetSensorAt(pScan->GetOdometricPose()); // test if we have turned enough - kt_double deltaHeading = math::NormalizeAngle( - scannerPose.GetHeading() - lastScannerPose.GetHeading()); - if (fabs(deltaHeading) >= m_pMinimumTravelHeading->GetValue()) { + kt_double deltaHeading = + math::NormalizeAngle(scannerPose.GetHeading() - lastScannerPose.GetHeading()); + if (fabs(deltaHeading) >= m_pMinimumTravelHeading->GetValue()) + { return true; } // test if we have moved enough - kt_double squaredTravelDistance = lastScannerPose.GetPosition().SquaredDistance( - scannerPose.GetPosition()); - if (squaredTravelDistance >= math::Square(m_pMinimumTravelDistance->GetValue()) - KT_TOLERANCE) { + kt_double squaredTravelDistance = + lastScannerPose.GetPosition().SquaredDistance(scannerPose.GetPosition()); + if (squaredTravelDistance >= math::Square(m_pMinimumTravelDistance->GetValue()) - KT_TOLERANCE) + { return true; } @@ -3133,7 +3411,8 @@ const LocalizedRangeScanVector Mapper::GetAllProcessedScans() const { LocalizedRangeScanVector allScans; - if (m_pMapperSensorManager != NULL) { + if (m_pMapperSensorManager != NULL) + { allScans = m_pMapperSensorManager->GetAllScans(); } @@ -3144,7 +3423,7 @@ const LocalizedRangeScanVector Mapper::GetAllProcessedScans() const * Adds a listener * @param pListener */ -void Mapper::AddListener(MapperListener * pListener) +void Mapper::AddListener(MapperListener* pListener) { m_Listeners.push_back(pListener); } @@ -3153,94 +3432,99 @@ void Mapper::AddListener(MapperListener * pListener) * Removes a listener * @param pListener */ -void Mapper::RemoveListener(MapperListener * pListener) +void Mapper::RemoveListener(MapperListener* pListener) { - std::vector::iterator iter = std::find(m_Listeners.begin(), - m_Listeners.end(), pListener); - if (iter != m_Listeners.end()) { + std::vector::iterator iter = + std::find(m_Listeners.begin(), m_Listeners.end(), pListener); + if (iter != m_Listeners.end()) + { m_Listeners.erase(iter); } } -void Mapper::FireInfo(const std::string & rInfo) const +void Mapper::FireInfo(const std::string& rInfo) const { - const_forEach(std::vector, &m_Listeners) + const_forEach(std::vector, &m_Listeners) { (*iter)->Info(rInfo); } } -void Mapper::FireDebug(const std::string & rInfo) const +void Mapper::FireDebug(const std::string& rInfo) const { - const_forEach(std::vector, &m_Listeners) + const_forEach(std::vector, &m_Listeners) { - MapperDebugListener * pListener = dynamic_cast(*iter); + MapperDebugListener* pListener = dynamic_cast(*iter); - if (pListener != NULL) { + if (pListener != NULL) + { pListener->Debug(rInfo); } } } -void Mapper::FireLoopClosureCheck(const std::string & rInfo) const +void Mapper::FireLoopClosureCheck(const std::string& rInfo) const { - const_forEach(std::vector, &m_Listeners) + const_forEach(std::vector, &m_Listeners) { - MapperLoopClosureListener * pListener = dynamic_cast(*iter); + MapperLoopClosureListener* pListener = dynamic_cast(*iter); - if (pListener != NULL) { + if (pListener != NULL) + { pListener->LoopClosureCheck(rInfo); } } } -void Mapper::FireBeginLoopClosure(const std::string & rInfo) const +void Mapper::FireBeginLoopClosure(const std::string& rInfo) const { - const_forEach(std::vector, &m_Listeners) + const_forEach(std::vector, &m_Listeners) { - MapperLoopClosureListener * pListener = dynamic_cast(*iter); + MapperLoopClosureListener* pListener = dynamic_cast(*iter); - if (pListener != NULL) { + if (pListener != NULL) + { pListener->BeginLoopClosure(rInfo); } } } -void Mapper::FireEndLoopClosure(const std::string & rInfo) const +void Mapper::FireEndLoopClosure(const std::string& rInfo) const { - const_forEach(std::vector, &m_Listeners) + const_forEach(std::vector, &m_Listeners) { - MapperLoopClosureListener * pListener = dynamic_cast(*iter); + MapperLoopClosureListener* pListener = dynamic_cast(*iter); - if (pListener != NULL) { + if (pListener != NULL) + { pListener->EndLoopClosure(rInfo); } } } -void Mapper::SetScanSolver(ScanSolver * pScanOptimizer) +void Mapper::SetScanSolver(ScanSolver* pScanOptimizer) { m_pScanOptimizer = pScanOptimizer; } -ScanSolver * Mapper::getScanSolver() +ScanSolver* Mapper::getScanSolver() { return m_pScanOptimizer; } -MapperGraph * Mapper::GetGraph() const +MapperGraph* Mapper::GetGraph() const { return m_pGraph; } -ScanMatcher * Mapper::GetSequentialScanMatcher() const +ScanMatcher* Mapper::GetSequentialScanMatcher() const { return m_pSequentialScanMatcher; } -ScanMatcher * Mapper::GetLoopScanMatcher() const +ScanMatcher* Mapper::GetLoopScanMatcher() const { return m_pGraph->GetLoopScanMatcher(); } -} // namespace karto +} // namespace karto BOOST_CLASS_EXPORT(karto::BreadthFirstTraversal) diff --git a/package.xml b/package.xml index 155c9b6ac..d474056c2 100644 --- a/package.xml +++ b/package.xml @@ -2,11 +2,12 @@ slam_toolbox - 2.4.0 + 2.6.0 This package provides a sped up improved slam karto with updated SDK and visualization and modification toolsets Steve Macenski + Michel Hidalgo LGPL Steve Macenski @@ -14,33 +15,26 @@ pluginlib eigen + maidbot_msg_utils + maidbot_std_srvs message_filters nav_msgs rclcpp sensor_msgs tf2_ros tf2 - tf2_sensor_msgs visualization_msgs std_srvs boost - interactive_markers std_msgs suitesparse liblapack-dev libceres-dev tf2_geometry_msgs tbb - libqt5-core - libqt5-widgets - qtbase5-dev - nav2_map_server + builtin_interfaces rosidl_default_generators - rviz_common - rviz_default_plugins - rviz_ogre_vendor - rviz_rendering eigen pluginlib message_filters @@ -49,28 +43,18 @@ sensor_msgs tf2 tf2_ros - tf2_sensor_msgs visualization_msgs std_srvs boost - interactive_markers std_msgs suitesparse liblapack-dev libceres-dev tf2_geometry_msgs tbb - libqt5-core - libqt5-widgets - nav2_map_server builtin_interfaces rosidl_default_generators - libqt5-core - libqt5-gui - libqt5-opengl - libqt5-widgets - ament_cmake_gtest launch launch_testing @@ -84,7 +68,6 @@ ament_cmake - diff --git a/rviz_plugin/slam_toolbox_rviz_plugin.cpp b/rviz_plugin/slam_toolbox_rviz_plugin.cpp deleted file mode 100644 index 1575c53c8..000000000 --- a/rviz_plugin/slam_toolbox_rviz_plugin.cpp +++ /dev/null @@ -1,578 +0,0 @@ -/* - * slam_toolbox - * Copyright (c) 2018, Simbe Robotics, Inc. - * - * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE - * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY - * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS - * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. - * - * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO - * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS - * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND - * CONDITIONS. - * - */ - -/* Author: Steven Macenski */ - -// Header -#include "rviz_plugin/slam_toolbox_rviz_plugin.hpp" -// ROS -#include -#include -// QT -#include -#include -#include -#include -#include -#include -#include -#include -#include -// STL -#include - - -namespace slam_toolbox -{ - -/*****************************************************************************/ -SlamToolboxPlugin::SlamToolboxPlugin(QWidget * parent) -: Panel(parent), - _match_type(PROCESS_FIRST_NODE_CMT) -/*****************************************************************************/ -{ - ros_node_ = std::make_shared("SlamToolboxPlugin"); - - bool paused_measure = false, interactive = false; - paused_measure = ros_node_->declare_parameter( - "/slam_toolbox/paused_new_measurements", paused_measure); - interactive = ros_node_->declare_parameter( - "/slam_toolbox/interactive_mode", interactive); - - _initialposeSub = - ros_node_->create_subscription( - "/initialpose", 10, - std::bind(&SlamToolboxPlugin::InitialPoseCallback, this, std::placeholders::_1)); - - _serialize = - ros_node_->create_client( - "/slam_toolbox/serialize_map"); - _load_map = - ros_node_->create_client( - "/slam_toolbox/deserialize_map"); - _clearChanges = ros_node_->create_client( - "/slam_toolbox/clear_changes"); - _saveChanges = ros_node_->create_client( - "/slam_toolbox/manual_loop_closure"); - _saveMap = ros_node_->create_client( - "/slam_toolbox/save_map"); - _clearQueue = ros_node_->create_client( - "/slam_toolbox/clear_queue"); - _interactive = - ros_node_->create_client( - "/slam_toolbox/toggle_interactive_mode"); - _pause_measurements = ros_node_->create_client( - "/slam_toolbox/pause_new_measurements"); - _load_submap_for_merging = - ros_node_->create_client( - "/map_merging/add_submap"); - _merge = ros_node_->create_client( - "/map_merging/merge_submaps"); - - _vbox = new QVBoxLayout(); - _hbox1 = new QHBoxLayout(); - _hbox2 = new QHBoxLayout(); - _hbox3 = new QHBoxLayout(); - _hbox4 = new QHBoxLayout(); - _hbox5 = new QHBoxLayout(); - _hbox6 = new QHBoxLayout(); - _hbox7 = new QHBoxLayout(); - _hbox8 = new QHBoxLayout(); - _hbox9 = new QHBoxLayout(); - _hbox10 = new QHBoxLayout(); - - QFrame * _line = new QFrame(); - _line->setFrameShape(QFrame::HLine); - _line->setFrameShadow(QFrame::Sunken); - - _button1 = new QPushButton(this); - _button1->setText("Clear Changes"); - connect(_button1, SIGNAL(clicked()), this, SLOT(ClearChanges())); - _button2 = new QPushButton(this); - _button2->setText("Save Changes"); - connect(_button2, SIGNAL(clicked()), this, SLOT(SaveChanges())); - _button3 = new QPushButton(this); - _button3->setText("Save Map"); - connect(_button3, SIGNAL(clicked()), this, SLOT(SaveMap())); - _button4 = new QPushButton(this); - _button4->setText("Clear Measurement Queue"); - connect(_button4, SIGNAL(clicked()), this, SLOT(ClearQueue())); - _button5 = new QPushButton(this); - _button5->setText("Add Submap"); - connect(_button5, SIGNAL(clicked()), this, SLOT(LoadSubmap())); - _button6 = new QPushButton(this); - _button6->setText("Generate Map"); - connect(_button6, SIGNAL(clicked()), this, SLOT(GenerateMap())); - _button7 = new QPushButton(this); - _button7->setText("Serialize Map"); - connect(_button7, SIGNAL(clicked()), this, SLOT(SerializeMap())); - _button8 = new QPushButton(this); - _button8->setText("Deserialize Map"); - connect(_button8, SIGNAL(clicked()), this, SLOT(DeserializeMap())); - - _label1 = new QLabel(this); - _label1->setText("Interactive Mode"); - _label2 = new QLabel(this); - _label2->setText("Accept New Scans"); - _label4 = new QLabel(this); - _label4->setText("Merge Map Tool"); - _label4->setAlignment(Qt::AlignCenter); - _label5 = new QLabel(this); - _label5->setText("Create Map Tool"); - _label5->setAlignment(Qt::AlignCenter); - _label6 = new QLabel(this); - _label6->setText("X"); - _label6->setAlignment(Qt::AlignCenter); - _label7 = new QLabel(this); - _label7->setText("Y"); - _label7->setAlignment(Qt::AlignCenter); - _label8 = new QLabel(this); - _label8->setText("θ"); - _label8->setAlignment(Qt::AlignCenter); - - _check1 = new QCheckBox(); - _check1->setChecked(interactive); - connect(_check1, SIGNAL(stateChanged(int)), this, SLOT(InteractiveCb(int))); - _check2 = new QCheckBox(); - _check2->setChecked(!paused_measure); - connect(_check2, SIGNAL(stateChanged(int)), this, - SLOT(PauseMeasurementsCb(int))); - _radio1 = new QRadioButton(tr("Start At Dock")); - _radio1->setChecked(true); - _radio2 = new QRadioButton(tr("Start At Pose Est.")); - _radio3 = new QRadioButton(tr("Start At Curr. Odom")); - _radio4 = new QRadioButton(tr("Localize")); - - connect(_radio1, SIGNAL(clicked()), this, SLOT(FirstNodeMatchCb())); - connect(_radio2, SIGNAL(clicked()), this, SLOT(PoseEstMatchCb())); - connect(_radio3, SIGNAL(clicked()), this, SLOT(CurEstMatchCb())); - connect(_radio4, SIGNAL(clicked()), this, SLOT(LocalizeCb())); - - _line1 = new QLineEdit(); - _line2 = new QLineEdit(); - _line3 = new QLineEdit(); - _line4 = new QLineEdit(); - _line5 = new QLineEdit(); - _line6 = new QLineEdit(); - _line7 = new QLineEdit(); - - _button1->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - _button2->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - _button3->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - _button4->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - _button5->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - _button6->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - _button7->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - _button8->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - _check1->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - _check2->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - _line1->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - _line2->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - _line3->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - _line4->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - _line5->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - _line6->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - _line7->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - - _hbox1->addWidget(_check1); - _hbox1->addWidget(_label1); - _hbox1->addWidget(_check2); - _hbox1->addWidget(_label2); - - _hbox2->addWidget(_button1); - _hbox2->addWidget(_button2); - - _hbox3->addWidget(_button3); - _hbox3->addWidget(_line1); - - _hbox4->addWidget(_button4); - - _hbox5->addWidget(_button5); - _hbox5->addWidget(_line2); - - _hbox6->addWidget(_button6); - - _hbox7->addWidget(_button7); - _hbox7->addWidget(_line3); - - _hbox8->addWidget(_button8); - _hbox8->addWidget(_line4); - - _hbox9->addWidget(_radio1); - _hbox9->addWidget(_radio2); - _hbox9->addWidget(_radio3); - _hbox9->addWidget(_radio4); - _hbox9->addStretch(1); - - _hbox10->addWidget(_label6); - _hbox10->addWidget(_line5); - _hbox10->addWidget(_label7); - _hbox10->addWidget(_line6); - _hbox10->addWidget(_label8); - _hbox10->addWidget(_line7); - - _vbox->addWidget(_label5); - _vbox->addLayout(_hbox1); - _vbox->addLayout(_hbox2); - _vbox->addLayout(_hbox3); - _vbox->addLayout(_hbox7); - _vbox->addLayout(_hbox8); - _vbox->addLayout(_hbox9); - _vbox->addLayout(_hbox10); - _vbox->addLayout(_hbox4); - _vbox->addWidget(_line); - _vbox->addWidget(_label4); - _vbox->addLayout(_hbox5); - _vbox->addLayout(_hbox6); - - setLayout(_vbox); - - _thread = - std::make_unique( - &SlamToolboxPlugin::updateCheckStateIfExternalChange, this); -} - -/*****************************************************************************/ -SlamToolboxPlugin::~SlamToolboxPlugin() -/*****************************************************************************/ -{ - _thread->join(); - _thread.reset(); -} - -/*****************************************************************************/ -void SlamToolboxPlugin::InitialPoseCallback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) -/*****************************************************************************/ -{ - _match_type = PROCESS_NEAR_REGION_CMT; - RCLCPP_INFO( - ros_node_->get_logger(), - "Setting initial pose from rviz; you can now deserialize a map given that pose."); - _radio2->setChecked(true); - _line5->setText(QString::number(msg->pose.pose.position.x, 'f', 2)); - _line6->setText(QString::number(msg->pose.pose.position.y, 'f', 2)); - tf2::Quaternion quat_tf; - tf2::convert(msg->pose.pose.orientation , quat_tf); - tf2::Matrix3x3 m(quat_tf); - double roll, pitch, yaw; - m.getRPY(roll, pitch, yaw); - _line7->setText(QString::number(yaw, 'f', 2)); -} - -/*****************************************************************************/ -void SlamToolboxPlugin::SerializeMap() -/*****************************************************************************/ -{ - auto request = - std::make_shared(); - request->filename = _line3->text().toStdString(); - auto result_future = _serialize->async_send_request(request); - - if (rclcpp::spin_until_future_complete(ros_node_, result_future, - std::chrono::seconds(5)) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_WARN(ros_node_->get_logger(), - "SlamToolbox: Failed to serialize" - " pose graph to file, is service running?"); - } -} - -/*****************************************************************************/ -void SlamToolboxPlugin::DeserializeMap() -/*****************************************************************************/ -{ - typedef slam_toolbox::srv::DeserializePoseGraph::Request procType; - - auto request = - std::make_shared(); - request->filename = _line4->text().toStdString(); - if (_match_type == PROCESS_FIRST_NODE_CMT) { - request->match_type = procType::START_AT_FIRST_NODE; - } else if (_match_type == PROCESS_NEAR_REGION_CMT) { - try - { - request->match_type = procType::START_AT_GIVEN_POSE; - request->initial_pose.x = std::stod(_line5->text().toStdString()); - request->initial_pose.y = std::stod(_line6->text().toStdString()); - request->initial_pose.theta = std::stod(_line7->text().toStdString()); - } - catch (const std::invalid_argument& ia) - { - RCLCPP_WARN(ros_node_->get_logger(), "Initial pose invalid."); - return; - } - } else if (_match_type == LOCALIZE_CMT) { - try - { - request->match_type = procType::LOCALIZE_AT_POSE; - request->initial_pose.x = std::stod(_line5->text().toStdString()); - request->initial_pose.y = std::stod(_line6->text().toStdString()); - request->initial_pose.theta = std::stod(_line7->text().toStdString()); - } - catch (const std::invalid_argument& ia) - { - RCLCPP_WARN(ros_node_->get_logger(), "Initial pose invalid."); - return; - } - } else { - RCLCPP_WARN( - ros_node_->get_logger(), - "No match type selected, cannot send request."); - return; - } - - auto result_future = _load_map->async_send_request(request); - - if (rclcpp::spin_until_future_complete(ros_node_, result_future, - std::chrono::seconds(5)) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_WARN( - ros_node_->get_logger(), - "SlamToolbox: Failed to deserialize mapper object " - "from file, is service running?"); - } -} - -/*****************************************************************************/ -void SlamToolboxPlugin::LoadSubmap() -/*****************************************************************************/ -{ - auto request = std::make_shared(); - request->filename = _line2->text().toStdString(); - auto result_future = _load_submap_for_merging->async_send_request(request); - - if (rclcpp::spin_until_future_complete(ros_node_, result_future, - std::chrono::seconds(5)) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_WARN( - ros_node_->get_logger(), - "MergeMaps: Failed to load pose graph from file, is service running?"); - } -} -/*****************************************************************************/ -void SlamToolboxPlugin::GenerateMap() -/*****************************************************************************/ -{ - auto request = std::make_shared(); - auto result_future = _merge->async_send_request(request); - - if (rclcpp::spin_until_future_complete(ros_node_, result_future, - std::chrono::seconds(5)) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_WARN( - ros_node_->get_logger(), - "MergeMaps: Failed to merge maps, is service running?"); - } -} - -/*****************************************************************************/ -void SlamToolboxPlugin::ClearChanges() -/*****************************************************************************/ -{ - auto request = std::make_shared(); - auto result_future = _clearChanges->async_send_request(request); - - if (rclcpp::spin_until_future_complete(ros_node_, result_future, - std::chrono::seconds(5)) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_WARN( - ros_node_->get_logger(), - "SlamToolbox: Failed to clear changes, is service running?"); - } -} - -/*****************************************************************************/ -void SlamToolboxPlugin::SaveChanges() -/*****************************************************************************/ -{ - auto request = std::make_shared(); - auto result_future = _saveChanges->async_send_request(request); - - if (rclcpp::spin_until_future_complete(ros_node_, result_future, - std::chrono::seconds(5)) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_WARN( - ros_node_->get_logger(), - "SlamToolbox: Failed to save changes, is service running?"); - } -} - -/*****************************************************************************/ -void SlamToolboxPlugin::SaveMap() -/*****************************************************************************/ -{ - auto request = std::make_shared(); - request->name.data = _line1->text().toStdString(); - auto result_future = _saveMap->async_send_request(request); - - if (rclcpp::spin_until_future_complete(ros_node_, result_future, - std::chrono::seconds(5)) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_WARN( - ros_node_->get_logger(), - "SlamToolbox: Failed to save map as %s, is service running?", - request->name.data.c_str()); - } -} - -/*****************************************************************************/ -void SlamToolboxPlugin::ClearQueue() -/*****************************************************************************/ -{ - auto request = std::make_shared(); - auto result_future = _clearQueue->async_send_request(request); - - if (rclcpp::spin_until_future_complete(ros_node_, result_future, - std::chrono::seconds(5)) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_WARN( - ros_node_->get_logger(), - "Failed to clear queue, is service running?"); - } -} - -/*****************************************************************************/ -void SlamToolboxPlugin::InteractiveCb(int state) -/*****************************************************************************/ -{ - auto request = - std::make_shared(); - auto result_future = _interactive->async_send_request(request); - - if (rclcpp::spin_until_future_complete(ros_node_, result_future, - std::chrono::seconds(5)) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_WARN( - ros_node_->get_logger(), - "SlamToolbox: Failed to toggle interactive mode, is service running?"); - } -} - -/*****************************************************************************/ -void SlamToolboxPlugin::PauseMeasurementsCb(int state) -/*****************************************************************************/ -{ - auto request = std::make_shared(); - auto result_future = _pause_measurements->async_send_request(request); - - if (rclcpp::spin_until_future_complete(ros_node_, result_future, - std::chrono::seconds(5)) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_WARN( - ros_node_->get_logger(), - "SlamToolbox: Failed to toggle pause measurements, is service running?"); - } -} - -/*****************************************************************************/ -void SlamToolboxPlugin::FirstNodeMatchCb() -/*****************************************************************************/ -{ - if (_radio1->isChecked() == Qt::Unchecked) { - return; - } else { - _match_type = PROCESS_FIRST_NODE_CMT; - RCLCPP_INFO( - ros_node_->get_logger(), - "Processing at first node selected."); - } -} - -/*****************************************************************************/ -void SlamToolboxPlugin::PoseEstMatchCb() -/*****************************************************************************/ -{ - if (_radio2->isChecked() == Qt::Unchecked) { - return; - } else { - _match_type = PROCESS_NEAR_REGION_CMT; - RCLCPP_INFO( - ros_node_->get_logger(), - "Processing at current pose estimate selected."); - } -} - -/*****************************************************************************/ -void SlamToolboxPlugin::CurEstMatchCb() -/*****************************************************************************/ -{ - if (_radio3->isChecked() == Qt::Unchecked) { - return; - } else { - _match_type = PROCESS_CMT; - RCLCPP_INFO( - ros_node_->get_logger(), - "Processing at current odometry selected."); - } -} - -/*****************************************************************************/ -void SlamToolboxPlugin::LocalizeCb() -/*****************************************************************************/ -{ - if (_radio4->isChecked() == Qt::Unchecked) { - return; - } else { - _match_type = LOCALIZE_CMT; - RCLCPP_INFO( - ros_node_->get_logger(), - "Processing localization selected."); - } -} - -/*****************************************************************************/ -void SlamToolboxPlugin::updateCheckStateIfExternalChange() -/*****************************************************************************/ -{ - rclcpp::Rate r(1); - bool paused_measure = false, interactive = false; - auto node = std::make_shared("SlamToolboxStateUpdateNode"); - auto parameters_client = - std::make_shared(node, "slam_toolbox"); - - while (rclcpp::ok()) { - auto parameters = parameters_client->get_parameters( - {"paused_new_measurements", "interactive_mode"}); - paused_measure = parameters[0].as_bool(); - interactive = parameters[1].as_bool(); - - bool oldState = _check1->blockSignals(true); - _check1->setChecked(interactive); - _check1->blockSignals(oldState); - - oldState = _check2->blockSignals(true); - _check2->setChecked(!paused_measure); - _check2->blockSignals(oldState); - - r.sleep(); - } -} - -} // namespace slam_toolbox - -#include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(slam_toolbox::SlamToolboxPlugin, rviz_common::Panel) diff --git a/rviz_plugin/slam_toolbox_rviz_plugin.hpp b/rviz_plugin/slam_toolbox_rviz_plugin.hpp deleted file mode 100644 index 09c17d2fe..000000000 --- a/rviz_plugin/slam_toolbox_rviz_plugin.hpp +++ /dev/null @@ -1,164 +0,0 @@ -/* - * slam_toolbox - * Copyright (c) 2018, Simbe Robotics, Inc. - * - * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE - * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY - * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS - * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. - * - * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO - * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS - * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND - * CONDITIONS. - * - */ - -/* Author: Steven Macenski */ - -#ifndef RVIZ_PLUGIN__SLAM_TOOLBOX_RVIZ_PLUGIN_H_ -#define RVIZ_PLUGIN__SLAM_TOOLBOX_RVIZ_PLUGIN_H_ - -#include -#include -// QT -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -// STL -#include -#include -#include -// ROS -#include "rclcpp/rclcpp.hpp" -#include "rviz_common/panel.hpp" -#include "slam_toolbox/toolbox_msgs.hpp" -#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" - - -class QLineEdit; -class QSpinBox; -class QComboBox; - -namespace rviz_common -{ -class VisualizationManager; -} // namespace rviz_common - -namespace slam_toolbox -{ - -enum ContinueMappingType -{ - PROCESS_CMT = 0, - PROCESS_FIRST_NODE_CMT = 1, - PROCESS_NEAR_REGION_CMT = 2, - LOCALIZE_CMT = 3 -}; - -class SlamToolboxPlugin : public rviz_common::Panel -{ - Q_OBJECT - -public: - explicit SlamToolboxPlugin(QWidget * parent = 0); - - virtual ~SlamToolboxPlugin(); - -private Q_SLOTS: - void ClearChanges(); - void SaveChanges(); - void SaveMap(); - void ClearQueue(); - void InteractiveCb(int state); - void PauseMeasurementsCb(int state); - void FirstNodeMatchCb(); - void PoseEstMatchCb(); - void CurEstMatchCb(); - void LocalizeCb(); - void LoadSubmap(); - void GenerateMap(); - void SerializeMap(); - void DeserializeMap(); - - void updateCheckStateIfExternalChange(); - -protected: - QVBoxLayout * _vbox; - QHBoxLayout * _hbox1; - QHBoxLayout * _hbox2; - QHBoxLayout * _hbox3; - QHBoxLayout * _hbox4; - QHBoxLayout * _hbox5; - QHBoxLayout * _hbox6; - QHBoxLayout * _hbox7; - QHBoxLayout * _hbox8; - QHBoxLayout * _hbox9; - QHBoxLayout * _hbox10; - - QPushButton * _button1; - QPushButton * _button2; - QPushButton * _button3; - QPushButton * _button4; - QPushButton * _button5; - QPushButton * _button6; - QPushButton * _button7; - QPushButton * _button8; - - QLineEdit * _line1; - QLineEdit * _line2; - QLineEdit * _line3; - QLineEdit * _line4; - QLineEdit * _line5; - QLineEdit * _line6; - QLineEdit * _line7; - - QCheckBox * _check1; - QCheckBox * _check2; - - QRadioButton * _radio1; - QRadioButton * _radio2; - QRadioButton * _radio3; - QRadioButton * _radio4; - - QLabel * _label1; - QLabel * _label2; - QLabel * _label4; - QLabel * _label5; - QLabel * _label6; - QLabel * _label7; - QLabel * _label8; - - QFrame * _line; - - rclcpp::Node::SharedPtr ros_node_; - rclcpp::Client::SharedPtr _clearChanges; - rclcpp::Client::SharedPtr _saveChanges; - rclcpp::Client::SharedPtr _saveMap; - rclcpp::Client::SharedPtr _clearQueue; - rclcpp::Client::SharedPtr _interactive; - rclcpp::Client::SharedPtr _pause_measurements; - rclcpp::Client::SharedPtr _load_submap_for_merging; - rclcpp::Client::SharedPtr _merge; - rclcpp::Client::SharedPtr _serialize; - rclcpp::Client::SharedPtr _load_map; - - std::unique_ptr _thread; - - ContinueMappingType _match_type; - - rclcpp::Subscription::SharedPtr _initialposeSub; - - void InitialPoseCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr pose); -}; - -} // namespace slam_toolbox - -#endif // RVIZ_PLUGIN__SLAM_TOOLBOX_RVIZ_PLUGIN_H_ diff --git a/rviz_plugins.xml b/rviz_plugins.xml deleted file mode 100644 index b8866e8ff..000000000 --- a/rviz_plugins.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - Panel to assist in Mapping and SLAM with the slam_toolbox - - diff --git a/solvers/ceres_solver.cpp b/solvers/ceres_solver.cpp index 0e49352ee..9c2df749c 100644 --- a/solvers/ceres_solver.cpp +++ b/solvers/ceres_solver.cpp @@ -145,6 +145,10 @@ void CeresSolver::Configure(rclcpp::Node::SharedPtr node) options_problem_.enable_fast_removal = true; } + // we do not want the problem definition to own these objects, otherwise they get + // deleted along with the problem + options_problem_.loss_function_ownership = ceres::Ownership::DO_NOT_TAKE_OWNERSHIP; + problem_ = new ceres::Problem(options_problem_); } @@ -158,6 +162,9 @@ CeresSolver::~CeresSolver() if (nodes_ != NULL) { delete nodes_; } + if (blocks_ != NULL) { + delete blocks_; + } if (problem_ != NULL) { delete problem_; } @@ -177,7 +184,10 @@ void CeresSolver::Compute() } // populate contraint for static initial pose - if (!was_constant_set_ && first_node_ != nodes_->end()) { + if (!was_constant_set_ && first_node_ != nodes_->end() && + problem_->HasParameterBlock(&first_node_->second(0)) && + problem_->HasParameterBlock(&first_node_->second(1)) && + problem_->HasParameterBlock(&first_node_->second(2))) { RCLCPP_DEBUG(node_->get_logger(), "CeresSolver: Setting first node as a constant pose:" "%0.2f, %0.2f, %0.2f.", first_node_->second(0), @@ -239,6 +249,8 @@ void CeresSolver::Reset() was_constant_set_ = false; if (problem_) { + // Note that this also frees anything the problem owns (i.e. local parameterization, cost + // function) delete problem_; } @@ -276,10 +288,51 @@ void CeresSolver::AddNode(karto::Vertex * pVertex) nodes_->insert(std::pair(id, pose2d)); if (nodes_->size() == 1) { + std::cout << "Intializing solver at node: " << pose2d.transpose() << std::endl; first_node_ = nodes_->find(id); } } +/*****************************************************************************/ +void CeresSolver::SetNodeConstant(const int & unique_id) +/*****************************************************************************/ +{ + boost::mutex::scoped_lock lock(nodes_mutex_); + GraphIterator nodeit = nodes_->find(unique_id); + if (nodeit != nodes_->end()) { + if(!problem_->HasParameterBlock(&nodeit->second(0))){ + // node is not constrained yet + return; + } + problem_->SetParameterBlockConstant(&nodeit->second(0)); + problem_->SetParameterBlockConstant(&nodeit->second(1)); + problem_->SetParameterBlockConstant(&nodeit->second(2)); + } else { + RCLCPP_ERROR(node_->get_logger(), "SetNodeConstant: Failed to find node matching id %i", + unique_id); + } +} + +/*****************************************************************************/ +void CeresSolver::SetNodeVariable(const int & unique_id) +/*****************************************************************************/ +{ + boost::mutex::scoped_lock lock(nodes_mutex_); + GraphIterator nodeit = nodes_->find(unique_id); + if (nodeit != nodes_->end()) { + if(!problem_->HasParameterBlock(&nodeit->second(0))){ + // node is not constrained yet + return; + } + problem_->SetParameterBlockVariable(&nodeit->second(0)); + problem_->SetParameterBlockVariable(&nodeit->second(1)); + problem_->SetParameterBlockVariable(&nodeit->second(2)); + } else { + RCLCPP_ERROR(node_->get_logger(), "SetNodeVariable: Failed to find node matching id %i", + unique_id); + } +} + /*****************************************************************************/ void CeresSolver::AddConstraint(karto::Edge * pEdge) /*****************************************************************************/ @@ -310,13 +363,14 @@ void CeresSolver::AddConstraint(karto::Edge * pEdge) Eigen::Vector3d pose2d(diff.GetX(), diff.GetY(), diff.GetHeading()); karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance().Inverse(); - Eigen::Matrix3d sqrt_information; - sqrt_information(0, 0) = precisionMatrix(0, 0); - sqrt_information(0, 1) = sqrt_information(1, 0) = precisionMatrix(0, 1); - sqrt_information(0, 2) = sqrt_information(2, 0) = precisionMatrix(0, 2); - sqrt_information(1, 1) = precisionMatrix(1, 1); - sqrt_information(1, 2) = sqrt_information(2, 1) = precisionMatrix(1, 2); - sqrt_information(2, 2) = precisionMatrix(2, 2); + Eigen::Matrix3d information; + information(0, 0) = precisionMatrix(0, 0); + information(0, 1) = information(1, 0) = precisionMatrix(0, 1); + information(0, 2) = information(2, 0) = precisionMatrix(0, 2); + information(1, 1) = precisionMatrix(1, 1); + information(1, 2) = information(2, 1) = precisionMatrix(1, 2); + information(2, 2) = precisionMatrix(2, 2); + Eigen::Matrix3d sqrt_information = information.llt().matrixU(); // populate residual and parameterization for heading normalization ceres::CostFunction * cost_function = PoseGraph2dErrorTerm::Create(pose2d(0), diff --git a/solvers/ceres_solver.hpp b/solvers/ceres_solver.hpp index b2757414f..5bf2293e2 100644 --- a/solvers/ceres_solver.hpp +++ b/solvers/ceres_solver.hpp @@ -13,13 +13,12 @@ #include #include #include -// god... getting this to work in ROS2 was a real pain -#include "../lib/karto_sdk/include/karto_sdk/Mapper.h" +#include "karto_sdk/Mapper.h" #include "solvers/ceres_utils.h" #include "rclcpp/rclcpp.hpp" #include "std_srvs/srv/empty.hpp" -#include "../include/slam_toolbox/toolbox_types.hpp" +#include "slam_toolbox/toolbox_types.hpp" namespace solver_plugins { @@ -56,6 +55,10 @@ class CeresSolver : public karto::ScanSolver virtual void ModifyNode(const int & unique_id, Eigen::Vector3d pose); // get a node's current pose yaw virtual void GetNodeOrientation(const int & unique_id, double & pose); + // fix the value of a node + virtual void SetNodeConstant(const int & unique_id); + // make the value of a node variable + virtual void SetNodeVariable(const int & unique_id); private: // karto diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index a56dc59ca..be88688dd 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -74,8 +74,6 @@ LifelongSlamToolbox::LifelongSlamToolbox(rclcpp::NodeOptions options) "https://github.com/SteveMacenski/slam_toolbox/wiki/" "Experimental-Lifelong-Mapping-Node for more information."); - // in lifelong mode, we cannot have interactive mode enabled - enable_interactive_mode_ = false; } /*****************************************************************************/ @@ -83,8 +81,12 @@ void LifelongSlamToolbox::laserCallback( sensor_msgs::msg::LaserScan::ConstSharedPtr scan) /*****************************************************************************/ { - // store scan timestamped - scan_timestamped = scan->header.stamp; + if (!waitForTransform(scan->header.frame_id, scan->header.stamp)) { + return; + } + + // store scan header + scan_header = scan->header; // no odom info Pose2 pose; if (!pose_helper_->getOdomPose(pose, scan->header.stamp)) { diff --git a/src/experimental/slam_toolbox_lifelong_node.cpp b/src/experimental/slam_toolbox_lifelong_node.cpp index 6a8401b78..36f710a28 100644 --- a/src/experimental/slam_toolbox_lifelong_node.cpp +++ b/src/experimental/slam_toolbox_lifelong_node.cpp @@ -17,23 +17,25 @@ /* Author: Steven Macenski */ -#include #include "slam_toolbox/experimental/slam_toolbox_lifelong.hpp" +#include -int main(int argc, char ** argv) +int main(int argc, char** argv) { rclcpp::init(argc, argv); int stack_size = 40000000; { auto temp_node = std::make_shared("slam_toolbox"); - temp_node->declare_parameter("stack_size_to_use"); - if (temp_node->get_parameter("stack_size_to_use", stack_size)) { + temp_node->declare_parameter("stack_size_to_use", rclcpp::ParameterType::PARAMETER_INTEGER); + if (temp_node->get_parameter("stack_size_to_use", stack_size)) + { RCLCPP_INFO(temp_node->get_logger(), "Node using stack size %i", (int)stack_size); const rlim_t max_stack_size = stack_size; struct rlimit stack_limit; getrlimit(RLIMIT_STACK, &stack_limit); - if (stack_limit.rlim_cur < stack_size) { + if (stack_limit.rlim_cur < stack_size) + { stack_limit.rlim_cur = stack_size; } setrlimit(RLIMIT_STACK, &stack_limit); diff --git a/src/experimental/slam_toolbox_map_and_localization.cpp b/src/experimental/slam_toolbox_map_and_localization.cpp new file mode 100644 index 000000000..345f68b1e --- /dev/null +++ b/src/experimental/slam_toolbox_map_and_localization.cpp @@ -0,0 +1,261 @@ +/* + * slam_toolbox + * Copyright (c) 2022, Steve Macenski + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +#include +#include +#include "slam_toolbox/experimental/slam_toolbox_map_and_localization.hpp" + +namespace slam_toolbox +{ + +/*****************************************************************************/ +MapAndLocalizationSlamToolbox::MapAndLocalizationSlamToolbox(rclcpp::NodeOptions options) +: LocalizationSlamToolbox(options) +/*****************************************************************************/ +{ + + localization_pose_sub_.reset(); + map_and_localization_pose_sub_ = create_subscription( + "initialpose", 1, + std::bind(&MapAndLocalizationSlamToolbox::localizePoseCallback, this, std::placeholders::_1)); + + ssSetLocalizationMode_ = create_service( + "slam_toolbox/set_localization_mode", + std::bind(&MapAndLocalizationSlamToolbox::setLocalizationModeCallback, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); +} + +/*****************************************************************************/ +void MapAndLocalizationSlamToolbox::configure() +/*****************************************************************************/ +{ + SlamToolbox::configure(); + toggleMode(false); +} + +/*****************************************************************************/ +bool MapAndLocalizationSlamToolbox::setLocalizationModeCallback( + const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr resp) +/*****************************************************************************/ +{ + toggleMode(req->data); + + resp->success = true; + return true; +} + +void MapAndLocalizationSlamToolbox::toggleMode(bool enable_localization) +{ + bool in_localization_mode = processor_type_ == PROCESS_LOCALIZATION; + if (in_localization_mode == enable_localization) + { + return; + } + + boost::mutex::scoped_lock lock(smapper_mutex_); + if (enable_localization) + { + RCLCPP_INFO(get_logger(), "Enabling localization ..."); + processor_type_ = PROCESS_LOCALIZATION; + + clear_localization_ = create_service( + "slam_toolbox/clear_localization_buffer", + std::bind(&MapAndLocalizationSlamToolbox::clearLocalizationBuffer, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + // in localization mode, disable map saver + map_saver_.reset(); + + if(!smapper_ || !solver_){ + RCLCPP_INFO(get_logger(), "Mapper not configured yet. Will not modify graph."); + return; + } + + // freeze all the non-localization nodes + const auto& localization_vertices = smapper_->getMapper()->GetLocalizationVertices(); + int first_localization_id = std::numeric_limits::max(); + if (!localization_vertices.empty()) + { + first_localization_id = localization_vertices.front().vertex->GetObject()->GetUniqueId(); + } + const auto& vertices = smapper_->getMapper()->GetGraph()->GetVertices(); + for (const auto& sensor_name : vertices) + { + for (const auto& vertex : sensor_name.second) + { + if (vertex.first >= first_localization_id) + { + continue; + } + solver_->SetNodeConstant(vertex.first); + } + } + } + else + { + RCLCPP_INFO(get_logger(), "Enabling mapping ..."); + processor_type_ = PROCESS; + clear_localization_.reset(); + if(!map_saver_){ + map_saver_ = std::make_unique(shared_from_this(), map_name_); + } + + if(!smapper_ || !solver_){ + RCLCPP_INFO(get_logger(), "Mapper not configured yet. Will not modify graph."); + return; + } + + if (!smapper_->getMapper()->GetLocalizationVertices().empty()) + { + smapper_->clearLocalizationBuffer(); + } + // TODO: We should do this if we ever wanted to be able to resume mapping + // // unfreeze vertices (note that since we just cleared the localization buffer, + // // all the remaining nodes are mapping nodes) + // const auto& vertices = smapper_->getMapper()->GetGraph()->GetVertices(); + // for (const auto& sensor_name : vertices) + // { + // for (const auto& vertex : sensor_name.second) + // { + // solver_->SetNodeVariable(vertex.first); + // } + // } + } +} + +/*****************************************************************************/ +void MapAndLocalizationSlamToolbox::loadPoseGraphByParams() +/*****************************************************************************/ +{ + if (processor_type_ == PROCESS_LOCALIZATION) { + LocalizationSlamToolbox::loadPoseGraphByParams(); + } + else { + SlamToolbox::loadPoseGraphByParams(); + } +} + +/*****************************************************************************/ +bool MapAndLocalizationSlamToolbox::serializePoseGraphCallback( + const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr resp) +/*****************************************************************************/ +{ + if (processor_type_ == PROCESS_LOCALIZATION) { + return LocalizationSlamToolbox::serializePoseGraphCallback(request_header, req, resp); + } + else { + return SlamToolbox::serializePoseGraphCallback(request_header, req, resp); + } +} + +/*****************************************************************************/ +bool MapAndLocalizationSlamToolbox::deserializePoseGraphCallback( + const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr resp) +/*****************************************************************************/ +{ + if (processor_type_ == PROCESS_LOCALIZATION) { + return LocalizationSlamToolbox::deserializePoseGraphCallback(request_header, req, resp); + } + else { + return SlamToolbox::deserializePoseGraphCallback(request_header, req, resp); + } +} + + +/*****************************************************************************/ +void MapAndLocalizationSlamToolbox::localizePoseCallback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) +/*****************************************************************************/ +{ + if (processor_type_ == PROCESS_LOCALIZATION) { + LocalizationSlamToolbox::localizePoseCallback(msg); + } + else { + RCLCPP_INFO(get_logger(), + "LocalizePoseCallback: Received initial pose callback " + "outside of localization mode. will start processing near pose."); + + boost::mutex::scoped_lock l(pose_mutex_); + processor_type_ = PROCESS; + if (process_near_pose_) { + process_near_pose_.reset(new Pose2(msg->pose.pose.position.x, + msg->pose.pose.position.y, tf2::getYaw(msg->pose.pose.orientation))); + } else { + process_near_pose_ = std::make_unique(msg->pose.pose.position.x, + msg->pose.pose.position.y, tf2::getYaw(msg->pose.pose.orientation)); + } + first_measurement_ = true; + + RCLCPP_INFO(get_logger(), + "LocalizePoseCallback: Localizing to: (%0.2f %0.2f), theta=%0.2f", + msg->pose.pose.position.x, msg->pose.pose.position.y, + tf2::getYaw(msg->pose.pose.orientation)); + + } +} + +/*****************************************************************************/ +void MapAndLocalizationSlamToolbox::laserCallback( + sensor_msgs::msg::LaserScan::ConstSharedPtr scan) +/*****************************************************************************/ +{ + if (!waitForTransform(scan->header.frame_id, scan->header.stamp)) { + return; + } + + // store scan header + scan_header = scan->header; + // no odom info + Pose2 pose; + if (!pose_helper_->getOdomPose(pose, scan->header.stamp)) { + RCLCPP_WARN(get_logger(), "Failed to compute odom pose"); + return; + } + + // ensure the laser can be used + LaserRangeFinder * laser = getLaser(scan); + + if (!laser) { + RCLCPP_WARN(get_logger(), "Failed to create laser device for" + " %s; discarding scan", scan->header.frame_id.c_str()); + return; + } + + addScan(laser, scan, pose); +} + +/*****************************************************************************/ +LocalizedRangeScan * MapAndLocalizationSlamToolbox::addScan( + LaserRangeFinder * laser, + const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan, + Pose2 & odom_pose) +/*****************************************************************************/ +{ + if (processor_type_ == PROCESS_LOCALIZATION) { + return LocalizationSlamToolbox::addScan(laser, scan, odom_pose); + } + else { + return SlamToolbox::addScan(laser, scan, odom_pose); + } +} + +} // namespace slam_toolbox diff --git a/src/experimental/slam_toolbox_map_and_localization_node.cpp b/src/experimental/slam_toolbox_map_and_localization_node.cpp new file mode 100644 index 000000000..5ba3b19f5 --- /dev/null +++ b/src/experimental/slam_toolbox_map_and_localization_node.cpp @@ -0,0 +1,49 @@ +/* + * slam_toolbox + * Copyright (c) 2022, Steve Macenski + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +#include "slam_toolbox/experimental/slam_toolbox_map_and_localization.hpp" +#include + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + int stack_size = 40000000; + { + auto temp_node = std::make_shared("slam_toolbox"); + temp_node->declare_parameter("stack_size_to_use", rclcpp::ParameterType::PARAMETER_INTEGER); + if (temp_node->get_parameter("stack_size_to_use", stack_size)) + { + RCLCPP_INFO(temp_node->get_logger(), "Node using stack size %i", (int)stack_size); + const rlim_t max_stack_size = stack_size; + struct rlimit stack_limit; + getrlimit(RLIMIT_STACK, &stack_limit); + if (stack_limit.rlim_cur < stack_size) + { + stack_limit.rlim_cur = stack_size; + } + setrlimit(RLIMIT_STACK, &stack_limit); + } + } + + rclcpp::NodeOptions options; + auto node = std::make_shared(options); + node->configure(); + node->loadPoseGraphByParams(); + rclcpp::spin(node->get_node_base_interface()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/laser_utils.cpp b/src/laser_utils.cpp index e7ead6023..5aed9b76d 100644 --- a/src/laser_utils.cpp +++ b/src/laser_utils.cpp @@ -125,6 +125,13 @@ karto::LaserRangeFinder * LaserAssistant::makeLaser(const double & mountingYaw) } node_->get_parameter("max_laser_range", max_laser_range); + if (max_laser_range <= 0) { + RCLCPP_WARN(node_->get_logger(), + "You've set maximum_laser_range to be negative," + "this isn't allowed so it will be set to (%.1f).", scan_.range_max); + max_laser_range = scan_.range_max; + } + if (max_laser_range > scan_.range_max) { RCLCPP_WARN(node_->get_logger(), "maximum laser range setting (%.1f m) exceeds the capabilities " @@ -191,4 +198,8 @@ void ScanHolder::addScan(const sensor_msgs::msg::LaserScan scan) current_scans_->push_back(scan); } +void ScanHolder::clearScans(){ + current_scans_->clear(); +} + } // namespace laser_utils diff --git a/src/loop_closure_assistant.cpp b/src/loop_closure_assistant.cpp index a8e6d812f..06fb0b50a 100644 --- a/src/loop_closure_assistant.cpp +++ b/src/loop_closure_assistant.cpp @@ -31,243 +31,133 @@ LoopClosureAssistant::LoopClosureAssistant( laser_utils::ScanHolder * scan_holder, PausedState & state, ProcessType & processor_type) : mapper_(mapper), scan_holder_(scan_holder), - interactive_mode_(false), node_(node), state_(state), + node_(node), state_(state), processor_type_(processor_type) /*****************************************************************************/ { node_->declare_parameter("paused_processing", false); node_->set_parameter(rclcpp::Parameter("paused_processing", false)); - node_->declare_parameter("interactive_mode", false); - node_->set_parameter(rclcpp::Parameter("interactive_mode", false)); - node_->get_parameter("enable_interactive_mode", enable_interactive_mode_); tfB_ = std::make_unique(node_); solver_ = mapper_->getScanSolver(); - ssClear_manual_ = node_->create_service( - "/slam_toolbox/clear_changes", std::bind(&LoopClosureAssistant::clearChangesCallback, - this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - - ssLoopClosure_ = node_->create_service( - "/slam_toolbox/manual_loop_closure", std::bind(&LoopClosureAssistant::manualLoopClosureCallback, - this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - scan_publisher_ = node_->create_publisher( - "/slam_toolbox/scan_visualization",10); - interactive_server_ = std::make_unique( - "slam_toolbox", - node_->get_node_base_interface(), - node_->get_node_clock_interface(), - node_->get_node_logging_interface(), - node_->get_node_topics_interface(), - node_->get_node_services_interface()); - ssInteractive_ = node_->create_service( - "/slam_toolbox/toggle_interactive_mode", std::bind(&LoopClosureAssistant::interactiveModeCallback, - this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - + "slam_toolbox/scan_visualization",10); marker_publisher_ = node_->create_publisher( - "/slam_toolbox/graph_visualization", rclcpp::QoS(1)); + "slam_toolbox/graph_visualization", rclcpp::QoS(1)); map_frame_ = node->get_parameter("map_frame").as_string(); } /*****************************************************************************/ -void LoopClosureAssistant::processInteractiveFeedback(const - visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr feedback) +void LoopClosureAssistant::publishGraph() /*****************************************************************************/ { - if (processor_type_ != PROCESS) - { - RCLCPP_ERROR_THROTTLE(node_->get_logger(), *node_->get_clock(), 5, - "Interactive mode is invalid outside processing mode."); + if(marker_publisher_->get_subscription_count() == 0){ return; } - - const int id = std::stoi(feedback->marker_name, nullptr, 10) - 1; - - // was depressed, something moved, and now released - if (feedback->event_type == - visualization_msgs::msg::InteractiveMarkerFeedback::MOUSE_UP && - feedback->mouse_point_valid) - { - addMovedNodes(id, Eigen::Vector3d(feedback->mouse_point.x, - feedback->mouse_point.y, tf2::getYaw(feedback->pose.orientation))); - } - - // is currently depressed, being moved before release - if (feedback->event_type == - visualization_msgs::msg::InteractiveMarkerFeedback::POSE_UPDATE) - { - // get scan - sensor_msgs::msg::LaserScan scan = scan_holder_->getCorrectedScan(id); - - // get correct orientation - tf2::Quaternion quat(0.,0.,0.,1.0), msg_quat(0.,0.,0.,1.0); - double node_yaw, first_node_yaw; - solver_->GetNodeOrientation(id, node_yaw); - solver_->GetNodeOrientation(0, first_node_yaw); - tf2::Quaternion q1(0.,0.,0.,1.0); - q1.setEuler(0., 0., node_yaw - 3.14159); - tf2::Quaternion q2(0.,0.,0.,1.0); - q2.setEuler(0., 0., 3.14159); - quat *= q1; - quat *= q2; - - // interactive move - tf2::convert(feedback->pose.orientation, msg_quat); - quat *= msg_quat; - quat.normalize(); - - // create correct transform - tf2::Transform transform; - transform.setOrigin(tf2::Vector3(feedback->pose.position.x, - feedback->pose.position.y, 0.)); - transform.setRotation(quat); - - // publish the scan visualization with transform - geometry_msgs::msg::TransformStamped msg; - tf2::convert(transform, msg.transform); - msg.child_frame_id = "scan_visualization"; - msg.header.frame_id = feedback->header.frame_id; - msg.header.stamp = node_->now(); - tfB_->sendTransform(msg); - - scan.header.frame_id = "scan_visualization"; - scan.header.stamp = node_->now(); - scan_publisher_->publish(scan); - } -} - - -/*****************************************************************************/ -void LoopClosureAssistant::publishGraph() -/*****************************************************************************/ -{ - interactive_server_->clear(); - std::unordered_map * graph = solver_->getGraph(); + + auto graph = solver_->getGraph(); if (graph->size() == 0) { return; } - RCLCPP_DEBUG(node_->get_logger(), "Graph size: %i", (int)graph->size()); - bool interactive_mode = false; - { - boost::mutex::scoped_lock lock(interactive_mutex_); - interactive_mode = interactive_mode_; + RCLCPP_DEBUG(node_->get_logger(), "Graph size: %zu", graph->size()); + + const auto & vertices = mapper_->GetGraph()->GetVertices(); + const auto & edges = mapper_->GetGraph()->GetEdges(); + const auto & localization_vertices = mapper_->GetLocalizationVertices(); + + int first_localization_id = std::numeric_limits::max(); + if (!localization_vertices.empty()) { + first_localization_id = localization_vertices.front().vertex->GetObject()->GetUniqueId(); } visualization_msgs::msg::MarkerArray marray; - visualization_msgs::msg::Marker m = vis_utils::toMarker(map_frame_, - "slam_toolbox", 0.1, node_); - - for (ConstGraphIterator it = graph->begin(); it != graph->end(); ++it) { - m.id = it->first + 1; - m.pose.position.x = it->second(0); - m.pose.position.y = it->second(1); - - if (interactive_mode && enable_interactive_mode_) { - visualization_msgs::msg::InteractiveMarker int_marker = - vis_utils::toInteractiveMarker(m, 0.3, node_); - interactive_server_->insert(int_marker, - std::bind( - &LoopClosureAssistant::processInteractiveFeedback, - this, std::placeholders::_1)); - } else { - marray.markers.push_back(m); - } - } - // if disabled, clears out old markers - interactive_server_->applyChanges(); - marker_publisher_->publish(marray); -} + // clear existing markers to account for any removed nodes + visualization_msgs::msg::Marker clear; + clear.header.stamp = node_->now(); + clear.action = visualization_msgs::msg::Marker::DELETEALL; + marray.markers.push_back(clear); -/*****************************************************************************/ -bool LoopClosureAssistant::manualLoopClosureCallback( - const std::shared_ptr request_header, - const std::shared_ptr req, - std::shared_ptr resp) -/*****************************************************************************/ -{ - if(!enable_interactive_mode_) - { - RCLCPP_WARN( - node_->get_logger(), "Called manual loop closure" - " with interactive mode disabled. Ignoring."); - return false; - } + visualization_msgs::msg::Marker m = vis_utils::toMarker(map_frame_, "slam_toolbox", 0.1, node_); - { - boost::mutex::scoped_lock lock(moved_nodes_mutex_); + // add map nodes + for (const auto & sensor_name : vertices) { + for (const auto & vertex : sensor_name.second) { + m.color.g = vertex.first < first_localization_id ? 0.0 : 1.0; + const auto & pose = vertex.second->GetObject()->GetCorrectedPose(); + m.id = vertex.first; + m.pose.position.x = pose.GetX(); + m.pose.position.y = pose.GetY(); - if (moved_nodes_.size() == 0) - { - RCLCPP_WARN( - node_->get_logger(), - "No moved nodes to attempt manual loop closure."); - return true; - } + marray.markers.push_back(m); - RCLCPP_INFO( - node_->get_logger(), - "LoopClosureAssistant: Attempting to manual " - "loop close with %i moved nodes.", (int)moved_nodes_.size()); - // for each in node map - std::map::const_iterator it = moved_nodes_.begin(); - for (it; it != moved_nodes_.end(); ++it) - { - moveNode(it->first, - Eigen::Vector3d(it->second(0),it->second(1), it->second(2))); } } - // optimize - mapper_->CorrectPoses(); - - //update visualization and clear out nodes completed - publishGraph(); - clearMovedNodes(); - return true; -} - - -/*****************************************************************************/ -bool LoopClosureAssistant::interactiveModeCallback( - const std::shared_ptr request_header, - const std::shared_ptr req, - std::shared_ptr resp) -/*****************************************************************************/ -{ - if(!enable_interactive_mode_) - { - RCLCPP_WARN( - node_->get_logger(), - "Called toggle interactive mode with interactive mode disabled. Ignoring."); - return false; + // add line markers for graph edges + visualization_msgs::msg::Marker edges_marker; + edges_marker.header.frame_id = map_frame_; + edges_marker.header.stamp = node_->now(); + edges_marker.id = 0; + edges_marker.ns = "slam_toolbox_edges"; + edges_marker.action = visualization_msgs::msg::Marker::ADD; + edges_marker.type = visualization_msgs::msg::Marker::LINE_LIST; + edges_marker.pose.orientation.w = 1; + edges_marker.scale.x = 0.05; + edges_marker.color.b = 1; + edges_marker.color.a = 1; + edges_marker.lifetime = rclcpp::Duration::from_seconds(0); + edges_marker.points.reserve(edges.size() * 2); + + visualization_msgs::msg::Marker localization_edges_marker; + localization_edges_marker.header.frame_id = map_frame_; + localization_edges_marker.header.stamp = node_->now(); + localization_edges_marker.id = 1; + localization_edges_marker.ns = "slam_toolbox_edges"; + localization_edges_marker.action = visualization_msgs::msg::Marker::ADD; + localization_edges_marker.type = visualization_msgs::msg::Marker::LINE_LIST; + localization_edges_marker.pose.orientation.w = 1; + localization_edges_marker.scale.x = 0.05; + localization_edges_marker.color.g = 1; + localization_edges_marker.color.b = 1; + localization_edges_marker.color.a = 1; + localization_edges_marker.lifetime = rclcpp::Duration::from_seconds(0); + localization_edges_marker.points.reserve(localization_vertices.size() * 3); + + for (const auto & edge : edges) { + int source_id = edge->GetSource()->GetObject()->GetUniqueId(); + const auto & pose0 = edge->GetSource()->GetObject()->GetCorrectedPose(); + geometry_msgs::msg::Point p0; + p0.x = pose0.GetX(); + p0.y = pose0.GetY(); + + int target_id = edge->GetTarget()->GetObject()->GetUniqueId(); + const auto & pose1 = edge->GetTarget()->GetObject()->GetCorrectedPose(); + geometry_msgs::msg::Point p1; + p1.x = pose1.GetX(); + p1.y = pose1.GetY(); + + if (source_id >= first_localization_id || target_id >= first_localization_id) { + localization_edges_marker.points.push_back(p0); + localization_edges_marker.points.push_back(p1); + } else { + edges_marker.points.push_back(p0); + edges_marker.points.push_back(p1); + } } - bool interactive_mode; - { - boost::mutex::scoped_lock lock_i(interactive_mutex_); - interactive_mode_ = !interactive_mode_; - interactive_mode = interactive_mode_; - node_->set_parameter(rclcpp::Parameter("interactive_mode", interactive_mode_)); - } + marray.markers.push_back(edges_marker); + marray.markers.push_back(localization_edges_marker); - RCLCPP_INFO(node_->get_logger(), - "SlamToolbox: Toggling %s interactive mode.", - interactive_mode ? "on" : "off"); - publishGraph(); - clearMovedNodes(); - - // set state so we don't overwrite changes in rviz while loop closing - state_.set(PROCESSING, interactive_mode); - state_.set(VISUALIZING_GRAPH, interactive_mode); - node_->set_parameter(rclcpp::Parameter("paused_processing", interactive_mode)); - return true; + // if disabled, clears out old markers + marker_publisher_->publish(marray); } + /*****************************************************************************/ void LoopClosureAssistant::moveNode( const int & id, const Eigen::Vector3d & pose) @@ -276,28 +166,6 @@ void LoopClosureAssistant::moveNode( solver_->ModifyNode(id, pose); } -/*****************************************************************************/ -bool LoopClosureAssistant::clearChangesCallback( - const std::shared_ptr request_header, - const std::shared_ptr req, - std::shared_ptr resp) -/*****************************************************************************/ -{ - if(!enable_interactive_mode_) - { - RCLCPP_WARN( - node_->get_logger(), - "Called Clear changes with interactive mode disabled. Ignoring."); - return false; - } - - RCLCPP_INFO( - node_->get_logger(), - "LoopClosureAssistant: Clearing manual loop closure nodes."); - publishGraph(); - clearMovedNodes(); - return true; -} /*****************************************************************************/ void LoopClosureAssistant::clearMovedNodes() @@ -319,4 +187,11 @@ void LoopClosureAssistant::addMovedNodes(const int & id, Eigen::Vector3d vec) moved_nodes_[id] = vec; } +/*****************************************************************************/ +void LoopClosureAssistant::setMapper(karto::Mapper * mapper) +/*****************************************************************************/ +{ + mapper_ = mapper; +} + } // namespace loop_closure_assistant diff --git a/src/map_saver.cpp b/src/map_saver.cpp index ce821a10d..a7189baec 100644 --- a/src/map_saver.cpp +++ b/src/map_saver.cpp @@ -28,7 +28,7 @@ MapSaver::MapSaver(rclcpp::Node::SharedPtr node, const std::string & map_name) : node_(node), map_name_(map_name), received_map_(false) /*****************************************************************************/ { - server_ = node_->create_service("/slam_toolbox/save_map", + server_ = node_->create_service("slam_toolbox/save_map", std::bind(&MapSaver::saveMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); @@ -53,6 +53,7 @@ bool MapSaver::saveMapCallback( RCLCPP_WARN(node_->get_logger(), "Map Saver: Cannot save map, no map yet received on topic %s.", map_name_.c_str()); + response->result = response->RESULT_NO_MAP_RECEIEVD; return false; } @@ -61,10 +62,20 @@ bool MapSaver::saveMapCallback( RCLCPP_INFO(node_->get_logger(), "SlamToolbox: Saving map as %s.", name.c_str()); int rc = system(("ros2 run nav2_map_server map_saver_cli -f " + name + " --ros-args -p map_subscribe_transient_local:=true").c_str()); + if (rc == 0) { + response->result = response->RESULT_SUCCESS; + } else { + response->result = response->RESULT_UNDEFINED_FAILURE; + } } else { RCLCPP_INFO(node_->get_logger(), "SlamToolbox: Saving map in current directory."); int rc = system("ros2 run nav2_map_server map_saver_cli --ros-args -p map_subscribe_transient_local:=true"); + if (rc == 0) { + response->result = response->RESULT_SUCCESS; + } else { + response->result = response->RESULT_UNDEFINED_FAILURE; + } } rclcpp::sleep_for(std::chrono::seconds(1)); diff --git a/src/merge_maps_kinematic.cpp b/src/merge_maps_kinematic.cpp deleted file mode 100644 index 794d65f69..000000000 --- a/src/merge_maps_kinematic.cpp +++ /dev/null @@ -1,379 +0,0 @@ -/* - * Author - * Copyright (c) 2018, Simbe Robotics, Inc. - * - * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE - * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY - * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS - * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. - * - * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO - * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS - * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND - * CONDITIONS. - * - */ - -/* Author: Steven Macenski */ - -#include -#include -#include "slam_toolbox/merge_maps_kinematic.hpp" -#include "slam_toolbox/serialization.hpp" - -/*****************************************************************************/ -MergeMapsKinematic::MergeMapsKinematic() -: Node("map_merging") -/*****************************************************************************/ -{ - RCLCPP_INFO(get_logger(), "MergeMapsKinematic: Starting up!"); - num_submaps_ = 0; -} - -/*****************************************************************************/ -void MergeMapsKinematic::configure() -/*****************************************************************************/ -{ - resolution_ = 0.05; - resolution_ = this->declare_parameter("resolution", resolution_); - - sstS_.push_back(this->create_publisher( - "/map", rclcpp::QoS(1))); - sstmS_.push_back(this->create_publisher( - "/map_metadata", rclcpp::QoS(1))); - - ssMap_ = this->create_service("/slam_toolbox/merge_submaps", - std::bind(&MergeMapsKinematic::mergeMapCallback, this, std::placeholders::_1, - std::placeholders::_2, std::placeholders::_3)); - ssSubmap_ = this->create_service("/slam_toolbox/add_submap", - std::bind(&MergeMapsKinematic::addSubmapCallback, this, std::placeholders::_1, - std::placeholders::_2, std::placeholders::_3)); - - tfB_ = std::make_unique(shared_from_this()); - - interactive_server_ = - std::make_unique( - "merge_maps_tool", shared_from_this()); -} - -/*****************************************************************************/ -MergeMapsKinematic::~MergeMapsKinematic() -/*****************************************************************************/ -{ -} - -/*****************************************************************************/ -bool MergeMapsKinematic::addSubmapCallback( - const std::shared_ptr request_header, - const std::shared_ptr req, - std::shared_ptr resp) -/*****************************************************************************/ -{ - std::unique_ptr mapper = std::make_unique(); - std::unique_ptr dataset = std::make_unique(); - - if (!serialization::read(req->filename, *mapper, - *dataset, shared_from_this())) - { - RCLCPP_ERROR(get_logger(), "addSubmapCallback: Failed to read " - "file: %s.", req->filename.c_str()); - return true; - } - - // we know the position because we put it there before any scans - LaserRangeFinder * laser = dynamic_cast( - dataset->GetLasers()[0]); - dataset->Add(laser, true); - dataset_vec_.push_back(std::move(dataset)); - - if (lasers_.find(laser->GetName().GetName()) == lasers_.end()) { - laser_utils::LaserMetadata laserMeta(laser, false); - lasers_[laser->GetName().GetName()] = laserMeta; - } - - LocalizedRangeScanVector scans = mapper->GetAllProcessedScans(); - scans_vec_.push_back(scans); - num_submaps_++; - - // create and publish map with marker that will move the map around - sstS_.push_back(this->create_publisher( - "/map_" + std::to_string(num_submaps_), rclcpp::QoS(1))); - sstmS_.push_back(this->create_publisher( - "/map_metadata_" + std::to_string(num_submaps_), rclcpp::QoS(1))); - sleep(1.0); - - nav_msgs::srv::GetMap::Response map; - nav_msgs::msg::OccupancyGrid & og = map.map; - try { - kartoToROSOccupancyGrid(scans, map); - } catch (const Exception & e) { - RCLCPP_WARN(get_logger(), "Failed to build grid to add submap, Exception: %s", - e.GetErrorMessage().c_str()); - return false; - } - - tf2::Transform transform; - transform.setIdentity(); - transform.setOrigin(tf2::Vector3(og.info.origin.position.x + - og.info.width * og.info.resolution / 2.0, - og.info.origin.position.y + og.info.height * og.info.resolution / 2.0, - 0.)); - og.info.origin.position.x = -(og.info.width * og.info.resolution / 2.0); - og.info.origin.position.y = -(og.info.height * og.info.resolution / 2.0); - og.header.stamp = this->now(); - og.header.frame_id = "map_" + std::to_string(num_submaps_); - sstS_[num_submaps_]->publish(og); - sstmS_[num_submaps_]->publish(og.info); - - geometry_msgs::msg::TransformStamped msg; - msg.transform = tf2::toMsg(transform); - msg.child_frame_id = "/map_" + std::to_string(num_submaps_); - msg.header.frame_id = "/map"; - msg.header.stamp = this->now(); - tfB_->sendTransform(msg); - - submap_marker_transform_[num_submaps_] = - tf2::Transform(tf2::Quaternion(0., 0., 0., 1.0), - tf2::Vector3(0, 0, 0)); // no initial correction -- identity mat - submap_locations_[num_submaps_] = - Eigen::Vector3d(transform.getOrigin().getX(), - transform.getOrigin().getY(), 0.); - - // create an interactive marker for the base of this frame and attach it - visualization_msgs::msg::Marker m = - vis_utils::toMarker("map", "merge_maps_tool", 2.0, shared_from_this()); - m.pose.position.x = transform.getOrigin().getX(); - m.pose.position.y = transform.getOrigin().getY(); - m.id = num_submaps_; - - visualization_msgs::msg::InteractiveMarker int_marker = - vis_utils::toInteractiveMarker(m, 2.4, shared_from_this()); - interactive_server_->insert( - int_marker, - std::bind(&MergeMapsKinematic::processInteractiveFeedback, this, std::placeholders::_1)); - interactive_server_->applyChanges(); - - RCLCPP_INFO(get_logger(), - "Map %s was loaded into topic %s!", req->filename.c_str(), - ("/map_" + std::to_string(num_submaps_)).c_str()); - return true; -} - -/*****************************************************************************/ -Pose2 MergeMapsKinematic::applyCorrection( - const - Pose2 & pose, - const tf2::Transform & submap_correction) -/*****************************************************************************/ -{ - tf2::Transform pose_tf, pose_corr; - tf2::Quaternion q(0., 0., 0., 1.0); - q.setRPY(0., 0., pose.GetHeading()); - pose_tf.setOrigin(tf2::Vector3(pose.GetX(), pose.GetY(), 0.)); - pose_tf.setRotation(q); - pose_corr = submap_correction * pose_tf; - return Pose2(pose_corr.getOrigin().x(), pose_corr.getOrigin().y(), - tf2::getYaw(pose_corr.getRotation())); -} - -/*****************************************************************************/ -Vector2 MergeMapsKinematic::applyCorrection( - const - Vector2 & pose, - const tf2::Transform & submap_correction) -/*****************************************************************************/ -{ - tf2::Transform pose_tf, pose_corr; - pose_tf.setOrigin(tf2::Vector3(pose.GetX(), pose.GetY(), 0.)); - pose_tf.setRotation(tf2::Quaternion(0., 0., 0., 1.0)); - pose_corr = submap_correction * pose_tf; - return Vector2(pose_corr.getOrigin().x(), - pose_corr.getOrigin().y()); -} - -/*****************************************************************************/ -void MergeMapsKinematic::transformScan( - LocalizedRangeScansIt iter, - tf2::Transform & submap_correction) -/*****************************************************************************/ -{ - // TRANSFORM BARYCENTERR POSE - const Pose2 bary_center_pose = (*iter)->GetBarycenterPose(); - auto bary_center_pose_corr = - applyCorrection(bary_center_pose, submap_correction); - (*iter)->SetBarycenterPose(bary_center_pose_corr); - - // TRANSFORM BOUNDING BOX POSITIONS - BoundingBox2 bbox = (*iter)->GetBoundingBox(); - const Vector2 bbox_min_corr = - applyCorrection(bbox.GetMinimum(), submap_correction); - bbox.SetMinimum(bbox_min_corr); - const Vector2 bbox_max_corr = - applyCorrection(bbox.GetMaximum(), submap_correction); - bbox.SetMaximum(bbox_max_corr); - (*iter)->SetBoundingBox(bbox); - - // TRANSFORM UNFILTERED POINTS USED - PointVectorDouble UPR_vec = (*iter)->GetPointReadings(); - for (PointVectorDouble::iterator it_upr = UPR_vec.begin(); - it_upr != UPR_vec.end(); ++it_upr) - { - const Vector2 upr_corr = applyCorrection( - *it_upr, submap_correction); - it_upr->SetX(upr_corr.GetX()); - it_upr->SetY(upr_corr.GetY()); - } - (*iter)->SetPointReadings(UPR_vec); - - // TRANSFORM CORRECTED POSE - const Pose2 corrected_pose = (*iter)->GetCorrectedPose(); - Pose2 robot_pose_corr = applyCorrection( - corrected_pose, submap_correction); - (*iter)->SetCorrectedPose(robot_pose_corr); - kt_bool dirty = false; - (*iter)->SetIsDirty(dirty); - - // TRANSFORM ODOM POSE - Pose2 odom_pose = (*iter)->GetOdometricPose(); - Pose2 robot_pose_odom = applyCorrection( - odom_pose, submap_correction); - (*iter)->SetOdometricPose(robot_pose_odom); -} - -/*****************************************************************************/ -bool MergeMapsKinematic::mergeMapCallback( - const std::shared_ptr request_header, - const std::shared_ptr req, - std::shared_ptr resp) -/*****************************************************************************/ -{ - RCLCPP_INFO(get_logger(), "Merging maps!"); - - // transform all the scans into the new global map coordinates - int id = 0; - LocalizedRangeScanVector transformed_scans; - for (LocalizedRangeScansVecIt it_LRV = scans_vec_.begin(); - it_LRV != scans_vec_.end(); ++it_LRV) - { - id++; - for (LocalizedRangeScansIt iter = it_LRV->begin(); - iter != it_LRV->end(); ++iter) - { - tf2::Transform submap_correction = submap_marker_transform_[id]; - transformScan(iter, submap_correction); - transformed_scans.push_back((*iter)); - } - } - - // create the map - nav_msgs::srv::GetMap::Response map; - try { - kartoToROSOccupancyGrid(transformed_scans, map); - } catch (const Exception & e) { - RCLCPP_WARN(get_logger(), - "Failed to build grid to merge maps together, Exception: %s", - e.GetErrorMessage().c_str()); - } - - // publish - map.map.header.stamp = this->now(); - map.map.header.frame_id = "map"; - sstS_[0]->publish(map.map); - sstmS_[0]->publish(map.map.info); - return true; -} - -/*****************************************************************************/ -void MergeMapsKinematic::kartoToROSOccupancyGrid( - const LocalizedRangeScanVector & scans, - nav_msgs::srv::GetMap::Response & map) -/*****************************************************************************/ -{ - OccupancyGrid * occ_grid = NULL; - occ_grid = OccupancyGrid::CreateFromScans(scans, resolution_); - if (!occ_grid) { - RCLCPP_INFO(get_logger(), - "MergeMapsKinematic: Could not make occupancy grid."); - } else { - map.map.info.resolution = resolution_; - vis_utils::toNavMap(occ_grid, map.map); - } - - delete occ_grid; -} - -/*****************************************************************************/ -void MergeMapsKinematic::processInteractiveFeedback( - visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr feedback) -/*****************************************************************************/ -{ - const int id = std::stoi(feedback->marker_name, nullptr, 10); - - if (feedback->event_type == - visualization_msgs::msg::InteractiveMarkerFeedback::MOUSE_UP && - feedback->mouse_point_valid) - { - tf2Scalar yaw = tf2::getYaw(feedback->pose.orientation); - tf2::Quaternion quat(0., 0., 0., 1.0); - tf2::fromMsg(feedback->pose.orientation, quat); // relative - - tf2::Transform previous_submap_correction; - previous_submap_correction.setIdentity(); - previous_submap_correction.setOrigin(tf2::Vector3(submap_locations_[id](0), - submap_locations_[id](1), 0.)); - - // update internal knowledge of submap locations - submap_locations_[id] = Eigen::Vector3d(feedback->pose.position.x, - feedback->pose.position.y, - submap_locations_[id](2) + yaw); - - // add the map_N frame there - tf2::Transform new_submap_location; - new_submap_location.setOrigin(tf2::Vector3(submap_locations_[id](0), - submap_locations_[id](1), 0.)); - new_submap_location.setRotation(quat); - - geometry_msgs::msg::TransformStamped msg; - msg.transform = tf2::toMsg(new_submap_location); - msg.child_frame_id = "/map_" + std::to_string(id); - msg.header.frame_id = "/map"; - msg.header.stamp = this->now(); - tfB_->sendTransform(msg); - - submap_marker_transform_[id] = submap_marker_transform_[id] * - previous_submap_correction.inverse() * new_submap_location; - } - - if (feedback->event_type == - visualization_msgs::msg::InteractiveMarkerFeedback::POSE_UPDATE) - { - tf2Scalar yaw = tf2::getYaw(feedback->pose.orientation); - tf2::Quaternion quat(0., 0., 0., 1.0); - tf2::fromMsg(feedback->pose.orientation, quat); // relative - - // add the map_N frame there - tf2::Transform new_submap_location; - new_submap_location.setOrigin(tf2::Vector3(feedback->pose.position.x, - feedback->pose.position.y, 0.)); - new_submap_location.setRotation(quat); - - geometry_msgs::msg::TransformStamped msg; - msg.transform = tf2::toMsg(new_submap_location); - msg.child_frame_id = "/map_" + std::to_string(id); - msg.header.frame_id = "/map"; - msg.header.stamp = this->now(); - tfB_->sendTransform(msg); - } -} - -/*****************************************************************************/ -int main(int argc, char ** argv) -/*****************************************************************************/ -{ - rclcpp::init(argc, argv); - auto merging_node = std::make_shared(); - merging_node->configure(); - rclcpp::spin(merging_node->get_node_base_interface()); - rclcpp::shutdown(); - return 0; -} diff --git a/src/slam_mapper.cpp b/src/slam_mapper.cpp index 6a60f09d4..cfa5c6354 100644 --- a/src/slam_mapper.cpp +++ b/src/slam_mapper.cpp @@ -207,6 +207,12 @@ void SMapper::configure(const rclcpp::Node::SharedPtr & node) correlation_search_space_dimension); } node->get_parameter("correlation_search_space_dimension", correlation_search_space_dimension); + if (correlation_search_space_dimension <= 0) { + RCLCPP_WARN(node->get_logger(), + "You've set correlation_search_space_dimension to be negative," + "this isn't allowed so it will be set to default value 0.5."); + correlation_search_space_dimension = 0.5; + } mapper_->setParamCorrelationSearchSpaceDimension(correlation_search_space_dimension); double correlation_search_space_resolution = 0.01; @@ -216,6 +222,12 @@ void SMapper::configure(const rclcpp::Node::SharedPtr & node) correlation_search_space_resolution); } node->get_parameter("correlation_search_space_resolution", correlation_search_space_resolution); + if (correlation_search_space_resolution <= 0) { + RCLCPP_WARN(node->get_logger(), + "You've set correlation_search_space_resolution to be negative," + "this isn't allowed so it will be set to default value 0.01."); + correlation_search_space_resolution = 0.01; + } mapper_->setParamCorrelationSearchSpaceResolution(correlation_search_space_resolution); double correlation_search_space_smear_deviation = 0.1; @@ -227,6 +239,12 @@ void SMapper::configure(const rclcpp::Node::SharedPtr & node) node->get_parameter( "correlation_search_space_smear_deviation", correlation_search_space_smear_deviation); + if (correlation_search_space_smear_deviation <= 0) { + RCLCPP_WARN(node->get_logger(), + "You've set correlation_search_space_smear_deviation to be negative," + "this isn't allowed so it will be set to default value 0.1."); + correlation_search_space_smear_deviation = 0.1; + } mapper_->setParamCorrelationSearchSpaceSmearDeviation(correlation_search_space_smear_deviation); // Setting Correlation Parameters, Loop Closure Parameters @@ -235,6 +253,12 @@ void SMapper::configure(const rclcpp::Node::SharedPtr & node) node->declare_parameter("loop_search_space_dimension", loop_search_space_dimension); } node->get_parameter("loop_search_space_dimension", loop_search_space_dimension); + if (loop_search_space_dimension <= 0) { + RCLCPP_WARN(node->get_logger(), + "You've set loop_search_space_dimension to be negative," + "this isn't allowed so it will be set to default value 8.0."); + loop_search_space_dimension = 8.0; + } mapper_->setParamLoopSearchSpaceDimension(loop_search_space_dimension); double loop_search_space_resolution = 0.05; @@ -242,6 +266,12 @@ void SMapper::configure(const rclcpp::Node::SharedPtr & node) node->declare_parameter("loop_search_space_resolution", loop_search_space_resolution); } node->get_parameter("loop_search_space_resolution", loop_search_space_resolution); + if (loop_search_space_resolution <= 0) { + RCLCPP_WARN(node->get_logger(), + "You've set loop_search_space_resolution to be negative," + "this isn't allowed so it will be set to default value 0.05."); + loop_search_space_resolution = 0.05; + } mapper_->setParamLoopSearchSpaceResolution(loop_search_space_resolution); double loop_search_space_smear_deviation = 0.03; @@ -249,6 +279,12 @@ void SMapper::configure(const rclcpp::Node::SharedPtr & node) node->declare_parameter("loop_search_space_smear_deviation", loop_search_space_smear_deviation); } node->get_parameter("loop_search_space_smear_deviation", loop_search_space_smear_deviation); + if (loop_search_space_smear_deviation <= 0) { + RCLCPP_WARN(node->get_logger(), + "You've set loop_search_space_smear_deviation to be negative," + "this isn't allowed so it will be set to default value 0.03."); + loop_search_space_smear_deviation = 0.03; + } mapper_->setParamLoopSearchSpaceSmearDeviation(loop_search_space_smear_deviation); // Setting Scan Matcher Parameters diff --git a/src/slam_toolbox_async.cpp b/src/slam_toolbox_async.cpp index 3ef1fa84a..9c9fa7dfc 100644 --- a/src/slam_toolbox_async.cpp +++ b/src/slam_toolbox_async.cpp @@ -35,8 +35,12 @@ void AsynchronousSlamToolbox::laserCallback( sensor_msgs::msg::LaserScan::ConstSharedPtr scan) /*****************************************************************************/ { - // store scan timestamped - scan_timestamped = scan->header.stamp; + if (!waitForTransform(scan->header.frame_id, scan->header.stamp)) { + return; + } + + // store scan header + scan_header = scan->header; // no odom info Pose2 pose; if (!pose_helper_->getOdomPose(pose, scan->header.stamp)) { diff --git a/src/slam_toolbox_async_node.cpp b/src/slam_toolbox_async_node.cpp index 934366816..c96b7718e 100644 --- a/src/slam_toolbox_async_node.cpp +++ b/src/slam_toolbox_async_node.cpp @@ -17,23 +17,25 @@ /* Author: Steven Macenski */ -#include #include "slam_toolbox/slam_toolbox_async.hpp" +#include -int main(int argc, char ** argv) +int main(int argc, char** argv) { rclcpp::init(argc, argv); int stack_size = 40000000; { auto temp_node = std::make_shared("slam_toolbox"); - temp_node->declare_parameter("stack_size_to_use"); - if (temp_node->get_parameter("stack_size_to_use", stack_size)) { + temp_node->declare_parameter("stack_size_to_use", rclcpp::ParameterType::PARAMETER_INTEGER); + if (temp_node->get_parameter("stack_size_to_use", stack_size)) + { RCLCPP_INFO(temp_node->get_logger(), "Node using stack size %i", (int)stack_size); const rlim_t max_stack_size = stack_size; struct rlimit stack_limit; getrlimit(RLIMIT_STACK, &stack_limit); - if (stack_limit.rlim_cur < stack_size) { + if (stack_limit.rlim_cur < stack_size) + { stack_limit.rlim_cur = stack_size; } setrlimit(RLIMIT_STACK, &stack_limit); diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index 5848720d3..1d79e6aba 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -16,32 +16,31 @@ */ /* Author: Steven Macenski */ -#include -#include -#include -#include #include "slam_toolbox/slam_toolbox_common.hpp" #include "slam_toolbox/serialization.hpp" +#include +#include +#include +#include + +#include "maidbot_msg_utils/occupancy_grid_compression.h" namespace slam_toolbox { /*****************************************************************************/ -SlamToolbox::SlamToolbox() -: SlamToolbox(rclcpp::NodeOptions()) +SlamToolbox::SlamToolbox() : SlamToolbox(rclcpp::NodeOptions()) /*****************************************************************************/ { } /*****************************************************************************/ SlamToolbox::SlamToolbox(rclcpp::NodeOptions options) -: Node("slam_toolbox", "", options), - solver_loader_("slam_toolbox", "karto::ScanSolver"), - processor_type_(PROCESS), - first_measurement_(true), - process_near_pose_(nullptr), - transform_timeout_(rclcpp::Duration::from_seconds(0.5)), - minimum_time_interval_(std::chrono::nanoseconds(0)) + : Node("slam_toolbox", "", options), solver_loader_("slam_toolbox", "karto::ScanSolver"), + processor_type_(PROCESS), first_measurement_(true), process_near_pose_(nullptr), + transform_timeout_(rclcpp::Duration::from_seconds(0.5)), + minimum_time_interval_(std::chrono::nanoseconds(0)), + maximum_match_interval_(rclcpp::Duration::from_seconds(-1.0)), slam_running_(true) /*****************************************************************************/ { smapper_ = std::make_unique(); @@ -56,35 +55,40 @@ void SlamToolbox::configure() setROSInterfaces(); setSolver(); - laser_assistant_ = std::make_unique( - shared_from_this(), tf_.get(), base_frame_); - pose_helper_ = std::make_unique( - tf_.get(), base_frame_, odom_frame_); + // pause new scan processing + if (!isPaused(NEW_MEASUREMENTS) && paused_at_startup_) + { + toggleScanProcessing(); + slam_running_ = false; + } + + laser_assistant_ = + std::make_unique(shared_from_this(), tf_.get(), base_frame_); + pose_helper_ = std::make_unique(tf_.get(), base_frame_, odom_frame_); scan_holder_ = std::make_unique(lasers_); - map_saver_ = std::make_unique(shared_from_this(), - map_name_); - closure_assistant_ = - std::make_unique( - shared_from_this(), smapper_->getMapper(), scan_holder_.get(), - state_, processor_type_); + map_saver_ = std::make_unique(shared_from_this(), map_name_); + closure_assistant_ = std::make_unique( + shared_from_this(), smapper_->getMapper(), scan_holder_.get(), state_, processor_type_); reprocessing_transform_.setIdentity(); double transform_publish_period = 0.05; transform_publish_period = - this->declare_parameter("transform_publish_period", - transform_publish_period); - threads_.push_back(std::make_unique( - boost::bind(&SlamToolbox::publishTransformLoop, - this, transform_publish_period))); - threads_.push_back(std::make_unique( - boost::bind(&SlamToolbox::publishVisualizations, this))); + this->declare_parameter("transform_publish_period", transform_publish_period); + if (transform_publish_period > 0) + { + threads_.push_back(std::make_unique( + boost::bind(&SlamToolbox::publishTransformLoop, this, transform_publish_period))); + } + threads_.push_back( + std::make_unique(boost::bind(&SlamToolbox::publishVisualizations, this))); } /*****************************************************************************/ SlamToolbox::~SlamToolbox() /*****************************************************************************/ { - for (int i = 0; i != threads_.size(); i++) { + for (int i = 0; i != threads_.size(); i++) + { threads_[i]->join(); } @@ -104,16 +108,20 @@ void SlamToolbox::setSolver() { // Set solver to be used in loop closure std::string solver_plugin = std::string("solver_plugins::CeresSolver"); - solver_plugin = this->declare_parameter("solver_plugin", solver_plugin); + solver_plugin = this->declare_parameter("solver_plugin", solver_plugin); - try { + try + { solver_ = solver_loader_.createSharedInstance(solver_plugin); - RCLCPP_INFO(get_logger(), "Using solver plugin %s", - solver_plugin.c_str()); + RCLCPP_INFO(get_logger(), "Using solver plugin %s", solver_plugin.c_str()); solver_->Configure(shared_from_this()); - } catch (const pluginlib::PluginlibException & ex) { - RCLCPP_FATAL(get_logger(), "Failed to create %s, is it " - "registered and built? Exception: %s.", solver_plugin.c_str(), ex.what()); + } + catch (const pluginlib::PluginlibException& ex) + { + RCLCPP_FATAL(get_logger(), + "Failed to create %s, is it " + "registered and built? Exception: %s.", + solver_plugin.c_str(), ex.what()); exit(1); } smapper_->getMapper()->SetScanSolver(solver_.get()); @@ -145,26 +153,37 @@ void SlamToolbox::setParams() throttle_scans_ = 1; throttle_scans_ = this->declare_parameter("throttle_scans", throttle_scans_); - enable_interactive_mode_ = false; - enable_interactive_mode_ = this->declare_parameter("enable_interactive_mode", - enable_interactive_mode_); + position_covariance_scale_ = 1.0; + position_covariance_scale_ = + this->declare_parameter("position_covariance_scale", position_covariance_scale_); + + yaw_covariance_scale_ = 1.0; + yaw_covariance_scale_ = this->declare_parameter("yaw_covariance_scale", yaw_covariance_scale_); - double tmp_val = 0.5; - tmp_val = this->declare_parameter("transform_timeout", tmp_val); - transform_timeout_ = rclcpp::Duration::from_seconds(tmp_val); - tmp_val = this->declare_parameter("minimum_time_interval", tmp_val); - minimum_time_interval_ = rclcpp::Duration::from_seconds(tmp_val); + double tmp_val = 0.5; + tmp_val = this->declare_parameter("transform_timeout", tmp_val); + transform_timeout_ = rclcpp::Duration::from_seconds(tmp_val); + tmp_val = this->declare_parameter("minimum_time_interval", tmp_val); + minimum_time_interval_ = rclcpp::Duration::from_seconds(tmp_val); + tmp_val = this->declare_parameter("maximum_match_interval", -1.0); + maximum_match_interval_ = rclcpp::Duration::from_seconds(tmp_val); bool debug = false; - debug = this->declare_parameter("debug_logging", debug); - if (debug) { - rcutils_ret_t rtn = rcutils_logging_set_logger_level("logger_name", - RCUTILS_LOG_SEVERITY_DEBUG); + debug = this->declare_parameter("debug_logging", debug); + if (debug) + { + rcutils_ret_t rtn = rcutils_logging_set_logger_level("logger_name", RCUTILS_LOG_SEVERITY_DEBUG); } smapper_->configure(shared_from_this()); - this->declare_parameter("paused_new_measurements"); - this->set_parameter({"paused_new_measurements", false}); + this->declare_parameter("paused_new_measurements"); + // some systems dont have the tf transforms and lidar ready at startup which + // produces a lot of spam. this allows the user to decide when they want to start + // the lidar/TF processing. + if (!this->get_parameter_or("paused_new_measurements", paused_at_startup_, false)) + { + this->set_parameter({"paused_new_measurements", paused_at_startup_}); + } } /*****************************************************************************/ @@ -172,67 +191,72 @@ void SlamToolbox::setROSInterfaces() /*****************************************************************************/ { double tmp_val = 30.; - tmp_val = this->declare_parameter("tf_buffer_duration", tmp_val); - tf_ = std::make_unique(this->get_clock(), - tf2::durationFromSec(tmp_val)); - auto timer_interface = std::make_shared( - get_node_base_interface(), - get_node_timers_interface()); + tmp_val = this->declare_parameter("tf_buffer_duration", tmp_val); + tf_ = std::make_unique(this->get_clock(), tf2::durationFromSec(tmp_val)); + auto timer_interface = std::make_shared(get_node_base_interface(), + get_node_timers_interface()); tf_->setCreateTimerInterface(timer_interface); tfL_ = std::make_unique(*tf_); tfB_ = std::make_unique(shared_from_this()); - sst_ = this->create_publisher( + pose_pub_ = this->create_publisher("pose", 10); + status_pub_ = this->create_publisher("localization/status", 10); + sst_ = this->create_publisher( map_name_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); sstm_ = this->create_publisher( - map_name_ + "_metadata", - rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); - ssMap_ = this->create_service("/slam_toolbox/dynamic_map", - std::bind(&SlamToolbox::mapCallback, this, std::placeholders::_1, - std::placeholders::_2, std::placeholders::_3)); - ssPauseMeasurements_ = this->create_service( - "/slam_toolbox/pause_new_measurements", - std::bind(&SlamToolbox::pauseNewMeasurementsCallback, - this, std::placeholders::_1, - std::placeholders::_2, std::placeholders::_3)); + map_name_ + "_metadata", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + ssMap_ = this->create_service( + "slam_toolbox/dynamic_map", std::bind(&SlamToolbox::mapCallback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); + ssPauseMeasurements_ = this->create_service( + "slam_toolbox/pause_new_measurements", + std::bind(&SlamToolbox::pauseNewMeasurementsCallback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); ssSerialize_ = this->create_service( - "/slam_toolbox/serialize_map", - std::bind(&SlamToolbox::serializePoseGraphCallback, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + "slam_toolbox/serialize_map", + std::bind(&SlamToolbox::serializePoseGraphCallback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); ssDesserialize_ = this->create_service( "slam_toolbox/deserialize_map", - std::bind(&SlamToolbox::deserializePoseGraphCallback, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - - scan_filter_sub_ = - std::make_unique>( - shared_from_this().get(), scan_topic_, rmw_qos_profile_sensor_data); - scan_filter_ = - std::make_unique>( - *scan_filter_sub_, *tf_, odom_frame_, 1, shared_from_this()); - scan_filter_->registerCallback( + std::bind(&SlamToolbox::deserializePoseGraphCallback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); + ssStart_ = this->create_service( + "slam_toolbox/start", + std::bind(&SlamToolbox::startSlamCallback, this, std::placeholders::_1, std::placeholders::_2)); + ssStop_ = this->create_service( + "slam_toolbox/stop", + std::bind(&SlamToolbox::stopSlamCallback, this, std::placeholders::_1, std::placeholders::_2)); + + scan_sub_ = create_subscription( + scan_topic_, rclcpp::SensorDataQoS().keep_last(1), std::bind(&SlamToolbox::laserCallback, this, std::placeholders::_1)); } /*****************************************************************************/ -void SlamToolbox::publishTransformLoop( - const double & transform_publish_period) +void SlamToolbox::publishTransformLoop(const double& transform_publish_period) /*****************************************************************************/ { - if (transform_publish_period == 0) { + if (transform_publish_period == 0) + { return; } rclcpp::Rate r(1.0 / transform_publish_period); - while (rclcpp::ok()) { + while (rclcpp::ok()) + { { boost::mutex::scoped_lock lock(map_to_odom_mutex_); - geometry_msgs::msg::TransformStamped msg; - msg.transform = tf2::toMsg(map_to_odom_); - msg.child_frame_id = odom_frame_; - msg.header.frame_id = map_frame_; - msg.header.stamp = scan_timestamped + transform_timeout_; - tfB_->sendTransform(msg); + rclcpp::Time scan_timestamp = scan_header.stamp; + // Avoid publishing tf with initial 0.0 scan timestamp + if (scan_timestamp.seconds() > 0.0 && !scan_header.frame_id.empty()) + { + geometry_msgs::msg::TransformStamped msg; + msg.transform = tf2::toMsg(map_to_odom_); + msg.child_frame_id = odom_frame_; + msg.header.frame_id = map_frame_; + msg.header.stamp = scan_timestamp + transform_timeout_; + tfB_->sendTransform(msg); + } } r.sleep(); } @@ -242,25 +266,27 @@ void SlamToolbox::publishTransformLoop( void SlamToolbox::publishVisualizations() /*****************************************************************************/ { - nav_msgs::msg::OccupancyGrid & og = map_.map; - og.info.resolution = resolution_; - og.info.origin.position.x = 0.0; - og.info.origin.position.y = 0.0; - og.info.origin.position.z = 0.0; - og.info.origin.orientation.x = 0.0; - og.info.origin.orientation.y = 0.0; - og.info.origin.orientation.z = 0.0; - og.info.origin.orientation.w = 1.0; - og.header.frame_id = map_frame_; + nav_msgs::msg::OccupancyGrid& og = map_.map; + og.info.resolution = resolution_; + og.info.origin.position.x = 0.0; + og.info.origin.position.y = 0.0; + og.info.origin.position.z = 0.0; + og.info.origin.orientation.x = 0.0; + og.info.origin.orientation.y = 0.0; + og.info.origin.orientation.z = 0.0; + og.info.origin.orientation.w = 1.0; + og.header.frame_id = map_frame_; double map_update_interval = 10; - map_update_interval = this->declare_parameter("map_update_interval", - map_update_interval); + map_update_interval = this->declare_parameter("map_update_interval", map_update_interval); rclcpp::Rate r(1.0 / map_update_interval); - while (rclcpp::ok()) { + while (rclcpp::ok()) + { updateMap(); - if (!isPaused(VISUALIZING_GRAPH)) { + if (!isPaused(VISUALIZING_GRAPH)) + { + boost::mutex::scoped_lock lock(smapper_mutex_); closure_assistant_->publishGraph(); } r.sleep(); @@ -274,19 +300,21 @@ void SlamToolbox::loadPoseGraphByParams() std::string filename; geometry_msgs::msg::Pose2D pose; bool dock = false; - if (shouldStartWithPoseGraph(filename, pose, dock)) { + if (shouldStartWithPoseGraph(filename, pose, dock)) + { std::shared_ptr req = std::make_shared(); std::shared_ptr resp = std::make_shared(); req->initial_pose = pose; - req->filename = filename; - if (dock) { - req->match_type = - slam_toolbox::srv::DeserializePoseGraph::Request::START_AT_FIRST_NODE; - } else { - req->match_type = - slam_toolbox::srv::DeserializePoseGraph::Request::START_AT_GIVEN_POSE; + req->filename = filename; + if (dock) + { + req->match_type = slam_toolbox::srv::DeserializePoseGraph::Request::START_AT_FIRST_NODE; + } + else + { + req->match_type = slam_toolbox::srv::DeserializePoseGraph::Request::START_AT_GIVEN_POSE; } deserializePoseGraphCallback(nullptr, req, resp); @@ -294,38 +322,49 @@ void SlamToolbox::loadPoseGraphByParams() } /*****************************************************************************/ -bool SlamToolbox::shouldStartWithPoseGraph( - std::string & filename, - geometry_msgs::msg::Pose2D & pose, bool & start_at_dock) +bool SlamToolbox::shouldStartWithPoseGraph(std::string& filename, geometry_msgs::msg::Pose2D& pose, + bool& start_at_dock) /*****************************************************************************/ { // if given a map to load at run time, do it. this->declare_parameter("map_file_name", std::string("")); - auto map_start_pose = this->declare_parameter("map_start_pose"); - auto map_start_at_dock = this->declare_parameter("map_start_at_dock"); + auto map_start_pose = + this->declare_parameter("map_start_pose", rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY); + auto map_start_at_dock = + this->declare_parameter("map_start_at_dock", rclcpp::ParameterType::PARAMETER_BOOL); filename = this->get_parameter("map_file_name").as_string(); - if (!filename.empty()) { + if (!filename.empty()) + { std::vector read_pose; - if (map_start_pose.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) { - read_pose = map_start_pose.get>(); + if (map_start_pose.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) + { + read_pose = map_start_pose.get>(); start_at_dock = false; - if (read_pose.size() != 3) { + if (read_pose.size() != 3) + { RCLCPP_ERROR(get_logger(), "LocalizationSlamToolbox: Incorrect " - "number of arguments for map starting pose. Must be in format: " - "[x, y, theta]. Starting at the origin"); - pose.x = 0.; - pose.y = 0.; + "number of arguments for map starting pose. Must be in format: " + "[x, y, theta]. Starting at the origin"); + pose.x = 0.; + pose.y = 0.; pose.theta = 0.; - } else { - pose.x = read_pose[0]; - pose.y = read_pose[1]; + } + else + { + pose.x = read_pose[0]; + pose.y = read_pose[1]; pose.theta = read_pose[2]; } - } else if (map_start_at_dock.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) { + } + else if (map_start_at_dock.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) + { start_at_dock = map_start_at_dock.get(); - } else { - RCLCPP_ERROR(get_logger(), "LocalizationSlamToolbox: Map starting " - "pose not specified. Set either map_start_pose or map_start_at_dock."); + } + else + { + RCLCPP_ERROR(get_logger(), + "LocalizationSlamToolbox: Map starting " + "pose not specified. Set either map_start_pose or map_start_at_dock."); return false; } @@ -336,19 +375,23 @@ bool SlamToolbox::shouldStartWithPoseGraph( } /*****************************************************************************/ -LaserRangeFinder * SlamToolbox::getLaser( - const - sensor_msgs::msg::LaserScan::ConstSharedPtr & scan) +LaserRangeFinder* SlamToolbox::getLaser(const sensor_msgs::msg::LaserScan::ConstSharedPtr& scan) /*****************************************************************************/ { - const std::string & frame = scan->header.frame_id; - if (lasers_.find(frame) == lasers_.end()) { - try { + const std::string& frame = scan->header.frame_id; + if (lasers_.find(frame) == lasers_.end()) + { + try + { lasers_[frame] = laser_assistant_->toLaserMetadata(*scan); dataset_->Add(lasers_[frame].getLaser(), true); - } catch (tf2::TransformException & e) { - RCLCPP_ERROR(get_logger(), "Failed to compute laser pose, " - "aborting initialization (%s)", e.what()); + } + catch (tf2::TransformException& e) + { + RCLCPP_ERROR(get_logger(), + "Failed to compute laser pose, " + "aborting initialization (%s)", + e.what()); return nullptr; } } @@ -356,27 +399,53 @@ LaserRangeFinder * SlamToolbox::getLaser( return lasers_[frame].getLaser(); } +/*****************************************************************************/ +bool SlamToolbox::waitForTransform(const std::string& scan_frame, const rclcpp::Time& stamp) +/*****************************************************************************/ +{ + if (!tf_->_frameExists(odom_frame_)) + { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, "tf frame [%s] doesn't exist yet.'", + odom_frame_.c_str()); + return false; + } + + if (!tf_->_frameExists(scan_frame)) + { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, "tf frame [%s] doesn't exist yet.'", + scan_frame.c_str()); + return false; + } + + if (!tf_->canTransform(odom_frame_, scan_frame, stamp, transform_timeout_)) + { + RCLCPP_WARN(get_logger(), "Failed to get transform %s -> %s.", scan_frame.c_str(), + odom_frame_.c_str()); + return false; + } + + return true; +} + /*****************************************************************************/ bool SlamToolbox::updateMap() /*****************************************************************************/ { - if (sst_->get_subscription_count() == 0) { - return true; + if (sst_->get_subscription_count() == 0) + { + return false; } boost::mutex::scoped_lock lock(smapper_mutex_); - OccupancyGrid * occ_grid = smapper_->getOccupancyGrid(resolution_); - if (!occ_grid) { + OccupancyGrid* occ_grid = smapper_->getOccupancyGrid(resolution_); + if (!occ_grid) + { return false; } - vis_utils::toNavMap(occ_grid, map_.map); - // publish map as current - map_.map.header.stamp = scan_timestamped; - sst_->publish( - std::move(std::make_unique(map_.map))); - sstm_->publish( - std::move(std::make_unique(map_.map.info))); + map_.map.header.stamp = scan_header.stamp; + sst_->publish(std::move(std::make_unique(map_.map))); + sstm_->publish(std::move(std::make_unique(map_.map.info))); delete occ_grid; occ_grid = nullptr; @@ -385,10 +454,8 @@ bool SlamToolbox::updateMap() /*****************************************************************************/ tf2::Stamped SlamToolbox::setTransformFromPoses( - const Pose2 & corrected_pose, - const Pose2 & odom_pose, - const rclcpp::Time & t, - const bool & update_reprocessing_transform) + const Pose2& corrected_pose, const Pose2& odom_pose, const rclcpp::Time& t, + const bool& update_reprocessing_transform) /*****************************************************************************/ { // Compute the map->odom transform @@ -396,26 +463,28 @@ tf2::Stamped SlamToolbox::setTransformFromPoses( tf2::Quaternion q(0., 0., 0., 1.0); q.setRPY(0., 0., corrected_pose.GetHeading()); tf2::Stamped base_to_map( - tf2::Transform(q, tf2::Vector3(corrected_pose.GetX(), - corrected_pose.GetY(), 0.0)).inverse(), tf2_ros::fromMsg(t), base_frame_); - try { + tf2::Transform(q, tf2::Vector3(corrected_pose.GetX(), corrected_pose.GetY(), 0.0)).inverse(), + tf2_ros::fromMsg(t), base_frame_); + try + { geometry_msgs::msg::TransformStamped base_to_map_msg, odom_to_map_msg; // https://github.com/ros2/geometry2/issues/176 // not working for some reason... // base_to_map_msg = tf2::toMsg(base_to_map); - base_to_map_msg.header.stamp = tf2_ros::toMsg(base_to_map.stamp_); - base_to_map_msg.header.frame_id = base_to_map.frame_id_; + base_to_map_msg.header.stamp = tf2_ros::toMsg(base_to_map.stamp_); + base_to_map_msg.header.frame_id = base_to_map.frame_id_; base_to_map_msg.transform.translation.x = base_to_map.getOrigin().getX(); base_to_map_msg.transform.translation.y = base_to_map.getOrigin().getY(); base_to_map_msg.transform.translation.z = base_to_map.getOrigin().getZ(); - base_to_map_msg.transform.rotation = tf2::toMsg(base_to_map.getRotation()); + base_to_map_msg.transform.rotation = tf2::toMsg(base_to_map.getRotation()); odom_to_map_msg = tf_->transform(base_to_map_msg, odom_frame_); tf2::fromMsg(odom_to_map_msg, odom_to_map); - } catch (tf2::TransformException & e) { - RCLCPP_ERROR(get_logger(), "Transform from base_link to odom failed: %s", - e.what()); + } + catch (tf2::TransformException& e) + { + RCLCPP_ERROR(get_logger(), "Transform from base_link to odom failed: %s", e.what()); return odom_to_map; } @@ -423,158 +492,215 @@ tf2::Stamped SlamToolbox::setTransformFromPoses( // estimate the homogenous transformation between the old and new // odometry frames and transform the new session // into the older session's frame - if (update_reprocessing_transform) { + if (update_reprocessing_transform) + { tf2::Transform odom_to_base_serialized = base_to_map.inverse(); tf2::Quaternion q1(0., 0., 0., 1.0); q1.setRPY(0., 0., tf2::getYaw(odom_to_base_serialized.getRotation())); odom_to_base_serialized.setRotation(q1); tf2::Transform odom_to_base_current = smapper_->toTfPose(odom_pose); - reprocessing_transform_ = - odom_to_base_serialized * odom_to_base_current.inverse(); + reprocessing_transform_ = odom_to_base_serialized * odom_to_base_current.inverse(); } // set map to odom for our transformation thread to publish boost::mutex::scoped_lock lock(map_to_odom_mutex_); - map_to_odom_ = tf2::Transform(tf2::Quaternion(odom_to_map.getRotation() ), - tf2::Vector3(odom_to_map.getOrigin() ) ).inverse(); + map_to_odom_ = tf2::Transform(tf2::Quaternion(odom_to_map.getRotation()), + tf2::Vector3(odom_to_map.getOrigin())) + .inverse(); return odom_to_map; } /*****************************************************************************/ -LocalizedRangeScan * SlamToolbox::getLocalizedRangeScan( - LaserRangeFinder * laser, - const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan, - Pose2 & odom_pose) +LocalizedRangeScan* SlamToolbox::getLocalizedRangeScan( + LaserRangeFinder* laser, const sensor_msgs::msg::LaserScan::ConstSharedPtr& scan, + Pose2& odom_pose) /*****************************************************************************/ { // Create a vector of doubles for lib - std::vector readings = laser_utils::scanToReadings( - *scan, lasers_[scan->header.frame_id].isInverted()); + std::vector readings = + laser_utils::scanToReadings(*scan, lasers_[scan->header.frame_id].isInverted()); // transform by the reprocessing transform - tf2::Transform pose_original = smapper_->toTfPose(odom_pose); + tf2::Transform pose_original = smapper_->toTfPose(odom_pose); tf2::Transform tf_pose_transformed = reprocessing_transform_ * pose_original; - Pose2 transformed_pose = smapper_->toKartoPose(tf_pose_transformed); + Pose2 transformed_pose = smapper_->toKartoPose(tf_pose_transformed); // create localized range scan - LocalizedRangeScan * range_scan = new LocalizedRangeScan( - laser->GetName(), readings); + LocalizedRangeScan* range_scan = new LocalizedRangeScan(laser->GetName(), readings); range_scan->SetOdometricPose(transformed_pose); range_scan->SetCorrectedPose(transformed_pose); - range_scan->SetTime(rclcpp::Time(scan->header.stamp).nanoseconds()/1.e9); + range_scan->SetTime(rclcpp::Time(scan->header.stamp).nanoseconds() / 1.e9); return range_scan; } /*****************************************************************************/ -bool SlamToolbox::shouldProcessScan( - const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan, - const Pose2 & pose) +bool SlamToolbox::shouldProcessScan(const sensor_msgs::msg::LaserScan::ConstSharedPtr& scan, + const Pose2& pose) /*****************************************************************************/ { static Pose2 last_pose; static rclcpp::Time last_scan_time = rclcpp::Time(0.); - static double min_dist2 = - smapper_->getMapper()->getParamMinimumTravelDistance() * - smapper_->getMapper()->getParamMinimumTravelDistance(); + static double min_dist2 = smapper_->getMapper()->getParamMinimumTravelDistance() * + smapper_->getMapper()->getParamMinimumTravelDistance(); static int scan_ctr = 0; scan_ctr++; // we give it a pass on the first measurement to get the ball rolling - if (first_measurement_) { - last_scan_time = scan->header.stamp; - last_pose = pose; + if (first_measurement_) + { + last_scan_time = scan->header.stamp; + last_pose = pose; first_measurement_ = false; return true; } // we are in a paused mode, reject incomming information - if (isPaused(NEW_MEASUREMENTS)) { + if (isPaused(NEW_MEASUREMENTS)) + { return false; } // throttled out - if ((scan_ctr % throttle_scans_) != 0) { + if ((scan_ctr % throttle_scans_) != 0) + { return false; } // not enough time - if (rclcpp::Time(scan->header.stamp) - last_scan_time < minimum_time_interval_) { + if (rclcpp::Time(scan->header.stamp) - last_scan_time < minimum_time_interval_) + { return false; } // check moved enough, within 10% for correction error const double dist2 = last_pose.SquaredDistance(pose); - if (dist2 < 0.8 * min_dist2 || scan_ctr < 5) { + if (dist2 < 0.8 * min_dist2 || scan_ctr < 5) + { return false; } - last_pose = pose; + last_pose = pose; last_scan_time = scan->header.stamp; return true; } /*****************************************************************************/ -LocalizedRangeScan * SlamToolbox::addScan( - LaserRangeFinder * laser, - PosedScan & scan_w_pose) +LocalizedRangeScan* SlamToolbox::addScan(LaserRangeFinder* laser, PosedScan& scan_w_pose) /*****************************************************************************/ { return addScan(laser, scan_w_pose.scan, scan_w_pose.pose); } /*****************************************************************************/ -LocalizedRangeScan * SlamToolbox::addScan( - LaserRangeFinder * laser, - const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan, - Pose2 & odom_pose) +LocalizedRangeScan* SlamToolbox::addScan(LaserRangeFinder* laser, + const sensor_msgs::msg::LaserScan::ConstSharedPtr& scan, + Pose2& odom_pose) /*****************************************************************************/ { + static rclcpp::Time last_match_time = rclcpp::Time(0.); + // get our localized range scan - LocalizedRangeScan * range_scan = getLocalizedRangeScan( - laser, scan, odom_pose); + LocalizedRangeScan* range_scan = getLocalizedRangeScan(laser, scan, odom_pose); // Add the localized range scan to the smapper boost::mutex::scoped_lock lock(smapper_mutex_); bool processed = false, update_reprocessing_transform = false; - if (processor_type_ == PROCESS) { - processed = smapper_->getMapper()->Process(range_scan); - } else if (processor_type_ == PROCESS_FIRST_NODE) { - processed = smapper_->getMapper()->ProcessAtDock(range_scan); - processor_type_ = PROCESS; + Matrix3 covariance; + covariance.SetToIdentity(); + + // whether or not the scan was processed as only a scan match without updating + // the graph and scan buffer + bool match_only = false; + + if (processor_type_ == PROCESS) + { + boost::mutex::scoped_lock l(pose_mutex_); + if (process_near_pose_) + { + range_scan->SetOdometricPose(*process_near_pose_); + range_scan->SetCorrectedPose(range_scan->GetOdometricPose()); + process_near_pose_.reset(nullptr); + } + processed = smapper_->getMapper()->Process(range_scan, &covariance); + + // if the scan was not processed into the map because of insuffcient travel + // distance, then check if enough time as passed to just perform a scan + // match without updating the graph or scan buffer + rclcpp::Time stamp = scan->header.stamp; + bool match_only = !processed && maximum_match_interval_.seconds() >= 0.0 && + stamp - last_match_time > maximum_match_interval_; + if (match_only) + { + processed = smapper_->getMapper()->Process(range_scan, &covariance, true); + } + } + else if (processor_type_ == PROCESS_FIRST_NODE) + { + processed = smapper_->getMapper()->ProcessAtDock(range_scan, &covariance); + processor_type_ = PROCESS; update_reprocessing_transform = true; - } else if (processor_type_ == PROCESS_NEAR_REGION) { + } + else if (processor_type_ == PROCESS_NEAR_REGION || + (processor_type_ == PROCESS_LOCALIZATION && process_near_pose_)) + { boost::mutex::scoped_lock l(pose_mutex_); - if (!process_near_pose_) { + if (!process_near_pose_) + { RCLCPP_ERROR(get_logger(), "Process near region called without a " - "valid region request. Ignoring scan."); + "valid region request. Ignoring scan."); return nullptr; } range_scan->SetOdometricPose(*process_near_pose_); range_scan->SetCorrectedPose(range_scan->GetOdometricPose()); process_near_pose_.reset(nullptr); - processed = smapper_->getMapper()->ProcessAgainstNodesNearBy(range_scan); + processed = smapper_->getMapper()->ProcessAgainstNodesNearBy(range_scan, false, &covariance); update_reprocessing_transform = true; - processor_type_ = PROCESS; - } else { - RCLCPP_FATAL(get_logger(), - "SlamToolbox: No valid processor type set! Exiting."); + + if (processor_type_ != PROCESS_LOCALIZATION) + { + processor_type_ = PROCESS; + } + } + else if (processor_type_ == PROCESS_LOCALIZATION) + { + processed = smapper_->getMapper()->ProcessLocalization(range_scan, &covariance, match_only); + + rclcpp::Time stamp = scan->header.stamp; + bool match_only = !processed && maximum_match_interval_.seconds() >= 0.0 && + stamp - last_match_time > maximum_match_interval_; + if (match_only) + { + processed = smapper_->getMapper()->ProcessLocalization(range_scan, &covariance, true); + } + } + else + { + RCLCPP_FATAL(get_logger(), "SlamToolbox: No valid processor type set! Exiting."); exit(-1); } // if successfully processed, create odom to map transformation // and add our scan to storage - if (processed) { - if (enable_interactive_mode_) { - scan_holder_->addScan(*scan); + if (processed) + { + last_match_time = scan->header.stamp; + + setTransformFromPoses(range_scan->GetCorrectedPose(), odom_pose, scan->header.stamp, + update_reprocessing_transform); + if (processor_type_ != PROCESS_LOCALIZATION) + { + // localization bookkeeping clashes with dataset bookkeeping, so best to + // avoid using them together. + dataset_->Add(range_scan); } - setTransformFromPoses(range_scan->GetCorrectedPose(), odom_pose, - scan->header.stamp, update_reprocessing_transform); - dataset_->Add(range_scan); - } else { + publishPose(range_scan->GetCorrectedPose(), covariance, scan->header.stamp); + } + else + { delete range_scan; range_scan = nullptr; } @@ -582,18 +708,49 @@ LocalizedRangeScan * SlamToolbox::addScan( return range_scan; } +/*****************************************************************************/ +void SlamToolbox::publishPose(const Pose2& pose, const Matrix3& cov, const rclcpp::Time& t) +/*****************************************************************************/ +{ + geometry_msgs::msg::PoseWithCovarianceStamped pose_msg; + pose_msg.header.stamp = t; + pose_msg.header.frame_id = map_frame_; + + tf2::Quaternion q(0., 0., 0., 1.0); + q.setRPY(0., 0., pose.GetHeading()); + tf2::Transform transform(q, tf2::Vector3(pose.GetX(), pose.GetY(), 0.0)); + tf2::toMsg(transform, pose_msg.pose.pose); + + pose_msg.pose.covariance[0] = cov(0, 0) * position_covariance_scale_; // x + pose_msg.pose.covariance[1] = cov(0, 1) * position_covariance_scale_; // xy + pose_msg.pose.covariance[6] = cov(1, 0) * position_covariance_scale_; // xy + pose_msg.pose.covariance[7] = cov(1, 1) * position_covariance_scale_; // y + pose_msg.pose.covariance[35] = cov(2, 2) * yaw_covariance_scale_; // yaw + + pose_pub_->publish(pose_msg); + + std_msgs::msg::Bool status; + status.data = true; + status_pub_->publish(status); +} + /*****************************************************************************/ bool SlamToolbox::mapCallback( const std::shared_ptr request_header, - const std::shared_ptr req, - std::shared_ptr res) + const std::shared_ptr req, + std::shared_ptr res) /*****************************************************************************/ { - if (map_.map.info.width && map_.map.info.height) { + if (map_.map.info.width && map_.map.info.height) + { boost::mutex::scoped_lock lock(smapper_mutex_); - *res = map_; + maidbot_msg_utils::compressOccupancyGrid(map_.map, res->map); + res->success = true; return true; - } else { + } + else + { + res->success = false; return false; } } @@ -601,23 +758,17 @@ bool SlamToolbox::mapCallback( /*****************************************************************************/ bool SlamToolbox::pauseNewMeasurementsCallback( const std::shared_ptr request_header, - const std::shared_ptr req, - std::shared_ptr resp) + const std::shared_ptr req, + std::shared_ptr resp) /*****************************************************************************/ { - bool curr_state = isPaused(NEW_MEASUREMENTS); - state_.set(NEW_MEASUREMENTS, !curr_state); - - this->set_parameter({"paused_new_measurements", !curr_state}); - RCLCPP_INFO(get_logger(), "SlamToolbox: Toggled to %s", - !curr_state ? "pause taking new measurements." : - "actively taking new measurements."); - resp->status = true; + toggleScanProcessing(); + resp->success = true; return true; } /*****************************************************************************/ -bool SlamToolbox::isPaused(const PausedApplication & app) +bool SlamToolbox::isPaused(const PausedApplication& app) /*****************************************************************************/ { return state_.get(app); @@ -630,23 +781,28 @@ bool SlamToolbox::serializePoseGraphCallback( std::shared_ptr resp) /*****************************************************************************/ { + if (!slam_running_) + { + RCLCPP_WARN(get_logger(), "serializePoseGraphCallback: Ignoring request to serialize pose " + "graph because SLAM is not running."); + return false; + } std::string filename = req->filename; // if we're inside the snap, we need to write to commonly accessible space - if (snap_utils::isInSnap()) { + if (snap_utils::isInSnap()) + { filename = snap_utils::getSnapPath() + std::string("/") + filename; } boost::mutex::scoped_lock lock(smapper_mutex_); - serialization::write(filename, *smapper_->getMapper(), - *dataset_, shared_from_this()); + serialization::write(filename, *smapper_->getMapper(), *dataset_, shared_from_this()); return true; } /*****************************************************************************/ -void SlamToolbox::loadSerializedPoseGraph( - std::unique_ptr & mapper, - std::unique_ptr & dataset) +void SlamToolbox::loadSerializedPoseGraph(std::unique_ptr& mapper, + std::unique_ptr& dataset) /*****************************************************************************/ { boost::mutex::scoped_lock lock(smapper_mutex_); @@ -654,21 +810,26 @@ void SlamToolbox::loadSerializedPoseGraph( solver_->Reset(); // add the nodes and constraints to the optimizer - VerticeMap mapper_vertices = mapper->GetGraph()->GetVertices(); + VerticeMap mapper_vertices = mapper->GetGraph()->GetVertices(); VerticeMap::iterator vertex_map_it = mapper_vertices.begin(); - for (vertex_map_it; vertex_map_it != mapper_vertices.end(); ++vertex_map_it) { + for (vertex_map_it; vertex_map_it != mapper_vertices.end(); ++vertex_map_it) + { ScanMap::iterator vertex_it = vertex_map_it->second.begin(); - for (vertex_it; vertex_it != vertex_map_it->second.end(); ++vertex_it) { - if (vertex_it->second != nullptr) { + for (vertex_it; vertex_it != vertex_map_it->second.end(); ++vertex_it) + { + if (vertex_it->second != nullptr) + { solver_->AddNode(vertex_it->second); } } } - EdgeVector mapper_edges = mapper->GetGraph()->GetEdges(); + EdgeVector mapper_edges = mapper->GetGraph()->GetEdges(); EdgeVector::iterator edges_it = mapper_edges.begin(); - for (edges_it; edges_it != mapper_edges.end(); ++edges_it) { - if (*edges_it != nullptr) { + for (edges_it; edges_it != mapper_edges.end(); ++edges_it) + { + if (*edges_it != nullptr) + { solver_->AddConstraint(*edges_it); } } @@ -678,32 +839,35 @@ void SlamToolbox::loadSerializedPoseGraph( // move the memory to our working dataset smapper_->setMapper(mapper.release()); smapper_->configure(shared_from_this()); + closure_assistant_->setMapper(smapper_->getMapper()); dataset_.reset(dataset.release()); - if (!smapper_->getMapper()) { - RCLCPP_FATAL(get_logger(), - "loadSerializedPoseGraph: Could not properly load " - "a valid mapping object. Did you modify something by hand?"); + if (!smapper_->getMapper()) + { + RCLCPP_FATAL(get_logger(), "loadSerializedPoseGraph: Could not properly load " + "a valid mapping object. Did you modify something by hand?"); exit(-1); } - if (dataset_->GetLasers().size() < 1) { + if (dataset_->GetLasers().size() < 1) + { RCLCPP_FATAL(get_logger(), "loadSerializedPoseGraph: Cannot deserialize " - "dataset with no laser objects."); + "dataset with no laser objects."); exit(-1); } // create a current laser sensor - LaserRangeFinder * laser = - dynamic_cast( - dataset_->GetLasers()[0]); - Sensor * pSensor = dynamic_cast(laser); - if (pSensor) { + LaserRangeFinder* laser = dynamic_cast(dataset_->GetLasers()[0]); + Sensor* pSensor = dynamic_cast(laser); + if (pSensor) + { SensorManager::GetInstance()->RegisterSensor(pSensor); lasers_.clear(); - } else { + } + else + { RCLCPP_ERROR(get_logger(), "Invalid sensor pointer in dataset." - " Unable to register sensor."); + " Unable to register sensor."); } solver_->Compute(); @@ -716,59 +880,190 @@ bool SlamToolbox::deserializePoseGraphCallback( std::shared_ptr resp) /*****************************************************************************/ { - if (req->match_type == slam_toolbox::srv::DeserializePoseGraph::Request::UNSET) { + if (req->match_type == slam_toolbox::srv::DeserializePoseGraph::Request::UNSET) + { RCLCPP_ERROR(get_logger(), "Deserialization called without valid" - " processor type set. Undefined behavior!"); + " processor type set. Undefined behavior!"); + return false; + } + + if (slam_running_) + { + RCLCPP_WARN(get_logger(), "deserializePoseGraphCallback: Ignoring request to deserialize pose " + "graph because SLAM is running."); return false; } std::string filename = req->filename; - if (filename.empty()) { + if (filename.empty()) + { RCLCPP_WARN(get_logger(), "No map file given!"); return true; } // if we're inside the snap, we need to write to commonly accessible space - if (snap_utils::isInSnap()) { + if (snap_utils::isInSnap()) + { filename = snap_utils::getSnapPath() + std::string("/") + filename; } std::unique_ptr dataset = std::make_unique(); - std::unique_ptr mapper = std::make_unique(); - - if (!serialization::read(filename, *mapper, *dataset, shared_from_this())) { - RCLCPP_ERROR(get_logger(), "DeserializePoseGraph: Failed to read " - "file: %s.", filename.c_str()); + std::unique_ptr mapper = std::make_unique(); + + if (!serialization::read(filename, *mapper, *dataset, shared_from_this())) + { + RCLCPP_ERROR(get_logger(), + "DeserializePoseGraph: Failed to read " + "file: %s.", + filename.c_str()); return true; } - RCLCPP_DEBUG(get_logger(), "DeserializePoseGraph: Successfully read file."); + RCLCPP_INFO(get_logger(), "DeserializePoseGraph: Successfully read file."); loadSerializedPoseGraph(mapper, dataset); updateMap(); first_measurement_ = true; boost::mutex::scoped_lock l(pose_mutex_); - switch (req->match_type) { + switch (req->match_type) + { case procType::START_AT_FIRST_NODE: processor_type_ = PROCESS_FIRST_NODE; break; case procType::START_AT_GIVEN_POSE: processor_type_ = PROCESS_NEAR_REGION; - process_near_pose_ = std::make_unique(req->initial_pose.x, - req->initial_pose.y, req->initial_pose.theta); + process_near_pose_ = + std::make_unique(req->initial_pose.x, req->initial_pose.y, req->initial_pose.theta); break; case procType::LOCALIZE_AT_POSE: processor_type_ = PROCESS_LOCALIZATION; - process_near_pose_ = std::make_unique(req->initial_pose.x, - req->initial_pose.y, req->initial_pose.theta); + process_near_pose_ = + std::make_unique(req->initial_pose.x, req->initial_pose.y, req->initial_pose.theta); break; default: - RCLCPP_FATAL(get_logger(), - "Deserialization called without valid processor type set."); + RCLCPP_FATAL(get_logger(), "Deserialization called without valid processor type set."); } return true; } -} // namespace slam_toolbox +/*****************************************************************************/ +void SlamToolbox::toggleScanProcessing() +/*****************************************************************************/ +{ + bool curr_state = isPaused(NEW_MEASUREMENTS); + state_.set(NEW_MEASUREMENTS, !curr_state); + + if (isPaused(NEW_MEASUREMENTS)) + { + scan_sub_.reset(); + } + else + { + scan_sub_ = create_subscription( + scan_topic_, rclcpp::SensorDataQoS().keep_last(1), + std::bind(&SlamToolbox::laserCallback, this, std::placeholders::_1)); + } + + this->set_parameter({"paused_new_measurements", !curr_state}); + RCLCPP_INFO(get_logger(), "SlamToolbox: Toggled to %s", + !curr_state ? "pause taking new measurements." : "actively taking new measurements."); +} +/*****************************************************************************/ +void SlamToolbox::startSlamCallback(const std::shared_ptr req, + std::shared_ptr resp) +/*****************************************************************************/ +{ + if (slam_running_) + { + RCLCPP_WARN(get_logger(), "SLAM was already started!"); + resp->success = false; + return; + } + if (isPaused(NEW_MEASUREMENTS)) + { + toggleScanProcessing(); + } + slam_running_ = true; + resp->success = true; +} +/*****************************************************************************/ +void SlamToolbox::stopSlamCallback(const std::shared_ptr req, + std::shared_ptr resp) +/*****************************************************************************/ +{ + if (!slam_running_) + { + RCLCPP_WARN(get_logger(), "SLAM was already stopped!"); + resp->success = false; + return; + } + if (!isPaused(NEW_MEASUREMENTS)) + { + toggleScanProcessing(); + } + resetSlam(); + slam_running_ = false; + resp->success = true; +} +/*****************************************************************************/ +void SlamToolbox::resetSlam() +/*****************************************************************************/ +{ + const bool paused_before_reset = isPaused(NEW_MEASUREMENTS); + + // pause new scan processing + if (!isPaused(NEW_MEASUREMENTS)) + { + toggleScanProcessing(); + } + + RCLCPP_WARN(get_logger(), "Starting SLAM reset."); + boost::mutex::scoped_lock lock(smapper_mutex_); + if (smapper_ && processor_type_ == ProcessType::PROCESS_LOCALIZATION) + { + smapper_->clearLocalizationBuffer(); + } + + dataset_->Clear(); + + std::unique_ptr mapper = std::make_unique(); + + solver_->Reset(); + mapper->SetScanSolver(solver_.get()); + + // move the memory to our working dataset + smapper_->setMapper(mapper.release()); + smapper_->configure(shared_from_this()); + // reset the closure_assistant's mapper (since its address just changed) + closure_assistant_->setMapper(smapper_->getMapper()); + + // reset slam_toolbox bookkeeping + lasers_.clear(); + scan_holder_->clearScans(); + first_measurement_ = true; + // no need to lock since we paused scan callbacks + processor_type_ = PROCESS; + process_near_pose_.reset(); + + nav_msgs::msg::OccupancyGrid& og = map_.map; + og.info.resolution = resolution_; + og.info.origin.position.x = 0.0; + og.info.origin.position.y = 0.0; + og.info.origin.position.z = 0.0; + og.info.origin.orientation.x = 0.0; + og.info.origin.orientation.y = 0.0; + og.info.origin.orientation.z = 0.0; + og.info.origin.orientation.w = 1.0; + og.header.frame_id = map_frame_; + + // resume new scan processing (unless it was already paused) + if (isPaused(NEW_MEASUREMENTS) && !paused_before_reset) + { + toggleScanProcessing(); + } + RCLCPP_WARN(get_logger(), "Finished SLAM reset."); +} + +} // namespace slam_toolbox diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp index 749c1b247..c5442caa2 100644 --- a/src/slam_toolbox_localization.cpp +++ b/src/slam_toolbox_localization.cpp @@ -31,17 +31,14 @@ LocalizationSlamToolbox::LocalizationSlamToolbox(rclcpp::NodeOptions options) processor_type_ = PROCESS_LOCALIZATION; localization_pose_sub_ = this->create_subscription( - "/initialpose", 1, + "initialpose", 1, std::bind(&LocalizationSlamToolbox::localizePoseCallback, this, std::placeholders::_1)); clear_localization_ = this->create_service( - "/slam_toolbox/clear_localization_buffer", + "slam_toolbox/clear_localization_buffer", std::bind(&LocalizationSlamToolbox::clearLocalizationBuffer, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - // in localization mode, we cannot allow for interactive mode - enable_interactive_mode_ = false; - // in localization mode, disable map saver map_saver_.reset(); } @@ -118,8 +115,12 @@ void LocalizationSlamToolbox::laserCallback( sensor_msgs::msg::LaserScan::ConstSharedPtr scan) /*****************************************************************************/ { - // store scan timestamped - scan_timestamped = scan->header.stamp; + if (!waitForTransform(scan->header.frame_id, scan->header.stamp)) { + return; + } + + // store scan header + scan_header = scan->header; // no odom info Pose2 pose; if (!pose_helper_->getOdomPose(pose, scan->header.stamp)) { @@ -160,6 +161,10 @@ LocalizedRangeScan * LocalizationSlamToolbox::addScan( // Add the localized range scan to the smapper boost::mutex::scoped_lock lock(smapper_mutex_); bool processed = false, update_reprocessing_transform = false; + + Matrix3 covariance; + covariance.SetToIdentity(); + if (processor_type_ == PROCESS_NEAR_REGION) { if (!process_near_pose_) { RCLCPP_ERROR(get_logger(), @@ -172,13 +177,13 @@ LocalizedRangeScan * LocalizationSlamToolbox::addScan( range_scan->SetOdometricPose(*process_near_pose_); range_scan->SetCorrectedPose(range_scan->GetOdometricPose()); process_near_pose_.reset(nullptr); - processed = smapper_->getMapper()->ProcessAgainstNodesNearBy(range_scan, true); + processed = smapper_->getMapper()->ProcessAgainstNodesNearBy(range_scan, true, &covariance); // reset to localization mode update_reprocessing_transform = true; processor_type_ = PROCESS_LOCALIZATION; } else if (processor_type_ == PROCESS_LOCALIZATION) { - processed = smapper_->getMapper()->ProcessLocalization(range_scan); + processed = smapper_->getMapper()->ProcessLocalization(range_scan, &covariance); update_reprocessing_transform = false; } else { RCLCPP_FATAL(get_logger(), "LocalizationSlamToolbox: " @@ -194,6 +199,8 @@ LocalizedRangeScan * LocalizationSlamToolbox::addScan( // compute our new transform setTransformFromPoses(range_scan->GetCorrectedPose(), odom_pose, scan->header.stamp, update_reprocessing_transform); + + publishPose(range_scan->GetCorrectedPose(), covariance, scan->header.stamp); } return range_scan; diff --git a/src/slam_toolbox_localization_node.cpp b/src/slam_toolbox_localization_node.cpp index 8a9acfe97..a38ec9879 100644 --- a/src/slam_toolbox_localization_node.cpp +++ b/src/slam_toolbox_localization_node.cpp @@ -17,23 +17,25 @@ /* Author: Steven Macenski */ -#include #include "slam_toolbox/slam_toolbox_localization.hpp" +#include -int main(int argc, char ** argv) +int main(int argc, char** argv) { rclcpp::init(argc, argv); int stack_size = 40000000; { auto temp_node = std::make_shared("slam_toolbox"); - temp_node->declare_parameter("stack_size_to_use"); - if (temp_node->get_parameter("stack_size_to_use", stack_size)) { + temp_node->declare_parameter("stack_size_to_use", rclcpp::ParameterType::PARAMETER_INTEGER); + if (temp_node->get_parameter("stack_size_to_use", stack_size)) + { RCLCPP_INFO(temp_node->get_logger(), "Node using stack size %i", (int)stack_size); const rlim_t max_stack_size = stack_size; struct rlimit stack_limit; getrlimit(RLIMIT_STACK, &stack_limit); - if (stack_limit.rlim_cur < stack_size) { + if (stack_limit.rlim_cur < stack_size) + { stack_limit.rlim_cur = stack_size; } setrlimit(RLIMIT_STACK, &stack_limit); diff --git a/src/slam_toolbox_sync.cpp b/src/slam_toolbox_sync.cpp index 43f5bd188..bdcc4018c 100644 --- a/src/slam_toolbox_sync.cpp +++ b/src/slam_toolbox_sync.cpp @@ -28,7 +28,7 @@ SynchronousSlamToolbox::SynchronousSlamToolbox(rclcpp::NodeOptions options) : SlamToolbox(options) /*****************************************************************************/ { - ssClear_ = this->create_service("/slam_toolbox/clear_queue", + ssClear_ = this->create_service("slam_toolbox/clear_queue", std::bind(&SynchronousSlamToolbox::clearQueueCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); @@ -65,8 +65,12 @@ void SynchronousSlamToolbox::laserCallback( sensor_msgs::msg::LaserScan::ConstSharedPtr scan) /*****************************************************************************/ { - // store scan timestamped - scan_timestamped = scan->header.stamp; + if (!waitForTransform(scan->header.frame_id, scan->header.stamp)) { + return; + } + + // store scan header + scan_header = scan->header; // no odom info Pose2 pose; if (!pose_helper_->getOdomPose(pose, scan->header.stamp)) { diff --git a/src/slam_toolbox_sync_node.cpp b/src/slam_toolbox_sync_node.cpp index 07088ff02..149a63908 100644 --- a/src/slam_toolbox_sync_node.cpp +++ b/src/slam_toolbox_sync_node.cpp @@ -17,23 +17,25 @@ /* Author: Steven Macenski */ -#include #include "slam_toolbox/slam_toolbox_sync.hpp" +#include -int main(int argc, char ** argv) +int main(int argc, char** argv) { rclcpp::init(argc, argv); int stack_size = 40000000; { auto temp_node = std::make_shared("slam_toolbox"); - temp_node->declare_parameter("stack_size_to_use"); - if (temp_node->get_parameter("stack_size_to_use", stack_size)) { + temp_node->declare_parameter("stack_size_to_use", rclcpp::ParameterType::PARAMETER_INTEGER); + if (temp_node->get_parameter("stack_size_to_use", stack_size)) + { RCLCPP_INFO(temp_node->get_logger(), "Node using stack size %i", (int)stack_size); const rlim_t max_stack_size = stack_size; struct rlimit stack_limit; getrlimit(RLIMIT_STACK, &stack_limit); - if (stack_limit.rlim_cur < stack_size) { + if (stack_limit.rlim_cur < stack_size) + { stack_limit.rlim_cur = stack_size; } setrlimit(RLIMIT_STACK, &stack_limit); diff --git a/srvs/Pause.srv b/srvs/Pause.srv deleted file mode 100644 index 24054a6a5..000000000 --- a/srvs/Pause.srv +++ /dev/null @@ -1,4 +0,0 @@ -# trigger pause toggle - ---- -bool status \ No newline at end of file diff --git a/srvs/SaveMap.srv b/srvs/SaveMap.srv index f38351323..c3a85f0d3 100644 --- a/srvs/SaveMap.srv +++ b/srvs/SaveMap.srv @@ -1,2 +1,8 @@ std_msgs/String name --- +# Result code defintions +uint8 RESULT_SUCCESS=0 +uint8 RESULT_NO_MAP_RECEIEVD=1 +uint8 RESULT_UNDEFINED_FAILURE=255 + +uint8 result diff --git a/srvs/SerializePoseGraph.srv b/srvs/SerializePoseGraph.srv index 811117977..398b323e7 100644 --- a/srvs/SerializePoseGraph.srv +++ b/srvs/SerializePoseGraph.srv @@ -1,2 +1,7 @@ string filename ---- \ No newline at end of file +--- +# Result code defintions +uint8 RESULT_SUCCESS=0 +uint8 RESULT_FAILED_TO_WRITE_FILE=255 + +uint8 result \ No newline at end of file