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/CMakeLists.txt b/CMakeLists.txt index 80f9eef25..1aa2654b8 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,28 +45,21 @@ 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) @@ -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}) @@ -165,9 +137,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 +157,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 +176,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..712cb650c 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 @@ -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..52bbcb936 100644 --- a/config/mapper_params_localization.yaml +++ b/config/mapper_params_localization.yaml @@ -35,19 +35,19 @@ slam_toolbox: minimum_travel_distance: 0.5 minimum_travel_heading: 0.5 scan_buffer_size: 3 - scan_buffer_maximum_scan_distance: 10.0 - link_match_minimum_response_fine: 0.1 + scan_buffer_maximum_scan_distance: 5.0 + 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.0349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0698 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..5cd08f287 100644 --- a/include/slam_toolbox/get_pose_helper.hpp +++ b/include/slam_toolbox/get_pose_helper.hpp @@ -22,7 +22,7 @@ #include #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" #include "slam_toolbox/toolbox_types.hpp" -#include "../lib/karto_sdk/include/karto_sdk/Mapper.h" +#include "karto_sdk/Mapper.h" namespace pose_utils { @@ -59,6 +59,29 @@ class GetPoseHelper 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_; std::string base_frame_, odom_frame_; 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..7384daa9a 100644 --- a/include/slam_toolbox/loop_closure_assistant.hpp +++ b/include/slam_toolbox/loop_closure_assistant.hpp @@ -28,8 +28,6 @@ #include "tf2_ros/transform_broadcaster.h" #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 +47,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 +59,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..42d52f481 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 { diff --git a/include/slam_toolbox/slam_toolbox_common.hpp b/include/slam_toolbox/slam_toolbox_common.hpp index 9274ba545..e1129f4b1 100644 --- a/include/slam_toolbox/slam_toolbox_common.hpp +++ b/include/slam_toolbox/slam_toolbox_common.hpp @@ -37,10 +37,14 @@ #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 "std_msgs/msg/bool.hpp" +#include "std_srvs/srv/set_bool.hpp" +#include "std_srvs/srv/trigger.hpp" #include "pluginlib/class_loader.hpp" +#include "maidbot_std_srvs/srv/get_compressed_map.hpp" + #include "slam_toolbox/toolbox_types.hpp" #include "slam_toolbox/slam_mapper.hpp" #include "slam_toolbox/snap_utils.hpp" @@ -61,8 +65,8 @@ class SlamToolbox : public rclcpp::Node public: explicit SlamToolbox(rclcpp::NodeOptions); SlamToolbox(); - ~SlamToolbox(); - void configure(); + virtual ~SlamToolbox(); + virtual void configure(); virtual void loadPoseGraphByParams(); protected: @@ -74,13 +78,15 @@ class SlamToolbox : public rclcpp::Node void setParams(); void setSolver(); void setROSInterfaces(); + void resetSlam(); // callbacks virtual void laserCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr scan) = 0; + 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); + const std::shared_ptr req, + std::shared_ptr res); virtual bool serializePoseGraphCallback( const std::shared_ptr request_header, const std::shared_ptr req, @@ -89,6 +95,12 @@ 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 &); @@ -100,6 +112,7 @@ class SlamToolbox : public rclcpp::Node karto::Pose2 & karto_pose); karto::LocalizedRangeScan * addScan(karto::LaserRangeFinder * laser, PosedScan & scanWPose); bool updateMap(); + 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, @@ -114,37 +127,48 @@ class SlamToolbox : public rclcpp::Node 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); + 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_; 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..8216d01c6 100644 --- a/include/slam_toolbox/toolbox_types.hpp +++ b/include/slam_toolbox/toolbox_types.hpp @@ -27,8 +27,7 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.h" #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..99b8135d6 100644 --- a/lib/karto_sdk/CMakeLists.txt +++ b/lib/karto_sdk/CMakeLists.txt @@ -18,8 +18,7 @@ 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,7 +26,7 @@ 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}) 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..5db0165d2 100644 --- a/lib/karto_sdk/include/karto_sdk/Mapper.h +++ b/lib/karto_sdk/include/karto_sdk/Mapper.h @@ -21,7 +21,7 @@ #include #include #include -#include +#include #include #include #include @@ -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 @@ -933,13 +933,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 +1057,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 +1493,7 @@ class KARTO_EXPORT ScanMatcher m_pCorrelationGrid(NULL), m_pSearchSpaceProbs(NULL), m_pGridLookup(NULL), + m_pPoseResponse(NULL), m_doPenalize(false) { } @@ -1519,13 +1530,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 +1946,7 @@ struct LocalizationScanVertex Vertex * vertex; }; -typedef std::queue LocalizationScanVertices; +typedef std::deque LocalizationScanVertices; class KARTO_EXPORT Mapper : public Module { @@ -1980,9 +2004,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 +2020,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 +2108,11 @@ class KARTO_EXPORT Mapper : public Module m_pGraph->CorrectPoses(); } + const LocalizationScanVertices& GetLocalizationVertices() + { + return m_LocalizationScanVertices; + } + protected: void InitializeParameters(); @@ -2342,16 +2383,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 +2423,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..e36bf2ecf 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,50 @@ 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; + // tbb::parallel_for_each(m_yPoses, (*this)); + + // SCAN MATCHING HERE. + for (const double y : m_yPoses) + { + (*this)(y); + } // 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 +837,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 +855,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 +895,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 +916,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 +938,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 +956,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 +979,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 +1024,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 +1081,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 +1102,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 +1124,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 +1134,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 +1143,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 +1152,8 @@ void ScanMatcher::AddScan( m_pCorrelationGrid->GetDataPointer()[gridIndex] = GridStates_Occupied; // smear grid - if (doSmear == true) { + if (doSmear == true) + { m_pCorrelationGrid->SmearPoint(gridPoint); } } @@ -1109,14 +1165,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 +1184,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 +1234,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 +1269,11 @@ kt_double ScanMatcher::GetResponse(kt_int32u angleIndex, kt_int32s gridPositionI return response; } - //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// -template +template class BreadthFirstTraversal : public GraphTraversal { public: @@ -1221,8 +1283,7 @@ class BreadthFirstTraversal : public GraphTraversal BreadthFirstTraversal() { } - explicit BreadthFirstTraversal(Graph * pGraph) - : GraphTraversal(pGraph) + explicit BreadthFirstTraversal(Graph* pGraph) : GraphTraversal(pGraph) { } @@ -1240,12 +1301,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 +1320,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 +1358,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 +1372,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 +1400,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 +1418,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 +1438,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 +1465,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 +1495,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 +1518,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 +1537,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 +1560,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 +1574,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 +1631,101 @@ 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) +void MapperGraph::LinkScans(LocalizedRangeScan* pFromScan, LocalizedRangeScan* pToScan, + const Pose2& rMean, const Matrix3& rCovariance) { - kt_bool isNewEdge = true; - Edge * pEdge = AddEdge(pFromScan, pToScan, isNewEdge); + kt_bool isNewEdge = true; + Edge* pEdge = AddEdge(pFromScan, pToScan, isNewEdge); - if (pEdge == NULL) { + 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 +1733,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 +1746,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 +1761,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 +1782,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 +1823,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 +1872,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 +1911,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 +1944,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 +1985,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 +2022,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 +2040,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 +2055,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(); } } @@ -2011,14 +2103,16 @@ LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure( void MapperGraph::CorrectPoses() { // 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; } scan->SetCorrectedPoseAndUpdate(iter->second); @@ -2030,13 +2124,14 @@ void MapperGraph::CorrectPoses() 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 +2143,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 +2152,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 +2171,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 +2237,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 +2296,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 +2353,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 +2613,6 @@ void Mapper::setParamCorrelationSearchSpaceSmearDeviation(double d) m_pCorrelationSearchSpaceSmearDeviation->SetValue((kt_double)d); } - // Correlation Parameters - Loop Closure Parameters void Mapper::setParamLoopSearchSpaceDimension(double d) { @@ -2556,7 +2629,6 @@ void Mapper::setParamLoopSearchSpaceSmearDeviation(double d) m_pLoopSearchSpaceSmearDeviation->SetValue((kt_double)d); } - // Scan Matcher Parameters void Mapper::setParamDistanceVariancePenalty(double d) { @@ -2598,32 +2670,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 +2707,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 +2715,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 +2834,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 +2908,8 @@ kt_bool Mapper::ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool ad m_pMapperSensorManager->SetLastScan(pScan); - if (addScanToLocalizationBuffer) { + if (addScanToLocalizationBuffer) + { AddScanToLocalizationBuffer(pScan, scan_vertex); } @@ -2820,93 +2919,105 @@ 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); + } 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 +3025,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 +3107,7 @@ void Mapper::ClearLocalizationBuffer() oldLSV.scan = NULL; } - m_LocalizationScanVertices.pop(); + m_LocalizationScanVertices.pop_front(); } std::vector names = m_pMapperSensorManager->GetSensorNames(); @@ -2950,41 +3120,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 +3164,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 +3183,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 +3205,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 +3257,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 +3269,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 +3314,8 @@ const LocalizedRangeScanVector Mapper::GetAllProcessedScans() const { LocalizedRangeScanVector allScans; - if (m_pMapperSensorManager != NULL) { + if (m_pMapperSensorManager != NULL) + { allScans = m_pMapperSensorManager->GetAllScans(); } @@ -3144,7 +3326,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 +3335,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..ecd884707 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ slam_toolbox - 2.4.0 + 2.5.1 This package provides a sped up improved slam karto with updated SDK and visualization and modification toolsets @@ -14,33 +14,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 +42,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 +67,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..cc9949c4f 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_; } @@ -239,6 +246,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 +285,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 +360,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..79fb606ee 100644 --- a/src/experimental/slam_toolbox_lifelong_node.cpp +++ b/src/experimental/slam_toolbox_lifelong_node.cpp @@ -27,7 +27,7 @@ int main(int argc, char ** argv) int stack_size = 40000000; { auto temp_node = std::make_shared("slam_toolbox"); - temp_node->declare_parameter("stack_size_to_use"); + 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; 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..57024fd5d --- /dev/null +++ b/src/experimental/slam_toolbox_map_and_localization_node.cpp @@ -0,0 +1,47 @@ +/* + * 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 "slam_toolbox/experimental/slam_toolbox_map_and_localization.hpp" + +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..d93996f62 100644 --- a/src/laser_utils.cpp +++ b/src/laser_utils.cpp @@ -191,4 +191,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..84a7b6caf 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)); 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_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..0a7dff9b5 100644 --- a/src/slam_toolbox_async_node.cpp +++ b/src/slam_toolbox_async_node.cpp @@ -27,7 +27,7 @@ int main(int argc, char ** argv) int stack_size = 40000000; { auto temp_node = std::make_shared("slam_toolbox"); - temp_node->declare_parameter("stack_size_to_use"); + 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; diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index 5848720d3..87c27e700 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -23,6 +23,8 @@ #include "slam_toolbox/slam_toolbox_common.hpp" #include "slam_toolbox/serialization.hpp" +#include "maidbot_msg_utils/occupancy_grid_compression.h" + namespace slam_toolbox { @@ -41,7 +43,9 @@ SlamToolbox::SlamToolbox(rclcpp::NodeOptions options) first_measurement_(true), process_near_pose_(nullptr), transform_timeout_(rclcpp::Duration::from_seconds(0.5)), - minimum_time_interval_(std::chrono::nanoseconds(0)) + minimum_time_interval_(std::chrono::nanoseconds(0)), + maximum_match_interval_(rclcpp::Duration::from_seconds(-1.0)), + slam_running_(true) /*****************************************************************************/ { smapper_ = std::make_unique(); @@ -56,6 +60,12 @@ void SlamToolbox::configure() setROSInterfaces(); setSolver(); + // 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( @@ -73,9 +83,11 @@ void SlamToolbox::configure() 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))); + 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))); } @@ -145,15 +157,19 @@ 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); + 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); @@ -163,8 +179,13 @@ void SlamToolbox::setParams() } 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_}); + } } /*****************************************************************************/ @@ -182,36 +203,43 @@ void SlamToolbox::setROSInterfaces() tfL_ = std::make_unique(*tf_); tfB_ = std::make_unique(shared_from_this()); + 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", + 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", + 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", + "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::laserCallback, this, std::placeholders::_1)); + 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)); } /*****************************************************************************/ @@ -227,12 +255,16 @@ void SlamToolbox::publishTransformLoop( 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(); } @@ -259,8 +291,16 @@ void SlamToolbox::publishVisualizations() rclcpp::Rate r(1.0 / map_update_interval); while (rclcpp::ok()) { - updateMap(); + if (sst_->get_subscription_count() > 0) { + updateMap(); + sst_->publish( + std::move(std::make_unique(map_.map))); + sstm_->publish( + std::move(std::make_unique(map_.map.info))); + } + if (!isPaused(VISUALIZING_GRAPH)) { + boost::mutex::scoped_lock lock(smapper_mutex_); closure_assistant_->publishGraph(); } r.sleep(); @@ -301,8 +341,8 @@ bool SlamToolbox::shouldStartWithPoseGraph( { // 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()) { std::vector read_pose; @@ -357,26 +397,41 @@ LaserRangeFinder * SlamToolbox::getLaser( } /*****************************************************************************/ -bool SlamToolbox::updateMap() +bool SlamToolbox::waitForTransform(const std::string& scan_frame, const rclcpp::Time& stamp) /*****************************************************************************/ { - if (sst_->get_subscription_count() == 0) { - return true; + 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() +/*****************************************************************************/ +{ boost::mutex::scoped_lock lock(smapper_mutex_); 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; + + vis_utils::toNavMap(occ_grid, map_.map); delete occ_grid; occ_grid = nullptr; @@ -531,6 +586,8 @@ LocalizedRangeScan * SlamToolbox::addScan( 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); @@ -539,13 +596,37 @@ LocalizedRangeScan * SlamToolbox::addScan( boost::mutex::scoped_lock lock(smapper_mutex_); bool processed = false, update_reprocessing_transform = false; + 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) { - processed = smapper_->getMapper()->Process(range_scan); + 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); + 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_) { RCLCPP_ERROR(get_logger(), "Process near region called without a " @@ -555,9 +636,23 @@ LocalizedRangeScan * SlamToolbox::addScan( 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; + + 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."); @@ -567,13 +662,18 @@ LocalizedRangeScan * SlamToolbox::addScan( // if successfully processed, create odom to map transformation // and add our scan to storage if (processed) { - if (enable_interactive_mode_) { - scan_holder_->addScan(*scan); - } + last_match_time = scan->header.stamp; setTransformFromPoses(range_scan->GetCorrectedPose(), odom_pose, scan->header.stamp, update_reprocessing_transform); - dataset_->Add(range_scan); + if (processor_type_ != PROCESS_LOCALIZATION) + { + // localization bookkeeping clashes with dataset bookkeeping, so best to + // avoid using them together. + dataset_->Add(range_scan); + } + + publishPose(range_scan->GetCorrectedPose(), covariance, scan->header.stamp); } else { delete range_scan; range_scan = nullptr; @@ -582,18 +682,48 @@ 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) { - boost::mutex::scoped_lock lock(smapper_mutex_); - *res = map_; + if (map_.map.info.width && map_.map.info.height && updateMap()) { + maidbot_msg_utils::compressOccupancyGrid(map_.map, res->map); + res->success = true; return true; } else { + res->success = false; return false; } } @@ -601,18 +731,12 @@ 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; } @@ -630,6 +754,12 @@ 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 @@ -678,6 +808,7 @@ 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()) { @@ -722,6 +853,13 @@ bool SlamToolbox::deserializePoseGraphCallback( 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()) { @@ -742,7 +880,7 @@ bool SlamToolbox::deserializePoseGraphCallback( "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(); @@ -771,4 +909,112 @@ bool SlamToolbox::deserializePoseGraphCallback( return true; } +/*****************************************************************************/ +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..c950dd634 100644 --- a/src/slam_toolbox_localization.cpp +++ b/src/slam_toolbox_localization.cpp @@ -16,31 +16,25 @@ /* Author: Steven Macenski */ +#include "slam_toolbox/slam_toolbox_localization.hpp" #include #include -#include "slam_toolbox/slam_toolbox_localization.hpp" namespace slam_toolbox { /*****************************************************************************/ -LocalizationSlamToolbox::LocalizationSlamToolbox(rclcpp::NodeOptions options) -: SlamToolbox(options) +LocalizationSlamToolbox::LocalizationSlamToolbox(rclcpp::NodeOptions options) : SlamToolbox(options) /*****************************************************************************/ { - processor_type_ = PROCESS_LOCALIZATION; - localization_pose_sub_ = - this->create_subscription( - "/initialpose", 1, - std::bind(&LocalizationSlamToolbox::localizePoseCallback, - this, std::placeholders::_1)); + processor_type_ = PROCESS_LOCALIZATION; + localization_pose_sub_ = this->create_subscription( + "initialpose", 1, + std::bind(&LocalizationSlamToolbox::localizePoseCallback, this, std::placeholders::_1)); clear_localization_ = this->create_service( - "/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; + "slam_toolbox/clear_localization_buffer", + std::bind(&LocalizationSlamToolbox::clearLocalizationBuffer, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); // in localization mode, disable map saver map_saver_.reset(); @@ -53,19 +47,19 @@ void LocalizationSlamToolbox::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; - req->match_type = - slam_toolbox::srv::DeserializePoseGraph::Request::LOCALIZE_AT_POSE; - if (dock) { - RCLCPP_WARN(get_logger(), - "LocalizationSlamToolbox: Starting localization " - "at first node (dock) is correctly not supported."); + req->filename = filename; + req->match_type = slam_toolbox::srv::DeserializePoseGraph::Request::LOCALIZE_AT_POSE; + if (dock) + { + RCLCPP_WARN(get_logger(), "LocalizationSlamToolbox: Starting localization " + "at first node (dock) is correctly not supported."); } deserializePoseGraphCallback(nullptr, req, resp); @@ -80,8 +74,7 @@ bool LocalizationSlamToolbox::clearLocalizationBuffer( /*****************************************************************************/ { boost::mutex::scoped_lock lock(smapper_mutex_); - RCLCPP_INFO(get_logger(), - "LocalizationSlamToolbox: Clearing localization buffer."); + RCLCPP_INFO(get_logger(), "LocalizationSlamToolbox: Clearing localization buffer."); smapper_->clearLocalizationBuffer(); return true; } @@ -94,7 +87,7 @@ bool LocalizationSlamToolbox::serializePoseGraphCallback( /*****************************************************************************/ { RCLCPP_ERROR(get_logger(), "LocalizationSlamToolbox: Cannot call serialize map " - "in localization mode!"); + "in localization mode!"); return false; } @@ -105,66 +98,80 @@ bool LocalizationSlamToolbox::deserializePoseGraphCallback( std::shared_ptr resp) /*****************************************************************************/ { - if (req->match_type != procType::LOCALIZE_AT_POSE) { + if (req->match_type != procType::LOCALIZE_AT_POSE) + { RCLCPP_ERROR(get_logger(), "Requested a non-localization deserialization " - "in localization mode."); + "in localization mode."); return false; } return SlamToolbox::deserializePoseGraphCallback(request_header, req, resp); } /*****************************************************************************/ -void LocalizationSlamToolbox::laserCallback( - sensor_msgs::msg::LaserScan::ConstSharedPtr scan) +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)) { + 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(), "SynchronousSlamToolbox: Failed to create laser" - " device for %s; discarding scan", scan->header.frame_id.c_str()); + LaserRangeFinder* laser = getLaser(scan); + + if (!laser) + { + RCLCPP_WARN(get_logger(), + "SynchronousSlamToolbox: Failed to create laser" + " device for %s; discarding scan", + scan->header.frame_id.c_str()); return; } - if (shouldProcessScan(scan, pose)) { + if (shouldProcessScan(scan, pose)) + { addScan(laser, scan, pose); } } /*****************************************************************************/ -LocalizedRangeScan * LocalizationSlamToolbox::addScan( - LaserRangeFinder * laser, - const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan, - Pose2 & odom_pose) +LocalizedRangeScan* LocalizationSlamToolbox::addScan( + LaserRangeFinder* laser, const sensor_msgs::msg::LaserScan::ConstSharedPtr& scan, + Pose2& odom_pose) /*****************************************************************************/ { boost::mutex::scoped_lock l(pose_mutex_); - if (PROCESS_LOCALIZATION && process_near_pose_) { + if (PROCESS_LOCALIZATION && process_near_pose_) + { processor_type_ = PROCESS_NEAR_REGION; } - 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_NEAR_REGION) { - if (!process_near_pose_) { - RCLCPP_ERROR(get_logger(), - "Process near region called without a " - "valid region request. Ignoring scan."); + + Matrix3 covariance; + covariance.SetToIdentity(); + + if (processor_type_ == PROCESS_NEAR_REGION) + { + if (!process_near_pose_) + { + RCLCPP_ERROR(get_logger(), "Process near region called without a " + "valid region request. Ignoring scan."); return nullptr; } @@ -172,28 +179,37 @@ 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); + processor_type_ = PROCESS_LOCALIZATION; + } + else if (processor_type_ == PROCESS_LOCALIZATION) + { + processed = smapper_->getMapper()->ProcessLocalization(range_scan, &covariance); update_reprocessing_transform = false; - } else { + } + else + { RCLCPP_FATAL(get_logger(), "LocalizationSlamToolbox: " - "No valid processor type set! Exiting."); + "No valid processor type set! Exiting."); exit(-1); } // if successfully processed, create odom to map transformation - if (!processed) { + if (!processed) + { delete range_scan; range_scan = nullptr; - } else { + } + else + { // compute our new transform - setTransformFromPoses(range_scan->GetCorrectedPose(), odom_pose, - scan->header.stamp, update_reprocessing_transform); + setTransformFromPoses(range_scan->GetCorrectedPose(), odom_pose, scan->header.stamp, + update_reprocessing_transform); + + publishPose(range_scan->GetCorrectedPose(), covariance, scan->header.stamp); } return range_scan; @@ -201,24 +217,27 @@ LocalizedRangeScan * LocalizationSlamToolbox::addScan( /*****************************************************************************/ void LocalizationSlamToolbox::localizePoseCallback( - const - geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) /*****************************************************************************/ { - if (processor_type_ != PROCESS_LOCALIZATION) { - RCLCPP_ERROR(get_logger(), - "LocalizePoseCallback: Cannot process localization command " - "if not in localization mode."); + if (processor_type_ != PROCESS_LOCALIZATION) + { + RCLCPP_ERROR(get_logger(), "LocalizePoseCallback: Cannot process localization command " + "if not in localization mode."); return; } boost::mutex::scoped_lock l(pose_mutex_); - 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)); + 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; @@ -226,10 +245,9 @@ void LocalizationSlamToolbox::localizePoseCallback( boost::mutex::scoped_lock lock(smapper_mutex_); smapper_->clearLocalizationBuffer(); - 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)); + 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)); } -} // namespace slam_toolbox +} // namespace slam_toolbox diff --git a/src/slam_toolbox_localization_node.cpp b/src/slam_toolbox_localization_node.cpp index 8a9acfe97..ab885a819 100644 --- a/src/slam_toolbox_localization_node.cpp +++ b/src/slam_toolbox_localization_node.cpp @@ -27,7 +27,7 @@ int main(int argc, char ** argv) int stack_size = 40000000; { auto temp_node = std::make_shared("slam_toolbox"); - temp_node->declare_parameter("stack_size_to_use"); + 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; 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..50d6d5a39 100644 --- a/src/slam_toolbox_sync_node.cpp +++ b/src/slam_toolbox_sync_node.cpp @@ -27,7 +27,7 @@ int main(int argc, char ** argv) int stack_size = 40000000; { auto temp_node = std::make_shared("slam_toolbox"); - temp_node->declare_parameter("stack_size_to_use"); + 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; 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