diff --git a/mrpt_graphslam_2d/.gitignore b/mrpt_graphslam_2d/.gitignore new file mode 100644 index 0000000..393c7d4 --- /dev/null +++ b/mrpt_graphslam_2d/.gitignore @@ -0,0 +1 @@ +rosbags/records_with_gt.bag diff --git a/mrpt_graphslam_2d/CMakeLists.txt b/mrpt_graphslam_2d/CMakeLists.txt index 434ea85..646c0a7 100644 --- a/mrpt_graphslam_2d/CMakeLists.txt +++ b/mrpt_graphslam_2d/CMakeLists.txt @@ -9,24 +9,26 @@ add_definitions(-std=c++11) # Use C++11 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - std_msgs - sensor_msgs - geometry_msgs - nav_msgs - mrpt_bridge - mrpt_msgs - tf - ) + roscpp + rospy + std_msgs + sensor_msgs + geometry_msgs + nav_msgs + mrpt_bridge + mrpt_msgs + tf + tf2 + multimaster_msgs_fkie + ) ## System dependencies are found with CMake's conventions find_package(MRPT REQUIRED - base - gui - obs - maps - opengl - graphslam) + base + gui + obs + maps + opengl + graphslam) if (MRPT_VERSION VERSION_LESS 2.0.0) set(CMAKE_CXX_FLAGS "-Wno-deprecated-declarations ${CMAKE_CXX_FLAGS}") @@ -46,20 +48,21 @@ endif() ################################### catkin_package( - INCLUDE_DIRS include - #LIBRARIES mrpt_graphslam_2d - CATKIN_DEPENDS - roscpp - rospy - std_msgs - sensor_msgs - geometry_msgs - nav_msgs - mrpt_bridge - mrpt_msgs - tf - # DEPENDS system_lib - ) + INCLUDE_DIRS include + #LIBRARIES mrpt_graphslam_2d + CATKIN_DEPENDS + roscpp + rospy + std_msgs + sensor_msgs + geometry_msgs + nav_msgs + mrpt_bridge + mrpt_msgs + tf + multimaster_msgs_fkie + # DEPENDS system_lib + ) ########### ## Build ## @@ -73,45 +76,81 @@ include_directories( ) IF(${MRPT_VERSION} LESS 1.5.0) - - set(WARN_MSG "${WARN_MSG}\"${PROJECT_NAME}\" requires MRPT version 1.5.0 or higher ") - set(WARN_MSG "${WARN_MSG} - Current MRPT version: ${MRPT_VERSION}\n") - set(WARN_MSG "${WARN_MSG}Ignoring target \"${PROJECT_NAME}\"...") - MESSAGE(WARNING ${WARN_MSG}) + set(WARN_MSG "${WARN_MSG}\"${PROJECT_NAME}\" requires MRPT version 1.5.0 or higher ") + set(WARN_MSG "${WARN_MSG} - Current MRPT version: ${MRPT_VERSION}\n") + set(WARN_MSG "${WARN_MSG}Ignoring target \"${PROJECT_NAME}\"...") + MESSAGE(WARNING ${WARN_MSG}) else() -## Declare a C++ executable -add_executable(${PROJECT_NAME}_node - src/mrpt_graphslam_2d_node.cpp - src/CGraphSlamResources.cpp + set(SR_GRAPHSLAM_SRC + src/CConnectionManager.cpp + src/common.cpp ) -## Specify libraries to link a library or executable target against -target_link_libraries(${PROJECT_NAME}_node + set(MR_GRAPHSLAM_MERGER_SRC + src/CMapMerger.cpp + src/TNeighborAgentMapProps.cpp + ) + + add_library(${PROJECT_NAME} + ${SR_GRAPHSLAM_SRC} + ) + add_executable(${PROJECT_NAME}_node + ${SR_GRAPHSLAM_SRC} + src/${PROJECT_NAME}_node.cpp + ) + add_executable(${PROJECT_NAME}_mr_node + ${SR_GRAPHSLAM_SRC} + src/${PROJECT_NAME}_mr_node.cpp + ) + add_executable(map_merger_node + ${SR_GRAPHSLAM_SRC} + ${MR_GRAPHSLAM_MERGER_SRC} + src/map_merger_node.cpp + ) + + ## Specify libraries to link a library or executable target against + target_link_libraries(${PROJECT_NAME} + ${MRPT_LIBS} + ${catkin_LIBRARIES} + ) + target_link_libraries(${PROJECT_NAME}_node + ${MRPT_LIBS} + ${catkin_LIBRARIES} + ) + target_link_libraries(${PROJECT_NAME}_mr_node ${MRPT_LIBS} ${catkin_LIBRARIES} ) + target_link_libraries(map_merger_node + ${MRPT_LIBS} + ${catkin_LIBRARIES} + ) + + ############# + ## Install ## + ############# + + # Mark executables and/or libraries for installation + install( + TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node ${PROJECT_NAME}_mr_node map_merger_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + + # Mark other files for installation (e.g. launch and bag files, etc.) + install(DIRECTORY + launch + demo + config + rviz + rosbags + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) -############# -## Install ## -############# - -# Mark executables and/or libraries for installation -install(TARGETS mrpt_graphslam_2d_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -# Mark other files for installation (e.g. launch and bag files, etc.) -install(DIRECTORY - launch - demo - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -############# -## Testing ## -############# + ############# + ## Testing ## + ############# endif() diff --git a/mrpt_graphslam_2d/README.md b/mrpt_graphslam_2d/README.md index 7678d75..15a5eb5 100644 --- a/mrpt_graphslam_2d/README.md +++ b/mrpt_graphslam_2d/README.md @@ -14,18 +14,17 @@ mrpt\_graphslam\_2d available [here](// TODO - add it.) - [mrpt-graphslam library](http://reference.mrpt.org/devel/namespacemrpt_1_1graphslam.html) - [graphslam-engine application page](http://www.mrpt.org/list-of-mrpt-apps/application-graphslamengine/) - [graphslam-engine documentation](https://www.dropbox.com/s/u7phs612qf1l8bb/graphslam-engine-guide.pdf?dl=0) -- TODO - Upload and include Youtube demo video w/ ground-truth -## Algorithm demonstration +## Single robot algorithm -### Real-Time demo experiment - short loop +### Real-Time experiment - short loop - A sample rosbag is included in the rosbags/demo_short_loop directory. To run this just launch the sr_graphslam_demo.launch file: `roslaunch mrpt_graphslam_2d sr_graphslam_demo.launch start_rviz:=True` -- Demo rosbag contains laserScan measurements (and odometry which is not usuable in the algorithm due to the topic type) +- Demo rosbag contains Laser Scan measurements (and odometry which is not usuable in the algorithm due to the topic type) ### Real-Time experiment - ground-truth data included @@ -51,13 +50,37 @@ mrpt\_graphslam\_2d available [here](// TODO - add it.) to the ground-truth visualization - sr_graphslam_demo_gt file uses the *pass_all_args* XML directive which is only - available starting from the Jade distribution. If you have an older one, + available starting from the *ROS Jade* distribution. If you have an older one, just replace this with all the args in that file exclusively. For an - example on how to do this, see the sr_graphslam_demo.launch file + example on how to do this, see the + [sr_graphslam_demo.launch](https://github.com/mrpt-ros-pkg/mrpt_slam/blob/master/mrpt_graphslam_2d/launch/sr_graphslam_demo.launch) + file - Rosbag was recorded in the Mechanical Engineering School of the [National Technical University of Athens](http://www.mech.ntua.gr/en) -# Notes: +## Multi-robot algorithm + +### Multi-robot simulations in Gazebo - [csl_mr_slam](http://github.com/bergercookie/csl_mr_slam) + +Multi-robot simulations are supported in the Gazebo Simulator via the +[csl_hw_setup](https://github.com/bergercookie/csl_mr_slam/tree/master/csl_hw_setup) +ROS package. An example of running such a simulation is given +below. + +![](https://media.giphy.com/media/l0Iy3H3H4eJQFxqlW/giphy.gif) + +A complete example of executing multi-robot graphSLAM in the Gazebo simulation +environment is presented in the following video: + +[![Multi-robot simulations](http://img.youtube.com/vi/4RKS2jrvsYE/0.jpg)](http://www.youtube.com/watch?v=4RKS2jrvsYE) + +### Real-time multi-robot experiments + +As with its single-robot variant, support for running the algorithm in a +real-time multi-robot setup is provided via the **csl_mr_slam/csl_hw_setup** +package. + +An example of executing real-time multi-robot graphSLAM is given +[here](https://www.dropbox.com/s/zm2njljeprnsfaf/20170426_mr_graphslam_real_2.mp4?dl=0). -- Multi-Robot graphSLAM is not yet supported. diff --git a/mrpt_graphslam_2d/config/ros_laser_odometry_LC_MR_real.ini b/mrpt_graphslam_2d/config/ros_laser_odometry_LC_MR_real.ini new file mode 100644 index 0000000..220db9a --- /dev/null +++ b/mrpt_graphslam_2d/config/ros_laser_odometry_LC_MR_real.ini @@ -0,0 +1,155 @@ +//---------------------------------------------------------------------------- +// Config file for the using the graphslam-engine application +// +// ~ The MRPT Library ~ +//---------------------------------------------------------------------------- + +############################################################### +# Sat Jun 25 16:43:35 EEST 2016, Nikos Koukis, nickkouk@gmail.com +# Used, tested for the following specifications +# observations handled : CObservationOdometry, CObservation2DRangeScan, CActionRobotMovement2D +# Produced map : 2D +# Graph type : mrpt::graphs::CNetworkOfPoses2DInf +# NodeRegistrationDecider tested : CFixedIntervalsNRD +# EdgeRegistrationDecider : CLoopCloserERD_MR +# GraphSlamSolver : CLevMarqGSO +# +# File is used for multi-robot real-time usage. See the csl_hw_setup +# ROS package for more on the configuration. +############################################################### + + +[GeneralConfiguration] + +output_dir_fname = graphslam_results +user_decides_about_output_dir = false +save_3DScene = true +save_3DScene_fname = output_scene.3DScene +save_graph = true +save_graph_fname = output_graph.graph +ground_truth_file_format = NavSimul // variable for determining how to parse the groundtruth file. + +; Set the verbosity of the output messages. Only messages over the specified +; will be printed to the console. +; Available options in ascending order are as follows: +; LVL_DEBUG => 0, +; LVL_INFO => 1, +; LVL_WARN => 2, +; LVL_ERROR => 3 +class_verbosity = 0 + + +; min node difference for an edge to be considered as a loop closure +; Used in both the visualization procedure for updating the Loop closures +; counter as well as the optimization procedure since it affects how often we +; otpimize the full graph +LC_min_nodeid_diff= 10 + +####################################################### +[NodeRegistrationDeciderParameters] + +registration_max_distance = 0.05 +;registration_max_distance = 1 // meters +registration_max_angle = 5 // degrees +class_verbosity = 1 + +######################################################## +[EdgeRegistrationDeciderParameters] + +use_scan_matching = true // strongly suggested that this is set to TRUE +;ICP_goodness_thresh = 0.80 // threshold for accepting the ICP constraint in the graph +prev_nodes_for_ICP = 10 +;prev_nodes_for_ICP = 5 +LC_eigenvalues_ratio_thresh = 2 +LC_min_remote_nodes = 2 // how many out "remote" nodes should exist in a partition for the partition to be examined for potential loop closures +LC_check_curr_partition_only = true + +consec_icp_constraint_factor = 0.91 +lc_icp_constraint_factor = 0.80 + + +class_verbosity = 0 + +// Graph Partitioning Parameters +forceBisectionOnly = false +gridResolution = 0.10 +;gridResolution = 0.50 +;gridResolution = 5 +;gridResolution = 50 +minDistForCorrespondence = 0.20 +minimumNumberElementsEachCluster = 1 +minMahaDistForCorrespondence = 2.0 +;partitionThreshold = 0.7 +partitionThreshold = 1.0 +;partitionThreshold = 1.5 +useMapMatching = true + +// Loop Closing Parameters + + +[OptimizerParameters] + +optimization_on_second_thread = false +optimization_distance = 0.4; +;optimization_distance = -1 // optimize whole graph every time. + +// Levenberg-Marquardt parameters +verbose = false +profiler = false +max_iterations = 100 +scale_hessian = 0.2 +tau = 1e-3 + +class_verbosity = 1 + +######################################################## +# seems that it doesn't read hex, using cfg_file.read_int +# hardcode the integer values instead +# http://www.binaryhexconverter.com/hex-to-decimal-converter +[VisualizationParameters] + +visualize_optimized_graph = 1 + +optimized_show_ID_labels = 0 +optimized_show_ground_grid = 1 +optimized_show_edges = 1 +optimized_edge_color = 4278257152 +optimized_edge_width = 0.5 +optimized_show_node_corners = 1 +optimized_show_edge_rel_poses = 0 +optimized_edge_rel_poses_color = 1090486271 +optimized_nodes_edges_corner_scale = 0.0 +optimized_nodes_corner_scale = 0.1 +optimized_nodes_point_size = 1 +optimized_nodes_point_color = 1090486271 + +visualize_map = true +visualize_laser_scans = true +visualize_odometry_poses = true +visualize_ground_truth = true +visualize_estimated_trajectory = true +visualize_SLAM_metric = true // extra displayPlots showing the evolution of the SLAM metric +visualize_map_partitions = true +enable_curr_pos_viewport = true + +[ICP] +maxIterations = 100 // The maximum number of iterations to execute if convergence is not achieved before +minAbsStep_trans = 1e-6 // If the correction in all translation coordinates (X,Y,Z) is below this threshold (in meters), iterations are terminated: +minAbsStep_rot = 1e-6 // If the correction in all rotation coordinates (yaw,pitch,roll) is below this threshold (in radians), iterations are terminated: + +thresholdDist = 0.3 // Initial maximum distance for matching a pair of points +thresholdAng_DEG = 5 // An angular factor (in degrees) to increase the matching distance for distant points. + +ALFA = 0.8 // After convergence, the thresholds are multiplied by this constant and ICP keep running (provides finer matching) + +smallestThresholdDist = 0.05 // This is the smallest the distance threshold can become after stopping ICP and accepting the result. +onlyClosestCorrespondences = false // 1: Use the closest points only, 0: Use all the correspondences within the threshold (more robust sometimes, but slower) +doRANSAC = true + +# 0: icpClassic +# 1: icpLevenbergMarquardt +ICP_algorithm = icpLevenbergMarquardt + +# decimation to apply to the point cloud being registered against the map +# Reduce to "1" to obtain the best accuracy +corresponding_points_decimation = 5 diff --git a/mrpt_graphslam_2d/config/ros_odometry_2DRangeScans_LC_version.ini b/mrpt_graphslam_2d/config/ros_odometry_2DRangeScans_LC.ini similarity index 100% rename from mrpt_graphslam_2d/config/ros_odometry_2DRangeScans_LC_version.ini rename to mrpt_graphslam_2d/config/ros_odometry_2DRangeScans_LC.ini diff --git a/mrpt_graphslam_2d/config/ros_odometry_2DRangeScans_LC_MR.ini b/mrpt_graphslam_2d/config/ros_odometry_2DRangeScans_LC_MR.ini new file mode 100644 index 0000000..332393b --- /dev/null +++ b/mrpt_graphslam_2d/config/ros_odometry_2DRangeScans_LC_MR.ini @@ -0,0 +1,155 @@ +//---------------------------------------------------------------------------- +// Config file for the using the graphslam-engine application +// +// ~ The MRPT Library ~ +//---------------------------------------------------------------------------- + +############################################################### +# Sat Jun 25 16:43:35 EEST 2016, Nikos Koukis, nickkouk@gmail.com +# Used, tested for the following specifications +# observations handled : CObservationOdometry, CObservation2DRangeScan, CActionRobotMovement2D +# Produced map : 2D +# Graph type : mrpt::graphs::CNetworkOfPoses2DInf +# NodeRegistrationDecider tested : CFixedIntervalsNRD +# EdgeRegistrationDecider : CLoopCloserERD_MR +# GraphSlamSolver : CLevMarqGSO +# +# File is used for multi-robot simulations in Gazebo. See the csl_robots_gazebo +# ROS package for more on the configuration +############################################################### + + +[GeneralConfiguration] + +output_dir_fname = graphslam_results +user_decides_about_output_dir = false +save_3DScene = true +save_3DScene_fname = output_scene.3DScene +save_graph = true +save_graph_fname = output_graph.graph +ground_truth_file_format = NavSimul // variable for determining how to parse the groundtruth file. + +; Set the verbosity of the output messages. Only messages over the specified +; will be printed to the console. +; Available options in ascending order are as follows: +; LVL_DEBUG => 0, +; LVL_INFO => 1, +; LVL_WARN => 2, +; LVL_ERROR => 3 +class_verbosity = 0 + + +; min node difference for an edge to be considered as a loop closure +; Used in both the visualization procedure for updating the Loop closures +; counter as well as the optimization procedure since it affects how often we +; otpimize the full graph +LC_min_nodeid_diff= 10 + +####################################################### +[NodeRegistrationDeciderParameters] + +registration_max_distance = 0.4 +;registration_max_distance = 1 // meters +registration_max_angle = 15 // degrees +class_verbosity = 1 + +######################################################## +[EdgeRegistrationDeciderParameters] + +use_scan_matching = true // strongly suggested that this is set to TRUE +;ICP_goodness_thresh = 0.80 // threshold for accepting the ICP constraint in the graph +prev_nodes_for_ICP = 10 +;prev_nodes_for_ICP = 5 +LC_eigenvalues_ratio_thresh = 2 +LC_min_remote_nodes = 2 // how many out "remote" nodes should exist in a partition for the partition to be examined for potential loop closures +LC_check_curr_partition_only = true + +consec_icp_constraint_factor = 0.91 +lc_icp_constraint_factor = 0.80 + + +class_verbosity = 0 + +// Graph Partitioning Parameters +forceBisectionOnly = false +gridResolution = 0.10 +;gridResolution = 0.50 +;gridResolution = 5 +;gridResolution = 50 +minDistForCorrespondence = 0.20 +minimumNumberElementsEachCluster = 1 +minMahaDistForCorrespondence = 2.0 +;partitionThreshold = 0.7 +partitionThreshold = 1.0 +;partitionThreshold = 1.5 +useMapMatching = true + +// Loop Closing Parameters + + +[OptimizerParameters] + +optimization_on_second_thread = false +optimization_distance = 0.4; +;optimization_distance = -1 // optimize whole graph every time. + +// Levenberg-Marquardt parameters +verbose = false +profiler = false +max_iterations = 100 +scale_hessian = 0.2 +tau = 1e-3 + +class_verbosity = 1 + +######################################################## +# seems that it doesn't read hex, using cfg_file.read_int +# hardcode the integer values instead +# http://www.binaryhexconverter.com/hex-to-decimal-converter +[VisualizationParameters] + +visualize_optimized_graph = 1 + +optimized_show_ID_labels = 0 +optimized_show_ground_grid = 1 +optimized_show_edges = 1 +optimized_edge_color = 4278257152 +optimized_edge_width = 0.5 +optimized_show_node_corners = 1 +optimized_show_edge_rel_poses = 0 +optimized_edge_rel_poses_color = 1090486271 +optimized_nodes_edges_corner_scale = 0.0 +optimized_nodes_corner_scale = 0.1 +optimized_nodes_point_size = 1 +optimized_nodes_point_color = 1090486271 + +visualize_map = true +visualize_laser_scans = true +visualize_odometry_poses = true +visualize_ground_truth = true +visualize_estimated_trajectory = true +visualize_SLAM_metric = true // extra displayPlots showing the evolution of the SLAM metric +visualize_map_partitions = true +enable_curr_pos_viewport = true + +[ICP] +maxIterations = 100 // The maximum number of iterations to execute if convergence is not achieved before +minAbsStep_trans = 1e-6 // If the correction in all translation coordinates (X,Y,Z) is below this threshold (in meters), iterations are terminated: +minAbsStep_rot = 1e-6 // If the correction in all rotation coordinates (yaw,pitch,roll) is below this threshold (in radians), iterations are terminated: + +thresholdDist = 0.3 // Initial maximum distance for matching a pair of points +thresholdAng_DEG = 5 // An angular factor (in degrees) to increase the matching distance for distant points. + +ALFA = 0.8 // After convergence, the thresholds are multiplied by this constant and ICP keep running (provides finer matching) + +smallestThresholdDist = 0.05 // This is the smallest the distance threshold can become after stopping ICP and accepting the result. +onlyClosestCorrespondences = false // 1: Use the closest points only, 0: Use all the correspondences within the threshold (more robust sometimes, but slower) +doRANSAC = true + +# 0: icpClassic +# 1: icpLevenbergMarquardt +ICP_algorithm = icpLevenbergMarquardt + +# decimation to apply to the point cloud being registered against the map +# Reduce to "1" to obtain the best accuracy +corresponding_points_decimation = 5 diff --git a/mrpt_graphslam_2d/config/ros_odometry_2DRangeScans_LC_MR_real.ini b/mrpt_graphslam_2d/config/ros_odometry_2DRangeScans_LC_MR_real.ini new file mode 100644 index 0000000..2bfae67 --- /dev/null +++ b/mrpt_graphslam_2d/config/ros_odometry_2DRangeScans_LC_MR_real.ini @@ -0,0 +1,154 @@ +//---------------------------------------------------------------------------- +// Config file for the using the graphslam-engine application +// +// ~ The MRPT Library ~ +//---------------------------------------------------------------------------- + +############################################################### +# Sat Jun 25 16:43:35 EEST 2016, Nikos Koukis, nickkouk@gmail.com +# Used, tested for the following specifications +# observations handled : CObservationOdometry, CObservation2DRangeScan, CActionRobotMovement2D +# Produced map : 2D +# Graph type : mrpt::graphs::CNetworkOfPoses2DInf +# NodeRegistrationDecider tested : CFixedIntervalsNRD +# EdgeRegistrationDecider : CLoopCloserERD_MR +# GraphSlamSolver : CLevMarqGSO +# +# File is used for multi-robot real-time usage. See the csl_hw_setup +# ROS package for more on the configuration. +############################################################### + + +[GeneralConfiguration] + +output_dir_fname = graphslam_results +user_decides_about_output_dir = false +save_3DScene = true +save_3DScene_fname = output_scene.3DScene +save_graph = true +save_graph_fname = output_graph.graph +ground_truth_file_format = NavSimul // variable for determining how to parse the groundtruth file. + +; Set the verbosity of the output messages. Only messages over the specified +; will be printed to the console. +; Available options in ascending order are as follows: +; LVL_DEBUG => 0, +; LVL_INFO => 1, +; LVL_WARN => 2, +; LVL_ERROR => 3 +class_verbosity = 1 + + +; min node difference for an edge to be considered as a loop closure +; Used in both the visualization procedure for updating the Loop closures +; counter as well as the optimization procedure since it affects how often we +; otpimize the full graph +LC_min_nodeid_diff= 10 + +####################################################### +[NodeRegistrationDeciderParameters] + +registration_max_distance = 0.2 +;registration_max_distance = 1 // meters +registration_max_angle = 15 // degrees +class_verbosity = 1 + +######################################################## +[EdgeRegistrationDeciderParameters] + +use_scan_matching = true // strongly suggested that this is set to TRUE +;ICP_goodness_thresh = 0.80 // threshold for accepting the ICP constraint in the graph +prev_nodes_for_ICP = 5 +LC_eigenvalues_ratio_thresh = 2.5 +LC_min_remote_nodes = 10 // how many out "remote" nodes should exist in a partition for the partition to be examined for potential loop closures +LC_check_curr_partition_only = true + +consec_icp_constraint_factor = 0.96 +lc_icp_constraint_factor = 0.95 + + +class_verbosity = 1 + +// Graph Partitioning Parameters +forceBisectionOnly = false +gridResolution = 0.10 +;gridResolution = 0.50 +;gridResolution = 5 +;gridResolution = 50 +minDistForCorrespondence = 0.20 +minimumNumberElementsEachCluster = 1 +minMahaDistForCorrespondence = 2.0 +;partitionThreshold = 0.7 +partitionThreshold = 1.0 +;partitionThreshold = 1.5 +useMapMatching = true + +// Loop Closing Parameters + + +[OptimizerParameters] + +optimization_on_second_thread = false +optimization_distance = 0.4; +;optimization_distance = -1 // optimize whole graph every time. + +// Levenberg-Marquardt parameters +verbose = false +profiler = false +max_iterations = 100 +scale_hessian = 0.2 +tau = 1e-3 + +class_verbosity = 1 + +######################################################## +# seems that it doesn't read hex, using cfg_file.read_int +# hardcode the integer values instead +# http://www.binaryhexconverter.com/hex-to-decimal-converter +[VisualizationParameters] + +visualize_optimized_graph = 1 + +optimized_show_ID_labels = 0 +optimized_show_ground_grid = 1 +optimized_show_edges = 1 +optimized_edge_color = 4278257152 +optimized_edge_width = 0.5 +optimized_show_node_corners = 1 +optimized_show_edge_rel_poses = 0 +optimized_edge_rel_poses_color = 1090486271 +optimized_nodes_edges_corner_scale = 0.0 +optimized_nodes_corner_scale = 0.1 +optimized_nodes_point_size = 1 +optimized_nodes_point_color = 1090486271 + +visualize_map = true +visualize_laser_scans = true +visualize_odometry_poses = true +visualize_ground_truth = true +visualize_estimated_trajectory = true +visualize_SLAM_metric = true // extra displayPlots showing the evolution of the SLAM metric +visualize_map_partitions = true +enable_curr_pos_viewport = true + +[ICP] +maxIterations = 100 // The maximum number of iterations to execute if convergence is not achieved before +minAbsStep_trans = 1e-6 // If the correction in all translation coordinates (X,Y,Z) is below this threshold (in meters), iterations are terminated: +minAbsStep_rot = 1e-6 // If the correction in all rotation coordinates (yaw,pitch,roll) is below this threshold (in radians), iterations are terminated: + +thresholdDist = 0.3 // Initial maximum distance for matching a pair of points +thresholdAng_DEG = 5 // An angular factor (in degrees) to increase the matching distance for distant points. + +ALFA = 0.8 // After convergence, the thresholds are multiplied by this constant and ICP keep running (provides finer matching) + +smallestThresholdDist = 0.05 // This is the smallest the distance threshold can become after stopping ICP and accepting the result. +onlyClosestCorrespondences = false // 1: Use the closest points only, 0: Use all the correspondences within the threshold (more robust sometimes, but slower) +doRANSAC = true + +# 0: icpClassic +# 1: icpLevenbergMarquardt +ICP_algorithm = icpLevenbergMarquardt + +# decimation to apply to the point cloud being registered against the map +# Reduce to "1" to obtain the best accuracy +corresponding_points_decimation = 5 diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CConnectionManager.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CConnectionManager.h new file mode 100644 index 0000000..810c473 --- /dev/null +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CConnectionManager.h @@ -0,0 +1,195 @@ +/* +---------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | http://www.mrpt.org/ | + | | + | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | + | See: http://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See details in http://www.mrpt.org/License | + +---------------------------------------------------------------------------+ */ + +#ifndef CCONNECTIONMANAGER_H +#define CCONNECTIONMANAGER_H + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +namespace mrpt { namespace graphslam { namespace detail { + +/**\brief Class responsible of handling the network communication between SLAM + * agents in the Multi-Robot graphSLAM algorithm. + */ +class CConnectionManager +{ +public: + //typedef std::vector::iterator agents_it; + //typedef std::vector::const_iterator agents_cit; + typedef mrpt_msgs::GraphSlamAgents::_list_type::iterator agents_it; + typedef mrpt_msgs::GraphSlamAgents::_list_type::const_iterator agents_cit; + + /**\brief Constructor */ + CConnectionManager( + mrpt::utils::COutputLogger* logger, + ros::NodeHandle* nh_in); + /**\brief Destructor */ + ~CConnectionManager(); + /**\brief Fill the given vector with the SLAM Agents that the current manager + * can see and communicate with + * + * \param[in] ignore_self If true the GraphSlamAgent instance that is under + * the same namespace as the CConnectionManager is not going to be inserted + * in the agents_vec + * + * \sa updateNearbySlamAgents + */ + void getNearbySlamAgents(mrpt_msgs::GraphSlamAgents* agents_vec, + bool ignore_self=true); + /**\brief Read-only method for accessing list of nearby agents + */ + const mrpt_msgs::GraphSlamAgents& getNearbySlamAgents(); + /**\brief Read-only method for accessing list of nearby agents. + * This doesn't update the internal list of GraphSlamAgents but just + * the returns its latest cached version + */ + const mrpt_msgs::GraphSlamAgents& getNearbySlamAgentsCached() const; + + /**\brief Wrapper method around the private setup* class methods. + * + * Handy for setting up publishers, subscribers, services, TF-related stuff + * all at once from the user application + * + */ + void setupComm(); + /**\brief Get the agent ROS namespace */ + const std::string& getTrimmedNs() const; + +private: + /**\brief Namespace under which we are running. Corresponds to the + * agent_ID_str with which the nodes are going to be registered in the graph + */ + std::string own_ns; + /**\brief Update the internal list of nearby SLAM agents + * + * \sa getNearbySlamAgents + */ + void updateNearbySlamAgents(); + /**\name setup* ROS-related methods + *\brief Methods for setting up topic subscribers, publishers, and + * corresponding services + * + * \sa setupComm + */ + /**\{*/ + void setupSubs(); + void setupPubs(); + void setupSrvs(); + /**\}*/ + + + /**\brief ROSMaster ==> mrpt_msgs::GraphSlamAgent + * + * Assumption is that each ROSMaster instance holds exactly one + * mrpt_graphslam_2d node which publshes at a specific toic namespace -> + * /_/... + * + * \return False if the ros_master specified doesn't correspond to a valid + * GraphSlamAgent node. Should at least have a \b feedback topic + * namespace under its main topic namespace + */ + static bool convert( + const multimaster_msgs_fkie::ROSMaster& ros_master, + mrpt_msgs::GraphSlamAgent* slam_agent); + /**\brief GraphSlamAgent ==> ROSMaster. */ + static void convert( + const mrpt_msgs::GraphSlamAgent& slam_agent, + multimaster_msgs_fkie::ROSMaster* ros_master); + /**\brief Remove http:// prefix and port suffix from the string and return result + * + * \param[out] agent_port Port that the agent is running on. Extracted from + * the overall string + */ + static std::string extractHostnameOrIP(const std::string& str, + unsigned short* agent_port=NULL); + + /**\brief Pointer to the logging instance */ + mrpt::utils::COutputLogger* m_logger; + /**\brief Pointer to the Ros NodeHanle instance */ + ros::NodeHandle* m_nh; + + ros::ServiceClient m_DiscoverMasters_client; + /**\brief List of slam agents in the current agent's neighborhood + * + * \note vector includes the GraphSlamAgent that is at the same namespace as + * the current CConnectionManager instance + */ + mrpt_msgs::GraphSlamAgents m_nearby_slam_agents; + + bool has_setup_comm; + +}; + +} } } // end of namespaces + +/**\brief ROSMaster instances are considered the same if the "uri" field is the + * same + */ +/**\{*/ +bool operator==( + const multimaster_msgs_fkie::ROSMaster& master1, + const multimaster_msgs_fkie::ROSMaster& master2); +bool operator!=( + const multimaster_msgs_fkie::ROSMaster& master1, + const multimaster_msgs_fkie::ROSMaster& master2); +/**\{*/ + +/**\brief GraphSlamAgent instances are considered the same if the "agent_ID" field is the + * same and the topic_namespace is the same + */ +/**\{*/ +bool operator==( + const mrpt_msgs::GraphSlamAgent& agent1, + const mrpt_msgs::GraphSlamAgent& agent2); +bool operator!=( + const mrpt_msgs::GraphSlamAgent& agent1, + const mrpt_msgs::GraphSlamAgent& agent2); +bool operator<( + const mrpt_msgs::GraphSlamAgent& agent1, + const mrpt_msgs::GraphSlamAgent& agent2); +/**\}*/ + +/**\brief GraphSlamAgent and ROSMaster instances are considered the same if the + * corresponding "name" fields coincede + */ +/**\{*/ +bool operator==( + const multimaster_msgs_fkie::ROSMaster& master, + const mrpt_msgs::GraphSlamAgent& agent); +bool operator==( + const mrpt_msgs::GraphSlamAgent& agent, + const multimaster_msgs_fkie::ROSMaster& master); +bool operator!=( + const multimaster_msgs_fkie::ROSMaster& master, + const mrpt_msgs::GraphSlamAgent& agent); +bool operator!=( + const mrpt_msgs::GraphSlamAgent& agent, + const multimaster_msgs_fkie::ROSMaster& master); + +/**\}*/ + +#endif /* end of include guard: CCONNECTIONMANAGER_H */ diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamEngine_MR.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamEngine_MR.h new file mode 100644 index 0000000..3166d90 --- /dev/null +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamEngine_MR.h @@ -0,0 +1,556 @@ +/* +---------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | http://www.mrpt.org/ | + | | + | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | + | See: http://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See details in http://www.mrpt.org/License | + +---------------------------------------------------------------------------+ */ + +#ifndef CGRAPHSLAMENGINE_MR_H +#define CGRAPHSLAMENGINE_MR_H + +#include + +#include "mrpt_graphslam_2d/CGraphSlamEngine_ROS.h" +#include "mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_MR.h" +#include "mrpt_graphslam_2d/interfaces/CEdgeRegistrationDecider_MR.h" +#include "mrpt_graphslam_2d/CConnectionManager.h" +#include "mrpt_graphslam_2d/misc/common.h" + +#include +#include +#include +#include // service +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace mrpt { namespace graphslam { + +/** \brief mrpt::graphslam::CGraphSlamEngine derived class for executing + * multi-robot graphSLAM + */ +template +class CGraphSlamEngine_MR : public CGraphSlamEngine_ROS +{ +public: + typedef CGraphSlamEngine_ROS parent_t; + typedef CGraphSlamEngine_MR self_t; + typedef typename GRAPH_T::constraint_t constraint_t; + typedef typename constraint_t::type_value pose_t; + typedef std::pair< + mrpt::utils::TNodeID, + mrpt::obs::CObservation2DRangeScanPtr> MRPT_NodeIDWithLaserScan; + typedef std::map< + mrpt::utils::TNodeID, + mrpt::obs::CObservation2DRangeScanPtr> nodes_to_scans2D_t; + typedef std::vector partitions_t; + typedef typename mrpt::graphs::detail::THypothesis hypot_t; + typedef std::vector hypots_t; + typedef std::vector hypotsp_t; + typedef typename GRAPH_T::global_pose_t global_pose_t; + typedef typename mrpt::graphslam::detail::TNodeProps node_props_t; + typedef mrpt::graphslam::TUncertaintyPath path_t; + typedef std::vector paths_t; + typedef CEdgeRegistrationDecider_MR edge_reg_mr_t; + + CGraphSlamEngine_MR( + ros::NodeHandle* nh, + const std::string& config_file, + const std::string& rawlog_fname="", + const std::string& fname_GT="", + mrpt::graphslam::CWindowManager* win_manager=NULL, + mrpt::graphslam::deciders::CNodeRegistrationDecider* node_reg=NULL, + mrpt::graphslam::deciders::CEdgeRegistrationDecider* edge_reg=NULL, + mrpt::graphslam::optimizers::CGraphSlamOptimizer* optimizer=NULL + ); + + ~CGraphSlamEngine_MR(); + + bool _execGraphSlamStep( + mrpt::obs::CActionCollectionPtr& action, + mrpt::obs::CSensoryFramePtr& observations, + mrpt::obs::CObservationPtr& observation, + size_t& rawlog_entry); + + void initClass(); + + /**\brief Struct responsible for holding properties (nodeIDs, node + * positions, LaserScans) that have been registered by a nearby + * GraphSlamAgent. + */ + struct TNeighborAgentProps { + /**\brief Constructor */ + TNeighborAgentProps( + CGraphSlamEngine_MR& engine_in, + const mrpt_msgs::GraphSlamAgent& agent_in); + /**\brief Destructor */ + ~TNeighborAgentProps(); + + /**\brief Wrapper for calling setupSubs, setupSrvs + */ + void setupComm(); + /**\brief Setup the necessary subscribers for fetching nodes, laserScans for + * the current neighbor + */ + void setupSubs(); + /**\brief Setup necessary services for neighbor. + */ + void setupSrvs(); + /**\name Subscriber callback methods + * Methods to be called when data is received on the subscribed topics + */ + /**\{ */ + /**\brief Update nodeIDs + corresponding estimated poses */ + void fetchUpdatedNodesList( + const mrpt_msgs::NodeIDWithPose_vec::ConstPtr& nodes); + /**\brief Fill the LaserScan of the last registered nodeID */ + void fetchLastRegdIDScan( + const mrpt_msgs::NodeIDWithLaserScan::ConstPtr& last_regd_id_scan); + /**\} */ + + /**\brief Return cached list of nodeIDs (with their corresponding poses, + * LaserScans) + * + * \param[in] only_unused Include only the nodes that have not already been + * used in the current CGraphSlamEngine's graph + * \param[out] nodeIDs Pointer to vector of nodeIDs that are actually + * returned. This argument is redundant but may be convinient in case that + * just the nodeIDs are required + * \param[out] node_params Pointer to the map of nodeIDs \rightarrow + * Corresponding properties that is to be filled by the method + * + * \note Method also calls resetFlags + * \sa resetFlags + */ + void getCachedNodes( + mrpt::vector_uint* nodeIDs=NULL, + std::map< + mrpt::utils::TNodeID, + node_props_t>* nodes_params=NULL, + bool only_unused=true) const; + /**\brief Fill the optimal paths for each combination of the given nodeIDs. + */ + void fillOptPaths( + const std::set& nodeIDs, + paths_t* opt_paths) const; + /**\brief Using the fetched LaserScans and nodeID positions, compute the + * occupancy gridmap of the given neighbor + */ + void computeGridMap() const; + const mrpt::maps::COccupancyGridMap2DPtr& getGridMap() const; + /**\brief Fill the given occupancy gridmap object with the current neighbor's grdmap. + */ + void getGridMap(mrpt::maps::COccupancyGridMap2DPtr& map) const; + /**\brief Return True if there are new data (node positions and + * corresponding LaserScans available) + * + * Caller is responsible of setting the underlying has_new_* variables to + * false using the resetFlags method upon successful integration of (some + * of) the nodes + */ + bool hasNewData() const; + std::string getAgentNs() const { return this->agent.topic_namespace.data; } + void resetFlags() const; + bool operator==( + const TNeighborAgentProps& other) const { + return (this->agent == other.agent); + } + bool operator<( + const TNeighborAgentProps& other) const { + return (this->agent < other.agent); + } + /**\brief Utility method for fetching the ROS LaserScan that corresponds to + * a nodeID + */ + const sensor_msgs::LaserScan* getLaserScanByNodeID( + const mrpt::utils::TNodeID nodeID) const; + + /**\brief GraphSlamAgent instance of the neighbor + * + * UPDATE: This should be a real instance instead of a reference + */ + const mrpt_msgs::GraphSlamAgent agent; + /**\brief Decide whether there are enough new nodes + scans that have not been + * integrated in the graph yet. + * + * Method takes in account only the nodeIDs that have a valid LaserScan + * + * \return True if there are more than \a new_batch_size valid nodes + scans + * available for integration, false otherwise + */ + bool hasNewNodesBatch(int new_batch_size); + + /**\brief Set the color related to the current neighbor agent + * + * Handy in visualization operations + */ + void setTColor(const mrpt::utils::TColor& color_in) { color = color_in; } + /**\brief Unique color of current TNeighborAgentProps instance + */ + mrpt::utils::TColor color; + + /**\name Neighbor cached properties */ + /**\{ */ + /**\brief NodeIDs that I have received from this graphSLAM agent. */ + std::set nodeIDs_set; + /**\brief Poses that I have received from this graphSLAM agent. */ + typename GRAPH_T::global_poses_t poses; + /**\brief ROS LaserScans that I have received from this graphSLAM agent. */ + std::vector ros_scans; + /**\brief Have I already integrated this node in my graph? + * \note CGraphSlamEngine_MR instance is responsible of setting these values to + * true when it integrates them in own graph + */ + std::map nodeID_to_is_integrated; + /**\} */ + + /**\name Subscriber/Services Instances */ + /**\{ */ + ros::Subscriber last_regd_nodes_sub; + ros::Subscriber last_regd_id_scan_sub; + + ros::ServiceClient cm_graph_srvclient; + /**\} */ + + /**\brief Constant reference to the outer class + */ + CGraphSlamEngine_MR& engine; + /**\name Full topic names / service names + * \brief Names of the full topic paths that the neighbor publishes nodes, + * LaserScans at. + */ + /**\{ */ + std::string last_regd_nodes_topic; + std::string last_regd_id_scan_topic; + + std::string cm_graph_service; + /**\} */ + + /**\brief Pointer to the gridmap object of the neighbor + */ + mutable mrpt::maps::COccupancyGridMap2DPtr gridmap_cached; + + mutable bool has_new_nodes; + mutable bool has_new_scans; + + int m_queue_size; + /**\brief NodeHandle passed by the calling CGraphSlamEngine_MR class + */ + ros::NodeHandle* nh; + bool has_setup_comm; + + /**\brief CPose2D connecting own graph origin to neighbor first received + * and integrated node. + * If this pose is 0 then it is not yet computed. + */ + mrpt::poses::CPose2D tf_self_to_neighbor_first_integrated_pose; + /**\brief Last nodeID/CPose2D of neighbor that was integrated - Pose is taken + * with regards to neighbor's frame of reference. + */ + std::pair< + mrpt::utils::TNodeID, + mrpt::poses::CPose2D> last_integrated_pair_neighbor_frame; + + }; + typedef std::vector neighbors_t; + + const neighbors_t& getVecOfNeighborAgentProps() const { + return m_neighbors; + } + /**\brief Return true if current CGraphSlamEngine_MR object initially + * registered this nodeID, false otherwise. + * \param[in] nodeID nodeID for which the query is made. + * \exc runtime_error In case given nodeID isn't registered at all in the + * graph + */ + bool isOwnNodeID( + const mrpt::utils::TNodeID nodeID, + const global_pose_t* pose_out=NULL) const; + + +private: + /**\brief Traverse all neighbors for which I know the intra-graph + * transformation, run addNodeBatchFromNeighbor. + */ + bool addNodeBatchesFromAllNeighbors(); + /**\brief neighbors for which I know the intra-graph + * transformation, add new batches of fetches nodeIDs and scans + * + * Try to integrate the newly received nodeIDs and laser Scans in own graph + * as a batch + * + * \note Batch integration (instead of individual nodeID + LaserScan every + * time a new one is received) is preferred since the neighbor is given time + * to optimize the node positions + */ + bool addNodeBatchFromNeighbor(TNeighborAgentProps* neighbor); + /**\brief Using map-merging find a potentuial transofrmation between own + * graph map to another agent's map and using that transformation add other + * agent's nodes to own graph + * + * Method delegates task to the findMatchesWithNeighboor method. + * + * \return True on successful graph integration with any neighbor, false + * otherwise + */ + bool findTFsWithAllNeighbors(); + /** + * \brief Method is used only when I haven't found any transformation between + * mine and the current neighbor's graph. When I do find one, I just add + * every new nodeID + LS directly in own graph since I can now connect the + * new ones with the already integrated nodes of the neighbor. + * + * \sa findTFsWithAllNeighbors + */ + bool findTFWithNeighbor(TNeighborAgentProps* neighbor); + /**\brief Fill the TNeighborAgentProps instance based on the given agent_ID_str + * string that is provided. + * + * \return False if the agent_ID_str doesn't correspond to any registered + * TNeighborAgentProps, True otherwise + */ + bool getNeighborByAgentID( + const std::string& agent_ID_str, + TNeighborAgentProps*& neighbor) const; + /**\brief Update the last registered NodeIDs and corresponding positions of + * the current agent. + * + * \note Method is automatically called from usePublishersBroadcasters, but + * it publishes to the corresponding topics only on new node additions. + * Node positions may change due to optimization but we will be notified of + * this change anyway after a new node addition. + * + * \return true on publication to corresponding topics, false otherwise + * \sa pubLastRegdIDScan + */ + bool pubUpdatedNodesList(); + /**\brief Update the last registered NodeID and LaserScan of the current + * agent. + * + * \note Method is automatically called from usePublishersBroadcasters, but + * it publishes to the corresponding topics only on new node/LS additions + * + * \return true on publication to corresponding topics, false otherwise + * \sa pubUpdatedNodesList + */ + bool pubLastRegdIDScan(); + + void usePublishersBroadcasters(); + + void setupSubs(); + void setupPubs(); + void setupSrvs(); + + /**\brief Compute and fill the Condensed Measurements Graph + */ + bool getCMGraph( + mrpt_msgs::GetCMGraph::Request& req, + mrpt_msgs::GetCMGraph::Response& res); + + void readParams(); + /**\brief Read the parameters related to the map-matching operation + */ + void readGridMapAlignmentParams(); + void readROSParameters(); + void printParams(); + mrpt::poses::CPose3D getLSPoseForGridMapVisualization( + const mrpt::utils::TNodeID nodeID) const; + void setObjectPropsFromNodeID( + const mrpt::utils::TNodeID nodeID, + mrpt::opengl::CSetOfObjectsPtr& viz_object); + /**\brief Overriden method that takes in account registration of multiple + * nodes of other running graphSLAM agents + * + */ + void monitorNodeRegistration( + bool registered=false, + std::string class_name="Class"); + /**\brief Fill given set with the nodeIDs that were initially registered by + * the current graphSLAM engine (and not by any neighboring agent. + */ + void getAllOwnNodes( + std::set* nodes_set) const; + void getNodeIDsOfEstimatedTrajectory( + std::set* nodes_set) const; + void getRobotEstimatedTrajectory( + typename GRAPH_T::global_poses_t* graph_poses) const; + + + /**\brief GraphSlamAgent instance pointer to TNeighborAgentProps + * + * \note elements of vector should persist even if the neighbor is no longer + * in range since they contain previous laser scans. + */ + neighbors_t m_neighbors; + /**\brief Map from neighbor instance to transformation from own to neighbor's graph + * + * As long as the corresponding variable for a specific agent is False + * current engine keeps trying the map-matching proc to find a common + * transformation between own and neighbor graphs. + */ + std::map m_neighbor_to_found_initial_tf; + + /**\name Subscribers - Publishers + * + * ROS Topic Subscriber/Publisher/Service instances + * */ + /**\{*/ + + ros::Publisher m_list_neighbors_pub; + /**\brief Publisher of the laserScan + the corresponding (last) registered node. + */ + ros::Publisher m_last_regd_id_scan_pub; + /**\brief Publisher of the last registered nodeIDs and positions. + * + * \note see m_num_last_regd_nodes variable for the exact number of + * published nodeID and position pairs. + */ + ros::Publisher m_last_regd_nodes_pub; + + ros::ServiceServer m_cm_graph_srvserver; + /**\}*/ + + /**\name Topic Names + * + * Names of the topics that the class instance subscribes or publishes to + */ + /**\{*/ + + /**\brief Condensed Measurements topic \a namespace */ + std::string m_mr_ns; + /**\brief Name of topic at which we publish information about the agents that + * we can currently communicate with. + */ + std::string m_list_neighbors_topic; + /**\brief Name of the topic that the last \b registered laser scan (+ + * corresponding nodeID) is published at + */ + std::string m_last_regd_id_scan_topic; + /**\brief Name of the topic that the last X registered nodes + positions + * are going to be published at + */ + std::string m_last_regd_nodes_topic; + + /**\brief Name of the server which is to be called when other agent wants to have a + * subgraph of certain nodes returned. + */ + std::string m_cm_graph_service; + + /**\}*/ + + /**\brief Last known size of the m_nodes_to_laser_scans2D map + */ + size_t m_nodes_to_laser_scans2D_last_size; + /**\brief Last known size of the m_nodes map + */ + size_t m_graph_nodes_last_size; + + /**\brief Max number of last registered NodeIDs + corresponding positions to publish. + * + * This is necessary for the other GraphSLAM agents so that they can use this + * information to localize the current agent in their own map and later + * make a querry for the Condensed Measurements Graph. + */ + int m_num_last_regd_nodes; + + + /**\brief CConnectionManager instance */ + mrpt::graphslam::detail::CConnectionManager m_conn_manager; + + + /**\brief NodeHandle pointer - inherited by the parent. Redefined here for + * convenience. + */ + ros::NodeHandle* m_nh; + + /**\brief Display the Deciders/Optimizers with which we are running as well + * as the namespace of the current agent. + */ + /**\{ */ + double m_offset_y_nrd; + double m_offset_y_erd; + double m_offset_y_gso; + double m_offset_y_namespace; + + int m_text_index_nrd; + int m_text_index_erd; + int m_text_index_gso; + int m_text_index_namespace; + /**\} */ + + /**\brief Indicates whether multiple nodes were just registered. + * Used for checking correct node registration in the monitorNodeRgistration + * method. + */ + bool m_registered_multiple_nodes; + + /**\brief Lowest number of nodes that should exist in a group of nodes before + * evaluating it. + * + * \note This should be set >= 3 + * \sa m_intra_group_node_count_thresh_minadv + */ + int m_intra_group_node_count_thresh; + /**\brief Minimum advised limit of m_intra_group_node_count_thresh + * \sa m_intra_group_node_count_thresh + */ + int m_intra_group_node_count_thresh_minadv; + /**\brief AsyncSpinner that is used to query the CM-Graph service in case a + * new request arrives + */ + ros::AsyncSpinner cm_graph_async_spinner; + + /**\brief Indicates if the program is going to pause on + * multiple-robot nodes registration + * + * \note For debugging reasons only, + * \todo Consider having this as a macro that is to be disabled on release + * code + */ + bool m_pause_exec_on_mr_registration; + + /**\brief After an intra-graph transformation is found between own graph and + * a neighbor's map, newly fetched neighbor's nodes are added in batches. + */ + int m_nodes_integration_batch_size; + + /**\brief Parameters used during the alignment operation + */ + mrpt::slam::CGridMapAligner::TConfigParams m_alignment_options; + + /**\brief Be conservative when it comes to deciding the initial + * transformation of own graph with regards to graphs of the neighboring + * agents. If true engine won't use map merging but instead will be waiting + * for rendez-vous with other agents to determine the tf. + * + * \warning Rendez-vous behavior is not yet implemented. + */ + bool m_conservative_find_initial_tfs_to_neighbors; + mrpt::utils::TColorManager neighbor_colors_manager; + +}; + + +} } // end of namespaces + +// pseudo-split decleration from implementation +#include "mrpt_graphslam_2d/CGraphSlamEngine_MR_impl.h" + +#endif /* end of include guard: CGRAPHSLAMENGINE_MR_H */ diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamEngine_MR_impl.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamEngine_MR_impl.h new file mode 100644 index 0000000..073a2d0 --- /dev/null +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamEngine_MR_impl.h @@ -0,0 +1,1451 @@ +/* +---------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | http://www.mrpt.org/ | + | | | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | + | See: http://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See details in http://www.mrpt.org/License | + +---------------------------------------------------------------------------+ */ + +#ifndef CGRAPHSLAMENGINE_MR_IMPL_H +#define CGRAPHSLAMENGINE_MR_IMPL_H + +namespace mrpt { namespace graphslam { + +template +CGraphSlamEngine_MR::CGraphSlamEngine_MR( + ros::NodeHandle* nh, + const std::string& config_file, + const std::string& rawlog_fname/* ="" */, + const std::string& fname_GT /* ="" */, + mrpt::graphslam::CWindowManager* win_manager /* = NULL */, + mrpt::graphslam::deciders::CNodeRegistrationDecider* node_reg /* = NULL */, + mrpt::graphslam::deciders::CEdgeRegistrationDecider* edge_reg /* = NULL */, + mrpt::graphslam::optimizers::CGraphSlamOptimizer* optimizer /* = NULL */): + parent_t::CGraphSlamEngine_ROS( + nh, + config_file, + rawlog_fname, + fname_GT, + win_manager, + node_reg, + edge_reg, + optimizer), + m_conn_manager( + dynamic_cast(this), nh), + m_nh(nh), + m_graph_nodes_last_size(0), + m_registered_multiple_nodes(false), + m_intra_group_node_count_thresh_minadv(25), + cm_graph_async_spinner(/* threads_num: */ 1, &this->custom_service_queue), + m_pause_exec_on_mr_registration(false) +{ + this->initClass(); +} + +template +CGraphSlamEngine_MR::~CGraphSlamEngine_MR() { + MRPT_LOG_DEBUG_STREAM( + "In Destructor: Deleting CGraphSlamEngine_MR instance..."); + cm_graph_async_spinner.stop(); + + for (typename neighbors_t::iterator + neighbors_it = m_neighbors.begin(); + neighbors_it != m_neighbors.end(); + ++neighbors_it) { + delete *neighbors_it; + } + +} + +template +bool CGraphSlamEngine_MR::addNodeBatchesFromAllNeighbors() { + MRPT_START; + using namespace mrpt::graphslam::detail; + + bool ret_val = false; + for (const auto& neighbor : m_neighbors) { + if (!isEssentiallyZero( + neighbor->tf_self_to_neighbor_first_integrated_pose) && // intra-graph TF found + neighbor->hasNewData() && + neighbor->hasNewNodesBatch(m_nodes_integration_batch_size)) { + bool loc_ret_val = this->addNodeBatchFromNeighbor(neighbor); + if (loc_ret_val) { ret_val = true; } + } + } + + return ret_val; + MRPT_END; +} // end of addNodeBatchesFromAllNeighbors + +template +bool CGraphSlamEngine_MR:: +addNodeBatchFromNeighbor(TNeighborAgentProps* neighbor) { + MRPT_START; + ASSERT_(neighbor); + using namespace mrpt::utils; + using namespace mrpt::poses; + using namespace mrpt::math; + using namespace std; + + + vector_uint nodeIDs; + std::map nodes_params; + neighbor->getCachedNodes(&nodeIDs, &nodes_params, /*only unused=*/ true); + + // + // get the condensed measurements graph of the new nodeIDs + // + mrpt_msgs::GetCMGraph cm_graph_srv; + //mrpt_msgs::GetCMGraphRequest::_nodeIDs_type& cm_graph_nodes = + //cm_graph_srv.request.nodeIDs; + for (const auto n : nodeIDs) { + cm_graph_srv.request.nodeIDs.push_back(n); + } + + MRPT_LOG_DEBUG_STREAM("Asking for the graph."); + bool res = neighbor->cm_graph_srvclient.call(cm_graph_srv); // blocking + if (!res) { + MRPT_LOG_ERROR_STREAM("Service call for CM_Graph failed."); + return false; // skip this if failed to fetch other's graph + } + MRPT_LOG_DEBUG_STREAM("Fetched graph successfully."); + MRPT_LOG_DEBUG_STREAM(cm_graph_srv.response.cm_graph); + + GRAPH_T other_graph; + mrpt_bridge::convert(cm_graph_srv.response.cm_graph, other_graph); + + // + // merge batch of nodes in own graph + // + MRPT_LOG_WARN_STREAM("Merging new batch from \"" << neighbor->getAgentNs() << "\"..." << endl + << "Batch: " << getSTLContainerAsString(cm_graph_srv.request.nodeIDs)); + hypots_t graph_conns; + // build a hypothesis connecting the new batch with the last integrated + // pose of the neighbor + { + pair node_props_to_connect = *nodes_params.begin(); + + hypot_t graph_conn; + constraint_t c; + c.mean = node_props_to_connect.second.pose - + neighbor->last_integrated_pair_neighbor_frame.second; + c.cov_inv.unit(); + graph_conn.setEdge(c); + + graph_conn.from = INVALID_NODEID; + // get the nodeID of the last integrated neighbor node after remapping + // in own graph numbering + for (const auto n : this->m_graph.nodes) { + if (n.second.agent_ID_str == neighbor->getAgentNs() && + n.second.nodeID_loc == neighbor->last_integrated_pair_neighbor_frame.first) { + graph_conn.from = n.first; + break; + } + } + ASSERT_(graph_conn.from != INVALID_NODEID); + graph_conn.to = node_props_to_connect.first; // other + + MRPT_LOG_DEBUG_STREAM("Hypothesis for adding the batch of nodes: " << graph_conn); + graph_conns.push_back(graph_conn); + } + + std::map old_to_new_mappings; + this->m_graph.mergeGraph( + other_graph, graph_conns, + /*hypots_from_other_to_self=*/ false, + &old_to_new_mappings); + + // + // Mark Nodes/LaserScans as integrated + // + MRPT_LOG_WARN_STREAM("Marking used nodes as integrated - Integrating LSs"); + nodes_to_scans2D_t new_nodeIDs_to_scans_pairs; + for (typename vector_uint::const_iterator + n_cit = nodeIDs.begin(); + n_cit != nodeIDs.end(); + ++n_cit) { + + // Mark the neighbor's used nodes as integrated + neighbor->nodeID_to_is_integrated.at(*n_cit) = true; + + // Integrate LaserScans at the newly integrated nodeIDs in own + // nodes_to_laser_scans2D map + new_nodeIDs_to_scans_pairs.insert(make_pair( + old_to_new_mappings.at(*n_cit), + nodes_params.at(*n_cit).scan)); + MRPT_LOG_INFO_STREAM("Adding nodeID-LS of nodeID: " + << "\t[old:] " << *n_cit << endl + << "| [new:] " << old_to_new_mappings.at(*n_cit)); + } + + this->m_nodes_to_laser_scans2D.insert( + new_nodeIDs_to_scans_pairs.begin(), + new_nodeIDs_to_scans_pairs.end()); + edge_reg_mr_t* edge_reg_mr = + dynamic_cast(this->m_edge_reg); + edge_reg_mr->addBatchOfNodeIDsAndScans(new_nodeIDs_to_scans_pairs); + + this->execDijkstraNodesEstimation(); + neighbor->resetFlags(); + + // keep track of the last integrated nodeID + { + TNodeID last_node = *nodeIDs.rbegin(); + neighbor->last_integrated_pair_neighbor_frame = make_pair( + last_node, + nodes_params.at(last_node).pose); + } + + if (m_pause_exec_on_mr_registration) this->pauseExec(); + return true; + MRPT_END; +} // end of addNodeBatchFromNeighbor + +template +bool CGraphSlamEngine_MR::findTFsWithAllNeighbors() { + MRPT_START; + using namespace mrpt::utils; + using namespace mrpt::math; + + if (this->m_graph.nodeCount() < m_intra_group_node_count_thresh) { + return false; + } + + + // cache the gridmap so that it is computed only once during all the + // findMatches* calls + this->computeMap(); + + bool ret_val = false; // at *any* node registration, change to true + + // iterate over all neighbors - run map-matching proc. + for (typename neighbors_t::iterator + neighbors_it = m_neighbors.begin(); + neighbors_it != m_neighbors.end(); + ++neighbors_it) { + + // run matching proc with neighbor only if I haven't found an initial + // intra-graph TF + if (!m_neighbor_to_found_initial_tf.at(*neighbors_it)) { + + bool loc_ret_val = findTFWithNeighbor(*neighbors_it); + if (loc_ret_val) { + MRPT_LOG_DEBUG_STREAM( + "Updating own cached map after successful node registration..."); + // update own map if new nodes have been added. + this->computeMap(); + + // mark current neighbor tf as found. + m_neighbor_to_found_initial_tf.at(*neighbors_it) = true; + + ret_val = true; + } + } + } + + return ret_val; + MRPT_END; +} // end of findTFsWithAllNeighbors + +template +bool CGraphSlamEngine_MR:: +findTFWithNeighbor(TNeighborAgentProps* neighbor) { + MRPT_START; + using namespace mrpt::slam; + using namespace mrpt::math; + using namespace mrpt::poses; + using namespace mrpt::utils; + using namespace mrpt::maps; + using namespace mrpt::graphslam::detail; + + bool ret_val = false; + + vector_uint neighbor_nodes; + std::map nodes_params; + neighbor->getCachedNodes(&neighbor_nodes, &nodes_params, /*only unused=*/ false); + if (neighbor_nodes.size() < m_intra_group_node_count_thresh || + !neighbor->hasNewData()) { + return false; + } + + // + // run alignment procedure + // + COccupancyGridMap2DPtr neighbor_gridmap = neighbor->getGridMap(); + CGridMapAligner gridmap_aligner; + gridmap_aligner.options = m_alignment_options; + + CGridMapAligner::TReturnInfo results; + float run_time = 0; + // TODO - make use of agents' rendez-vous in the initial estimation + CPosePDFGaussian init_estim; + init_estim.mean = neighbor->tf_self_to_neighbor_first_integrated_pose; + this->logFmt(LVL_INFO, + "Trying to align the maps, initial estimation: %s", + init_estim.mean.asString().c_str()); + neighbor_gridmap->saveMetricMapRepresentationToFile(this->getLoggerName() + "_other"); + this->m_gridmap_cached->saveMetricMapRepresentationToFile(this->getLoggerName() + "_self"); + const CPosePDFPtr pdf_tmp = gridmap_aligner.AlignPDF( + this->m_gridmap_cached.pointer(), neighbor_gridmap.pointer(), + init_estim, + &run_time, &results); + this->logFmt(LVL_INFO, "Elapsed Time: %f", run_time); + CPosePDFSOGPtr pdf_out = CPosePDFSOG::Create(); + pdf_out->copyFrom(*pdf_tmp); + + CPose2D pose_out; CMatrixDouble33 cov_out; + pdf_out->getMostLikelyCovarianceAndMean(cov_out, pose_out); + + this->logFmt(LVL_INFO, "%s\n", + getGridMapAlignmentResultsAsString(*pdf_tmp, results).c_str()); + + // dismiss this? + if (results.goodness > 0.999 || + isEssentiallyZero(pose_out)) { + return false; + } + + // + // + // Map-merging operation is successful. Integrate graph into own. + ////////////////////////////////////////////////////////////////// + + // + // ask for condensed measurements graph + // + mrpt_msgs::GetCMGraph cm_graph_srv; + typedef mrpt_msgs::GetCMGraphRequest::_nodeIDs_type nodeID_type; + nodeID_type& matched_nodeIDs = + cm_graph_srv.request.nodeIDs; + + // which nodes to ask the condensed graph for + // I assume that no nodes of the other graph have been integrated yet. + for (typename vector_uint::const_iterator + n_cit = neighbor_nodes.begin(); + n_cit != neighbor_nodes.end(); + ++n_cit) { + matched_nodeIDs.push_back(*n_cit); // all used nodes + } + + MRPT_LOG_DEBUG_STREAM("Asking for the graph."); + bool res = neighbor->cm_graph_srvclient.call(cm_graph_srv); // blocking + if (!res) { + MRPT_LOG_ERROR_STREAM("Service call for CM_Graph failed."); + return false; // skip this if failed to fetch other's graph + } + MRPT_LOG_DEBUG_STREAM("Fetched graph successfully."); + MRPT_LOG_DEBUG_STREAM(cm_graph_srv.response.cm_graph); + + GRAPH_T other_graph; + mrpt_bridge::convert(cm_graph_srv.response.cm_graph, other_graph); + + // + // merge graphs + // + MRPT_LOG_WARN_STREAM("Merging graph of \"" << neighbor->getAgentNs() << "\"..."); + hypots_t graph_conns; + // build the only hypothesis connecting graph with neighbor subgraph + // own origin -> first valid nodeID of neighbor + { + hypot_t graph_conn; + constraint_t c; + c.mean = pose_out; + cov_out.inv(c.cov_inv); // assign the inverse of the found covariance to c + graph_conn.setEdge(c); + // TODO - change this. + graph_conn.from = 0; // self + graph_conn.to = *neighbor_nodes.begin(); // other + + graph_conn.goodness = results.goodness; + graph_conns.push_back(graph_conn); + } + + std::map old_to_new_mappings; + this->m_graph.mergeGraph( + other_graph, graph_conns, + /*hypots_from_other_to_self=*/ false, + &old_to_new_mappings); + + // + // Mark Nodes/LaserScans as integrated + // + MRPT_LOG_WARN_STREAM("Marking used nodes as integrated - Integrating LSs"); + nodes_to_scans2D_t new_nodeIDs_to_scans_pairs; + for (typename vector_uint::const_iterator + n_cit = neighbor_nodes.begin(); + n_cit != neighbor_nodes.end(); + ++n_cit) { + + // Mark the neighbor's used nodes as integrated + neighbor->nodeID_to_is_integrated.at(*n_cit) = true; + + // Integrate LaserScans at the newly integrated nodeIDs in own + // nodes_to_laser_scans2D map + new_nodeIDs_to_scans_pairs.insert(make_pair( + old_to_new_mappings.at(*n_cit), + nodes_params.at(*n_cit).scan)); + MRPT_LOG_INFO_STREAM("Adding nodeID-LS of nodeID: " + << "\t[old:] " << *n_cit << endl + << "| [new:] " << old_to_new_mappings.at(*n_cit)); + } + + this->m_nodes_to_laser_scans2D.insert( + new_nodeIDs_to_scans_pairs.begin(), + new_nodeIDs_to_scans_pairs.end()); + edge_reg_mr_t* edge_reg_mr = + dynamic_cast(this->m_edge_reg); + edge_reg_mr->addBatchOfNodeIDsAndScans(new_nodeIDs_to_scans_pairs); + + // Call for a Full graph visualization update and Dijkstra update - + // CGraphSlamOptimizer + //MRPT_LOG_WARN_STREAM("Optimizing graph..." << endl); + //this->m_optimizer->optimizeGraph(); + MRPT_LOG_WARN_STREAM("Executing Dijkstra..." << endl); + this->execDijkstraNodesEstimation(); + + neighbor->resetFlags(); + + // update the initial tf estimation + neighbor->tf_self_to_neighbor_first_integrated_pose = pose_out; + + // keep track of the last integrated nodeID + { + TNodeID last_node = *neighbor_nodes.rbegin(); + neighbor->last_integrated_pair_neighbor_frame = make_pair( + last_node, + nodes_params.at(last_node).pose); + } + + MRPT_LOG_WARN_STREAM("Nodes of neighbor [" << neighbor->getAgentNs() + << "] have been integrated successfully"); + + if (m_pause_exec_on_mr_registration) this->pauseExec(); + ret_val = true; + + return ret_val; + MRPT_END; +} // end if findTFWithNeighbor + +template +bool CGraphSlamEngine_MR::_execGraphSlamStep( + mrpt::obs::CActionCollectionPtr& action, + mrpt::obs::CSensoryFramePtr& observations, + mrpt::obs::CObservationPtr& observation, + size_t& rawlog_entry) { + MRPT_START; + using namespace mrpt::graphslam::deciders; + using namespace mrpt; + + // call parent method + bool continue_exec = parent_t::_execGraphSlamStep( + action, observations, observation, rawlog_entry); + + // find matches between own nodes and those of the neighbors + bool did_register_from_map_merge; + if (!m_conservative_find_initial_tfs_to_neighbors) { + did_register_from_map_merge = this->findTFsWithAllNeighbors(); + } + else { // intra-graph TF is available - add new nodes + THROW_EXCEPTION("Conservative intra-graph TF computation is not yet implemented."); + } + + bool did_register_from_batches = this->addNodeBatchesFromAllNeighbors(); + m_registered_multiple_nodes = (did_register_from_map_merge || did_register_from_batches); + + if (m_registered_multiple_nodes) { + if (this->m_enable_visuals) { + this->updateAllVisuals(); + } + } + + return continue_exec; + MRPT_END; +} // end of _execGraphSlamStep + +template +void CGraphSlamEngine_MR::initClass() { + using namespace mrpt::graphslam; + using namespace mrpt::utils; + + // initialization of topic namespace names + // TODO - put these into seperate method + m_mr_ns = "mr_info"; + + // initialization of topic names / services + m_list_neighbors_topic = m_mr_ns + "/" + "neighbors"; + m_last_regd_id_scan_topic = m_mr_ns + "/" + "last_nodeID_laser_scan"; + m_last_regd_nodes_topic = m_mr_ns + "/" + "last_regd_nodes"; + m_cm_graph_service = m_mr_ns + "/" + "get_cm_graph"; + + this->m_class_name = "CGraphSlamEngine_MR_" + m_conn_manager.getTrimmedNs(); + this->setLoggerName(this->m_class_name); + this->setupComm(); + + // Make sure that master_discovery and master_sync are up before trying to + // connect to other agents + std::vector nodes_up; + ros::master::getNodes(nodes_up); + // If both master_dicovery and master_sync are running, then the + // /master_sync/get_sync_info service should be available + MRPT_LOG_INFO_STREAM("Waiting for master_discovery, master_sync nodes to come up...."); + ros::service::waitForService("/master_sync/get_sync_info"); // block until it is + MRPT_LOG_INFO_STREAM("master_discovery, master_sync are available."); + + this->m_node_reg->setClassName( + this->m_node_reg->getClassName() + "_" + m_conn_manager.getTrimmedNs()); + this->m_edge_reg->setClassName( + this->m_edge_reg->getClassName() + "_" + m_conn_manager.getTrimmedNs()); + this->m_optimizer->setClassName( + this->m_optimizer->getClassName() + "_" + m_conn_manager.getTrimmedNs()); + + // in case of Multi-robot specific deciders/optimizers (they + // inherit from the CDeciderOrOptimizer_ROS interface) set the + // CConnectionManager* + // NOTE: It's not certain, even though we are running multi-robot graphSLAM + // that all these classes do inherit from the + // CRegistrationDeciderOrOptimizer_MR base class. They might be more generic + // and not care about the multi-robot nature of the algorithm (e.g. + // optimization scheme) + { // NRD + CRegistrationDeciderOrOptimizer_MR* dec_opt_mr = + dynamic_cast*>(this->m_node_reg); + + if (dec_opt_mr) { + dec_opt_mr->setCConnectionManagerPtr(&m_conn_manager); + } + } + { // ERD + CRegistrationDeciderOrOptimizer_MR* dec_opt_mr = + dynamic_cast*>(this->m_edge_reg); + ASSERT_(dec_opt_mr); + dec_opt_mr->setCConnectionManagerPtr(&m_conn_manager); + dec_opt_mr->setCGraphSlamEnginePtr(this); + } + { // GSO + CRegistrationDeciderOrOptimizer_MR* dec_opt_mr = + dynamic_cast*>(this->m_optimizer); + if (dec_opt_mr) { + dec_opt_mr->setCConnectionManagerPtr(&m_conn_manager); + } + } + // display messages with the names of the deciders, optimizer and agent string ID + if (this->m_enable_visuals) { + // NRD + this->m_win_manager->assignTextMessageParameters( + /* offset_y* = */ &m_offset_y_nrd, + /* text_index* = */ &m_text_index_nrd); + this->m_win_manager->addTextMessage(this->m_offset_x_left, -m_offset_y_nrd, + mrpt::format("NRD: %s", this->m_node_reg->getClassName().c_str()), + TColorf(0,0,0), + /* unique_index = */ m_text_index_nrd); + + // ERD + this->m_win_manager->assignTextMessageParameters( + /* offset_y* = */ &m_offset_y_erd, + /* text_index* = */ &m_text_index_erd); + this->m_win_manager->addTextMessage( + this->m_offset_x_left, -m_offset_y_erd, + mrpt::format("ERD: %s", this->m_edge_reg->getClassName().c_str()), + TColorf(0,0,0), + /* unique_index = */ m_text_index_erd); + + // GSO + this->m_win_manager->assignTextMessageParameters( + /* offset_y* = */ &m_offset_y_gso, + /* text_index* = */ &m_text_index_gso); + this->m_win_manager->addTextMessage( + this->m_offset_x_left, -m_offset_y_gso, + mrpt::format("GSO: %s", this->m_optimizer->getClassName().c_str()), + TColorf(0,0,0), + /* unique_index = */ m_text_index_gso); + + // Agent Namespace + this->m_win_manager->assignTextMessageParameters( + /* offset_y* = */ &m_offset_y_namespace, + /* text_index* = */ &m_text_index_namespace); + this->m_win_manager->addTextMessage( + this->m_offset_x_left, -m_offset_y_namespace, + mrpt::format("Agent ID: %s", m_conn_manager.getTrimmedNs().c_str()), + TColorf(0,0,0), + /* unique_index = */ m_text_index_namespace); + + } + this->readParams(); + + this->m_optimized_map_color = neighbor_colors_manager.getNextTColor(); + + // start the spinner for asynchronously servicing cm_graph requests + cm_graph_async_spinner.start(); +} // end of initClass + +template +mrpt::poses::CPose3D CGraphSlamEngine_MR:: +getLSPoseForGridMapVisualization( + const mrpt::utils::TNodeID nodeID) const { + MRPT_START; + using namespace mrpt::graphs::detail; + + // if this is my own node ID return the corresponding CPose2D, otherwise + // return the pose connecting own graph with the neighbor's graph + + const TMRSlamNodeAnnotations* node_annots = dynamic_cast( + &this->m_graph.nodes.at(nodeID)); + + // Is the current nodeID registered by a neighboring agent or is it my own? + TNeighborAgentProps* neighbor = NULL; + this->getNeighborByAgentID(node_annots->agent_ID_str, neighbor); + + CPose3D laser_pose; + if (!neighbor) { // own node + laser_pose = parent_t::getLSPoseForGridMapVisualization(nodeID); + } + else { // not by me - return TF to neighbor graph + laser_pose = CPose3D(neighbor->tf_self_to_neighbor_first_integrated_pose); + } + return laser_pose; + + MRPT_END; +} + +template +bool CGraphSlamEngine_MR:: +getNeighborByAgentID(const std::string& agent_ID_str, + TNeighborAgentProps*& neighbor) const { + MRPT_START; + + bool ret= false; + for (auto neighbors_it = m_neighbors.begin(); + neighbors_it != m_neighbors.end(); + ++neighbors_it) { + + if ((*neighbors_it)->getAgentNs() == agent_ID_str) { + ret = true; + neighbor = *neighbors_it; + break; + } + } + + return ret; + MRPT_END; +} + +template +void CGraphSlamEngine_MR::usePublishersBroadcasters() { + MRPT_START; + using namespace mrpt_bridge; + using namespace mrpt_msgs; + using namespace std; + using namespace mrpt::math; + using ::operator==; + + // call the parent class + parent_t::usePublishersBroadcasters(); + + // update list of neighbors that the current agent can communicate with. + mrpt_msgs::GraphSlamAgents nearby_slam_agents; + m_conn_manager.getNearbySlamAgents( + &nearby_slam_agents, + /*ignore_self = */ true); + m_list_neighbors_pub.publish(nearby_slam_agents); + + // Initialize TNeighborAgentProps + // for each *new* GraphSlamAgent we should add a TNeighborAgentProps instance, + // and initialize its subscribers so that we fetch every new LaserScan and list of + // modified nodes it publishes + { + for (GraphSlamAgents::_list_type::const_iterator + it = nearby_slam_agents.list.begin(); + it != nearby_slam_agents.list.end(); + ++it) { + + const GraphSlamAgent& gsa = *it; + + // Is the current GraphSlamAgent already registered? + const auto search = [gsa](const TNeighborAgentProps* neighbor) { + return (neighbor->agent == gsa); + }; + typename neighbors_t::const_iterator neighbor_it = find_if( + m_neighbors.begin(), + m_neighbors.end(), search); + + if (neighbor_it == m_neighbors.end()) { // current gsa not found, add it + + MRPT_LOG_DEBUG_STREAM("Initializing NeighborAgentProps instance for " + << gsa.name.data); + m_neighbors.push_back(new TNeighborAgentProps(*this, gsa)); + TNeighborAgentProps* latest_neighbor = m_neighbors.back(); + latest_neighbor->setTColor(neighbor_colors_manager.getNextTColor()); + m_neighbor_to_found_initial_tf.insert(make_pair( + latest_neighbor, false)); + latest_neighbor->setupComm(); + } + } + } + + this->pubUpdatedNodesList(); + this->pubLastRegdIDScan(); + + MRPT_END; +} // end of usePublishersBroadcasters + +template +bool CGraphSlamEngine_MR::pubUpdatedNodesList() { + MRPT_START; + using namespace mrpt_msgs; + + // update the last X NodeIDs + positions; Do it only when a new node is inserted. + // notified of this change anyway after a new node addition + if (this->m_graph_nodes_last_size == this->m_graph.nodes.size()) { + return false; + } + MRPT_LOG_DEBUG_STREAM("Updating list of node poses"); + + // send at most m_num_last_rgd_nodes + typename GRAPH_T::global_poses_t poses_to_send; + int poses_counter = 0; + // fill the NodeIDWithPose_vec msg + NodeIDWithPose_vec ros_nodes; + + // send up to m_num_last_regd_nodes Nodes - start from end. + for (typename GRAPH_T::global_poses_t::const_reverse_iterator + cit = this->m_graph.nodes.rbegin(); + (poses_counter <= m_num_last_regd_nodes && + cit != this->m_graph.nodes.rend()); + ++cit) { + + // Do not resend nodes of others, registered in own graph. + if (cit->second.agent_ID_str != m_conn_manager.getTrimmedNs()) { + continue; // skip this. + } + + NodeIDWithPose curr_node_w_pose; + // send basics - NodeID, Pose + curr_node_w_pose.nodeID = cit->first; + mrpt_bridge::convert(cit->second, curr_node_w_pose.pose); + + // send mr-fields + curr_node_w_pose.str_ID.data = cit->second.agent_ID_str; + curr_node_w_pose.nodeID_loc = cit->second.nodeID_loc; + + ros_nodes.vec.push_back(curr_node_w_pose); + + poses_counter++; + } + + m_last_regd_nodes_pub.publish(ros_nodes); + + // update the last known size + m_graph_nodes_last_size = this->m_graph.nodeCount(); + + bool res = false; + if (poses_counter) { + res = true; + } + + return res; + MRPT_END; +} // end of pubUpdatedNodesList + +template +bool CGraphSlamEngine_MR::pubLastRegdIDScan() { + MRPT_START; + using namespace mrpt_msgs; + using namespace mrpt_bridge; + + // Update the last registered scan + associated nodeID + // Last registered scan always corresponds to the *last* element of the of the + // published NodeIDWithPose_vec that is published above. + // + // - Check if map is empty + // - Have I already published the last laser scan? + if (!this->m_nodes_to_laser_scans2D.empty() && + m_nodes_to_laser_scans2D_last_size < this->m_nodes_to_laser_scans2D.size()) { + // last registered scan + MRPT_NodeIDWithLaserScan mrpt_last_regd_id_scan = + *(this->m_nodes_to_laser_scans2D.rbegin()); + + + // if this is a mr-registered nodeID+LS, skip it. + if (!this->isOwnNodeID(mrpt_last_regd_id_scan.first)) { + return false; + } + + //MRPT_LOG_DEBUG_STREAM + //<< "Publishing LaserScan of nodeID \"" + //<< mrpt_last_regd_id_scan.first << "\""; + ASSERT_(mrpt_last_regd_id_scan.second); + + // convert to ROS msg + mrpt_msgs::NodeIDWithLaserScan ros_last_regd_id_scan; + convert(*(mrpt_last_regd_id_scan.second), ros_last_regd_id_scan.scan); + ros_last_regd_id_scan.nodeID = mrpt_last_regd_id_scan.first; + + m_last_regd_id_scan_pub.publish(ros_last_regd_id_scan); + + // update the last known size. + m_nodes_to_laser_scans2D_last_size = + this->m_nodes_to_laser_scans2D.size(); + + return true; + } + else { + return false; + } + + + MRPT_END; +} // end of pubLastRegdIDScan + + +template +bool CGraphSlamEngine_MR::isOwnNodeID( + const mrpt::utils::TNodeID nodeID, + const global_pose_t* pose_out/*=NULL*/) const { + MRPT_START; + using namespace mrpt::graphs::detail; + + // make sure give node is in the graph + typename GRAPH_T::global_poses_t::const_iterator global_pose_search = + this->m_graph.nodes.find(nodeID); + ASSERTMSG_(global_pose_search != this->m_graph.nodes.end(), + mrpt::format( + "isOwnNodeID called for nodeID \"%lu\" which is not found in the graph", + static_cast(nodeID))); + + // fill pointer to global_pose_t + if (pose_out) { + pose_out = &global_pose_search->second; + } + + bool res = false; + const TMRSlamNodeAnnotations* node_annots = dynamic_cast( + &global_pose_search->second); + if ((node_annots && node_annots->agent_ID_str == m_conn_manager.getTrimmedNs()) || + (!node_annots)) { + res = true; + } + + return res; + + MRPT_END; +} // end of isOwnNodeID + +template +void CGraphSlamEngine_MR::readParams() { + this->readROSParameters(); + this->readGridMapAlignmentParams(); +} + +template +void CGraphSlamEngine_MR::readGridMapAlignmentParams() { + using namespace mrpt::slam; + + std::string alignment_opts_ns = "alignment_opts"; + + m_nh->param(alignment_opts_ns + "/" + "min_ICP_goodness", + m_alignment_options.min_ICP_goodness, 0.60); + m_nh->param(alignment_opts_ns + "/" + "maxKLd_for_merge", + m_alignment_options.maxKLd_for_merge, 0.90); + m_nh->param(alignment_opts_ns + "/" + "ransac_minSetSizeRatio", + m_alignment_options.ransac_minSetSizeRatio, 0.40); + m_nh->param(alignment_opts_ns + "/" + "nodes_integration_batch_size", + m_nodes_integration_batch_size, 4); + + // GridMap Alignment options to be used in merging. + m_alignment_options.methodSelection = CGridMapAligner::amModifiedRANSAC; + ////options.methodSelection = CGridMapAligner::amRobustMatch; // ASSERTION ERROR + + //m_alignment_options.loadFromConfigFileName( + //"/home/bergercookie/mrpt/share/mrpt/config_files/grid-matching/gridmatch_example.ini", + //"grid-match"); + +} + +template +void CGraphSlamEngine_MR::readROSParameters() { + // call parent method first in case the parent wants to ask for any ROS + // parameters + parent_t::readROSParameters(); + + m_nh->param(m_mr_ns + "/" + "conservative_find_initial_tfs_to_neighbors", + m_conservative_find_initial_tfs_to_neighbors, false); + ASSERTMSG_(!m_conservative_find_initial_tfs_to_neighbors, + "Conservative behavior is not yet implemented."); + + m_nh->param(m_mr_ns + "/" + "num_last_registered_nodes", + m_num_last_regd_nodes, 10); + + m_nh->param(m_mr_ns + "/" + "intra_group_node_count_thresh", + m_intra_group_node_count_thresh, m_intra_group_node_count_thresh_minadv); + // warn user if they choose smaller threshold. + if (m_intra_group_node_count_thresh < m_intra_group_node_count_thresh_minadv) { + MRPT_LOG_ERROR_STREAM("intra_group_node_count_thresh [" + << m_intra_group_node_count_thresh + << "is set lower than the advised minimum [" + << m_intra_group_node_count_thresh_minadv + << "]"); + } +} + +template +void CGraphSlamEngine_MR::printParams() { + parent_t::printParams(); + m_alignment_options.dumpToConsole(); +} + + + +template +void CGraphSlamEngine_MR::setupSubs() { } + +template +void CGraphSlamEngine_MR::setupPubs() { + using namespace mrpt_msgs; + using namespace sensor_msgs; + + m_list_neighbors_pub = m_nh->advertise( + m_list_neighbors_topic, + this->m_queue_size, + /*latch = */ true); + + // last registered laser scan - by default this corresponds to the last + // nodeID of the vector of last registered nodes. + m_last_regd_id_scan_pub = m_nh->advertise( + m_last_regd_id_scan_topic, + this->m_queue_size, + /*latch = */ true); + + // last X nodeIDs + positions + m_last_regd_nodes_pub = m_nh->advertise( + m_last_regd_nodes_topic, + this->m_queue_size, + /*latch = */ true); + + + +} + +template +void CGraphSlamEngine_MR::setupSrvs() { + // cm_graph service requests are handled by the custom custom_service_queue CallbackQueue + ros::CallbackQueueInterface* global_queue = m_nh->getCallbackQueue(); + m_nh->setCallbackQueue(&this->custom_service_queue); + + m_cm_graph_srvserver = m_nh->advertiseService( + m_cm_graph_service, + &CGraphSlamEngine_MR::getCMGraph, this); + + m_nh->setCallbackQueue(global_queue); +} + +template +bool CGraphSlamEngine_MR::getCMGraph( + mrpt_msgs::GetCMGraph::Request& req, + mrpt_msgs::GetCMGraph::Response& res) { + using namespace std; + using namespace mrpt::utils; + using namespace mrpt::math; + + const size_t min_nodeIDs = 2; + + set nodes_set(req.nodeIDs.begin(), req.nodeIDs.end()); + MRPT_LOG_INFO_STREAM("Called the GetCMGraph service for nodeIDs: " + << getSTLContainerAsString(nodes_set)); + + bool ret_val = false; + if (nodes_set.size() < 2) { } + else { + // fill the given Response with the ROS NetworkOfPoses + GRAPH_T mrpt_subgraph; + this->m_graph.extractSubGraph(nodes_set, &mrpt_subgraph, + /*root_node = */ INVALID_NODEID, + /*auto_expand_set=*/false); + mrpt_bridge::convert(mrpt_subgraph, res.cm_graph); + ret_val = true; + } + + return ret_val; +} // end of getCMGraph + +template +void CGraphSlamEngine_MR::setObjectPropsFromNodeID( + const mrpt::utils::TNodeID nodeID, + mrpt::opengl::CSetOfObjectsPtr& viz_object) { + using namespace mrpt::utils; + MRPT_START; + + // who registered this node? + TNeighborAgentProps* neighbor = NULL; + std::string& agent_ID_str = this->m_graph.nodes.at(nodeID).agent_ID_str; + bool is_not_own = getNeighborByAgentID(agent_ID_str, neighbor); + + TColor obj_color; + if (!is_not_own) { // I registered this. + // use the standard maps color + obj_color = this->m_optimized_map_color; + } + else { + ASSERT_(neighbor); + obj_color = neighbor->color; + } + viz_object->setColor_u8(obj_color); + + MRPT_END; +}// end of setObjectPropsFromNodeID + + +template +void CGraphSlamEngine_MR::monitorNodeRegistration( + bool registered/*=false*/, + std::string class_name/*="Class"*/) { + MRPT_START; + using namespace mrpt::utils; + + if (m_registered_multiple_nodes) { + MRPT_LOG_ERROR_STREAM("m_registered_multiple_nodes = TRUE!"); + m_registered_multiple_nodes = !m_registered_multiple_nodes; + this->m_nodeID_max = this->m_graph.nodeCount()-1; + } + else { + parent_t::monitorNodeRegistration(registered, class_name); + } + + MRPT_END; +} // end of monitorNodeRegistration + +template +void CGraphSlamEngine_MR::getRobotEstimatedTrajectory( + typename GRAPH_T::global_poses_t* graph_poses) const { + MRPT_START; + ASSERT_(graph_poses); + + for (const auto& n : this->m_graph.nodes) { + if (n.second.agent_ID_str == m_conn_manager.getTrimmedNs()) { + graph_poses->insert(n); + } + } + + MRPT_END; +} // end of getRobotEstimatedTrajectory + +template +void CGraphSlamEngine_MR:: +getAllOwnNodes(std::set* nodes_set) const { + ASSERT_(nodes_set); + nodes_set->clear(); + + for (const auto& n : this->m_graph.nodes) { + if (n.second.agent_ID_str == m_conn_manager.getTrimmedNs()) { + nodes_set->insert(n.first); + } + } +} // end of getAllOwnNodes + +template +void CGraphSlamEngine_MR:: +getNodeIDsOfEstimatedTrajectory( + std::set* nodes_set) const { + ASSERT_(nodes_set); + this->getAllOwnNodes(nodes_set); +} // end of getNodeIDsOfEstimatedTrajectory + +////////////////////////////////////////////////////////////////////////////////////// + +template +CGraphSlamEngine_MR::TNeighborAgentProps::TNeighborAgentProps( + CGraphSlamEngine_MR& engine_in, + const mrpt_msgs::GraphSlamAgent& agent_in): + engine(engine_in), + agent(agent_in), + has_setup_comm(false) +{ + using namespace mrpt::utils; + + nh = engine.m_nh; + m_queue_size = engine.m_queue_size; + this->resetFlags(); + + // fill the full paths of the topics to subscribe + // ASSUMPTION: agents namespaces start at the root "/" + this->last_regd_nodes_topic = "/" + agent.topic_namespace.data + "/" + + engine.m_last_regd_nodes_topic; + this->last_regd_id_scan_topic = "/" + agent.topic_namespace.data + "/" + + engine.m_last_regd_id_scan_topic; + this->cm_graph_service = "/" + agent.topic_namespace.data + "/" + + engine.m_cm_graph_service; + + engine.logFmt(LVL_DEBUG, + "In constructor of TNeighborAgentProps for topic namespace: %s", + agent.topic_namespace.data.c_str()); + + // initialize the occupancy map + gridmap_cached = mrpt::maps::COccupancyGridMap2D::Create(); +} + +template +CGraphSlamEngine_MR::TNeighborAgentProps::~TNeighborAgentProps() { } + +template +void CGraphSlamEngine_MR::TNeighborAgentProps::setupComm() { + using namespace mrpt::utils; + + this->setupSubs(); + this->setupSrvs(); + + engine.logFmt( + LVL_DEBUG, + "TNeighborAgentProps: Successfully set up subscribers/services " + "to agent topics for namespace [%s].", + agent.topic_namespace.data.c_str()); + + has_setup_comm = true; +} + +template +void CGraphSlamEngine_MR::TNeighborAgentProps::setupSrvs() { + + cm_graph_srvclient = + nh->serviceClient(cm_graph_service); +} + + +template +void CGraphSlamEngine_MR::TNeighborAgentProps::setupSubs() { + using namespace mrpt_msgs; + using namespace mrpt::utils; + + last_regd_nodes_sub = nh->subscribe( + last_regd_nodes_topic, + m_queue_size, + &TNeighborAgentProps::fetchUpdatedNodesList, this); + last_regd_id_scan_sub = nh->subscribe( + last_regd_id_scan_topic, + m_queue_size, + &TNeighborAgentProps::fetchLastRegdIDScan, this); +} + +template +void CGraphSlamEngine_MR::TNeighborAgentProps:: +fetchUpdatedNodesList( + const mrpt_msgs::NodeIDWithPose_vec::ConstPtr& nodes) { + MRPT_START; + using namespace mrpt_msgs; + using namespace mrpt_bridge; + using namespace std; + using namespace mrpt::utils; + + typedef typename GRAPH_T::constraint_t::type_value pose_t; + engine.logFmt(LVL_DEBUG, "In fetchUpdatedNodesList method."); + + for (NodeIDWithPose_vec::_vec_type::const_iterator + n_it = nodes->vec.begin(); + n_it != nodes->vec.end(); + ++n_it) { + + // insert in the set if not already there. + TNodeID nodeID = static_cast(n_it->nodeID); + std::pair::iterator, bool> res = + nodeIDs_set.insert(nodeID); + // if I just inserted this node mark it as not used (in own graph) + if (res.second) { // insertion took place. + engine.logFmt(LVL_INFO, "Just fetched a new nodeID: %lu", + static_cast(nodeID)); + nodeID_to_is_integrated.insert(make_pair(n_it->nodeID, false)); + } + + // update the poses + pose_t curr_pose; + // note: use "operator[]" instead of "insert" so that if the key already + // exists, the corresponding value is changed rather than ignored. + poses[static_cast(n_it->nodeID)] = + convert(n_it->pose, curr_pose); + } + has_new_nodes = true; + + //// Mon Mar 6 17:11:23 EET 2017, Nikos Koukis + // TODO When using 3 graphSLAM agents GDB Shows that it crashes on + // engine.logFmt lines. Fix this. + // + //engine.logFmt(LVL_DEBUG, // THIS CRASHES - GDB WHERE + //"NodeIDs for topic namespace: %s -> [%s]", + //agent.topic_namespace.data.c_str(), + //mrpt::math::getSTLContainerAsString(vector( + //nodeIDs_set.begin(), nodeIDs_set.end())).c_str()); + // print poses just for verification + //engine.logFmt(LVL_DEBUG, "Poses for topic namespace: %s", + //agent.topic_namespace.data.c_str()); + //for (typename GRAPH_T::global_poses_t::const_iterator + //p_it = poses.begin(); + //p_it != poses.end(); + //++p_it) { + //std::string p_str; p_it->second.asString(p_str); + //engine.logFmt(LVL_DEBUG, "nodeID: %lu | pose: %s", + //static_cast(p_it->first), + //p_str.c_str()); + //} + // TODO - These also seem to crash sometimes + //cout << "Agent information: " << agent << endl; + //cout << "Nodes: " << mrpt::math::getSTLContainerAsString(vector(nodeIDs_set.begin(), nodeIDs_set.end())); + //print nodeIDs just for verification + + MRPT_END; +} // end of fetchUpdatedNodesList + +template +void CGraphSlamEngine_MR::TNeighborAgentProps:: +fetchLastRegdIDScan( + const mrpt_msgs::NodeIDWithLaserScan::ConstPtr& last_regd_id_scan) { + MRPT_START; + using namespace std; + using namespace mrpt::utils; + ASSERT_(last_regd_id_scan); + engine.logFmt(LVL_DEBUG, "In fetchLastRegdIDScan method."); + + // make sure I haven't received any LaserScan for the current nodeID + // TODO + TNodeID curr_node = static_cast(last_regd_id_scan->nodeID); + + // Pose may not be available due to timing in publishing of the corresponding + // topics. Just keep it in ROS form and then convert them to MRPT form when + // needed. + ros_scans.push_back(*last_regd_id_scan); + has_new_scans = true; + + MRPT_END; +} // end of fetchLastRegdIDScan + +template +void CGraphSlamEngine_MR::TNeighborAgentProps::getCachedNodes( + vector_uint* nodeIDs/*=NULL*/, + std::map< + mrpt::utils::TNodeID, + node_props_t>* nodes_params/*=NULL*/, + bool only_unused/*=true*/) const { + MRPT_START; + using namespace mrpt::obs; + using namespace mrpt::utils; + using namespace mrpt::math; + using namespace std; + using namespace mrpt::poses; + + // at least one of the two args should be given. + ASSERT_(nodeIDs || nodes_params); + + // traverse all nodes + for (std::set::const_iterator + n_it = nodeIDs_set.begin(); + n_it != nodeIDs_set.end(); + ++n_it) { + + // Should I return only the unused ones? + // If so, if the current nodeID is integrated, skip it. + if (only_unused && nodeID_to_is_integrated.at(*n_it)) { continue; } + + // add the node properties + if (nodes_params) { + std::pair params; // current pair to be inserted. + const pose_t* p = &poses.at(*n_it); + + params.first = *n_it; + params.second.pose = *p; + CObservation2DRangeScanPtr mrpt_scan = CObservation2DRangeScan::Create(); + const sensor_msgs::LaserScan* ros_laser_scan = + this->getLaserScanByNodeID(*n_it); + + // if LaserScan not found, skip nodeID altogether. + // + // Its natural to have more poses than LaserScans since on every node + // registration I send a modified list of X node positions and just one + // LaserScan + if (!ros_laser_scan) { continue; } + + mrpt_bridge::convert(*ros_laser_scan, CPose3D(*p), *mrpt_scan); + params.second.scan = mrpt_scan; + + // insert the pair + nodes_params->insert(params); + + // debugging message + //std::stringstream ss; + //ss << "\ntID: " << nodes_params->rbegin()->first + //<< " ==> Props: " << nodes_params->rbegin()->second; + //engine.logFmt(LVL_DEBUG, "%s", ss.str().c_str()); + } + + // add the nodeID + if (nodeIDs) { + // do not return nodeIDs that don't have valid laserScans + // this is also checked at the prior if + if (!nodes_params && !this->getLaserScanByNodeID(*n_it)) { + continue; + } + + nodeIDs->push_back(*n_it); + } + } + + MRPT_END; +} // end of TNeighborAgentProps::getCachedNodes + +template +void CGraphSlamEngine_MR::TNeighborAgentProps::resetFlags() const { + has_new_nodes = false; + has_new_scans = false; + +} + +template +bool CGraphSlamEngine_MR::TNeighborAgentProps:: +hasNewNodesBatch(int new_batch_size) { + MRPT_START; + + auto is_not_integrated = [this](const std::pair pair) { + return (pair.second == false && getLaserScanByNodeID(pair.first)); + }; + int not_integrated_num = count_if( + nodeID_to_is_integrated.begin(), + nodeID_to_is_integrated.end(), + is_not_integrated); + + return (not_integrated_num >= new_batch_size); + + MRPT_END; +} + + +template +const sensor_msgs::LaserScan* +CGraphSlamEngine_MR::TNeighborAgentProps:: +getLaserScanByNodeID( + const mrpt::utils::TNodeID nodeID) const { + MRPT_START; + + // assert that the current nodeID exists in the nodeIDs_set + ASSERT_(nodeIDs_set.find(nodeID) != nodeIDs_set.end()); + + for (std::vector::const_iterator + it = ros_scans.begin(); + it != ros_scans.end(); + ++it) { + if (it->nodeID == nodeID) { + return &it->scan; + } + } + + // if out here, LaserScan doesn't exist. + return 0; + MRPT_END; +} // end of TNeighborAgentProps::getLaserScanByNodeID + +template +void +CGraphSlamEngine_MR::TNeighborAgentProps::fillOptPaths( + const std::set& nodeIDs, + paths_t* opt_paths) const { + MRPT_START; + using namespace std; + using namespace mrpt::utils; + typedef std::set::const_iterator set_cit; + + // make sure that all given nodes belong in the nodeIDs_set. + ASSERTMSG_(includes(nodeIDs_set.begin(), nodeIDs_set.end(), + nodeIDs.begin(), nodeIDs.end()), + "nodeIDs is not a strict subset of the current neighbor's nodes"); + ASSERT_(opt_paths); + + for (set_cit cit = nodeIDs.begin(); + cit != std::prev(nodeIDs.end()); + ++cit) { + + for (set_cit cit2 = std::next(cit); + cit2 != nodeIDs.end(); + ++cit2) { + + + constraint_t c = constraint_t(poses.at(*cit2) - poses.at(*cit)); + c.cov_inv = + mrpt::math::CMatrixFixedNumeric(); + c.cov_inv.unit(); + c.cov_inv *= 500; + + opt_paths->push_back(path_t( + /*start=*/ *cit, + /*end=*/ *cit2, + /*constraint=*/ c)); + + engine.logFmt(LVL_DEBUG, + "Filling optimal path for nodes: %lu => %lu: %s", + static_cast(*cit), + static_cast(*cit2), + opt_paths->back().curr_pose_pdf.getMeanVal().asString().c_str()); + + } + } + MRPT_END; +} // end of TNeighborAgentProps::fillOptPaths + +template +void CGraphSlamEngine_MR::TNeighborAgentProps:: +computeGridMap() const { + MRPT_START; + using namespace mrpt::poses; + using namespace mrpt::utils; + using namespace mrpt; + using namespace mrpt::math; + + gridmap_cached->clear(); + + vector_uint nodeIDs; + std::map nodes_params; + // get list of nodes, laser scans + this->getCachedNodes(&nodeIDs, &nodes_params, false); + + // iterate over poses, scans. Add them to the gridmap. + for (typename map::const_iterator + it = nodes_params.begin(); + it != nodes_params.end(); + ++it) { + const CPose3D curr_pose_3d; // do not add the actual pose! + gridmap_cached->insertObservation(it->second.scan.pointer(), &curr_pose_3d); + } + + MRPT_END; +} + +template +const mrpt::maps::COccupancyGridMap2DPtr& +CGraphSlamEngine_MR::TNeighborAgentProps:: +getGridMap() const { + MRPT_START; + if (hasNewData()) { + this->computeGridMap(); + } + + return gridmap_cached; + MRPT_END; +} + +template +void CGraphSlamEngine_MR::TNeighborAgentProps:: +getGridMap(mrpt::maps::COccupancyGridMap2DPtr& map) const { + MRPT_START; + ASSERT_(map.present()); + + if (hasNewData()) { + this->computeGridMap(); + } + + map->copyMapContentFrom(*gridmap_cached); + MRPT_END; +} + +template +bool CGraphSlamEngine_MR::TNeighborAgentProps::hasNewData() const { + return has_new_nodes && has_new_scans; +} + +} } // end of namespaces + +#endif /* end of include guard: CGRAPHSLAMENGINE_MR_IMPL_H */ diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamEngine_ROS.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamEngine_ROS.h new file mode 100644 index 0000000..4251e2e --- /dev/null +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamEngine_ROS.h @@ -0,0 +1,109 @@ +/* +---------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | http://www.mrpt.org/ | + | | + | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | + | See: http://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See details in http://www.mrpt.org/License | + +---------------------------------------------------------------------------+ */ + +#ifndef CGRAPHSLAMENGINE_ROS_H +#define CGRAPHSLAMENGINE_ROS_H + +#include "mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_ROS.h" + +#include +#include + +namespace mrpt { namespace graphslam { + +/**\brief Class template that provides a wrapper around the MRPT + * CGraphSlamEngine class template and implements methods for interacting with + * ROS. + * + */ +template +class CGraphSlamEngine_ROS : public CGraphSlamEngine +{ +public: + typedef CGraphSlamEngine parent; + + CGraphSlamEngine_ROS( + ros::NodeHandle* nh, + const std::string& config_file, + const std::string& rawlog_fname="", + const std::string& fname_GT="", + mrpt::graphslam::CWindowManager* win_manager=NULL, + mrpt::graphslam::deciders::CNodeRegistrationDecider* node_reg=NULL, + mrpt::graphslam::deciders::CEdgeRegistrationDecider* edge_reg=NULL, + mrpt::graphslam::optimizers::CGraphSlamOptimizer* optimizer=NULL + ); + virtual ~CGraphSlamEngine_ROS(); + /**\brief Wrapper method around the protected setup* class methods. + * + * Handy for setting up publishers, subscribers, services + * all at once. + * + */ + void setupComm(); + + /**\brief Initialize object instance. */ + void initClass(); + ros::NodeHandle* m_nh; +protected: + /**\brief Read the problem configuration parameters + * + * \sa readROSParameters + */ + void readParams(); + /**\brief Provide feedback about the SLAM operation + * + * Method makes the necessary calls to all the publishers of the class and + * broadcaster instances + */ + virtual void usePublishersBroadcasters(); + /**\brief Read configuration parameters from the ROS parameter server. + * + * \note Method is automatically called on object construction. + * + * \sa readParams, initClass + */ + void readROSParameters(); + virtual bool _execGraphSlamStep( + mrpt::obs::CActionCollectionPtr& action, + mrpt::obs::CSensoryFramePtr& observations, + mrpt::obs::CObservationPtr& observation, + size_t& rawlog_entry); + + /**\name setup* ROS-related methods + *\brief Methods for setting up topic subscribers, publishers, and + * corresponding services + * + * \sa setupComm + */ + /**\{*/ + virtual void setupSubs(); + virtual void setupPubs(); + virtual void setupSrvs(); + /**\}*/ + /**\brief Custom Callback queue for processing requests for the + * services outside the standard CallbackQueue. + * + * \note Logical thing would be to define it in CGraphSlamEngine_MR, but that + * results in an segfault with the following error message: + * ``` + * pure virtual method called + * terminate called without an active exception + * ``` + */ + ros::CallbackQueue custom_service_queue; + + + int m_queue_size; +}; + +} } // end of namespaces + +#include "mrpt_graphslam_2d/CGraphSlamEngine_ROS_impl.h" + +#endif /* end of include guard: CGRAPHSLAMENGINE_ROS_H */ diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamEngine_ROS_impl.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamEngine_ROS_impl.h new file mode 100644 index 0000000..a371497 --- /dev/null +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamEngine_ROS_impl.h @@ -0,0 +1,144 @@ +/* +---------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | http://www.mrpt.org/ | + | | + | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | + | See: http://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See details in http://www.mrpt.org/License | + +---------------------------------------------------------------------------+ */ + +#ifndef CGRAPHSLAMENGINE_ROS_IMPL_H +#define CGRAPHSLAMENGINE_ROS_IMPL_H +namespace mrpt { namespace graphslam { + +template +CGraphSlamEngine_ROS::CGraphSlamEngine_ROS( + ros::NodeHandle* nh, + const std::string& config_file, + const std::string& rawlog_fname/* ="" */, + const std::string& fname_GT /* ="" */, + mrpt::graphslam::CWindowManager* win_manager /* = NULL */, + mrpt::graphslam::deciders::CNodeRegistrationDecider* node_reg /* = NULL */, + mrpt::graphslam::deciders::CEdgeRegistrationDecider* edge_reg /* = NULL */, + mrpt::graphslam::optimizers::CGraphSlamOptimizer* optimizer /* = NULL */): + m_nh(nh), + parent::CGraphSlamEngine( + config_file, + rawlog_fname, + fname_GT, + win_manager, + node_reg, + edge_reg, + optimizer) +{ + this->initClass(); +} + +template +CGraphSlamEngine_ROS::~CGraphSlamEngine_ROS() { + MRPT_LOG_DEBUG_STREAM("In Destructor: Deleting CGraphSlamEngine_ROS instance..."); + ros::shutdown(); +} + +template +void CGraphSlamEngine_ROS::initClass() { + using namespace mrpt::graphslam; + + ASSERT_(m_nh); + + this->m_class_name = "CGraphSlamEngine_ROS"; + this->setLoggerName(this->m_class_name); + + // http://wiki.ros.org/rospy/Overview/Publishers%20and%20Subscribers#queue_size:_publish.28.29_behavior_and_queuing + m_queue_size = 10; + + this->setupComm(); + + // in case of ROS specific deciders/optimizers (they inherit from the CDeciderOrOptimizer_ROS interface) + // set the NodeHandle + { + CRegistrationDeciderOrOptimizer_ROS* dec_opt_ros = + dynamic_cast*>(this->m_node_reg); + + if (dec_opt_ros) { + dec_opt_ros->setNodeHandle(m_nh); + } + } + { + CRegistrationDeciderOrOptimizer_ROS* dec_opt_ros = + dynamic_cast*>(this->m_edge_reg); + if (dec_opt_ros) { + dec_opt_ros->setNodeHandle(m_nh); + } + } + { + CRegistrationDeciderOrOptimizer_ROS* dec_opt_ros = + dynamic_cast*>(this->m_optimizer); + if (dec_opt_ros) { + dec_opt_ros->setNodeHandle(m_nh); + } + } + + this->readParams(); +} + +template +void CGraphSlamEngine_ROS::readParams() { + + this->readROSParameters(); +} + +template +void CGraphSlamEngine_ROS::readROSParameters() { } + +template +bool CGraphSlamEngine_ROS::_execGraphSlamStep( + mrpt::obs::CActionCollectionPtr& action, + mrpt::obs::CSensoryFramePtr& observations, + mrpt::obs::CObservationPtr& observation, + size_t& rawlog_entry) { + + bool continue_exec = parent::_execGraphSlamStep( + action, observations, observation, rawlog_entry); + this->usePublishersBroadcasters(); + + return continue_exec; +} + +template +void CGraphSlamEngine_ROS::usePublishersBroadcasters() { } + +template +void CGraphSlamEngine_ROS::setupComm() { + MRPT_LOG_INFO_STREAM( + "Setting up subscribers, publishers, services..."); + + // setup subscribers, publishers, services... + this->setupSubs(); + this->setupPubs(); + this->setupSrvs(); + +} + +template +void CGraphSlamEngine_ROS::setupSubs() { + MRPT_LOG_DEBUG_STREAM("Setting up subscribers..."); + +} + +template +void CGraphSlamEngine_ROS::setupPubs() { + MRPT_LOG_DEBUG_STREAM("Setting up publishers..."); + +} + +template +void CGraphSlamEngine_ROS::setupSrvs() { + MRPT_LOG_DEBUG_STREAM("Setting up services..."); + +} + + +} } // end of namespaces + +#endif /* end of include guard: CGRAPHSLAMENGINE_ROS_IMPL_H */ diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamHandler_ROS.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamHandler_ROS.h new file mode 100644 index 0000000..243abc8 --- /dev/null +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamHandler_ROS.h @@ -0,0 +1,343 @@ +/* +---------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | http://www.mrpt.org/ | + | | + | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | + | See: http://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See details in http://www.mrpt.org/License | + +---------------------------------------------------------------------------+ */ + +#ifndef CGRAPHSLAMHANDLER_ROS_H +#define CGRAPHSLAMHANDLER_ROS_H + +// ROS +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +// MRPT +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mrpt_graphslam_2d/CGraphSlamEngine_ROS.h" +#include "mrpt_graphslam_2d/CGraphSlamEngine_MR.h" +#include "mrpt_graphslam_2d/TUserOptionsChecker_ROS.h" + + +// cpp headers +#include +#include +#include + +namespace mrpt { namespace graphslam { namespace apps { + +/**\brief Manage variables, ROS parameters and everything else related to the + * graphslam-engine ROS wrapper. + */ +template +class CGraphSlamHandler_ROS : + public CGraphSlamHandler +{ + public: + /**\brief type of graph constraints */ + typedef typename GRAPH_T::constraint_t constraint_t; + /**\brief type of underlying poses (2D/3D). */ + typedef typename GRAPH_T::constraint_t::type_value pose_t; + + /**\brief Handy self type */ + typedef CGraphSlamHandler_ROS self_t; + /**\brief Handy parent type */ + typedef CGraphSlamHandler parent_t; + + CGraphSlamHandler_ROS( + mrpt::utils::COutputLogger* logger, + TUserOptionsChecker* options_checker, + ros::NodeHandle* nh_in); + ~CGraphSlamHandler_ROS(); + + + void getParamsAsString(std::string* str_out); + std::string getParamsAsString(); + + /**\brief Read the problem configuration parameters + * + * \sa readROSParameters, printParams + */ + void readParams(); + /**\brief Print in a compact manner the overall problem configuration + * parameters + */ + void printParams(); + + /**\name Sniffing methods + * + * Sniff measurements off their corresponding topics + */ + /**\{*/ + /**\brief Callback method for handling incoming odometry measurements in a ROS + * topic. + */ + void sniffOdom(const nav_msgs::Odometry::ConstPtr& ros_odom); + /**\brief Callback method for handling incoming LaserScans objects in a ROS + * topic. + */ + void sniffLaserScan(const sensor_msgs::LaserScan::ConstPtr& ros_laser_scan); + void sniffCameraImage(); + /** TODO - Implement this */ + void sniff3DPointCloud(); + /**\}*/ + /**\brief Indicate whether graphslam execution can proceed normally. + * \return False if user has demanded to exit (pressed ), True otherwise + */ + bool continueExec(); + /**\brief Generate the relevant report directory/files after the graphSLAM + * execution. + */ + void generateReport(); + /**\brief Provide feedback about the SLAM operation using ROS publilshers, + * update the registered frames using the tf2_ros::TransformBroadcaster + * + * Method makes the necessary calls to all the publishers of the class and + * broadcaster instances + * + * \sa continueExec + * \return True if the graphSLAM execution is to continue normally, false + * otherwise + */ + bool usePublishersBroadcasters(); + /**\brief Wrapper method around the protected setup* class methods. + * + * Handy for setting up publishers, subscribers, services, TF-related stuff + * all at once from the user application + * + * \note method should be called right after the call to + * CGraphSlamHandler_ROS::readParams method + */ + void setupComm(); + + /**\brief Initialize the CGraphslamEngine_* object + * + * The CGraphSlamEngine instance is to be instaniated depending on the user + * application at hand. User should call this method just after reading the + * problem parameters. + */ + /**\{*/ + void initEngine_ROS(); + void initEngine_MR(); + /**\}*/ + + static const std::string sep_header; + static const std::string sep_subheader; + + private: + /**\brief Process an incoming measurement. + * + * Method is a wrapper around the _process method + * + * \note Method is automatically called when a new measurement is fetched + * from a subscribed topic + * + * \sa _process + */ + void processObservation(mrpt::obs::CObservationPtr& observ); + /**\brief Low level wrapper for executing the + * CGraphSlamEngine_ROS::execGraphSlamStep method + * + * \sa processObservation(); + */ + void _process(mrpt::obs::CObservationPtr& observ); + /**\brief read configuration parameters from the ROS parameter server. + * + * \sa readParams + */ + void readROSParameters(); + void readStaticTFs(); + /**\brief Fill in the given string with the parameters that have been read + * from the ROS parameter server + * + * \sa getParamsAsString, readROSParameters + */ + void getROSParameters(std::string* str_out); + /**\brief Verify that the parameters read are valid and can be used with the + * CGraphSlamEngine_ROS instance. + */ + void verifyUserInput(); + + /**\brief Reset the flags indicating whether we have received new data + * (odometry, laser scans etc.) + */ + void resetReceivedFlags(); + /**\name setup* ROS-related methods + *\brief Methods for setting up topic subscribers, publishers, and + * corresponding services + * + * \sa setupComm + */ + /**\{*/ + void setupSubs(); + void setupPubs(); + void setupSrvs(); + /**\}*/ + /**\brief Pointer to the Ros NodeHanle instance */ + ros::NodeHandle* m_nh; + + // ROS server parameters + /**\name node, edge, optimizer modules in string representation */ + /**\{*/ + std::string m_node_reg; + std::string m_edge_reg; + std::string m_optimizer; + /**\}*/ + + /**\brief Minimum logging level + * + * \note Value is fetched from the ROS Parameter Server (not from the + * external .ini file. + */ + mrpt::utils::VerbosityLevel m_min_logging_level; + + bool m_has_read_config; + /**\name Received measurements - boolean flags + * + * \brief Flags that indicate if any new measurements have arrived in the + * corresponding topics. + */ + /**\{*/ + bool m_received_odom; + bool m_received_laser_scan; + bool m_received_camera; + bool m_received_point_cloud; + /**\}*/ + + /**\name Processed measurements + * + * Measurements that the class can the class instance is keeping track + * of and passes to the CGraphSlamEngine_ROS instance. + */ + /**\{*/ + /**\brief Received laser scan - converted into MRPT CObservation* format */ + mrpt::obs::CObservationOdometryPtr m_mrpt_odom; + mrpt::obs::CObservation2DRangeScanPtr m_mrpt_laser_scan; + /**\}*/ + + /**\name Subscribers - Publishers + * + * ROS Topic Subscriber/Publisher instances + * */ + /**\{*/ + ros::Subscriber m_odom_sub; + ros::Subscriber m_laser_scan_sub; + ros::Subscriber m_camera_scan_sub; + ros::Subscriber m_point_cloud_scan_sub; + + ros::Publisher m_curr_robot_pos_pub; + ros::Publisher m_robot_trajectory_pub; + ros::Publisher m_robot_tr_poses_pub; + ros::Publisher m_gt_trajectory_pub; // TODO + ros::Publisher m_SLAM_eval_metric_pub; // TODO + ros::Publisher m_odom_trajectory_pub; + ros::Publisher m_gridmap_pub; + ros::Publisher m_stats_pub; + /**\}*/ + + /**\name Topic Names + * + * Names of the topics that the class instance subscribes or publishes to + */ + /**\{*/ + std::string m_odom_topic; + std::string m_laser_scan_topic; + std::string m_camera_topic; + std::string m_point_cloud_topic; + + std::string m_curr_robot_pos_topic; + std::string m_robot_trajectory_topic; + std::string m_robot_tr_poses_topic; + std::string m_odom_trajectory_topic; + std::string m_gridmap_topic; + std::string m_stats_topic; + /**\}*/ + + /**\name TransformBroadcasters - TransformListeners + */ + /**\{*/ + tf2_ros::Buffer m_buffer; + tf2_ros::TransformBroadcaster m_broadcaster; + /**\}*/ + + /**\name TF Frame IDs + * Names of the current TF Frames available + */ + /**\{*/ + /**\brief Frame that the robot starts from. In a single-robot SLAM + * setup this can be set to the world frame + */ + std::string m_anchor_frame_id; + std::string m_base_link_frame_id; + std::string m_odom_frame_id; + /**\}*/ + + /**\name Geometrical Configuration + * \brief Variables that setup the geometrical dimensions, distances between + * the different robot parts etc. + */ + /**\{*/ + geometry_msgs::TransformStamped m_anchor_odom_transform; + /**\}*/ + + /**\brief Odometry path of the robot. + * Handy mostly for visualization reasons. + */ + nav_msgs::Path m_odom_path; + + /**\brief Times a messge has been published => usePublishersBroadcasters method is called + */ + int m_pub_seq; + int m_stats_pub_seq; + + /**\brief Total counter of the processed measurements + */ + size_t m_measurement_cnt; + + /**\brief ROS topic publisher standard queue size */ + int m_queue_size; + + size_t m_graph_nodes_last_size; + + /**\brief Initial offset of the received odometry. + * + * Assumption is that in the beginning I have 0 position, thus the incoming + * odometry for the algorithm has to be 0 */ + bool m_first_time_in_sniff_odom; + pose_t m_input_odometry_offset; +}; + +} } } // end of namespaces + +#include "mrpt_graphslam_2d/CGraphSlamHandler_ROS_impl.h" + +#endif /* end of include guard: CGRAPHSLAMHANDLER_ROS_H */ diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamHandler_ROS_impl.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamHandler_ROS_impl.h new file mode 100644 index 0000000..7a24924 --- /dev/null +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamHandler_ROS_impl.h @@ -0,0 +1,752 @@ +/* +---------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | http://www.mrpt.org/ | + | | + | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | + | See: http://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See details in http://www.mrpt.org/License | + +---------------------------------------------------------------------------+ */ +#ifndef CGRAPHSLAMHANDLER_ROS_IMPL_H +#define CGRAPHSLAMHANDLER_ROS_IMPL_H + +namespace mrpt { namespace graphslam { namespace apps { + +using mrpt::utils::LVL_DEBUG; +using mrpt::utils::LVL_INFO; +using mrpt::utils::LVL_WARN; +using mrpt::utils::LVL_ERROR; + +// static member variables +template +const std::string CGraphSlamHandler_ROS::sep_header(40, '='); + +template +const std::string CGraphSlamHandler_ROS::sep_subheader(20, '-'); + +// Ctor +template +CGraphSlamHandler_ROS::CGraphSlamHandler_ROS( + mrpt::utils::COutputLogger* logger, + TUserOptionsChecker* options_checker, + ros::NodeHandle* nh_in): + m_nh(nh_in), + parent_t(logger, options_checker, /*enable_visuals=*/ false) +{ + using namespace mrpt::obs; + + ASSERT_(m_nh); + + // TODO - does this affect? + // Previous value = 0; + m_queue_size = 1; + + // variables initialization/assignment + m_has_read_config = false; + m_pub_seq = 0; + m_stats_pub_seq = 0; + this->resetReceivedFlags(); + + // measurements initialization + m_mrpt_odom = CObservationOdometry::Create(); + m_mrpt_odom->hasEncodersInfo = false; + m_mrpt_odom->hasVelocities = false; + m_first_time_in_sniff_odom = true; + + m_measurement_cnt = 0; + + // Thu Nov 3 23:36:49 EET 2016, Nikos Koukis + // WARNING: ROS Server Parameters have not been read yet. Make sure you know + // what to initialize at this stage! +} + +template +CGraphSlamHandler_ROS::~CGraphSlamHandler_ROS() { } + +template +void CGraphSlamHandler_ROS::readParams() { + this->readROSParameters(); + + ASSERT_(!this->m_ini_fname.empty()); + parent_t::readConfigFname(this->m_ini_fname); + + m_has_read_config = true; +} + +template +void CGraphSlamHandler_ROS::readROSParameters() { + using namespace mrpt::utils; + + // misc + { + std::string ns = "misc/"; + + // enable/disable visuals + bool m_disable_MRPT_visuals; + m_nh->param(ns + "disable_MRPT_visuals", m_disable_MRPT_visuals, false); + this->m_enable_visuals = !m_disable_MRPT_visuals; + + // verbosity level + int lvl; + m_nh->param(ns + "verbosity", + lvl, + static_cast(LVL_INFO)); + m_min_logging_level = static_cast(lvl); + this->m_logger->setMinLoggingLevel(m_min_logging_level); + } + // deciders, optimizer + { + std::string ns = "deciders_optimizers/"; + m_nh->param(ns + "NRD", m_node_reg, "CFixedIntervalsNRD"); + m_nh->param(ns + "ERD", m_edge_reg, "CICPCriteriaERD"); + m_nh->param(ns + "GSO", m_optimizer, "CLevMarqGSO"); + } + // filenames + { + std::string ns = "files/"; + + // configuration file - mandatory + std::string config_param_path = ns + "config"; + bool found_config = m_nh->getParam(ns + "config", this->m_ini_fname); + ASSERTMSG_(found_config, + mrpt::format( + "Configuration file was not set. Set %s and try again.\nExiting...", + config_param_path.c_str())); + + // ground-truth file + m_nh->getParam(ns + "ground_truth", this->m_gt_fname); + } + + // TF Frame IDs + // names of the frames of the corresponding robot parts + { + std::string ns = "frame_IDs/"; + + m_nh->param(ns + "anchor_frame" , m_anchor_frame_id, "map"); + m_nh->param(ns + "base_link_frame" , m_base_link_frame_id, "base_link"); + m_nh->param(ns + "odometry_frame" , m_odom_frame_id, "odom"); + + // take action based on the above frames + // + + } + + // ASSERT that the given user options are valid + // Fill the TuserOptionsChecker related structures + this->m_options_checker->createDeciderOptimizerMappings(); + this->m_options_checker->populateDeciderOptimizerProperties(); + this->verifyUserInput(); + + this->m_logger->logFmt(LVL_DEBUG, + "Successfully read parameters from ROS Parameter Server"); + + // Visuals initialization + if (this->m_enable_visuals) { + this->initVisualization(); + } +} // end of readROSParameters + +template +void CGraphSlamHandler_ROS::readStaticTFs() { } + +template +void CGraphSlamHandler_ROS::initEngine_ROS() { + + this->m_logger->logFmt(LVL_WARN, + "Initializing CGraphSlamEngine_ROS instance..."); + this->m_engine = new CGraphSlamEngine_ROS( + m_nh, + this->m_ini_fname, + /*rawlog_fname=*/ "", + this->m_gt_fname, + this->m_win_manager, + this->m_options_checker->node_regs_map[m_node_reg](), + this->m_options_checker->edge_regs_map[m_edge_reg](), + this->m_options_checker->optimizers_map[m_optimizer]()); + this->m_logger->logFmt(LVL_WARN, + "Successfully initialized CGraphSlamEngine_ROS instance."); + +} + +template +void CGraphSlamHandler_ROS::initEngine_MR() { + + this->m_options_checker->node_regs_map[m_node_reg](); + this->m_options_checker->edge_regs_map[m_edge_reg](); + this->m_options_checker->optimizers_map[m_optimizer](); + + this->m_logger->logFmt(LVL_WARN, + "Initializing CGraphSlamEngine_MR instance..."); + this->m_engine = new CGraphSlamEngine_MR( + m_nh, + this->m_ini_fname, + /*rawlog_fname=*/ "", + this->m_gt_fname, + this->m_win_manager, + this->m_options_checker->node_regs_map[m_node_reg](), + this->m_options_checker->edge_regs_map[m_edge_reg](), + this->m_options_checker->optimizers_map[m_optimizer]()); + this->m_logger->logFmt(LVL_WARN, + "Successfully initialized CGraphSlamEngine_MR instance."); + +} + +template +void CGraphSlamHandler_ROS::getROSParameters(std::string* str_out) { + using namespace mrpt::utils; + + ASSERT_(str_out); + + stringstream ss(""); + + + ss << "ROS Parameters: " << endl; + ss << sep_header << endl; + ss << endl; + + ss << "Deciders / Optimizers = " << endl; + ss << sep_subheader << endl; + ss << "Node Registration Decider = " << m_node_reg << endl; + ss << "Edge Registration Decider = " << m_edge_reg << endl; + ss << "GraphSLAM Optimizer = " << m_optimizer << endl; + ss << endl; + + ss << "Filenames: " << endl; + ss << sep_subheader << endl; + ss << "Configuration .ini file = " << this->m_ini_fname << endl; + ss << "Ground truth filename = " << (!this->m_gt_fname.empty() + ? this->m_gt_fname : "NONE") + << endl; + ss << endl; + + ss << "Miscellaneous: " << endl; + ss << sep_subheader << endl; + ss << "Enable MRPT visuals? = " << + (this->m_enable_visuals? "TRUE" : "FALSE") + << endl; + ss << "Logging verbosity Level = " << + COutputLogger::logging_levels_to_names[m_min_logging_level] << endl;; + ss << endl; + + *str_out = ss.str(); +} + +template +void CGraphSlamHandler_ROS::getParamsAsString(std::string* str_out) { + ASSERT_(str_out); + + // ros parameters + std::string ros_params(""); + this->getROSParameters(&ros_params); + std::string mrpt_params(""); + parent_t::getParamsAsString(&mrpt_params); + + *str_out += ros_params; + *str_out += "\n\n"; + *str_out += mrpt_params; + + // various parameters +} + +template +std::string CGraphSlamHandler_ROS::getParamsAsString() { + std::string params; + this->getParamsAsString(¶ms); + return params; +} + +template +void CGraphSlamHandler_ROS::printParams() { + parent_t::printParams(); + cout << this->getParamsAsString() << endl; +} + +template +void CGraphSlamHandler_ROS::verifyUserInput() { + this->m_logger->logFmt(LVL_DEBUG, "Verifying user input..."); + + + // verify the NRD, ERD, GSO parameters + bool node_success, edge_success, optimizer_success; + bool failed = false; + + node_success = + this->m_options_checker->checkRegistrationDeciderExists( + m_node_reg, "node"); + edge_success = + this->m_options_checker->checkRegistrationDeciderExists( + m_edge_reg, "edge"); + optimizer_success = + this->m_options_checker->checkOptimizerExists( + m_optimizer); + + if (!node_success) { + this->m_logger->logFmt(LVL_ERROR, + "\nNode Registration Decider \"%s\" is not available", + m_node_reg.c_str()); + this->m_options_checker->dumpRegistrarsToConsole("node"); + failed = true; + } + if (!edge_success) { + this->m_logger->logFmt(LVL_ERROR, + "\nEdge Registration Decider \"%s\" is not available.", + m_edge_reg.c_str()); + this->m_options_checker->dumpRegistrarsToConsole("edge"); + failed = true; + } + if (!optimizer_success) { + this->m_logger->logFmt(LVL_ERROR, + "\ngraphSLAM Optimizser \"%s\" is not available.", + m_optimizer.c_str()); + this->m_options_checker->dumpOptimizersToConsole(); + failed = true; + } + ASSERT_(!failed) + + // verify that the path to the files is correct + // .ini file + bool ini_exists = system::fileExists(this->m_ini_fname); + ASSERTMSG_(ini_exists, + format( + "\n.ini configuration file \"%s\"doesn't exist. " + "Please specify a valid pathname.\nExiting...\n", + this->m_ini_fname.c_str())); + // ground-truth file + if (!this->m_gt_fname.empty()) { + bool gt_exists = system::fileExists(this->m_gt_fname); + ASSERTMSG_(gt_exists, + format( + "\nGround truth file \"%s\"doesn't exist." + "Either unset the corresponding ROS parameter or specify a valid pathname.\n" + "Exiting...\n", + this->m_gt_fname.c_str())); + } + +} + +template +void CGraphSlamHandler_ROS::setupComm() { + + this->m_logger->logFmt(LVL_INFO, + "Setting up ROS-related subscribers, publishers, services..."); + + // setup subscribers, publishers, services... + this->setupSubs(); + this->setupPubs(); + this->setupSrvs(); + + // fetch the static geometrical transformations + //this->readStaticTFs(); + +} + +template +void CGraphSlamHandler_ROS::setupSubs() { + this->m_logger->logFmt(LVL_INFO, "Setting up the subscribers..."); + + // setup the names + std::string ns = "input/"; + + m_odom_topic = ns + "odom"; + m_laser_scan_topic = ns + "laser_scan"; + + // odometry + m_odom_sub = m_nh->subscribe( + m_odom_topic, + m_queue_size, + &self_t::sniffOdom, this); + + // laser_scans + m_laser_scan_sub = m_nh->subscribe( + m_laser_scan_topic, + m_queue_size, + &self_t::sniffLaserScan, this); + + // camera + // TODO + + // 3D point clouds + // TODO + +} + +template +void CGraphSlamHandler_ROS::setupPubs() { + this->m_logger->logFmt(LVL_INFO, "Setting up the publishers..."); + + // setup the names + std::string ns = "feedback/"; + + m_curr_robot_pos_topic = ns + "robot_position"; + m_robot_trajectory_topic = ns + "robot_trajectory"; + m_robot_tr_poses_topic = ns + "robot_tr_poses"; + m_odom_trajectory_topic = ns + "odom_trajectory"; + m_gridmap_topic = ns + "gridmap"; + m_stats_topic = ns + "graphslam_stats"; + + // setup the publishers + + // agent estimated position + m_curr_robot_pos_pub = m_nh->advertise( + m_curr_robot_pos_topic, + m_queue_size); + m_robot_trajectory_pub = m_nh->advertise( + m_robot_trajectory_topic, + m_queue_size); + m_robot_tr_poses_pub = m_nh->advertise( + m_robot_tr_poses_topic, + m_queue_size); + + // odometry nav_msgs::Path + m_odom_path.header.seq = 0; + m_odom_path.header.stamp = ros::Time::now(); + m_odom_path.header.frame_id = m_anchor_frame_id; + + m_odom_trajectory_pub = m_nh->advertise( + m_odom_trajectory_topic, + m_queue_size); + + // generated gridmap + m_gridmap_pub = m_nh->advertise( + m_gridmap_topic, + m_queue_size, + /*latch=*/true); + + m_stats_pub = m_nh->advertise( + m_stats_topic, + m_queue_size, + /*latch=*/true); + +} + +template +void CGraphSlamHandler_ROS::setupSrvs() { + this->m_logger->logFmt(LVL_INFO, "Setting up the services..."); + + // SLAM statistics + // Error statistics + +} + +template +bool CGraphSlamHandler_ROS::usePublishersBroadcasters() { + using namespace mrpt::utils; + MRPT_START; + bool ret_val = true; + + ros::Time timestamp = ros::Time::now(); + + // current MRPT robot pose + pose_t mrpt_pose = this->m_engine->getCurrentRobotPosEstimation(); + + // + // convert pose_t to corresponding geometry_msg::TransformStamped + // anchor frame <=> base_link + // + geometry_msgs::TransformStamped anchor_base_link_transform; + anchor_base_link_transform.header.stamp = ros::Time::now(); + anchor_base_link_transform.header.frame_id = m_anchor_frame_id; + anchor_base_link_transform.child_frame_id = m_base_link_frame_id; + + // translation + anchor_base_link_transform.transform.translation.x = mrpt_pose.x(); + anchor_base_link_transform.transform.translation.y = mrpt_pose.y(); + anchor_base_link_transform.transform.translation.z = 0; + + // rotation + anchor_base_link_transform.transform.rotation = + tf::createQuaternionMsgFromYaw(mrpt_pose.phi()); + + // TODO - potential error in the rotation, investigate this + m_broadcaster.sendTransform(anchor_base_link_transform); + + // anchor frame <=> odom frame + // + // make sure that we have received odometry information in the first + // place... + // the corresponding field would be initialized + if (!m_anchor_odom_transform.child_frame_id.empty()) { + m_broadcaster.sendTransform(m_anchor_odom_transform); + } + + // set an arrow indicating the current orientation of the robot + { + geometry_msgs::PoseStamped geom_pose; + geom_pose.header.stamp = timestamp; + geom_pose.header.seq = m_pub_seq; + geom_pose.header.frame_id = m_anchor_frame_id; // with regards to base_link... + + // position + mrpt_bridge::convert(mrpt_pose, geom_pose.pose); + m_curr_robot_pos_pub.publish(geom_pose); + } + + // robot trajectory + // publish the trajectory of the robot + { + this->m_logger->logFmt(LVL_DEBUG, + "Publishing the current robot trajectory"); + typename GRAPH_T::global_poses_t graph_poses; + this->m_engine->getRobotEstimatedTrajectory(&graph_poses); + + nav_msgs::Path path; + + // set the header + path.header.stamp = timestamp; + path.header.seq = m_pub_seq; + path.header.frame_id = m_anchor_frame_id; + + // + // fill in the pose as well as the nav_msgs::Path at the same time. + // + + geometry_msgs::PoseArray geom_poses; + geom_poses.header.stamp = timestamp; + geom_poses.header.frame_id = m_anchor_frame_id; + + for (auto n_cit = graph_poses.begin(); + n_cit != graph_poses.end(); + ++n_cit) { + + geometry_msgs::PoseStamped geom_pose_stamped; + geometry_msgs::Pose geom_pose; + + // grab the pose - convert to geometry_msgs::Pose format + const pose_t& mrpt_pose = n_cit->second; + mrpt_bridge::convert(mrpt_pose, geom_pose); + geom_poses.poses.push_back(geom_pose); + geom_pose_stamped.pose = geom_pose; + + // edit the header + geom_pose_stamped.header.stamp = timestamp; + geom_pose_stamped.header.seq = m_pub_seq; + geom_pose_stamped.header.frame_id = m_anchor_frame_id; + + path.poses.push_back(geom_pose_stamped); + } + + // publish only on new node addition + if (this->m_engine->getGraph().nodeCount() > m_graph_nodes_last_size) { + m_robot_tr_poses_pub.publish(geom_poses); + m_robot_trajectory_pub.publish(path); + } + } + + // Odometry trajectory - nav_msgs::Path + m_odom_trajectory_pub.publish(m_odom_path); + + // generated gridmap + // publish only on new node addition + if (this->m_engine->getGraph().nodeCount() > m_graph_nodes_last_size) { + std_msgs::Header h; + mrpt::system::TTimeStamp mrpt_time; + mrpt::maps::COccupancyGridMap2DPtr mrpt_gridmap = + mrpt::maps::COccupancyGridMap2D::Create(); + this->m_engine->getMap(mrpt_gridmap, &mrpt_time); + + // timestamp + mrpt_bridge::convert(mrpt_time, h.stamp); + h.seq = m_pub_seq; + h.frame_id = m_anchor_frame_id; + + // nav gridmap + nav_msgs::OccupancyGrid nav_gridmap; + mrpt_bridge::convert(*mrpt_gridmap, nav_gridmap, h); + m_gridmap_pub.publish(nav_gridmap); + } + + // GraphSlamStats publishing + { + mrpt_msgs::GraphSlamStats stats; + stats.header.seq = m_stats_pub_seq++; + + map node_stats; + map edge_stats; + vector def_energy_vec; + mrpt::system::TTimeStamp mrpt_time; + + this->m_engine->getGraphSlamStats(&node_stats, + &edge_stats, + &mrpt_time); + + // node/edge count + stats.nodes_total = node_stats["nodes_total"]; + stats.edges_total = edge_stats["edges_total"]; + if (edge_stats.find("ICP2D") != edge_stats.end()) { + stats.edges_ICP2D = edge_stats["ICP2D"]; + } + if (edge_stats.find("ICP3D") != edge_stats.end()) { + stats.edges_ICP3D = edge_stats["ICP3D"]; + } + if (edge_stats.find("Odometry") != edge_stats.end()) { + stats.edges_odom = edge_stats["Odometry"]; + } + stats.loop_closures = edge_stats["loop_closures"]; + + // SLAM evaluation metric + this->m_engine->getDeformationEnergyVector( + &stats.slam_evaluation_metric); + + mrpt_bridge::convert(mrpt_time, stats.header.stamp); + + m_stats_pub.publish(stats); + + } + + + + // update the last known size + m_graph_nodes_last_size = this->m_engine->getGraph().nodeCount(); + m_pub_seq++; + + if (this->m_enable_visuals) { + ret_val = this->queryObserverForEvents(); + } + + return ret_val; + MRPT_END; +} // end of usePublishersBroadcasters + +template +void CGraphSlamHandler_ROS::sniffLaserScan( + const sensor_msgs::LaserScan::ConstPtr& ros_laser_scan) { + using namespace std; + using namespace mrpt::utils; + using namespace mrpt::obs; + + this->m_logger->logFmt( + LVL_DEBUG, + "sniffLaserScan: Received a LaserScan msg. Converting it to MRPT format..."); + + // build the CObservation2DRangeScan + m_mrpt_laser_scan = CObservation2DRangeScan::Create(); + mrpt::poses::CPose3D rel_pose; // pose is 0. + mrpt_bridge::convert(*ros_laser_scan, rel_pose, *m_mrpt_laser_scan); + + m_received_laser_scan = true; + this->processObservation(m_mrpt_laser_scan); +} // end of sniffLaserScan + +template +void CGraphSlamHandler_ROS::sniffOdom( + const nav_msgs::Odometry::ConstPtr& ros_odom) { + using namespace std; + using namespace mrpt::utils; + using namespace mrpt::obs; + using namespace mrpt::poses; + + this->m_logger->logFmt( + LVL_DEBUG, + "sniffOdom: Received an odometry msg. Converting it to MRPT format..."); + + // update the odometry frame with regards to the anchor + { + // header + m_anchor_odom_transform.header.frame_id = m_anchor_frame_id; + m_anchor_odom_transform.header.stamp = ros_odom->header.stamp; + m_anchor_odom_transform.header.seq = ros_odom->header.seq; + + m_anchor_odom_transform.child_frame_id = m_odom_frame_id; + + // + // copy ros_odom ==> m_anchor_odom + // + + // translation + m_anchor_odom_transform.transform.translation.x = + ros_odom->pose.pose.position.x; + m_anchor_odom_transform.transform.translation.y = + ros_odom->pose.pose.position.y; + m_anchor_odom_transform.transform.translation.z = + ros_odom->pose.pose.position.z; + + // quaternion + m_anchor_odom_transform.transform.rotation = + ros_odom->pose.pose.orientation; + } + + // build and fill an MRPT CObservationOdometry instance for manipulation from + // the main algorithm + mrpt_bridge::convert( + /* src = */ ros_odom->header.stamp, + /* dst = */ m_mrpt_odom->timestamp); + mrpt_bridge::convert( + /* src = */ ros_odom->pose.pose, + /* dst = */ m_mrpt_odom->odometry); + + // if this is the first call odometry should be 0. Decrement by the + // corresponding offset + if (m_first_time_in_sniff_odom) { + m_input_odometry_offset = m_mrpt_odom->odometry; + m_first_time_in_sniff_odom = false; + } + // decrement by the (constant) offset + m_mrpt_odom->odometry = + m_mrpt_odom->odometry - m_input_odometry_offset; + + // add to the overall odometry path + { + geometry_msgs::PoseStamped pose_stamped; + pose_stamped.header = ros_odom->header; + + // just for convenience - convert the MRPT pose back to PoseStamped + mrpt_bridge::convert( + /* src = */ m_mrpt_odom->odometry, + /* des = */ pose_stamped.pose); + m_odom_path.poses.push_back(pose_stamped); + } + + // print the odometry - for debugging reasons... + stringstream ss; + ss << "Odometry - MRPT format:\t" << m_mrpt_odom->odometry << endl; + this->m_logger->logFmt(LVL_DEBUG, "%s", ss.str().c_str()); + + m_received_odom = true; + this->processObservation(m_mrpt_odom); +} // end of sniffOdom + +template +void CGraphSlamHandler_ROS::sniffCameraImage() { + THROW_EXCEPTION("Method is not implemented yet."); + +} +template +void CGraphSlamHandler_ROS::sniff3DPointCloud() { + THROW_EXCEPTION("Method is not implemented yet."); + +} +template +void CGraphSlamHandler_ROS::processObservation( + mrpt::obs::CObservationPtr& observ) { + using namespace mrpt::utils; + using namespace std; + + this->_process(observ); + this->resetReceivedFlags(); + +} + +template +void CGraphSlamHandler_ROS::_process( + mrpt::obs::CObservationPtr& observ) { + using namespace mrpt::utils; + + //this->m_logger->logFmt(LVL_DEBUG, "Calling execGraphSlamStep..."); + + // TODO - use the exit code of execGraphSlamStep to exit?? + if (!this->m_engine->isPaused()) { + this->m_engine->execGraphSlamStep(observ, m_measurement_cnt); + m_measurement_cnt++; + } +} + +template +void CGraphSlamHandler_ROS::resetReceivedFlags() { + m_received_odom = false; + m_received_laser_scan = false; + m_received_camera = false; + m_received_point_cloud = false; +} + +} } } // end of namespaces + +#endif /* end of include guard: CGRAPHSLAMHANDLER_ROS_IMPL_H */ diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamResources.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamResources.h deleted file mode 100644 index 8dedaad..0000000 --- a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamResources.h +++ /dev/null @@ -1,325 +0,0 @@ -/* +---------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | http://www.mrpt.org/ | - | | - | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | - | See: http://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See details in http://www.mrpt.org/License | - +---------------------------------------------------------------------------+ */ - -#ifndef CGRAPHSLAMRESOURCES_H -#define CGRAPHSLAMRESOURCES_H - -// ROS -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -// MRPT -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// cpp headers -#include -#include -#include - -/**\brief Manage variables, ROS parameters and everything else related to the - * graphslam-engine ROS wrapper. - */ -class CGraphSlamResources -{ -public: - /**\brief type of graph constraints */ - typedef typename mrpt::graphs::CNetworkOfPoses2DInf::constraint_t constraint_t; - /**\brief type of underlying poses (2D/3D). */ - typedef typename mrpt::graphs::CNetworkOfPoses2DInf::constraint_t::type_value pose_t; - - CGraphSlamResources( - mrpt::utils::COutputLogger* logger_in, - ros::NodeHandle* nh - ); - ~CGraphSlamResources(); - - - void getParamsAsString(std::string* str_out); - std::string getParamsAsString(); - - /**\brief Read the problem configuration parameters - * - * \sa readROSParameters, printParams - */ - void readParams(); - /**\brief Print in a compact manner the overall problem configuration - * parameters - */ - void printParams(); - - /**\name Sniffing methods - * - * Sniff measurements off their corresponding topics - */ - /**\{*/ - /**\brief Callback method for handling incoming odometry measurements in a ROS - * topic. - */ - void sniffOdom(const nav_msgs::Odometry::ConstPtr& ros_odom); - /**\brief Callback method for handling incoming LaserScans objects in a ROS - * topic. - */ - void sniffLaserScan(const sensor_msgs::LaserScan::ConstPtr& ros_laser_scan); - void sniffCameraImage(); - /** TODO - Implement this */ - void sniff3DPointCloud(); - /**\}*/ - /**\brief Indicate whether graphslam execution can proceed normally. - * \return False if user has demanded to exit (pressed ), True otherwise - */ - bool continueExec(); - /**\brief Generate the relevant report directory/files after the graphSLAM - * execution. - */ - void generateReport(); - /**\brief Provide feedback about the SLAM operation using ROS publilshers, - * update the registered frames using the tf2_ros::TransformBroadcaster - * - * Method makes the necessary calls to all the publishers of the class and - * broadcaster instances - * - * \sa continueExec - * \return True if the graphSLAM execution is to continue normally - */ - bool usePublishersBroadcasters(); - /**\brief Wrapper method around the setup* private class methods. - * - * Handy for setting up publishers, subscribers, services, TF-related stuff - * all at once from the user application - * - * \note method should be called right after the call to - * CGraphSlamResources::readParams method - */ - void setupCommunication(); - - - static const std::string sep_header; - static const std::string sep_subheader; -private: - /**\brief Initialize the CGraphslamEngine object based on the user input. */ - void initGraphSLAM(); - /**\brief Process an incoming measurement. - * - * Method is a wrapper around the _process method - * - * \note Method is automatically called when a new measurement is fetched - * from a subscribed topic - * - * \sa _process - */ - void processObservation(mrpt::obs::CObservationPtr& observ); - /**\brief Low level wrapper for executing the - * CGraphSlamEngine::execGraphSlamStep method - * - * \sa processObservation(); - */ - void _process(mrpt::obs::CObservationPtr& observ); - /**\brief read configuration parameters from the ROS parameter server. - * - * \sa readParams - */ - void readROSParameters(); - void readStaticTFs(); - /**\brief Fill in the given string with the parameters that have been read - * from the ROS parameter server - * - * \sa getParamsAsString, readROSParameters - */ - void getROSParameters(std::string* str_out); - /**\brief Verify that the parameters read are valid and can be used with the - * CGraphSlamEngine instance. - */ - void verifyUserInput(); - - /**\brief Reset the flags indicating whether we have received new data - * (odometry, laser scans etc.) - */ - void resetReceivedFlags(); - /**\name Methods for setting up topic subscribers, publishers, and - * corresponding services - * - * \sa setupCommunication - */ - /**\{*/ - void setupSubs(); - void setupPubs(); - void setupSrvs(); - /**\}*/ - - /**\brief Pointer to the logging instance */ - mrpt::utils::COutputLogger* m_logger; - ros::NodeHandle* nh; - - // ROS server parameters - /**\name node, edge, optimizer modules in string representation */ - /**\{*/ - std::string m_node_reg; - std::string m_edge_reg; - std::string m_optimizer; - /**\}*/ - - /**\name graphslam-engine various filenames */ - /**\{*/ - std::string m_ini_fname; /**<.ini configuration file */ - std::string m_gt_fname; /*** - m_graphslam_engine; - - /**\name Subscribers - Publishers - * - * ROS Topic Subscriber/Publisher instances - * */ - /**\{*/ - ros::Subscriber m_odom_sub; - ros::Subscriber m_laser_scan_sub; - ros::Subscriber m_camera_scan_sub; - ros::Subscriber m_point_cloud_scan_sub; - - ros::Publisher m_curr_robot_pos_pub; - ros::Publisher m_robot_trajectory_pub; - ros::Publisher m_robot_tr_poses_pub; - ros::Publisher m_gt_trajectory_pub; // TODO - ros::Publisher m_SLAM_eval_metric_pub; // TODO - ros::Publisher m_odom_trajectory_pub; - ros::Publisher m_gridmap_pub; - /**\}*/ - - /**\name Topic Names - * - * Names of the topics that the class instance subscribes or publishes to - */ - /**\{*/ - std::string m_odom_topic; - std::string m_laser_scan_topic; - std::string m_camera_topic; - std::string m_point_cloud_topic; - - std::string m_curr_robot_pos_topic; - std::string m_robot_trajectory_topic; - std::string m_robot_tr_poses_topic; - std::string m_odom_trajectory_topic; - std::string m_SLAM_eval_metric_topic; - std::string m_gridmap_topic; // TODO - /**\}*/ - - /**\name TransformBroadcasters - TransformListeners - */ - /**\{*/ - tf2_ros::Buffer m_buffer; - tf2_ros::TransformBroadcaster m_broadcaster; - /**\}*/ - - /**\name TF Frame IDs - * Names of the current TF Frames available - */ - /**\{*/ - /**\brief Frame that the robot starts from. In a single-robot SLAM - * setup this can be set to the world frame - */ - std::string m_anchor_frame_id; - std::string m_base_link_frame_id; - std::string m_odom_frame_id; - std::string m_laser_frame_id; - /**\}*/ - - /**\name Geometrical Configuration - * \brief Variables that setup the geometrical dimensions, distances between - * the different robot parts etc. - */ - /**\{*/ - geometry_msgs::TransformStamped m_base_laser_transform; // TODO - either use it or lose it - geometry_msgs::TransformStamped m_anchor_odom_transform; - /**\}*/ - - /**\brief Odometry path of the robot. - * Handy mostly for visualization reasons. - */ - nav_msgs::Path m_odom_path; - - /**\brief Times a messge has been published => usePublishersBroadcasters method is called - */ - int m_pub_seq; - - /**\brief Total counter of the processed measurements - */ - size_t m_measurement_cnt; - - int m_queue_size; -}; - -#endif /* end of include guard: CGRAPHSLAMRESOURCES_H */ diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CMapMerger.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CMapMerger.h new file mode 100644 index 0000000..be60285 --- /dev/null +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CMapMerger.h @@ -0,0 +1,109 @@ +/* +---------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | http://www.mrpt.org/ | + | | + | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | + | See: http://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See details in http://www.mrpt.org/License | + +---------------------------------------------------------------------------+ */ + +#ifndef CMAPMERGER_H +#define CMAPMERGER_H + +#include "mrpt_graphslam_2d/CConnectionManager.h" +#include "mrpt_graphslam_2d/TNeighborAgentMapProps.h" +#include "mrpt_graphslam_2d/misc/common.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +const mrpt::poses::CPose3D EMPTY_POSE; + +namespace mrpt { namespace graphslam { + +/**\brief Class responsible of the execution of the map_merger_node. + * + * Instance of the graph asks of the generated maps of all the running + * GraphSlamAgents computes a feasible map-merging, if any, and returns the + * resulting global map to the user. + */ +class CMapMerger +{ +public: + typedef std::map maps_t; + typedef std::map neighbor_to_is_used_t; + typedef std::map neighbor_to_rel_pose_t; + + /**\brief Robot trajectory visual object type */ + typedef std::map< + TNeighborAgentMapProps*, + mrpt::opengl::CSetOfLinesPtr> trajectories_t; + typedef std::vector neighbors_t; + CMapMerger( + mrpt::utils::COutputLogger* logger_in, + ros::NodeHandle* nh_in); + ~CMapMerger(); + void mergeMaps(); + /**\brief Query and fetch the list of new graphSLAM agents. + * + * \return True if execution is to continue normally. + */ + bool updateState(); +private: + /**\brief Compact method for monitoring the given keystrokes for the given + * observer. + */ + void monitorKeystrokes(mrpt::graphslam::CWindowObserver* win_observer); + void initWindowVisuals(mrpt::graphslam::CWindowManager* win_manager); + mrpt::graphslam::CWindowManager* initWindowVisuals(); + + /**\brief CConnectionManager instance for fetching the running graphSLAM + * agents + */ + neighbors_t m_neighbors; + std::map m_neighbors_to_windows; + mrpt::utils::COutputLogger* m_logger; + ros::NodeHandle* m_nh; + mrpt::graphslam::detail::CConnectionManager m_conn_manager; + + /**\brief Topic namespace under which current node is going to be publishing. + */ + std::string m_global_ns; + /**\brief Topic namespace under which, options that are used during the map + * alignment procedure are fetched from + */ + std::string m_options_ns; + std::string m_feedback_ns; + size_t m_queue_size; + mrpt::slam::CGridMapAligner::TConfigParams m_alignment_options; + + std::string quit_keypress1; + std::string quit_keypress2; + std::string map_merge_keypress; + + bool save_map_merging_results; + + mrpt::graphslam::CWindowManager* m_fused_map_win_manager; + +}; // end of CMapMerger + +} } // end of namespaces + +#endif /* end of include guard: CMAPMERGER_H */ diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/ERD/CLoopCloserERD_MR.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/ERD/CLoopCloserERD_MR.h new file mode 100644 index 0000000..ff9919b --- /dev/null +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/ERD/CLoopCloserERD_MR.h @@ -0,0 +1,72 @@ +/* +---------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | http://www.mrpt.org/ | + | | + | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | + | See: http://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See details in http://www.mrpt.org/License | + +---------------------------------------------------------------------------+ */ + +#ifndef CLOOPCLOSERERD_MR_H +#define CLOOPCLOSERERD_MR_H + +#include "mrpt_graphslam_2d/interfaces/CEdgeRegistrationDecider_MR.h" +#include +#include +#include + +namespace mrpt { namespace graphslam { namespace deciders { + +/** Loop Closer Edge Registration Decider class that can also be used in + * multi-robot SLAM operations since it can utilize information from other + * robot agents and correct own graph according to the strategy described in [1]. + * + * \note Multi-robot-related classes are suffixed with _MR. + */ +template +class CLoopCloserERD_MR : + public virtual CLoopCloserERD, + public virtual CEdgeRegistrationDecider_MR +{ +public: + /**\brief Handy typedefs */ + /**\{*/ + /**\brief type of graph constraints */ + typedef CLoopCloserERD lc_parent_t; /**< parent class */ + typedef CEdgeRegistrationDecider_MR mr_parent_t; /**< parent class */ + typedef CLoopCloserERD_MR decider_t; /**< handy self type */ + typedef typename lc_parent_t::constraint_t constraint_t; + typedef typename lc_parent_t::pose_t pose_t; + typedef typename lc_parent_t::range_ops_t range_ops_t; + typedef typename lc_parent_t::partitions_t partitions_t; + typedef typename lc_parent_t::nodes_to_scans2D_t nodes_to_scans2D_t; + typedef mrpt::graphslam::CGraphSlamEngine_MR engine_t; + typedef typename GRAPH_T::global_pose_t global_pose_t; + /**\}*/ + + // Ctor, Dtor + CLoopCloserERD_MR(); + ~CLoopCloserERD_MR(); + + // member implementations + bool updateState( + mrpt::obs::CActionCollectionPtr action, + mrpt::obs::CSensoryFramePtr observations, + mrpt::obs::CObservationPtr observation ); + void addBatchOfNodeIDsAndScans( + const std::map< + mrpt::utils::TNodeID, + mrpt::obs::CObservation2DRangeScanPtr>& nodeIDs_to_scans2D); + void addScanMatchingEdges(mrpt::utils::TNodeID curr_nodeID); + void fetchNodeIDsForScanMatching( + const mrpt::utils::TNodeID& curr_nodeID, + std::set* nodes_set); + +protected: + +}; + +} } } // end of namespaces + +#include "CLoopCloserERD_MR_impl.h" +#endif /* end of include guard: CLOOPCLOSERERD_MR_H */ diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/ERD/CLoopCloserERD_MR_impl.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/ERD/CLoopCloserERD_MR_impl.h new file mode 100644 index 0000000..a7d3f2f --- /dev/null +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/ERD/CLoopCloserERD_MR_impl.h @@ -0,0 +1,97 @@ +/* +---------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | http://www.mrpt.org/ | + | | + | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | + | See: http://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See details in http://www.mrpt.org/License | + +---------------------------------------------------------------------------+ */ + +#ifndef CLOOPCLOSERERD_MR_IMPL_H +#define CLOOPCLOSERERD_MR_IMPL_H + +namespace mrpt { namespace graphslam { namespace deciders { + +// Ctors, Dtors +template +CLoopCloserERD_MR::CLoopCloserERD_MR() { + // CLoopCloser Ctor is automatically called. + + // since this is for MR-SLAM, we do expect more than one node registered + // between successive calls + this->m_override_registered_nodes_check = true; + + this->initializeLoggers("CLoopCloserERD_MR"); +} + +template +void CLoopCloserERD_MR::addBatchOfNodeIDsAndScans( + const std::map< + mrpt::utils::TNodeID, + mrpt::obs::CObservation2DRangeScanPtr>& nodeIDs_to_scans2D) { + mr_parent_t::addBatchOfNodeIDsAndScans(nodeIDs_to_scans2D); + + this->updateMapPartitions(/*full update=*/ true, + /* is_first_time_node_reg = */ false); + +} // end of addBatchOfNodeIDsAndScans + +template +void CLoopCloserERD_MR::addScanMatchingEdges( + mrpt::utils::TNodeID curr_nodeID) { + MRPT_START; + + // Do scan-matching only if I have initially registered curr_nodeID in the + // graph. + bool is_own = this->m_engine->isOwnNodeID(curr_nodeID); + if (is_own) { + lc_parent_t::addScanMatchingEdges(curr_nodeID); + } + + MRPT_END; +} + +template +void CLoopCloserERD_MR::fetchNodeIDsForScanMatching( + const mrpt::utils::TNodeID& curr_nodeID, + std::set* nodes_set) { + ASSERT_(nodes_set); + + size_t fetched_nodeIDs = 0; + for (int nodeID_i = static_cast(curr_nodeID)-1; + ((fetched_nodeIDs <= this->m_laser_params.prev_nodes_for_ICP) && + (nodeID_i >= 0)); + --nodeID_i) { + bool is_own = this->m_engine->isOwnNodeID(nodeID_i); + if (is_own) { + nodes_set->insert(nodeID_i); + fetched_nodeIDs++; + } + } +} + + +template +CLoopCloserERD_MR::~CLoopCloserERD_MR() { + // CLoopCloser Dtor is automatically called. +} + +// Member methods implementations +template +bool CLoopCloserERD_MR::updateState( + mrpt::obs::CActionCollectionPtr action, + mrpt::obs::CSensoryFramePtr observations, + mrpt::obs::CObservationPtr observation ) { + + bool success = lc_parent_t::updateState(action, observations, observation); + + // search for possible edges with the other agent's graph. + // TODO + + return success; +} + + +} } } // end of namespaces + +#endif /* end of include guard: CLOOPCLOSERERD_MR_IMPL_H */ diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/NRD/CFixedIntervalsNRD_MR.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/NRD/CFixedIntervalsNRD_MR.h new file mode 100644 index 0000000..775260d --- /dev/null +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/NRD/CFixedIntervalsNRD_MR.h @@ -0,0 +1,41 @@ +/* +---------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | http://www.mrpt.org/ | + | | + | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | + | See: http://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See details in http://www.mrpt.org/License | + +---------------------------------------------------------------------------+ */ + +#ifndef CFIXEDINTERVALSNRD_MR_H +#define CFIXEDINTERVALSNRD_MR_H + +#include "mrpt_graphslam_2d/interfaces/CNodeRegistrationDecider_MR.h" +#include + +namespace mrpt { namespace graphslam { namespace deciders { + +template +class CFixedIntervalsNRD_MR : + public virtual CFixedIntervalsNRD, + public virtual CNodeRegistrationDecider_MR +{ + public: + typedef CNodeRegistrationDecider_MR parent_cm; + typedef CFixedIntervalsNRD parent_mrpt; + typedef typename GRAPH_T::global_pose_t global_pose_t; + + CFixedIntervalsNRD_MR(); + + private: + + +}; + +} } } // end of namespaces + +#include "CFixedIntervalsNRD_MR_impl.h" + + +#endif /* end of include guard: CFIXEDINTERVALSNRD_MR_H */ + diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/NRD/CFixedIntervalsNRD_MR_impl.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/NRD/CFixedIntervalsNRD_MR_impl.h new file mode 100644 index 0000000..7a14935 --- /dev/null +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/NRD/CFixedIntervalsNRD_MR_impl.h @@ -0,0 +1,25 @@ +/* +---------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | http://www.mrpt.org/ | + | | + | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | + | See: http://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See details in http://www.mrpt.org/License | + +---------------------------------------------------------------------------+ */ + +#ifndef CFIXEDINTERVALSNRD_MR_IMPL_H +#define CFIXEDINTERVALSNRD_MR_IMPL_H + +namespace mrpt { namespace graphslam { namespace deciders { + +template +CFixedIntervalsNRD_MR::CFixedIntervalsNRD_MR() { + this->initializeLoggers("CFixedIntervalsNRD_MR"); +} + +} } } // end of namespaces + + + +#endif /* end of include guard: CFIXEDINTERVALSNRD_MR_IMPL_H */ + diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/NRD/CICPCriteriaNRD_MR.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/NRD/CICPCriteriaNRD_MR.h new file mode 100644 index 0000000..2bc78c4 --- /dev/null +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/NRD/CICPCriteriaNRD_MR.h @@ -0,0 +1,39 @@ +/* +---------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | http://www.mrpt.org/ | + | | + | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | + | See: http://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See details in http://www.mrpt.org/License | + +---------------------------------------------------------------------------+ */ + +#ifndef CICPCRITERIANRD_MR_H +#define CICPCRITERIANRD_MR_H + +#include "mrpt_graphslam_2d/interfaces/CNodeRegistrationDecider_MR.h" +#include + +namespace mrpt { namespace graphslam { namespace deciders { + +template +class CICPCriteriaNRD_MR : + public virtual CICPCriteriaNRD, + public virtual CNodeRegistrationDecider_MR +{ + public: + typedef CNodeRegistrationDecider_MR parent_cm; + typedef CICPCriteriaNRD parent_mrpt; + typedef typename GRAPH_T::global_pose_t global_pose_t; + + CICPCriteriaNRD_MR(); + + private: + + +}; + +} } } // end of namespaces + +#include "CICPCriteriaNRD_MR_impl.h" + +#endif /* end of include guard: CICPCRITERIANRD_MR_H */ diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/NRD/CICPCriteriaNRD_MR_impl.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/NRD/CICPCriteriaNRD_MR_impl.h new file mode 100644 index 0000000..66d3a22 --- /dev/null +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/NRD/CICPCriteriaNRD_MR_impl.h @@ -0,0 +1,22 @@ +/* +---------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | http://www.mrpt.org/ | + | | + | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | + | See: http://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See details in http://www.mrpt.org/License | + +---------------------------------------------------------------------------+ */ + +#ifndef CICPCRITERIANRD_MR_IMPL_H +#define CICPCRITERIANRD_MR_IMPL_H + +namespace mrpt { namespace graphslam { namespace deciders { + +template +CICPCriteriaNRD_MR::CICPCriteriaNRD_MR() { + this->initializeLoggers("CICPCriteriaNRD_MR"); +} + +} } } // end of namespaces + +#endif /* end of include guard: CICPCRITERIANRD_MR_IMPL_H */ diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/TNeighborAgentMapProps.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/TNeighborAgentMapProps.h new file mode 100644 index 0000000..0db3b52 --- /dev/null +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/TNeighborAgentMapProps.h @@ -0,0 +1,84 @@ +/* +---------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | http://www.mrpt.org/ | + | | + | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | + | See: http://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See details in http://www.mrpt.org/License | + +---------------------------------------------------------------------------+ */ +#ifndef TNEIGHBORAGENTMAPPROPS_H +#define TNEIGHBORAGENTMAPPROPS_H + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace mrpt { namespace graphslam { + +/**\brief Struct holding a COccupancyGridMap2D (as well as subscribers for + * updating it) for a specific running GraphSlamAgent instance. + * + * \note used in the global map-merging procedure + */ +struct TNeighborAgentMapProps +{ + TNeighborAgentMapProps( + mrpt::utils::COutputLogger* logger_in, + const mrpt_msgs::GraphSlamAgent& agent_in, + ros::NodeHandle* nh_in); + + void readROSParameters(); + void setupComm(); + void setupSubs(); + void setupPubs(); + /**\brief Callback method to be called when new data in the map topic is + * available + */ + void updateGridMap( + const nav_msgs::OccupancyGrid::ConstPtr& nav_gridmap); + /**\brief Callback method to be called when new data in the Estimated + * Trajectory topic is available + */ + void updateRobotTrajectory( + const nav_msgs::Path::ConstPtr& nav_robot_traj); + + /**\brief Return how many nodes have been registered in the fetched robot + * trajectory + */ + size_t getNodeCount() { return nav_robot_trajectory->poses.size(); } + + ros::NodeHandle* nh; + /**\brief Pointer to the GraphSlamAgent instance of the neighbor */ + const mrpt_msgs::GraphSlamAgent& agent; + /**\brief Map generated by the corresponding agent - in ROS form */ + nav_msgs::OccupancyGrid::ConstPtr nav_map; + /**\brief Trajectory of the correspondign agent - in ROS form */ + nav_msgs::Path::ConstPtr nav_robot_trajectory; + /**\name Subscriber instances */ + /**\{ */ + /**\brief Map subscriber instance */ + ros::Subscriber map_sub; + ros::Subscriber robot_trajectory_sub; + /**\} */ + /**\name Topic names */ + /**\{ */ + /**\brief Map topic to subscribe and fetch the map from */ + std::string map_topic; + std::string robot_trajectory_topic; + /**\} */ + size_t queue_size; + bool has_init_class; + bool has_setup_comm; + mrpt::utils::COutputLogger* m_logger; + + +}; // end of TNeighborAgentMapProps + +} } // end of namespaces + +#endif /* end of include guard: TNEIGHBORAGENTMAPPROPS_H */ diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/TUserOptionsChecker_ROS.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/TUserOptionsChecker_ROS.h new file mode 100644 index 0000000..9b31694 --- /dev/null +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/TUserOptionsChecker_ROS.h @@ -0,0 +1,60 @@ +/* +---------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | http://www.mrpt.org/ | + | | + | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | + | See: http://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See details in http://www.mrpt.org/License | + +---------------------------------------------------------------------------+ */ + +#ifndef TUSEROPTIONSCHECKER_ROS_H +#define TUSEROPTIONSCHECKER_ROS_H + +#include + +#include +#include +#include "mrpt_graphslam_2d/NRD/CFixedIntervalsNRD_MR.h" +#include "mrpt_graphslam_2d/NRD/CICPCriteriaNRD_MR.h" +#include "mrpt_graphslam_2d/ERD/CLoopCloserERD_MR.h" + +namespace mrpt { namespace graphslam { namespace apps { + +template +struct TUserOptionsChecker_ROS: + public mrpt::graphslam::apps::TUserOptionsChecker { + + /**\name handy typedefs for the creation of deciders/optimzer instances from + * the corresponding strings + * + * \note ROS-related classes are suffixed with _ROS + */ + /**\{*/ + typedef std::map< + std::string, + mrpt::graphslam::deciders::CNodeRegistrationDecider*(*)()> + node_regs_t; + typedef std::map< + std::string, + mrpt::graphslam::deciders::CEdgeRegistrationDecider*(*)()> + edge_regs_t; + typedef std::map< + std::string, + mrpt::graphslam::optimizers::CGraphSlamOptimizer*(*)()> + optimizers_t; + /**\brief Parent class */ + typedef mrpt::graphslam::apps::TUserOptionsChecker parent; + /**\}*/ + + TUserOptionsChecker_ROS(); + ~TUserOptionsChecker_ROS(); + void createDeciderOptimizerMappings(); + void populateDeciderOptimizerProperties(); + +}; + + +} } } // end of namespaces +#include "mrpt_graphslam_2d/TUserOptionsChecker_ROS_impl.h" + +#endif /* end of include guard: TUSEROPTIONSCHECKER_ROS_H */ diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/TUserOptionsChecker_ROS_impl.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/TUserOptionsChecker_ROS_impl.h new file mode 100644 index 0000000..52ff3db --- /dev/null +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/TUserOptionsChecker_ROS_impl.h @@ -0,0 +1,93 @@ +/* +---------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | http://www.mrpt.org/ | + | | + | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | + | See: http://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See details in http://www.mrpt.org/License | + +---------------------------------------------------------------------------+ */ +#ifndef TUSEROPTIONSCHECKER_ROS_IMPL_H +#define TUSEROPTIONSCHECKER_ROS_IMPL_H + +namespace mrpt { namespace graphslam { namespace apps { + +template +TUserOptionsChecker_ROS::TUserOptionsChecker_ROS() { +} + +template +TUserOptionsChecker_ROS::~TUserOptionsChecker_ROS() { +} + +template +void TUserOptionsChecker_ROS::createDeciderOptimizerMappings() { + using namespace std; + using namespace mrpt::graphs; + using namespace mrpt::graphslam::apps; + using namespace mrpt::graphslam::deciders; + parent::createDeciderOptimizerMappings(); + + // node registration deciders + this->node_regs_map["CICPCriteriaNRD_MR"] = + parent::template createNodeRegistrationDecider>; + this->node_regs_map["CFixedIntervalsNRD_MR"] = + parent::template createNodeRegistrationDecider>; + + + // edge registration deciders + this->edge_regs_map["CLoopCloserERD_MR"] = + parent::template createEdgeRegistrationDecider>; + + // optimizers + +} + +template +void TUserOptionsChecker_ROS::populateDeciderOptimizerProperties() { + using namespace mrpt::graphslam::apps; + using namespace std; + + parent::populateDeciderOptimizerProperties(); + { // CICPCriteriaNRD_MR + TRegistrationDeciderProps* dec = new TRegistrationDeciderProps; + dec->name = "CICPCriteriaNRD_MR"; + dec->description = + "Multi-robot SLAM implementation of the CICPCriteriaNRD class based on \"Condensed Measurements\""; + dec->type = "Node"; + dec->rawlog_format = "Both"; + dec->observations_used.push_back("CObservation2DRangeScan - Format #1, #2"); + dec->is_mr_slam_class = "true"; + + this->regs_descriptions.push_back(dec); + } + { // CFixedIntervalsNRD_MR + TRegistrationDeciderProps* dec = new TRegistrationDeciderProps; + dec->name = "CFixedIntervalsNRD_MR"; + dec->description = + "Multi-robot SLAM implementation of the CFixedIntervalsNRD class based on \"Condensed Measurements\""; + dec->type = "Node"; + dec->rawlog_format = "Both"; + dec->observations_used.push_back("CObservation2DRangeScan - Format #1, #2"); + dec->is_mr_slam_class = "true"; + + this->regs_descriptions.push_back(dec); + } + + { // CLoopCloserERD_MR + TRegistrationDeciderProps* dec = new TRegistrationDeciderProps; + dec->name = "CLoopCloserERD_MR"; + dec->description = + "Multi-robot SLAM implementation of the CLoopCloserERD class based on \"Condensed Measurements\""; + dec->type = "Edge"; + dec->rawlog_format = "Both"; + dec->observations_used.push_back("CObservation2DRangeScan - Format #1, #2"); + dec->is_mr_slam_class = "true"; + + this->regs_descriptions.push_back(dec); + } + +} + +} } } //end namespaces + +#endif /* end of include guard: TUSEROPTIONSCHECKER_ROS_IMPL_H */ diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CEdgeRegistrationDecider_MR.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CEdgeRegistrationDecider_MR.h new file mode 100644 index 0000000..e69df4a --- /dev/null +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CEdgeRegistrationDecider_MR.h @@ -0,0 +1,55 @@ +/* +---------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | http://www.mrpt.org/ | + | | + | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | + | See: http://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See details in http://www.mrpt.org/License | + +---------------------------------------------------------------------------+ */ +#ifndef CEDGEREGISTRATIONDECIDER_MR_H +#define CEDGEREGISTRATIONDECIDER_MR_H + +#include "mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_MR.h" +#include "mrpt_graphslam_2d/CConnectionManager.h" +#include + +#include + +namespace mrpt { namespace graphslam { namespace deciders { + +/**\brief Edge Registration Decider virtual method. + * + * \b Edge Registration Decider classes that are to be used in a multi-robot + * SLAM scheme according to the Condensed Measurements multi-robot strategy by + * M.T. Lazaro et al. [1] are to inherit from this method. + * + * \note Condensed Measurements-related classes are suffixed with _MR. + * + * \note For an example of inheriting from this class, see the + * mrpt::graphslam::deciders::CLoopCloserERD_MR. + * + * [1] Multi-robot + * SLAM using Condensed Measurements - M.T. Lazaro, L.M. Paz, P. Pinies, + * J.A. Castellanos, G. Grisetti + */ +template +class CEdgeRegistrationDecider_MR : + public virtual mrpt::graphslam::CRegistrationDeciderOrOptimizer_MR, + public virtual mrpt::graphslam::deciders::CRangeScanEdgeRegistrationDecider +{ +public: + CEdgeRegistrationDecider_MR (); + ~CEdgeRegistrationDecider_MR (); + virtual void addBatchOfNodeIDsAndScans( + const std::map< + mrpt::utils::TNodeID, + mrpt::obs::CObservation2DRangeScanPtr>& nodeIDs_to_scans2D); + +protected: +}; + +} } } // end of namespaces + +#include "CEdgeRegistrationDecider_MR_impl.h" +#endif /* end of include guard: CEDGEREGISTRATIONDECIDER_MR_H */ diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CEdgeRegistrationDecider_MR_impl.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CEdgeRegistrationDecider_MR_impl.h new file mode 100644 index 0000000..1d7f383 --- /dev/null +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CEdgeRegistrationDecider_MR_impl.h @@ -0,0 +1,39 @@ +/* +---------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | http://www.mrpt.org/ | + | | + | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | + | See: http://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See details in http://www.mrpt.org/License | + +---------------------------------------------------------------------------+ */ + +#ifndef CEDGEREGISTRATIONDECIDER_MR_IMPL_H +#define CEDGEREGISTRATIONDECIDER_MR_IMPL_H + +namespace mrpt { namespace graphslam { namespace deciders { + +template +CEdgeRegistrationDecider_MR::CEdgeRegistrationDecider_MR() {} + +template +CEdgeRegistrationDecider_MR::~CEdgeRegistrationDecider_MR() {} + +template +void CEdgeRegistrationDecider_MR::addBatchOfNodeIDsAndScans( + const std::map< + mrpt::utils::TNodeID, + mrpt::obs::CObservation2DRangeScanPtr>& nodeIDs_to_scans2D) { + + this->m_nodes_to_laser_scans2D.insert( + nodeIDs_to_scans2D.begin(), + nodeIDs_to_scans2D.end()); + + // update the last known number of nodeIDs + this->m_last_total_num_nodes = this->m_graph->nodeCount(); + +} // end of addBatchOfNodeIDsAndScans + + +} } } // end of namespaces + +#endif /* end of include guard: CEDGEREGISTRATIONDECIDER_MR_IMPL_H */ diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CGraphSlamOptimizer_MR.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CGraphSlamOptimizer_MR.h new file mode 100644 index 0000000..a91510b --- /dev/null +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CGraphSlamOptimizer_MR.h @@ -0,0 +1,33 @@ +/* +---------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | http://www.mrpt.org/ | + | | + | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | + | See: http://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See details in http://www.mrpt.org/License | + +---------------------------------------------------------------------------+ */ + +#ifndef CGRAPHSLAMOPTIMIZER_MR_H +#define CGRAPHSLAMOPTIMIZER_MR_H + +namespace mrpt { namespace graphslam { namespace optimizers { + +/**\brief Interface for implementing graphSLAM optimizer classes specific to + * the Condensed Measurements MR-SLAM case + */ +class CGraphSlamOptimizer_MR : + public virtual mrpt::graphslam::CRegistrationDeciderOrOptimizer_MR + public virtual CGraphSlamOptimizer +{ + public: + CGraphSlamOptimizer_MR (); + ~CGraphSlamOptimizer_MR (); + + private: + + +} } } // end of namespaces + +#endif /* end of include guard: CGRAPHSLAMOPTIMIZER_MR_H */ + + diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CGraphSlamOptimizer_MR_impl.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CGraphSlamOptimizer_MR_impl.h new file mode 100644 index 0000000..32f7cda --- /dev/null +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CGraphSlamOptimizer_MR_impl.h @@ -0,0 +1,23 @@ +/* +---------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | http://www.mrpt.org/ | + | | + | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | + | See: http://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See details in http://www.mrpt.org/License | + +---------------------------------------------------------------------------+ */ + +#ifndef CGRAPHSLAMOPTIMIZER_MR_IMPL_H +#define CGRAPHSLAMOPTIMIZER_MR_IMPL_H + +namespace mrpt { namespace graphslam { namespace optimizers { + +template +CGraphSlamOptimizer_MR::CGraphSlamOptimizer_MR() {} + +template +CGraphSlamOptimizer_MR::~CGraphSlamOptimizer_MR() {} + +} } } // end of namespaces + +#endif /* end of include guard: CGRAPHSLAMOPTIMIZER_MR_IMPL_H */ diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CNodeRegistrationDecider_MR.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CNodeRegistrationDecider_MR.h new file mode 100644 index 0000000..b30a59c --- /dev/null +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CNodeRegistrationDecider_MR.h @@ -0,0 +1,58 @@ +/* +---------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | http://www.mrpt.org/ | + | | + | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | + | See: http://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See details in http://www.mrpt.org/License | + +---------------------------------------------------------------------------+ */ + +#ifndef CNODEREGISTRATIONDECIDER_MR_H +#define CNODEREGISTRATIONDECIDER_MR_H + +#include "mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_MR.h" +#include +#include + +namespace mrpt { namespace graphslam { namespace deciders { + +/**\brief Node Registration Decider Interface Class. + * + * \b Node Registration Decider classes that are to be used in a multi-robot + * SLAM scheme according to the Condensed Measurements multi-robot strategy by + * M.T. Lazaro et al. [1] are to inherit from this method. + * + * \note Condensed Measurements-related classes are suffixed with _MR. + * + * \note For an example of inheriting from this class, see the + * mrpt::graphslam::deciders::CFixedIntervalsNRD_MR. + * + * [1] Multi-robot + * SLAM using Condensed Measurements - M.T. Lazaro, L.M. Paz, P. Pinies, + * J.A. Castellanos, G. Grisetti + */ +template +class CNodeRegistrationDecider_MR : + public virtual mrpt::graphslam::CRegistrationDeciderOrOptimizer_MR, + public virtual CNodeRegistrationDecider +{ +public: + typedef typename GRAPH_T::global_pose_t global_pose_t; + + CNodeRegistrationDecider_MR (); + ~CNodeRegistrationDecider_MR (); +protected: + /**\brief Decorate a pose according to the TMRSlamNodeAnnotation fields + * + * \note Do this only for the nodes that are initially registered in the graph by + * the current CGraphSlamEngine_t class. Nodes of other graphSLAM-agents that + * are to be integrated must have already filled these fields. + */ + void addNodeAnnotsToPose(global_pose_t* pose) const; +}; + +} } } // end of namespaces + +#include "CNodeRegistrationDecider_MR_impl.h" +#endif /* end of include guard: CNODEREGISTRATIONDECIDER_MR_H */ diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CNodeRegistrationDecider_MR_impl.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CNodeRegistrationDecider_MR_impl.h new file mode 100644 index 0000000..d7da899 --- /dev/null +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CNodeRegistrationDecider_MR_impl.h @@ -0,0 +1,39 @@ +/* +---------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | http://www.mrpt.org/ | + | | + | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | + | See: http://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See details in http://www.mrpt.org/License | + +---------------------------------------------------------------------------+ */ + +#ifndef CNODEREGISTRATIONDECIDER_MR_IMPL_H +#define CNODEREGISTRATIONDECIDER_MR_IMPL_H + +namespace mrpt { namespace graphslam { namespace deciders { + +template +CNodeRegistrationDecider_MR::CNodeRegistrationDecider_MR() {} + +template +CNodeRegistrationDecider_MR::~CNodeRegistrationDecider_MR() {} + +template +void CNodeRegistrationDecider_MR::addNodeAnnotsToPose( + global_pose_t* pose) const { } + +template<> +void CNodeRegistrationDecider_MR::addNodeAnnotsToPose( + global_pose_t* pose) const { + ASSERT_(pose); + + pose->agent_ID_str = this->own_ns; + // ASSUMPTION: addNodeAnnotsToPose is going to be called right before the + // actual registration. + // Mark it with the nodeID that is up-next + pose->nodeID_loc = this->m_graph->nodeCount(); +} + +} } } // end of namespaces + +#endif /* end of include guard: CNODEREGISTRATIONDECIDER_MR_IMPL_H */ diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_MR.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_MR.h new file mode 100644 index 0000000..204b272 --- /dev/null +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_MR.h @@ -0,0 +1,56 @@ +#ifndef CREGISTRATIONDECIDEROROPTIMIZER_MR_H +#define CREGISTRATIONDECIDEROROPTIMIZER_MR_H + +#include "mrpt_graphslam_2d/CGraphSlamEngine_MR.h" +#include "mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_ROS.h" +#include "mrpt_graphslam_2d/CConnectionManager.h" + +// forward declaration +namespace mrpt { namespace graphslam { +template class CGraphSlamEngine_MR; +} }// end of namespaces + +namespace mrpt { namespace graphslam { + +/**\brief Interface for implementing deciders/optimizers related to the Condensed + * Measurements multi-robot graphSLAM algorithm. + * + * \warning Beware that this class does not inherit from the + * mrpt::graphslam::CNodeRegistrationDeciderOrOptimizer. + * + * \note Condensed Measurements-related classes are suffixed with _MR. + * + */ +template +class CRegistrationDeciderOrOptimizer_MR : + public mrpt::graphslam::CRegistrationDeciderOrOptimizer_ROS +{ +public: + typedef CGraphSlamEngine_MR engine_t; + + CRegistrationDeciderOrOptimizer_MR(); + ~CRegistrationDeciderOrOptimizer_MR(); + + void setCGraphSlamEnginePtr(const engine_t* engine); + virtual void setCConnectionManagerPtr( + mrpt::graphslam::detail::CConnectionManager* conn_manager); + +protected: + /**\brief Pointer to the CConnectionManager instance + */ + mrpt::graphslam::detail::CConnectionManager* m_conn_manager; + /**\brief Constant pointer to the CGraphSlamEngine instance. + * + */ + const engine_t* m_engine; + std::string own_ns; + + +}; + +} } // end of namespaces + +// template methods implementations +#include "mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_MR_impl.h" + +#endif /* end of include guard: CREGISTRATIONDECIDEROROPTIMIZER_MR_H */ diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_MR_impl.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_MR_impl.h new file mode 100644 index 0000000..7bdd9ce --- /dev/null +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_MR_impl.h @@ -0,0 +1,34 @@ +#ifndef CREGISTRATIONDECIDEROROPTIMIZER_MR_IMPL_H +#define CREGISTRATIONDECIDEROROPTIMIZER_MR_IMPL_H + +namespace mrpt { namespace graphslam { + +template +CRegistrationDeciderOrOptimizer_MR::CRegistrationDeciderOrOptimizer_MR() { + this->is_mr_slam_class = true; +} + +template +CRegistrationDeciderOrOptimizer_MR::~CRegistrationDeciderOrOptimizer_MR() { } + +template +void CRegistrationDeciderOrOptimizer_MR::setCConnectionManagerPtr( + mrpt::graphslam::detail::CConnectionManager* conn_manager) { + ASSERTMSG_(conn_manager, "\nInvalid CConnectionManager* pointer.\n"); + + m_conn_manager = conn_manager; + own_ns = m_conn_manager->getTrimmedNs(); +} + +template +void CRegistrationDeciderOrOptimizer_MR::setCGraphSlamEnginePtr( + const engine_t* engine) { + ASSERTMSG_(engine, "CGraphSlamEngine pointer is NULL"); + m_engine = engine; +} + + + +} } // end of namespaces + +#endif /* end of include guard: CREGISTRATIONDECIDEROROPTIMIZER_MR_IMPL_H */ diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_ROS.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_ROS.h new file mode 100644 index 0000000..66f5d5e --- /dev/null +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_ROS.h @@ -0,0 +1,39 @@ +#ifndef CREGISTRATIONDECIDEROROPTIMIZER_ROS_H +#define CREGISTRATIONDECIDEROROPTIMIZER_ROS_H + +#include + +#include +#include + +namespace mrpt { namespace graphslam { + +/**\brief Interface class that all ROS-specific deciders/optimizers can inherit + * from. + * + * \note ROS-related classes are suffixed with _ROS + */ +template +class CRegistrationDeciderOrOptimizer_ROS : + public virtual mrpt::graphslam::CRegistrationDeciderOrOptimizer +{ +public: + CRegistrationDeciderOrOptimizer_ROS(); + virtual ~CRegistrationDeciderOrOptimizer_ROS(); + + virtual void setNodeHandle(ros::NodeHandle* nh); + +protected: + /**\brief NodeHandle instance + */ + ros::NodeHandle* m_nh; + + +}; + + +} } // end of namespaces + +#include "mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_ROS_impl.h" + +#endif /* end of include guard: CREGISTRATIONDECIDEROROPTIMIZER_ROS_H */ diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_ROS_impl.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_ROS_impl.h new file mode 100644 index 0000000..cf0363c --- /dev/null +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_ROS_impl.h @@ -0,0 +1,22 @@ +#ifndef CREGISTRATIONDECIDEROROPTIMIZER_ROS_IMPL_H +#define CREGISTRATIONDECIDEROROPTIMIZER_ROS_IMPL_H + +namespace mrpt { namespace graphslam { + + +template +CRegistrationDeciderOrOptimizer_ROS::CRegistrationDeciderOrOptimizer_ROS() { } +template +CRegistrationDeciderOrOptimizer_ROS::~CRegistrationDeciderOrOptimizer_ROS() { } + +template +void CRegistrationDeciderOrOptimizer_ROS::setNodeHandle(ros::NodeHandle* nh) { + ASSERTMSG_(nh, "\nInvalid NodeHandle instance was provided.\n"); + + m_nh = nh; +} + + +} } // end of namespaces + +#endif /* end of include guard: CREGISTRATIONDECIDEROROPTIMIZER_ROS_IMPL_H */ diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/misc/common.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/misc/common.h new file mode 100644 index 0000000..33c7758 --- /dev/null +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/misc/common.h @@ -0,0 +1,30 @@ +/* +---------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | http://www.mrpt.org/ | + | | + | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | + | See: http://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See details in http://www.mrpt.org/License | + +---------------------------------------------------------------------------+ */ + +/**\file common.h + * + * Defined methods used in different parts of the mrpt_graphslam_2d application. + */ + +#include +#include +#include +#include +#include +#include + +namespace mrpt { namespace graphslam { namespace detail { + +std::string getGridMapAlignmentResultsAsString( + const mrpt::poses::CPosePDF& pdf, + const mrpt::slam::CGridMapAligner::TReturnInfo& ret_info); + +bool isEssentiallyZero(const mrpt::poses::CPose2D& p); + +} } } // end of namespaces diff --git a/mrpt_graphslam_2d/launch/sr_graphslam.launch b/mrpt_graphslam_2d/launch/graphslam.launch similarity index 59% rename from mrpt_graphslam_2d/launch/sr_graphslam.launch rename to mrpt_graphslam_2d/launch/graphslam.launch index fded0a4..cbcb8bf 100644 --- a/mrpt_graphslam_2d/launch/sr_graphslam.launch +++ b/mrpt_graphslam_2d/launch/graphslam.launch @@ -13,21 +13,24 @@ Refer to the README.md file for usage information + - + + + @@ -41,24 +44,38 @@ Refer to the README.md file for usage information - + - - + + + + + + + + + + + + + + + + + + + - - - diff --git a/mrpt_graphslam_2d/launch/mr_graphslam.launch b/mrpt_graphslam_2d/launch/mr_graphslam.launch index 844aa9d..53b3122 100644 --- a/mrpt_graphslam_2d/launch/mr_graphslam.launch +++ b/mrpt_graphslam_2d/launch/mr_graphslam.launch @@ -3,65 +3,39 @@ - - - - - + + + - - - - - + + + + - - - + + - - - - - - - - - - - - - - - - - - - - - - + + + - - + - - - - - - - - + + + + diff --git a/mrpt_graphslam_2d/launch/sr_graphslam_demo.launch b/mrpt_graphslam_2d/launch/sr_graphslam_demo.launch index c3432ca..bd2c539 100644 --- a/mrpt_graphslam_2d/launch/sr_graphslam_demo.launch +++ b/mrpt_graphslam_2d/launch/sr_graphslam_demo.launch @@ -11,25 +11,28 @@ single robot situations. More elaborate demo files are built on top of this one - - - - - - - + + + + + + + + + - - + + - + + + + - - - - - + + + @@ -37,18 +40,22 @@ single robot situations. More elaborate demo files are built on top of this one - + + - + + - + + diff --git a/mrpt_graphslam_2d/package.xml b/mrpt_graphslam_2d/package.xml index 89bda2d..db0d59d 100644 --- a/mrpt_graphslam_2d/package.xml +++ b/mrpt_graphslam_2d/package.xml @@ -25,6 +25,7 @@ mrpt_bridge mrpt_msgs tf + multimaster_msgs_fkie roscpp rospy @@ -36,6 +37,8 @@ mrpt_bridge mrpt_msgs tf + multimaster_msgs_fkie + rviz diff --git a/mrpt_graphslam_2d/rosbags/demo_short_loop/demo.bag b/mrpt_graphslam_2d/rosbags/demo_short_loop/demo.bag index c85ed5e..df075a5 100644 Binary files a/mrpt_graphslam_2d/rosbags/demo_short_loop/demo.bag and b/mrpt_graphslam_2d/rosbags/demo_short_loop/demo.bag differ diff --git a/mrpt_graphslam_2d/rviz/README.md b/mrpt_graphslam_2d/rviz/README.md new file mode 100644 index 0000000..6570162 --- /dev/null +++ b/mrpt_graphslam_2d/rviz/README.md @@ -0,0 +1,9 @@ +Directory contains rviz-related files + +Currently the \*gazebo\* rviz files refer to static topics and the user has to +manually change csldesktop to his own hostname for them to appropriately +visualize the procedure. Same goes for the \*real\* files. + +There are plans to dynamically create the rviz files based on the user's hostname / current configuration using the [rviz_generator package](https://github.com/bergercookie/rviz_generator) + + diff --git a/mrpt_graphslam_2d/rviz/graphslam_gazebo_1.rviz b/mrpt_graphslam_2d/rviz/graphslam_gazebo_1.rviz new file mode 100644 index 0000000..a2910c2 --- /dev/null +++ b/mrpt_graphslam_2d/rviz/graphslam_gazebo_1.rviz @@ -0,0 +1,254 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /map1 + - /estim_path1 + - /LaserScan1 + - /Image1 + - /Map1 + - /Axes1 + - /Axes2 + - /estim_pose1 + - /estim_pose1/Status1 + - /odom_path1 + Splitter Ratio: 0.5 + Tree Height: 477 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: Image +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: map + Topic: /csldesktop/feedback/gridmap + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 85; 0 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: estim_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /csldesktop/feedback/robot_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: "" + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /csldesktop/input/laser_scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /csldesktop/camera1/image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /csldesktop/feedback/gridmap + Unreliable: false + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.1 + Reference Frame: + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.1 + Reference Frame: csldesktop_anchor + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.1 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: false + Head Length: 0.3 + Head Radius: 0.1 + Name: estim_pose + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Arrow + Topic: /csldesktop/feedback/robot_position + Unreliable: false + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 0; 254 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: odom_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /csldesktop/feedback/odom_trajectory + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 3.4002 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.397505 + Y: -2.73194 + Z: 1.58828 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.8654 + Target Frame: + Value: Orbit (rviz) + Yaw: 2.61539 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1027 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000360fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000410000026c000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002b3000000ee0000001600ffffff000000010000010f00000360fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004100000360000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000028400fffffffb0000000800540069006d00650100000000000004500000000000000000000004fb0000036000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 0 + Y: 31 diff --git a/mrpt_graphslam_2d/rviz/graphslam_gazebo_2.rviz b/mrpt_graphslam_2d/rviz/graphslam_gazebo_2.rviz new file mode 100644 index 0000000..a4826ba --- /dev/null +++ b/mrpt_graphslam_2d/rviz/graphslam_gazebo_2.rviz @@ -0,0 +1,591 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /anchor_frame_11 + - /anchor_frame_21 + - /img_21 + - /robot_model_11 + - /robot_model_21 + Splitter Ratio: 0.5 + Tree Height: 328 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: img_2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: origin_axes + Radius: 0.1 + Reference Frame: + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: anchor_frame_1 + Radius: 0.1 + Reference Frame: csldesktop/anchor + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: anchor_frame_2 + Radius: 0.1 + Reference Frame: csldesktop_11312/anchor + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /csldesktop/ar_sys/result + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: img_1 + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /csldesktop_11312/ar_sys/result + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: img_2 + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + back_sonar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_sonar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + hokuyo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top_plate: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: robot_model_1 + Robot Description: csldesktop/robot_description + TF Prefix: csldesktop + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + back_sonar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_sonar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + hokuyo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top_plate: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: robot_model_2 + Robot Description: csldesktop_11312/robot_description + TF Prefix: csldesktop_11312 + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: laser_scan_1 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /csldesktop/input/laser_scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: laser_scan_2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /csldesktop_11312/input/laser_scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Angle Tolerance: 0.1 + Class: rviz/Odometry + Color: 255; 25; 0 + Enabled: false + Keep: 100 + Length: 1 + Name: odom_1 + Position Tolerance: 0.1 + Topic: /csldesktop/input/odom + Value: false + - Angle Tolerance: 0.1 + Class: rviz/Odometry + Color: 255; 25; 0 + Enabled: false + Keep: 100 + Length: 1 + Name: odom_2 + Position Tolerance: 0.1 + Topic: /csldesktop_11312/input/odom + Value: false + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: map_1 + Topic: /csldesktop/feedback/gridmap + Unreliable: false + Value: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: map_2 + Topic: /csldesktop_11312/feedback/gridmap + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 0; 255 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: odom_traj_1 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /csldesktop/feedback/odom_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 0; 255 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: odom_traj_2 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /csldesktop_11312/feedback/odom_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.1 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: estim_pose_1 + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Arrow + Topic: /csldesktop/feedback/robot_position + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.1 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: estim_pose_2 + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Arrow + Topic: /csldesktop_11312/feedback/robot_position + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 85; 0 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: estim_traj_1 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /csldesktop/feedback/robot_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 85; 0 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: estim_traj_2 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /csldesktop_11312/feedback/robot_trajectory + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 20.7017 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 3.57737 + Y: -5.15677 + Z: -4.08701 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 1.0448 + Target Frame: + Value: Orbit (rviz) + Yaw: 2.7704 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1027 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001a300000360fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004100000189000000dd00fffffffb0000000a0069006d0067005f003101000001d0000000f10000001600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0069006d0067005f003201000002c7000000da0000001600fffffffb0000000a0069006d0067005f003101000001d7000000b20000000000000000000000010000010f000001e0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000043000001e0000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000028400fffffffb0000000800540069006d00650100000000000004500000000000000000000005d70000036000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 0 + Y: 31 + img_1: + collapsed: false + img_2: + collapsed: false diff --git a/mrpt_graphslam_2d/rviz/graphslam_gazebo_3.rviz b/mrpt_graphslam_2d/rviz/graphslam_gazebo_3.rviz new file mode 100644 index 0000000..eeff793 --- /dev/null +++ b/mrpt_graphslam_2d/rviz/graphslam_gazebo_3.rviz @@ -0,0 +1,824 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /anchor_frame_11 + - /anchor_frame_21 + - /anchor_frame_31 + - /robot_model_31 + Splitter Ratio: 0.5 + Tree Height: 232 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: img_2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: origin_axes + Radius: 0.1 + Reference Frame: + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: anchor_frame_1 + Radius: 0.1 + Reference Frame: csldesktop/anchor + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: anchor_frame_2 + Radius: 0.1 + Reference Frame: csldesktop_11312/anchor + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: anchor_frame_3 + Radius: 0.1 + Reference Frame: csldesktop_11313/anchor + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /csldesktop/ar_sys/result + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: img_1 + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /csldesktop_11312/ar_sys/result + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: img_2 + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /csldesktop_11313/ar_sys/result + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: img_3 + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + back_sonar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_sonar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + hokuyo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top_plate: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: robot_model_1 + Robot Description: csldesktop/robot_description + TF Prefix: csldesktop + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + back_sonar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_sonar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + hokuyo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top_plate: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: robot_model_2 + Robot Description: csldesktop_11312/robot_description + TF Prefix: csldesktop_11312 + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + back_sonar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_sonar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + hokuyo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top_plate: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: robot_model_3 + Robot Description: csldesktop_11313/robot_description + TF Prefix: csldesktop_11313 + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: laser_scan_1 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /csldesktop/input/laser_scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: laser_scan_2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /csldesktop_11312/input/laser_scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: laser_scan_3 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /csldesktop_11313/input/laser_scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Angle Tolerance: 0.1 + Class: rviz/Odometry + Color: 255; 25; 0 + Enabled: false + Keep: 100 + Length: 1 + Name: odom_1 + Position Tolerance: 0.1 + Topic: /csldesktop/input/odom + Value: false + - Angle Tolerance: 0.1 + Class: rviz/Odometry + Color: 255; 25; 0 + Enabled: false + Keep: 100 + Length: 1 + Name: odom_2 + Position Tolerance: 0.1 + Topic: /csldesktop_11312/input/odom + Value: false + - Angle Tolerance: 0.1 + Class: rviz/Odometry + Color: 255; 25; 0 + Enabled: false + Keep: 100 + Length: 1 + Name: odom_3 + Position Tolerance: 0.1 + Topic: /csldesktop_11313/input/odom + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 85; 0 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: estim_traj_1 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /csldesktop/feedback/robot_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 85; 0 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: estim_traj_2 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /csldesktop_11312/feedback/robot_trajectory + Unreliable: false + Value: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: map_1 + Topic: /csldesktop/feedback/gridmap + Unreliable: false + Value: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: map_3 + Topic: /csldesktop_11313/feedback/gridmap + Unreliable: false + Value: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: map_2 + Topic: /csldesktop_11312/feedback/gridmap + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 0; 255 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: odom_traj_1 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /csldesktop/feedback/odom_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 0; 255 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: odom_traj_3 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /csldesktop_11313/feedback/odom_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 0; 255 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: odom_traj_2 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /csldesktop_11312/feedback/odom_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 85; 0 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: estim_traj_3 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /csldesktop_11313/feedback/robot_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.1 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: estim_pose_1 + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Arrow + Topic: /csldesktop/feedback/robot_position + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.1 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: estim_pose_3 + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Arrow + Topic: /csldesktop_11313/feedback/robot_position + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.1 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: estim_pose_2 + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Arrow + Topic: /csldesktop_11312/feedback/robot_position + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 18.9857 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 4.27584 + Y: -5.9404 + Z: -3.04498 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.7648 + Target Frame: + Value: Orbit (rviz) + Yaw: 2.75222 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1023 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001a30000035cfc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004100000129000000dd00fffffffb0000000a0069006d0067005f00310100000170000000b60000001600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0069006d0067005f0032010000022c000000a50000001600fffffffb0000000a0069006d0067005f003101000001d7000000b20000000000000000fb0000000a0069006d0067005f003301000002d7000000c60000001600ffffff000000010000010f000001e0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000043000001e0000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077e0000003efc0100000002fb0000000800540069006d006501000000000000077e0000028400fffffffb0000000800540069006d00650100000000000004500000000000000000000005d50000035c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1918 + X: 0 + Y: 31 + img_1: + collapsed: false + img_2: + collapsed: false + img_3: + collapsed: false diff --git a/mrpt_graphslam_2d/rviz/mr_graphslam.rviz b/mrpt_graphslam_2d/rviz/graphslam_real_1.rviz similarity index 62% rename from mrpt_graphslam_2d/rviz/mr_graphslam.rviz rename to mrpt_graphslam_2d/rviz/graphslam_real_1.rviz index acc9939..68c2787 100644 --- a/mrpt_graphslam_2d/rviz/mr_graphslam.rviz +++ b/mrpt_graphslam_2d/rviz/graphslam_real_1.rviz @@ -1,19 +1,23 @@ Panels: - Class: rviz/Displays - Help Height: 89 + Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 - - /Pose1 - - /PoseArray1 - - /Path1 - - /Path2 + - /map1 + - /estim_path1 + - /LaserScan1 + - /Image1 - /Map1 - - /TF1 + - /Axes1 + - /Axes2 + - /estim_pose1 + - /estim_pose1/Status1 + - /odom_path1 Splitter Ratio: 0.5 - Tree Height: 392 + Tree Height: 477 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -32,7 +36,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: Image Visualization Manager: Class: "" Displays: @@ -54,19 +58,35 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: map + Topic: /nickkoukubuntu/feedback/gridmap + Unreliable: false + Value: true - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.1 - Class: rviz/Pose - Color: 255; 25; 0 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 85; 0 Enabled: true - Head Length: 0.1 - Head Radius: 0.03 - Name: Pose - Shaft Length: 0.3 - Shaft Radius: 0.02 - Shape: Arrow - Topic: /pioneer_2AT_1/feedback/robot_position + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: estim_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /nickkoukubuntu/feedback/robot_trajectory Unreliable: false Value: true - Alpha: 1 @@ -79,110 +99,95 @@ Visualization Manager: Channel Name: intensity Class: rviz/LaserScan Color: 255; 255; 255 - Color Transformer: Intensity + Color Transformer: "" Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 Name: LaserScan - Position Transformer: XYZ + Position Transformer: "" Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.01 Style: Flat Squares - Topic: /graphslam_engine/input/laser_scan + Topic: /nickkoukubuntu/input/laser_scan Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - - Arrow Length: 0.3 - Class: rviz/PoseArray - Color: 255; 25; 0 + - Class: rviz/Image Enabled: true - Name: PoseArray - Topic: /pioneer_2AT_1/feedback/robot_tr_poses + Image Topic: /nickkoukubuntu/onboard_camera/image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed Unreliable: false Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 0; 0; 255 + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false Enabled: true - Line Style: Lines - Line Width: 0.03 - Name: Path - Offset: - X: 0 - Y: 0 - Z: 0 - Topic: /pioneer_2AT_1/feedback/odom_tr_poses + Name: Map + Topic: /nickkoukubuntu/feedback/gridmap Unreliable: false Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.1 + Reference Frame: + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.1 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: false + Head Length: 0.3 + Head Radius: 0.1 + Name: estim_pose + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Arrow + Topic: /nickkoukubuntu/feedback/robot_position + Unreliable: false + Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path - Color: 255; 85; 0 + Color: 0; 0; 254 Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 Line Style: Lines Line Width: 0.03 - Name: Path + Name: odom_path Offset: X: 0 Y: 0 Z: 0 - Topic: /pioneer_2AT_1/feedback/robot_trajectory - Unreliable: false - Value: true - - Alpha: 0.6 - Class: rviz/Map - Color Scheme: map - Draw Behind: false - Enabled: true - Name: Map - Topic: /pioneer_2AT_1/feedback/gridmap + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /nickkoukubuntu/feedback/odom_trajectory Unreliable: false Value: true - - Class: rviz/TF - Enabled: true - Frame Timeout: 15 - Frames: - ($arg tf_prefix)base_link: - Value: true - ($arg tf_prefix)map: - Value: true - ($arg tf_prefix)odom: - Value: true - All Enabled: true - pioneer_2AT_1_base_link: - Value: true - pioneer_2AT_1_laser: - Value: true - pioneer_2AT_1_map: - Value: true - pioneer_2AT_1_odom: - Value: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - pioneer_2AT_1_map: - pioneer_2AT_1_base_link: - pioneer_2AT_1_laser: - {} - pioneer_2AT_1_odom: - {} - Update Interval: 0 - Value: true Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: pioneer_2AT_1_map + Fixed Frame: mf1_anchor Frame Rate: 30 Name: root Tools: @@ -203,30 +208,32 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 11.6602 + Distance: 3.4002 Enable Stereo Rendering: Stereo Eye Separation: 0.06 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 1.31148 - Y: -1.55439 - Z: -0.150453 + X: -0.397505 + Y: -2.73194 + Z: 1.58828 Name: Current View Near Clip Distance: 0.01 - Pitch: 0.914797 + Pitch: 0.8654 Target Frame: Value: Orbit (rviz) - Yaw: 2.5054 + Yaw: 2.61539 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 711 + Height: 1027 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000019300000222fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006700fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004300000222000000e300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000222fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004300000222000000b300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000028300fffffffb0000000800540069006d00650100000000000004500000000000000000000002020000022200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000360fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000410000026c000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002b3000000ee0000001600ffffff000000010000010f00000360fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004100000360000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000028400fffffffb0000000800540069006d00650100000000000004500000000000000000000004fb0000036000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -235,6 +242,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1200 - X: 60 + Width: 1920 + X: 0 Y: 31 diff --git a/mrpt_graphslam_2d/rviz/graphslam_real_2.rviz b/mrpt_graphslam_2d/rviz/graphslam_real_2.rviz new file mode 100644 index 0000000..502b575 --- /dev/null +++ b/mrpt_graphslam_2d/rviz/graphslam_real_2.rviz @@ -0,0 +1,596 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /anchor_frame_11 + - /anchor_frame_21 + - /img_21 + - /map_21 + - /odom_traj_21 + - /estim_pose_21 + - /estim_traj_21 + - /odom_traj_11 + - /estim_pose_11 + - /estim_traj_11 + Splitter Ratio: 0.5 + Tree Height: 328 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: img_2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: origin_axes + Radius: 0.1 + Reference Frame: + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: anchor_frame_1 + Radius: 0.1 + Reference Frame: mf1_anchor + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: anchor_frame_2 + Radius: 0.1 + Reference Frame: mf3_anchor + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /nickkoukubuntu/onboard_camera/image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: img_1 + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /odroidxu3/onboard_camera/image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: img_2 + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + back_sonar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_sonar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + hokuyo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top_plate: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: robot_model_1 + Robot Description: robot_description + TF Prefix: nickkoukubuntu + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + back_sonar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_sonar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + hokuyo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top_plate: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: robot_model_2 + Robot Description: robot_description + TF Prefix: odroidxu3 + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: laser_scan_1 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /nickkoukubuntu/input/laser_scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: laser_scan_2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /odroidxu3/input/laser_scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Angle Tolerance: 0.1 + Class: rviz/Odometry + Color: 255; 25; 0 + Enabled: false + Keep: 100 + Length: 1 + Name: odom_1 + Position Tolerance: 0.1 + Topic: /nickkoukubuntu/input/odom + Value: false + - Angle Tolerance: 0.1 + Class: rviz/Odometry + Color: 255; 25; 0 + Enabled: false + Keep: 100 + Length: 1 + Name: odom_2 + Position Tolerance: 0.1 + Topic: /odroidxu3/input/odom + Value: false + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: map_2 + Topic: /odroidxu3/feedback/gridmap + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 0; 255 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: odom_traj_2 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /odroidxu3/feedback/odom_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.1 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: estim_pose_2 + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Arrow + Topic: /odroidxu3/feedback/robot_position + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 85; 0 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: estim_traj_2 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /odroidxu3/feedback/robot_trajectory + Unreliable: false + Value: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: map_1 + Topic: /nickkoukubuntu/feedback/gridmap + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 0; 255 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: odom_traj_1 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /nickkoukubuntu/feedback/odom_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.1 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: estim_pose_1 + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Arrow + Topic: /nickkoukubuntu/feedback/robot_position + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 85; 0 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: estim_traj_1 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /nickkoukubuntu/feedback/robot_trajectory + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 20.7017 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 3.048 + Y: -4.8197 + Z: -4.44887 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.9848 + Target Frame: + Value: Orbit (rviz) + Yaw: 2.5954 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1027 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001a300000360fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006f00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004300000189000000f300fffffffb0000000a0069006d0067005f003101000001d2000000f10000001600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0069006d0067005f003201000002c9000000da0000001600fffffffb0000000a0069006d0067005f003101000001d7000000b20000000000000000000000010000010f000001e0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000043000001e0000000bd00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000027900fffffffb0000000800540069006d00650100000000000004500000000000000000000005d70000036000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 0 + Y: 31 + img_1: + collapsed: false + img_2: + collapsed: false diff --git a/mrpt_graphslam_2d/rviz/graphslam_real_3.rviz b/mrpt_graphslam_2d/rviz/graphslam_real_3.rviz new file mode 100644 index 0000000..9671e2d --- /dev/null +++ b/mrpt_graphslam_2d/rviz/graphslam_real_3.rviz @@ -0,0 +1,552 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /img_31 + Splitter Ratio: 0.5 + Tree Height: 232 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: img_2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: origin_axes + Radius: 0.1 + Reference Frame: + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: anchor_frame_1 + Radius: 0.1 + Reference Frame: mf1_anchor + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: anchor_frame_2 + Radius: 0.1 + Reference Frame: mf3_anchor + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: anchor_frame_3 + Radius: 0.1 + Reference Frame: mf5_anchor + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /nickkoukubuntu/onboard_camera/image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: img_1 + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /odroidxu3/onboard_camera/image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: img_2 + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /odroidu2/onboard_camera/image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: img_3 + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: robot_model_1 + Robot Description: robot_description + TF Prefix: nickkoukubuntu + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: robot_model_2 + Robot Description: robot_description + TF Prefix: odroidxu3 + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: robot_model_3 + Robot Description: robot_description + TF Prefix: odroidu2 + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: laser_scan_1 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /nickkoukubuntu/input/laser_scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: laser_scan_2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /odroidxu3/input/laser_scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: laser_scan_3 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /odroidu2/input/laser_scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Angle Tolerance: 0.1 + Class: rviz/Odometry + Color: 255; 25; 0 + Enabled: false + Keep: 100 + Length: 1 + Name: odom_1 + Position Tolerance: 0.1 + Topic: /nickkoukubuntu/input/odom + Value: false + - Angle Tolerance: 0.1 + Class: rviz/Odometry + Color: 255; 25; 0 + Enabled: false + Keep: 100 + Length: 1 + Name: odom_2 + Position Tolerance: 0.1 + Topic: /odroidxu3/input/odom + Value: false + - Angle Tolerance: 0.1 + Class: rviz/Odometry + Color: 255; 25; 0 + Enabled: false + Keep: 100 + Length: 1 + Name: odom_3 + Position Tolerance: 0.1 + Topic: /odroidu2/input/odom + Value: false + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: map_1 + Topic: /nickkoukubuntu/feedback/gridmap + Unreliable: false + Value: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: map_2 + Topic: /odroidxu3/feedback/gridmap + Unreliable: false + Value: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: map_3 + Topic: /odroidu2/feedback/gridmap + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 85; 0 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: estim_traj_1 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /nickkoukubuntu/feedback/robot_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 85; 0 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: estim_traj_2 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /odroidxu3/feedback/robot_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 85; 0 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: estim_traj_3 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /odroidu2/feedback/robot_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 0; 255 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: odom_traj_1 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /nickkoukubuntu/feedback/odom_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 0; 255 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: odom_traj_2 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /odroidxu3/feedback/odom_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 0; 255 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: odom_traj_3 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /odroidu2/feedback/odom_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.1 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: estim_pose_1 + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Arrow + Topic: /nickkoukubuntu/feedback/robot_position + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.1 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: estim_pose_2 + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Arrow + Topic: /odroidxu3/feedback/robot_position + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.1 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: estim_pose_3 + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Arrow + Topic: /odroidu2/feedback/robot_position + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 28.726 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 4.92833 + Y: 3.92607 + Z: -4.21462 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 1.3298 + Target Frame: + Value: Orbit (rviz) + Yaw: 2.77223 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1023 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001a30000035cfc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004100000129000000dd00fffffffb0000000a0069006d0067005f00310100000170000000b60000001600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0069006d0067005f0032010000022c000000a50000001600fffffffb0000000a0069006d0067005f003101000001d7000000b20000000000000000fb0000000a0069006d0067005f003301000002d7000000c60000001600ffffff000000010000010f000001e0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000043000001e0000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077e0000003efc0100000002fb0000000800540069006d006501000000000000077e0000028400fffffffb0000000800540069006d00650100000000000004500000000000000000000005d50000035c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1918 + X: 0 + Y: 31 + img_1: + collapsed: false + img_2: + collapsed: false + img_3: + collapsed: false diff --git a/mrpt_graphslam_2d/rviz/sr_graphslam.rviz b/mrpt_graphslam_2d/rviz/sr_graphslam.rviz index 98c6cc1..a3f89d7 100644 --- a/mrpt_graphslam_2d/rviz/sr_graphslam.rviz +++ b/mrpt_graphslam_2d/rviz/sr_graphslam.rviz @@ -1,6 +1,6 @@ Panels: - Class: rviz/Displays - Help Height: 81 + Help Height: 89 Name: Displays Property Tree Widget: Expanded: @@ -14,7 +14,7 @@ Panels: - /Current Pose1 - /LaserScan1 Splitter Ratio: 0.5 - Tree Height: 404 + Tree Height: 710 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -64,19 +64,25 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: true + base_link: + Value: true + map: + Value: true Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: true Tree: - {} + map: + base_link: + {} Update Interval: 0 Value: true - Alpha: 0.7 Class: rviz/Map Color Scheme: map - Draw Behind: false + Draw Behind: true Enabled: true Name: map Topic: /feedback/gridmap @@ -87,6 +93,9 @@ Visualization Manager: Class: rviz/Path Color: 0; 0; 255 Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 Line Style: Lines Line Width: 0.03 Name: Odom Trajectory @@ -94,6 +103,10 @@ Visualization Manager: X: 0 Y: 0 Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 Topic: /feedback/odom_trajectory Unreliable: false Value: true @@ -102,6 +115,9 @@ Visualization Manager: Class: rviz/Path Color: 255; 85; 0 Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 Line Style: Lines Line Width: 0.03 Name: SLAM Trajectory @@ -109,6 +125,10 @@ Visualization Manager: X: 0 Y: 0 Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 Topic: /feedback/robot_trajectory Unreliable: false Value: true @@ -147,7 +167,7 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 @@ -164,7 +184,7 @@ Visualization Manager: Unreliable: false Use Fixed Frame: true Use rainbow: true - Value: false + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -189,30 +209,30 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 4.91671 + Distance: 11.2431 Enable Stereo Rendering: Stereo Eye Separation: 0.06 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -0.163164 - Y: 0.59622 - Z: 1.76263 + X: -1.04912 + Y: 1.19479 + Z: 0.964277 Name: Current View Near Clip Distance: 0.01 - Pitch: 1.5048 + Pitch: 0.889802 Target Frame: Value: Orbit (rviz) - Yaw: 1.63359 + Yaw: 2.57542 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 715 + Height: 1027 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a00000226fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006700fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000001d1000000980000006700fffffffb000000180069006d0067005f0074006f0070005f006c0065006600740100000043000001220000000000000000fb0000001a0069006d0067005f0074006f0070005f007200690067006800740100000043000002260000000000000000000000010000016a00000226fc0200000003fb000000100044006900730070006c006100790073010000004300000226000000e300fffffffb0000000a00560069006500770073000000004300000222000000b300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005000000003efc0100000002fb0000000800540069006d00650100000000000005000000028300fffffffb0000000800540069006d00650100000000000004500000000000000000000003900000022600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000226fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006f00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000001d1000000980000006f00fffffffb000000180069006d0067005f0074006f0070005f006c0065006600740100000043000001220000000000000000fb0000001a0069006d0067005f0074006f0070005f007200690067006800740100000043000002260000000000000000000000010000016a00000360fc0200000003fb000000100044006900730070006c006100790073010000004300000360000000f300fffffffb0000000a00560069006500770073000000004300000222000000bd00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000027900fffffffb0000000800540069006d00650100000000000004500000000000000000000006100000036000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -221,6 +241,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1280 + Width: 1920 X: 0 Y: 31 diff --git a/mrpt_graphslam_2d/rviz/sr_graphslam_demo_gt.rviz b/mrpt_graphslam_2d/rviz/sr_graphslam_demo_gt.rviz index ec01dfe..f7b07ba 100644 --- a/mrpt_graphslam_2d/rviz/sr_graphslam_demo_gt.rviz +++ b/mrpt_graphslam_2d/rviz/sr_graphslam_demo_gt.rviz @@ -112,7 +112,7 @@ Visualization Manager: Name: img_top_left Normalize Range: true Queue Size: 2 - Transport Hint: raw + Transport Hint: compressed Unreliable: false Value: true - Class: rviz/Image @@ -124,7 +124,7 @@ Visualization Manager: Name: img_top_right Normalize Range: true Queue Size: 2 - Transport Hint: raw + Transport Hint: compressed Unreliable: false Value: true - Alpha: 0.7 diff --git a/mrpt_graphslam_2d/src/CConnectionManager.cpp b/mrpt_graphslam_2d/src/CConnectionManager.cpp new file mode 100644 index 0000000..43eb598 --- /dev/null +++ b/mrpt_graphslam_2d/src/CConnectionManager.cpp @@ -0,0 +1,308 @@ +/* +---------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | http://www.mrpt.org/ | + | | + | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | + | See: http://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See details in http://www.mrpt.org/License | + +---------------------------------------------------------------------------+ */ + +#include "mrpt_graphslam_2d/CConnectionManager.h" + +using namespace mrpt::graphslam::detail; +using namespace std; +using namespace mrpt::utils; +using namespace mrpt::system; +using namespace mrpt::math; +using namespace multimaster_msgs_fkie; + +bool operator==( + const multimaster_msgs_fkie::ROSMaster& master1, + const multimaster_msgs_fkie::ROSMaster& master2) { + return master1.uri == master2.uri; +} +bool operator!=( + const multimaster_msgs_fkie::ROSMaster& master1, + const multimaster_msgs_fkie::ROSMaster& master2) { + return !(master1 == master2); +} + +bool operator==( + const mrpt_msgs::GraphSlamAgent& agent1, + const mrpt_msgs::GraphSlamAgent& agent2) { + return ( + agent1.agent_ID == agent2.agent_ID && + agent1.topic_namespace.data == agent2.topic_namespace.data); +} +//////////////////////////////////////////////////////////// + +bool operator!=( + const mrpt_msgs::GraphSlamAgent& agent1, + const mrpt_msgs::GraphSlamAgent& agent2) { + return !(agent1 == agent2); +} + +bool operator<( + const mrpt_msgs::GraphSlamAgent& agent1, + const mrpt_msgs::GraphSlamAgent& agent2) { + return agent1.agent_ID < agent2.agent_ID; +} +//////////////////////////////////////////////////////////// + +bool operator==( + const multimaster_msgs_fkie::ROSMaster& master, + const mrpt_msgs::GraphSlamAgent& agent) { + return (master.name == agent.name.data); +} + +bool operator!=( + const multimaster_msgs_fkie::ROSMaster& master, + const mrpt_msgs::GraphSlamAgent& agent) { + return !(master == agent); +} + +bool operator==( + const mrpt_msgs::GraphSlamAgent& agent, + const multimaster_msgs_fkie::ROSMaster& master) { + return (master == agent); +} +bool operator!=( + const mrpt_msgs::GraphSlamAgent& agent, + const multimaster_msgs_fkie::ROSMaster& master) { + return (master != agent); +} + +//////////////////////////////////////////////////////////// + +CConnectionManager::CConnectionManager( + mrpt::utils::COutputLogger* logger, + ros::NodeHandle* nh_in): + m_logger(logger), + m_nh(nh_in), + has_setup_comm(false) +{ + + ASSERT_(m_logger); + ASSERT_(m_nh); + + { + std::string own_ns_tmp = m_nh->getNamespace(); + // ignore starting "/" characters + own_ns = std::string( + own_ns_tmp.begin() + own_ns_tmp.find_first_not_of(" /"), + own_ns_tmp.end()); + } + + // keep this call below the topic names initializations + this->setupComm(); + +} + +CConnectionManager::~CConnectionManager() { } + +const std::string& CConnectionManager::getTrimmedNs() const { + return own_ns; +} + +void CConnectionManager::getNearbySlamAgents( + mrpt_msgs::GraphSlamAgents* agents_vec, + bool ignore_self/*= true */) { + + + ASSERTMSG_(agents_vec, "Invalid pointer to vector of GraphSlam Agents."); + this->updateNearbySlamAgents(); + *agents_vec = m_nearby_slam_agents; + + if (ignore_self) { + // remove the GraphSlamAgent instance whose topic namespace coincedes with + // the namespace that the CConnectionManager instance is running under. + auto search = [this](const mrpt_msgs::GraphSlamAgent& agent) { + return (agent.topic_namespace.data == this->own_ns); + }; + agents_it it = find_if( + agents_vec->list.begin(), + agents_vec->list.end(), search); + + // TODO - fix the following + // this agent should always exist + // TODO - well, sometimes it doesn't, investigate this + // UPDATE: Even when /master_discovery node is up the agents vector might + // be empty. + //ASSERT_(it != agents_vec->list.end()); + if (it != agents_vec->list.end()) { + agents_vec->list.erase(it); + } + else { + } + } +} + +const mrpt_msgs::GraphSlamAgents& +CConnectionManager::getNearbySlamAgentsCached() const { + return m_nearby_slam_agents; +} + +const mrpt_msgs::GraphSlamAgents& +CConnectionManager::getNearbySlamAgents() { + this->updateNearbySlamAgents(); + return this->getNearbySlamAgentsCached(); +} // end of getNearbySlamAgents + +void CConnectionManager::updateNearbySlamAgents() { + using ::operator==; + using namespace mrpt::utils; + using namespace mrpt::math; + ASSERT_(has_setup_comm); + + DiscoverMasters srv; + + // ask for the agents in the neighborhood + m_DiscoverMasters_client.call(srv); + std::vector* masters = &(srv.response.masters); + + // convert RosMaster(s) to mrpt_msgs::GraphSlamAgent(s) + for (std::vector::const_iterator + masters_it = masters->begin(); + masters_it != masters->end(); + ++masters_it) { + + // 3 cases: + // In RosMasters AND In mrpt_msgs::GraphSlamAgents => update relevant fields + // In RosMasters AND NOT In mrpt_msgs::GraphSlamAgents => add it to mrpt_msgs::GraphSlamAgents + // NOT In RosMasters AND In mrpt_msgs::GraphSlamAgents => Do nothing. + + // have I already registered the current agent? + auto search = [masters_it](const mrpt_msgs::GraphSlamAgent& agent) { + return agent == *masters_it; + }; + agents_it it = find_if( + m_nearby_slam_agents.list.begin(), + m_nearby_slam_agents.list.end(), search); + + if (it != m_nearby_slam_agents.list.end()) { // found, update relevant fields + // update timestamp + it->last_seen_time.data = ros::Time(masters_it->timestamp); + } + else { // not found, try to insert it. + mrpt_msgs::GraphSlamAgent new_agent; + bool is_agent = this->convert(*masters_it, &new_agent); + if (is_agent) { + m_nearby_slam_agents.list.push_back(new_agent); + }; + } + + } // for all ROSMaster(s) + +} // end of updateNearbySlamAgents + + +void CConnectionManager::setupComm() { + + this->setupSubs(); + this->setupPubs(); + this->setupSrvs(); + + has_setup_comm = true; +} // end of setupComm + +void CConnectionManager::setupSubs() { } +void CConnectionManager::setupPubs() { } +void CConnectionManager::setupSrvs() { + // call to the querier should be made after the + // multimaster_msgs_fkie::DiscoverMaster service is up and running + m_DiscoverMasters_client = + m_nh->serviceClient( + "/master_discovery/list_masters"); + + //ASSERT_(m_DiscoverMasters_client.isValid()); +} + +bool CConnectionManager::convert( + const multimaster_msgs_fkie::ROSMaster& ros_master, + mrpt_msgs::GraphSlamAgent* slam_agent) { + ASSERT_(slam_agent); + bool agent_namespace_found = false; + + slam_agent->name.data = ros_master.name; + slam_agent->is_online.data = static_cast(ros_master.online); + + // ip_address, hostname, port + std::string ip_addr = CConnectionManager::extractHostnameOrIP(ros_master.monitoruri); + slam_agent->ip_addr.data = ip_addr; + std::string hostname = CConnectionManager::extractHostnameOrIP( + ros_master.uri, &slam_agent->port); + slam_agent->hostname.data = hostname; + + // agent_ID - last field of the IP address + vector tokens; + mrpt::system::tokenize(ip_addr, ".", tokens); + slam_agent->agent_ID = atoi(tokens.rbegin()->c_str()); + + // robot topic namespace + { + //stringstream ss(""); + //ss << slam_agent->name.data << "_" << slam_agent->agent_ID; + //slam_agent->topic_namespace.data = ss.str().c_str(); + slam_agent->topic_namespace.data = slam_agent->name.data; + + // assert that there exists a subtopic namespace named feedback under this. + ros::master::V_TopicInfo topics; + bool got_topics = ros::master::getTopics(topics); + ASSERTMSG_(got_topics, "Unable to fetch topics. Exiting."); + + // get the namespaces under the current topic_namespace + const std::string& topic_ns = "/" + slam_agent->topic_namespace.data; + // TODO - What if this topic changes? from the configuration file + const std::string& feedback_ns = + "/" + slam_agent->topic_namespace.data + "/" + "feedback"; + + auto search = [&feedback_ns]( + const ros::master::TopicInfo& topic) { + return (strStarts(topic.name, feedback_ns)); + }; + ros::master::V_TopicInfo::const_iterator cit = find_if( + topics.begin(), topics.end(), search); + if (cit != topics.end()) { + agent_namespace_found = true; + } + } + + // timestamp + slam_agent->last_seen_time.data = ros::Time(ros_master.timestamp); + return agent_namespace_found; + +} // end of convert + +void CConnectionManager::convert( + const mrpt_msgs::GraphSlamAgent& slam_agent, + multimaster_msgs_fkie::ROSMaster* ros_master) { + ASSERT_(ros_master); + + ros_master->name = slam_agent.name.data; + { + stringstream ss(""); + ss << "http://" << slam_agent.ip_addr << ":" << slam_agent.port; + ros_master->uri = ss.str(); + } + ros_master->online = slam_agent.is_online.data; + ros_master->discoverer_name = "/master_discovery"; + + // TODO - timestamp + +} + +std::string CConnectionManager::extractHostnameOrIP(const std::string& str, + unsigned short* agent_port/*=NULL*/) { + // example for monitoruri: http://nickkouk-ubuntu:11311/ + std::string s = std::string(str.begin()+7, str.end()); + + vector tokens; + mrpt::system::tokenize(s, ":", tokens); + + if (agent_port) { + *agent_port = static_cast(atoi(tokens[1].c_str())); + } + + return tokens[0]; +} + diff --git a/mrpt_graphslam_2d/src/CGraphSlamResources.cpp b/mrpt_graphslam_2d/src/CGraphSlamResources.cpp deleted file mode 100644 index 310421e..0000000 --- a/mrpt_graphslam_2d/src/CGraphSlamResources.cpp +++ /dev/null @@ -1,743 +0,0 @@ -/* +---------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | http://www.mrpt.org/ | - | | - | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | - | See: http://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See details in http://www.mrpt.org/License | - +---------------------------------------------------------------------------+ */ - -#include "mrpt_graphslam_2d/CGraphSlamResources.h" - -// supplementary functions - TODO - where to put these? -template -std::string getVectorAsString(const T& t) { - using namespace std; - stringstream ss; - for (typename T::const_iterator it = t.begin(); it != t.end(); ++it) { - ss << *it << ", "; - } - return ss.str(); -} -template -void printVector(const T& t) { - std::cout << getVectorAsString(t) << std::endl; -} -template -void printVectorOfVectors(const T& t) { - int i = 0; - for (typename T::const_iterator it = t.begin(); it != t.end(); ++i, ++it) { - printf("Vector %d/%lu:\n\t", i, t.size()); - printVector(*it); - } -} - - -// static member variables -const std::string CGraphSlamResources::sep_header(40, '='); -const std::string CGraphSlamResources::sep_subheader(20, '-'); - -////////////////////////////////////////////////////////////////////////////// -// Ctor -CGraphSlamResources::CGraphSlamResources( - mrpt::utils::COutputLogger* logger_in, - ros::NodeHandle* nh_in): - m_logger(logger_in), - nh(nh_in) -{ - using namespace std; - using namespace mrpt::obs; - - ASSERT_(m_logger); - ASSERT_(nh_in); - - m_graphslam_handler = new CGraphSlamHandler(); - m_graphslam_handler->setOutputLoggerPtr(m_logger); - - // variables initialization/assignment - m_has_read_config = false; - m_pub_seq = 0; - this->resetReceivedFlags(); - - // measurements initialization - m_mrpt_odom = CObservationOdometry::Create(); - m_mrpt_odom->hasEncodersInfo = false; - m_mrpt_odom->hasVelocities = false; - - m_graphslam_engine = NULL; - - m_measurement_cnt = 0; - - // Thu Nov 3 23:36:49 EET 2016, Nikos Koukis - // WARNING: ROS Server Parameters have not been read yet. Make sure you know - // what to initialize at this stage! -} -////////////////////////////////////////////////////////////////////////////// -CGraphSlamResources::~CGraphSlamResources() { - using namespace mrpt::utils; - - - // cleaning heap... - m_logger->logFmt(LVL_DEBUG, "Releasing CGraphSlamEngine instance..."); - delete m_graphslam_engine; - m_logger->logFmt(LVL_DEBUG, "Releasing CGraphSlamHandler instance..."); - delete m_graphslam_handler; - -} -////////////////////////////////////////////////////////////////////////////// - -void CGraphSlamResources::readParams() { - this->readROSParameters(); - m_graphslam_handler->readConfigFname(m_ini_fname); - - - m_has_read_config = true; -} -void CGraphSlamResources::readROSParameters() { - using namespace mrpt::utils; - - // misc - { - std::string ns = "misc/"; - - // enable/disable visuals - nh->param(ns + "disable_MRPT_visuals", m_disable_MRPT_visuals, false); - - // verbosity level - int lvl; - nh->param(ns + "verbosity", - lvl, - static_cast(LVL_INFO)); - m_min_logging_level = static_cast(lvl); - this->m_logger->setMinLoggingLevel(m_min_logging_level); - } - // deciders, optimizer - { - std::string ns = "deciders_optimizers/"; - nh->param(ns + "NRD", m_node_reg, "CFixedIntervalsNRD"); - nh->param(ns + "ERD", m_edge_reg, "CICPCriteriaERD"); - nh->param(ns + "GSO", m_optimizer, "CLevMarqGSO"); - } - // filenames - { - std::string ns = "files/"; - - // configuration file - mandatory - std::string config_param_path = ns + "config"; - bool found_config = nh->getParam(ns + "config", m_ini_fname); - ASSERTMSG_(found_config, - mrpt::format("Configuration file was not set. Set %s and try again.\nExiting...", - config_param_path.c_str())); - - // ground-truth file - nh->getParam(ns + "ground_truth", m_gt_fname); - } - - // TF Frame IDs - // names of the frames of the corresponding robot parts - { - std::string ns = "frame_IDs/"; - - nh->param(ns + "anchor_frame" , m_anchor_frame_id , "map"); - nh->param(ns + "base_link_frame" , m_base_link_frame_id , "base_link"); - nh->param(ns + "odometry_frame" , m_odom_frame_id , "odom"); - nh->param(ns + "laser_frame" , m_laser_frame_id , "laser"); - - // take action based on the above frames - // - - } - - // ASSERT that the given user options are valid - this->verifyUserInput(); - - m_logger->logFmt(LVL_DEBUG, - "Successfully read parameters from ROS Parameter Server"); - - this->initGraphSLAM(); -} -////////////////////////////////////////////////////////////////////////////// -void CGraphSlamResources::readStaticTFs() { - using namespace mrpt::utils; - - // base_link => laser - m_logger->logFmt(LVL_WARN, "Looking up static transform...%s => %s", - m_laser_frame_id.c_str(), - m_base_link_frame_id.c_str()); - try { - m_base_laser_transform = m_buffer.lookupTransform( - /* target_fname = */ m_laser_frame_id, - /* source_fname = */ m_base_link_frame_id, - /* transform time = */ ros::Time(0)); - m_logger->logFmt(LVL_INFO, "OK."); - } - catch (tf2::LookupException &ex) { - m_logger->logFmt(LVL_WARN, "Transformation not found, using default..."); - - m_base_laser_transform.header.stamp = ros::Time::now(); - m_base_laser_transform.transform.translation.x = 0.0; - m_base_laser_transform.transform.translation.y = 0.0; - m_base_laser_transform.transform.translation.z = 0.0; - m_base_laser_transform.transform.rotation.x = 0.0; - m_base_laser_transform.transform.rotation.y = 0.0; - m_base_laser_transform.transform.rotation.z = 0.0; - m_base_laser_transform.transform.rotation.w = 1.0; - - m_base_laser_transform.header.frame_id = m_base_link_frame_id; - m_base_laser_transform.child_frame_id = m_laser_frame_id; - } - - - -} -////////////////////////////////////////////////////////////////////////////// -void CGraphSlamResources::initGraphSLAM() { - using namespace mrpt::graphs; - using namespace mrpt::graphslam; - using namespace mrpt::utils; - - m_logger->logFmt(LVL_DEBUG, "Initializing CGraphSlamEngine instance..."); - - - // TODO - quickfix for calling CGraphSlamEngine... - std::string rawlog_fname(""); - m_graphslam_handler->setRawlogFname(rawlog_fname); - - // Visuals initialization - if (!m_disable_MRPT_visuals) { - m_graphslam_handler->initVisualization(); - } - - m_graphslam_engine = new CGraphSlamEngine( - m_ini_fname, - rawlog_fname, - m_gt_fname, - m_graphslam_handler->win_manager, - m_graphslam_opts.node_regs_map[m_node_reg](), - m_graphslam_opts.edge_regs_map[m_edge_reg](), - m_graphslam_opts.optimizers_map[m_optimizer]()); - - m_logger->logFmt(LVL_DEBUG, "Successfully initialized CGraphSlamEngine instance."); -} -////////////////////////////////////////////////////////////////////////////// -void CGraphSlamResources::getROSParameters(std::string* str_out) { - using namespace std; - using namespace mrpt::utils; - - ASSERT_(str_out); - - stringstream ss(""); - - - ss << "ROS Parameters: " << endl; - ss << sep_header << endl; - ss << endl; - - ss << "Deciders / Optimizers = " << endl; - ss << sep_subheader << endl; - ss << "Node Registration Decider = " << m_node_reg << endl; - ss << "Edge Registration Decider = " << m_edge_reg << endl; - ss << "GraphSLAM Optimizer = " << m_optimizer << endl; - ss << endl; - - ss << "Filenames: " << endl; - ss << sep_subheader << endl; - ss << "Configuration .ini file = " << m_ini_fname << endl; - ss << "Ground truth filename = " << (!m_gt_fname.empty() ? m_gt_fname : "NONE") - << endl; - ss << endl; - - ss << "Miscellaneous: " << endl; - ss << sep_subheader << endl; - ss << "Enable MRPT visuals? = " << (m_disable_MRPT_visuals? "FALSE" : "TRUE") - << endl; - ss << "Logging verbosity Level = " << - COutputLogger::logging_levels_to_names[m_min_logging_level] << endl;; - ss << endl; - - *str_out = ss.str(); -} - -////////////////////////////////////////////////////////////////////////////// -void CGraphSlamResources::getParamsAsString(std::string* str_out) { - ASSERT_(str_out); - - // ros parameters - std::string ros_params(""); - this->getROSParameters(&ros_params); - *str_out += ros_params; - - // various parameters -} - -////////////////////////////////////////////////////////////////////////////// -std::string CGraphSlamResources::getParamsAsString() { - std::string params; - this->getParamsAsString(¶ms); - return params; -} - -////////////////////////////////////////////////////////////////////////////// -void CGraphSlamResources::printParams() { - using namespace std; - // print the problem parameters - cout << this->getParamsAsString() << endl; - - m_graphslam_handler->printParams(); - m_graphslam_engine->printParams(); - - -} -////////////////////////////////////////////////////////////////////////////// -void CGraphSlamResources::verifyUserInput() { - using namespace mrpt; - using namespace mrpt::utils; - m_logger->logFmt(LVL_DEBUG, "Verifying user input..."); - - - // verify the NRD, ERD, GSO parameters - bool node_success, edge_success, optimizer_success; - bool failed = false; - - node_success = m_graphslam_opts.checkRegistrationDeciderExists(m_node_reg, "node"); - edge_success = m_graphslam_opts.checkRegistrationDeciderExists(m_edge_reg, "edge"); - optimizer_success = m_graphslam_opts.checkOptimizerExists(m_optimizer); - - if (!node_success) { - m_logger->logFmt(LVL_ERROR, - "\nNode Registration Decider \"%s\" is not available", - m_node_reg.c_str()); - m_graphslam_opts.dumpRegistrarsToConsole("node"); - failed = true; - } - if (!edge_success) { - m_logger->logFmt(LVL_ERROR, - "\nEdge Registration Decider \"%s\" is not available.", - m_edge_reg.c_str()); - m_graphslam_opts.dumpRegistrarsToConsole("edge"); - failed = true; - } - if (!optimizer_success) { - m_logger->logFmt(LVL_ERROR, - "\ngraphSLAM Optimizser \"%s\" is not available.", - m_optimizer.c_str()); - m_graphslam_opts.dumpOptimizersToConsole(); - failed = true; - } - ASSERT_(!failed) - - // verify that the path to the files is correct - // .ini file - bool ini_exists = system::fileExists(m_ini_fname); - ASSERTMSG_(ini_exists, - format( - "\n.ini configuration file \"%s\"doesn't exist. Please specify a valid pathname.\nExiting...\n", - m_ini_fname.c_str())); - // ground-truth file - if (!m_gt_fname.empty()) { - bool gt_exists = system::fileExists(m_gt_fname); - ASSERTMSG_(gt_exists, - format( - "\nGround truth file \"%s\"doesn't exist. Either unset the corresponding ROS parameter or specify a valid pathname.\nExiting...\n", - m_gt_fname.c_str())); - } - -} - -////////////////////////////////////////////////////////////////////////////// -void CGraphSlamResources::setupCommunication() { - - // setup subscribers, publishers, services... - this->setupSubs(); - this->setupPubs(); - this->setupSrvs(); - - // fetch the static geometrical transformations - this->readStaticTFs(); - -} - -void CGraphSlamResources::setupSubs() { - using namespace mrpt::utils; - m_logger->logFmt(LVL_INFO, "Setting up the subscribers..."); - - // setup the names - std::string ns = "input/"; - - m_odom_topic = ns + "odom"; - m_laser_scan_topic = ns + "laser_scan"; - - // odometry - m_odom_sub = nh->subscribe( - m_odom_topic, - m_queue_size, - &CGraphSlamResources::sniffOdom, this); - - // laser_scans - m_laser_scan_sub = nh->subscribe( - m_laser_scan_topic, - m_queue_size, - &CGraphSlamResources::sniffLaserScan, this); - - // camera - // TODO - - // 3D point clouds - // TODO - -} -////////////////////////////////////////////////////////////////////////////// -void CGraphSlamResources::setupPubs() { - using namespace mrpt::utils; - m_logger->logFmt(LVL_INFO, "Setting up the publishers..."); - - // setup the names - std::string ns = "feedback/"; - - m_curr_robot_pos_topic = ns + "robot_position"; - m_robot_trajectory_topic = ns + "robot_trajectory"; - m_robot_tr_poses_topic = ns + "robot_tr_poses"; - m_odom_trajectory_topic = ns + "odom_trajectory"; - m_SLAM_eval_metric_topic = ns + "evaluation_metric"; - m_gridmap_topic = ns + "gridmap"; - - // agent estimated position - m_curr_robot_pos_pub = nh->advertise( - m_curr_robot_pos_topic, - m_queue_size); - m_robot_trajectory_pub = nh->advertise( - m_robot_trajectory_topic, - m_queue_size); - m_robot_tr_poses_pub = nh->advertise( - m_robot_tr_poses_topic, - m_queue_size); - - // odometry nav_msgs::Path - m_odom_path.header.seq = 0; - m_odom_path.header.stamp = ros::Time::now(); - m_odom_path.header.frame_id = m_anchor_frame_id; - - m_odom_trajectory_pub = nh->advertise( - m_odom_trajectory_topic, - m_queue_size); - - // generated gridmap - m_gridmap_pub = nh->advertise( - m_gridmap_topic, - m_queue_size); - - -} - -void CGraphSlamResources::setupSrvs() { - using namespace mrpt::utils; - m_logger->logFmt(LVL_INFO, "Setting up the services..."); - - // SLAM statistics - // Error statistics - // Graph statistics - - // TODO - Implement this... -} - -bool CGraphSlamResources::usePublishersBroadcasters() { - using namespace mrpt::poses; - using namespace mrpt::utils; - using namespace std; - - - ros::Time timestamp = ros::Time::now(); - - // current MRPT robot pose - pose_t mrpt_pose = m_graphslam_engine->getCurrentRobotPosEstimation(); - - // anchor frame <=> base_link - { - // fill the geometry_msgs::TransformStamped object - geometry_msgs::TransformStamped transform_stamped; - transform_stamped.header.stamp = ros::Time::now(); - transform_stamped.header.frame_id = m_anchor_frame_id; - transform_stamped.child_frame_id = m_base_link_frame_id; - - transform_stamped.transform.translation.x = mrpt_pose.x(); - transform_stamped.transform.translation.y = mrpt_pose.y(); - transform_stamped.transform.translation.z = 0; - - tf2::Quaternion q; - q.setRPY(0, 0, mrpt_pose.phi()); - tf2::Vector3 axis = q.getAxis(); - tf2Scalar w = q.getW(); - transform_stamped.transform.rotation.x = axis.getX(); - transform_stamped.transform.rotation.y = axis.getY(); - transform_stamped.transform.rotation.z = axis.getZ(); - transform_stamped.transform.rotation.w = w; - - m_broadcaster.sendTransform(transform_stamped); - - } - - // anchor frame <=> odom frame - // - // make sure that we have received odometry information in the first place... - // the corresponding field would be initialized - if (!m_anchor_odom_transform.child_frame_id.empty()) { - m_broadcaster.sendTransform(m_anchor_odom_transform); - } - - - // set an arrow indicating clearly the current orientation of the robot - { - geometry_msgs::PoseStamped geom_pose; - - geom_pose.header.stamp = timestamp; - geom_pose.header.seq = m_pub_seq; - geom_pose.header.frame_id = m_base_link_frame_id; // with regards to base_link... - - geometry_msgs::Point point; - point.x = 0; - point.y = 0; - point.z = 0; - geometry_msgs::Quaternion quat; - quat.x = 0; - quat.y = 0; - quat.z = 0; - quat.w = 1; - - geom_pose.pose.position = point; - geom_pose.pose.orientation = quat; - m_curr_robot_pos_pub.publish(geom_pose); - } - - // robot trajectory - // publish the trajectory of the robot - { - m_logger->logFmt(LVL_DEBUG, "Publishing the current robot trajectory"); - mrpt::graphs::CNetworkOfPoses2DInf::global_poses_t graph_poses; - m_graphslam_engine->getRobotEstimatedTrajectory(&graph_poses); - - nav_msgs::Path path; - - // set the header - path.header.stamp = timestamp; - path.header.seq = m_pub_seq; - path.header.frame_id = m_anchor_frame_id; - - // - // fill in the pose as well as the nav_msgs::Path at the same time. - // - - geometry_msgs::PoseArray geom_poses; - geom_poses.header.stamp = timestamp; - geom_poses.header.frame_id = m_anchor_frame_id; - - for (int i = 0; i != graph_poses.size(); ++i) { - geometry_msgs::PoseStamped geom_pose_stamped; - geometry_msgs::Pose geom_pose; - - // grab the pose - convert to geometry_msgs::Pose format - pose_t mrpt_pose = graph_poses.at(i); - mrpt_bridge::convert(mrpt_pose, geom_pose); - geom_poses.poses.push_back(geom_pose); - geom_pose_stamped.pose = geom_pose; - - // edit the header - geom_pose_stamped.header.stamp = timestamp; - geom_pose_stamped.header.seq = m_pub_seq; - geom_pose_stamped.header.frame_id = m_anchor_frame_id; - - path.poses.push_back(geom_pose_stamped); - } - - m_robot_tr_poses_pub.publish(geom_poses); - m_robot_trajectory_pub.publish(path); - } - - // SLAM Evaluation Metric - { - } - - // Odometry trajectory - nav_msgs::Path - m_odom_trajectory_pub.publish(m_odom_path); - - // generated gridmap - { - std_msgs::Header h; - mrpt::system::TTimeStamp mrpt_time; - mrpt::maps::COccupancyGridMap2D mrpt_gridmap; - m_graphslam_engine->getOccupancyGridMap2D(&mrpt_gridmap, &mrpt_time); - - // timestamp - mrpt_bridge::convert(mrpt_time, h.stamp); - h.seq = m_pub_seq; - h.frame_id = m_anchor_frame_id; - - // nav gridmap - nav_msgs::OccupancyGrid nav_gridmap; - mrpt_bridge::convert(mrpt_gridmap, nav_gridmap, h); - m_gridmap_pub.publish(nav_gridmap); - } - - m_pub_seq++; - - if (!m_disable_MRPT_visuals) { - return this->continueExec(); - } -} // USEPUBLISHERSBROADCASTERS - -////////////////////////////////////////////////////////////////////////////// -void CGraphSlamResources::sniffLaserScan(const sensor_msgs::LaserScan::ConstPtr& ros_laser_scan) { - using namespace std; - using namespace mrpt::utils; - using namespace mrpt::obs; - - m_logger->logFmt(LVL_DEBUG, "sniffLaserScan: Received a LaserScan msg. Converting it to MRPT format..."); - - // build the CObservation2DRangeScan - m_mrpt_laser_scan = CObservation2DRangeScan::Create(); - mrpt::poses::CPose3D rel_pose; - mrpt_bridge::convert(*ros_laser_scan, rel_pose, *m_mrpt_laser_scan); - - m_received_laser_scan = true; - this->processObservation(m_mrpt_laser_scan); -} - -void CGraphSlamResources::sniffOdom(const nav_msgs::Odometry::ConstPtr& ros_odom) { - using namespace std; - using namespace mrpt::utils; - using namespace mrpt::obs; - using namespace mrpt::poses; - - m_logger->logFmt(LVL_DEBUG, "sniffOdom: Received an odometry msg. Converting it to MRPT format..."); - - // update the odometry frame with regards to the anchor - { - // header - m_anchor_odom_transform.header.frame_id = m_anchor_frame_id; - m_anchor_odom_transform.header.stamp = ros_odom->header.stamp; - m_anchor_odom_transform.header.seq = ros_odom->header.seq; - - m_anchor_odom_transform.child_frame_id = m_odom_frame_id; - - // translation - m_anchor_odom_transform.transform.translation.x = ros_odom->pose.pose.position.x; - m_anchor_odom_transform.transform.translation.y = ros_odom->pose.pose.position.y; - m_anchor_odom_transform.transform.translation.z = ros_odom->pose.pose.position.z; - - // quaternion - m_anchor_odom_transform.transform.rotation.x = ros_odom->pose.pose.orientation.x; - m_anchor_odom_transform.transform.rotation.y = ros_odom->pose.pose.orientation.y; - m_anchor_odom_transform.transform.rotation.z = ros_odom->pose.pose.orientation.z; - m_anchor_odom_transform.transform.rotation.w = ros_odom->pose.pose.orientation.w; - } - - // add to the overall odometry path - { - geometry_msgs::PoseStamped pose_stamped; - pose_stamped.header = ros_odom->header; - pose_stamped.pose = ros_odom->pose.pose; - m_odom_path.poses.push_back(pose_stamped); - } - - // build and fill an MRPT CObservationOdometry instance for manipulation from - // the main algorithm - mrpt_bridge::convert( - /* src = */ ros_odom->header.stamp, - /* dst = */ m_mrpt_odom->timestamp); - mrpt_bridge::convert( - /* src = */ ros_odom->pose.pose, - /* dst = */ m_mrpt_odom->odometry); - - // print the odometry - for debugging reasons... - stringstream ss; - ss << "Odometry - MRPT format:\t" << m_mrpt_odom->odometry << endl; - m_logger->logFmt(LVL_DEBUG, "%s", ss.str().c_str()); - - m_received_odom = true; - this->processObservation(m_mrpt_odom); -} - -////////////////////////////////////////////////////////////////////////////// -void CGraphSlamResources::sniffCameraImage() { - THROW_EXCEPTION("Method is not implemented yet."); - -} -////////////////////////////////////////////////////////////////////////////// -void CGraphSlamResources::sniff3DPointCloud() { - THROW_EXCEPTION("Method is not implemented yet."); - -} -////////////////////////////////////////////////////////////////////////////// -void CGraphSlamResources::processObservation(mrpt::obs::CObservationPtr& observ) { - using namespace mrpt::utils; - using namespace std; - - this->_process(observ); - this->resetReceivedFlags(); - -} -////////////////////////////////////////////////////////////////////////////// -void CGraphSlamResources::generateReport() { - using namespace std; - using namespace mrpt::utils; - - m_logger->logFmt(LVL_INFO, "Generating overall report..."); - m_graphslam_engine->generateReportFiles(m_graphslam_handler->output_dir_fname); - // save the graph and the 3DScene - if (m_graphslam_handler->save_graph) { - m_logger->logFmt(LVL_INFO, "Saving the graph..."); - std::string save_graph_fname = - m_graphslam_handler->output_dir_fname + - "/" + - m_graphslam_handler->save_graph_fname; - m_graphslam_engine->saveGraph(&save_graph_fname); - } - if (!m_disable_MRPT_visuals && m_graphslam_handler->save_3DScene) { - m_logger->logFmt(LVL_INFO, "Saving the 3DScene object..."); - std::string save_3DScene_fname = - m_graphslam_handler->output_dir_fname + - "/" + - m_graphslam_handler->save_3DScene_fname; - - m_graphslam_engine->save3DScene(&save_3DScene_fname); - } - // get the occupancy gridmap that was built - if (m_graphslam_handler->save_gridmap) { - COccupancyGridMap2D gridmap; - m_graphslam_engine->getOccupancyGridMap2D(&gridmap); - gridmap.saveMetricMapRepresentationToFile( - m_graphslam_handler->output_dir_fname + - "/" + - m_graphslam_handler->save_gridmap_fname); - } - - -} - -bool CGraphSlamResources::continueExec() { - using namespace std; - using namespace mrpt::utils; - - m_logger->logFmt(LVL_DEBUG, "In continueExec check method"); - return m_graphslam_handler->queryObserverForEvents(); -} - -////////////////////////////////////////////////////////////////////////////// -void CGraphSlamResources::_process(mrpt::obs::CObservationPtr& observ) { - using namespace mrpt::utils; - using namespace std; - - m_logger->logFmt(LVL_DEBUG, "Calling execGraphSlamStep..."); - - m_graphslam_engine->execGraphSlamStep(observ, m_measurement_cnt); - m_measurement_cnt++; -} - -////////////////////////////////////////////////////////////////////////////// -void CGraphSlamResources::resetReceivedFlags() { - m_received_odom = false; - m_received_laser_scan = false; - m_received_camera = false; - m_received_point_cloud = false; -} - - diff --git a/mrpt_graphslam_2d/src/CMapMerger.cpp b/mrpt_graphslam_2d/src/CMapMerger.cpp new file mode 100644 index 0000000..c733ac5 --- /dev/null +++ b/mrpt_graphslam_2d/src/CMapMerger.cpp @@ -0,0 +1,611 @@ +/* +---------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | http://www.mrpt.org/ | + | | + | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | + | See: http://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See details in http://www.mrpt.org/License | + +---------------------------------------------------------------------------+ */ + +#include "mrpt_graphslam_2d/CMapMerger.h" + +using namespace mrpt::graphslam; +using namespace mrpt::graphslam::detail; +using namespace mrpt_msgs; +using namespace mrpt::maps; +using namespace mrpt::slam; +using namespace mrpt::math; +using namespace mrpt::system; +using namespace mrpt::poses; +using namespace mrpt::opengl; +using namespace mrpt::utils; +using namespace ros; +using namespace nav_msgs; +using namespace std; +using namespace mrpt_bridge; + +// helper methods + +//////////////////////////////////////////////////////////// + +/**\return True if the object was actually removed. */ +bool removeObjectFrom3DScene( + mrpt::gui::CDisplayWindow3D* win, + std::string obj_name="") { + using namespace mrpt::opengl; + ASSERT_(win); + bool res = true; + COpenGLScenePtr& scene = win->get3DSceneAndLock(); + + if (obj_name.empty()) { + cout << "Clearing entire scene." << endl; + scene->clear(); + } + else { + CRenderizablePtr obj = scene->getByName(obj_name); + if (obj) { + scene->removeObject(obj); + } + else { + res = false; + } + } + + win->unlockAccess3DScene(); + win->forceRepaint(); + return res; +} + +/**\return True if the object was actually removed. */ +bool removeObjectFrom3DScene( + mrpt::graphslam::CWindowManager* win_manager, + std::string obj_name="") { + ASSERT_(win_manager); + return removeObjectFrom3DScene(win_manager->win, obj_name); +} + +template +void addToWindow(mrpt::gui::CDisplayWindow3D* win, + const RENDERIZABLE_OBJECT& o, + const std::string& obj_name="", + const mrpt::poses::CPose3D& obj_pose=EMPTY_POSE) { + using namespace mrpt::opengl; + + COpenGLScenePtr &scene = win->get3DSceneAndLock(); + CSetOfObjectsPtr obj = CSetOfObjects::Create(); + + o.getAs3DObject(obj); + obj->setPose(obj_pose); + + if (!obj_name.empty()) { + obj->setName(obj_name); + } + + scene->insert(obj); + + win->unlockAccess3DScene(); + win->forceRepaint(); +} + +void addAxis(mrpt::gui::CDisplayWindow3D* win) { + using namespace mrpt; + using namespace mrpt::opengl; + ASSERT_(win); + + COpenGLScenePtr &scene = win->get3DSceneAndLock(); + opengl::CAxisPtr obj = opengl::CAxis::Create(-6, -6, -6, 6, 6, 6, 2, 2, true); + obj->setLocation(0, 0, 0); + scene->insert(obj); + win->unlockAccess3DScene(); + win->forceRepaint(); +} + +void addGrid(mrpt::gui::CDisplayWindow3D* win) { + using namespace mrpt; + using namespace mrpt::opengl; + ASSERT_(win); + + COpenGLScenePtr &scene = win->get3DSceneAndLock(); + opengl::CGridPlaneXYPtr obj = opengl::CGridPlaneXY::Create(-20, 20, -20, 20, 0, 1); + obj->setColor(0.7, 0.7, 0.7); + obj->setLocation(0, 0, 0); + scene->insert(obj); + + win->unlockAccess3DScene(); + win->forceRepaint(); +} + +void addSupWidgets(mrpt::gui::CDisplayWindow3D* win) { + ASSERT_(win); + + addAxis(win); + addGrid(win); +} + +//////////////////////////////////////////////////////////// + + +CMapMerger::CMapMerger( + mrpt::utils::COutputLogger* logger_in, + ros::NodeHandle* nh_in): + m_logger(logger_in), + m_nh(nh_in), + m_conn_manager(logger_in, nh_in), + m_queue_size(1), + quit_keypress1("q"), + quit_keypress2("Ctrl+c"), + map_merge_keypress("n"), + save_map_merging_results(true) +{ + ASSERT_(m_nh); + + m_global_ns = "/map_merger"; + + // GridMap Alignment options to be used in merging. + m_alignment_options.methodSelection = CGridMapAligner::amModifiedRANSAC; + ////options.methodSelection = CGridMapAligner::amRobustMatch; // ASSERTION ERROR + m_alignment_options.min_ICP_goodness = 0.60; + m_alignment_options.maxKLd_for_merge = 0.90; + m_alignment_options.ransac_minSetSizeRatio = 0.40; + //m_alignment_options.loadFromConfigFileName( + //"/home/bergercookie/mrpt/share/mrpt/config_files/grid-matching/gridmatch_example.ini", + //"grid-match"); + m_alignment_options.dumpToConsole(); + + // initialize CDisplayWindow for fused map + m_fused_map_win_manager = this->initWindowVisuals(); + m_fused_map_win_manager->win->setWindowTitle("Fused map"); + this->monitorKeystrokes(m_fused_map_win_manager->observer); + +} + +void CMapMerger::monitorKeystrokes(mrpt::graphslam::CWindowObserver* win_observer) { + ASSERT_(win_observer); + + win_observer->registerKeystroke(quit_keypress1, + "Finish execution"); + win_observer->registerKeystroke(map_merge_keypress, + "Compute next available grid-merging"); + + +} + +CMapMerger::~CMapMerger() { + + // delete the generated neighbors intances + for (neighbors_t::iterator n_it = m_neighbors.begin(); + n_it != m_neighbors.end(); + ++n_it) { + + delete m_neighbors_to_windows.at(*n_it)->observer; + delete m_neighbors_to_windows.at(*n_it)->win; + delete m_neighbors_to_windows.at(*n_it); + delete *n_it; + } + + // delete fused window manager + delete m_fused_map_win_manager->win; + delete m_fused_map_win_manager->observer; + delete m_fused_map_win_manager; + + m_logger->logFmt(LVL_WARN, "Exiting..."); +} + +bool CMapMerger::updateState() { + MRPT_START; + + // get the new GraphSlamAgents + const GraphSlamAgents& nearby_slam_agents = + m_conn_manager.getNearbySlamAgents(); + //m_logger->logFmt(LVL_DEBUG, "nearby_slam_agents size: %lu\n", + //static_cast(nearby_slam_agents.list.size())); + + for (GraphSlamAgents::_list_type::const_iterator + it = nearby_slam_agents.list.begin(); + it != nearby_slam_agents.list.end(); + ++it) { + const GraphSlamAgent& gsa = *it; + + // Is the current GraphSlamAgent already registered? + auto search = [gsa](const TNeighborAgentMapProps* neighbor) { + return (neighbor->agent == gsa); + }; + typename neighbors_t::iterator n_it = find_if( + m_neighbors.begin(), + m_neighbors.end(), search); + + if (n_it == m_neighbors.end()) { // current gsa not found, add it + + m_neighbors.push_back(new TNeighborAgentMapProps(m_logger, gsa, m_nh)); + TNeighborAgentMapProps* latest_neighbor = m_neighbors.back(); + latest_neighbor->setupComm(); + m_logger->logFmt(LVL_WARN, + "Initialized NeighborAgentMapProps instance for agent %s...", + latest_neighbor->agent.topic_namespace.data.c_str()); + + // initialize the window + mrpt::graphslam::CWindowManager* win_manager = initWindowVisuals(); + win_manager->win->setWindowTitle( + latest_neighbor->agent.topic_namespace.data); + m_neighbors_to_windows.insert(make_pair(latest_neighbor, win_manager)); + this->monitorKeystrokes(win_manager->observer); + + } + } // end for all fetched GraphSlamAgents + + // run through the open windows - Exit if instructed + bool continue_exec = true; + for (map::const_iterator + it = m_neighbors_to_windows.begin(); + it != m_neighbors_to_windows.end(); + ++it) { + + CWindowManager* win_manager = it->second; + + std::map events_map; + win_manager->observer->returnEventsStruct(&events_map); + + if (events_map.at(map_merge_keypress)) { + mergeMaps(); + } + win_manager->win->forceRepaint(); + + // continue or exit + if (!win_manager->isOpen() || + events_map.at(quit_keypress1) || + events_map.at(quit_keypress2)) { + continue_exec = false; + break; + } + } + + // Fetch the events for the main (fused map) window + if (continue_exec) { + std::map events_map; + m_fused_map_win_manager->observer->returnEventsStruct(&events_map); + if (events_map.at(map_merge_keypress)) { + mergeMaps(); + } + m_fused_map_win_manager->win->forceRepaint(); + continue_exec = + m_fused_map_win_manager->isOpen() && + !events_map.at(quit_keypress1) && + !events_map.at(quit_keypress2); + } + + return continue_exec; + + MRPT_END; +} // end of updateState + +void CMapMerger::mergeMaps() { + MRPT_START; + m_logger->logFmt(LVL_WARN, "In mergeMaps."); + + CGridMapAligner gridmap_aligner; + gridmap_aligner.options = m_alignment_options; + + // List of maps that is to be filled. + maps_t mrpt_gridmaps; + trajectories_t mrpt_trajectories; + + // traverse Neighbor instances - get their nav_msgs::OccupancyGrid maps, trajectories + for (neighbors_t::const_iterator n_it = m_neighbors.begin(); + n_it != m_neighbors.end(); + ++n_it) { + TNeighborAgentMapProps& neighbor = **n_it; + CWindowManager* neighbor_win_manager = m_neighbors_to_windows.at(*n_it); + + // reset visuals for each neighbor's window + removeObjectFrom3DScene(neighbor_win_manager->win); + addSupWidgets(neighbor_win_manager->win); + + if (neighbor.nav_map && neighbor.nav_robot_trajectory) { + // + // map + // + COccupancyGridMap2DPtr map = COccupancyGridMap2D::Create(); + m_logger->logFmt(LVL_INFO, "Adding map of agent \"%s\" to the stack", + neighbor.agent.topic_namespace.data.c_str()); + convert(*neighbor.nav_map, *map); + + // visualize map in corresponding window + addToWindow(neighbor_win_manager->win, *map); + mrpt_gridmaps.insert(make_pair(*n_it, map)); + + // + // trajectory + // + CSetOfLinesPtr curr_traj = CSetOfLines::Create(); + curr_traj->setPose(CPose3D(0,0,0.3,0,0,0)); + curr_traj->setLineWidth(1.5); + curr_traj->setColor(TColorf(0,0,1)); + // append a dummy line so that you can later use append using + // CSetOfLines::appendLienStrip method. + curr_traj->appendLine( + /* 1st */ 0, 0, 0, + /* 2nd */ 0, 0, 0); + + for (nav_msgs::Path::_poses_type::const_iterator + pth_it = neighbor.nav_robot_trajectory->poses.begin(); + pth_it != neighbor.nav_robot_trajectory->poses.end(); + ++pth_it) { + curr_traj->appendLineStrip(pth_it->pose.position.x, pth_it->pose.position.y, 0); + } + // visualize trajectory + { + COpenGLScenePtr &scene = neighbor_win_manager->win->get3DSceneAndLock(); + scene->insert(curr_traj); + + neighbor_win_manager->win->unlockAccess3DScene(); + neighbor_win_manager->win->forceRepaint(); + } + + // cache trajectory so that I can later visualize it in the fused map + { + // operate on copy of object - it is already inserted and used in + // another window + CObjectPtr tmp = curr_traj->duplicateGetSmartPtr(); + CSetOfLinesPtr curr_traj = static_cast(tmp); + ASSERT_(curr_traj); + mrpt_trajectories.insert(make_pair(&neighbor, curr_traj)); + } + + } // end if both nav_map and nav_robot_trajectory exist + } // end for all neighbors traversal + + // join all gathered MRPT gridmaps + if (mrpt_gridmaps.size() >= 2) { + + m_logger->logFmt(LVL_INFO, "Executing map merging for [%lu] gridmaps", + static_cast(mrpt_gridmaps.size())); + int merge_counter = 0; + + // save results + std::string output_dir_fname = "map_merger_results"; + if (save_map_merging_results) { + m_logger->logFmt(LVL_INFO, + "Saving map-merging results to \"%s\"", + output_dir_fname.c_str()); + + mrpt::system::TTimeStamp cur_date(getCurrentTime()); + //string cur_date_str(dateTimeToString(cur_date)); + //string cur_date_validstr(fileNameStripInvalidChars(cur_date_str)); + //std::string output_dir_fname = "map_merger_results_" + cur_date_validstr; + bool does_exist = directoryExists(output_dir_fname); + if (does_exist) { + deleteFilesInDirectory(output_dir_fname); + } + else { + m_logger->logFmt(LVL_INFO, "Initializing gridmaps output directory.\n"); + createDirectory(output_dir_fname); + } + } + + // initialize final fused map + COccupancyGridMap2DPtr fused_map = COccupancyGridMap2D::Create(); + fused_map->copyMapContentFrom(*mrpt_gridmaps.begin()->second); + ASSERT_(fused_map.present()); + + { + // clear the fused map visuals + COpenGLScenePtr& fused_scene = m_fused_map_win_manager->win->get3DSceneAndLock(); + fused_scene->clear(); + m_logger->logFmt(LVL_INFO, "Clearing the fused map visuals"); + + addSupWidgets(m_fused_map_win_manager->win); + + // add first map + CSetOfObjectsPtr first_map_obj = CSetOfObjects::Create(); + fused_map->getAs3DObject(first_map_obj); + // each map in the fused display will have a different name - based on + // the topic namespace + first_map_obj->setName(format("map_%s", + mrpt_gridmaps.begin()->first->agent.topic_namespace.data.c_str())); + fused_scene->insert(first_map_obj); + + m_fused_map_win_manager->win->unlockAccess3DScene(); + m_fused_map_win_manager->win->forceRepaint(); + } + + // save the first gridmap + if (save_map_merging_results) { + stringstream ss; + ss << output_dir_fname << "/" << "map" << merge_counter; + (mrpt_gridmaps.begin()->second)->saveMetricMapRepresentationToFile(ss.str()); + } + + // value at which to display each new gridmap in the display window + double off_z = 1; + double off_z_step = 1; + + // map of TNeighborAgentMapProps* to a corresponding flag specifying + // whether the COccupancyGridMap is correctly aligned can thus can be used. + neighbor_to_is_used_t neighbor_to_is_used; + // first gridmap frame coincedes with the global frame - used anyway + + // map from TNeighborAgentMapProps* to a corresponding relative pose with + // regards to the global fused map + neighbor_to_rel_pose_t neighbor_to_rel_pose; + for (neighbors_t::const_iterator + n_it = m_neighbors.begin(); + n_it != m_neighbors.end(); + ++n_it) { + neighbor_to_is_used[*n_it] = false; + neighbor_to_rel_pose[*n_it] = CPose2D(); + } + + // mark first as used - one trajectory should be there anyway + neighbor_to_is_used[*m_neighbors.begin()] = true; + + // for all captured gridmaps - except the first + for (maps_t::iterator + m_it = std::next(mrpt_gridmaps.begin()); + m_it != mrpt_gridmaps.end(); + ++m_it, ++merge_counter) { + + TNeighborAgentMapProps* curr_neighbor = m_it->first; + COccupancyGridMap2D* curr_gridmap = m_it->second.pointer(); + + // run alignment procedure + CGridMapAligner::TReturnInfo results; + float run_time = 0; + CPosePDFGaussian init_estim; + m_logger->logFmt(LVL_INFO, + "Trying to align the maps, initial estimation: %s", + init_estim.mean.asString().c_str()); + const CPosePDFPtr pdf_tmp = gridmap_aligner.AlignPDF( + fused_map.pointer(), curr_gridmap, + init_estim, + &run_time, &results); + m_logger->logFmt(LVL_INFO, "Elapsed Time: %f", run_time); + + + CPosePDFSOGPtr pdf_out = CPosePDFSOG::Create(); + pdf_out->copyFrom(*pdf_tmp); + + CPose2D pose_out; CMatrixDouble33 cov_out; + pdf_out->getMostLikelyCovarianceAndMean(cov_out, pose_out); + + // dismiss this? + if (results.goodness > 0.999 || isEssentiallyZero(pose_out)) { + continue; + } + + neighbor_to_rel_pose.at(curr_neighbor) = pose_out; + neighbor_to_is_used.at(curr_neighbor) = true; + + m_logger->logFmt(LVL_INFO, "%s\n", + getGridMapAlignmentResultsAsString(*pdf_tmp, results).c_str()); + + // save current gridmap + if (save_map_merging_results) { + stringstream ss; + ss << output_dir_fname << "/" << "map" << merge_counter; + curr_gridmap->saveMetricMapRepresentationToFile(ss.str()); + } + + // save correspondences image + if (save_map_merging_results) { + stringstream ss; + ss << output_dir_fname << "/" << "fusing_proc_with" + << "_" << merge_counter; + COccupancyGridMap2D::saveAsEMFTwoMapsWithCorrespondences( + ss.str(), fused_map.pointer(), curr_gridmap, results.correspondences); + } + + // Add current map to the fused map visualization + { + COpenGLScenePtr fused_scene = m_fused_map_win_manager->win->get3DSceneAndLock(); + + // insert current map + m_logger->logFmt(LVL_INFO, + "Adding map to the fused map visualiztion using transformation %s", + pose_out.asString().c_str()); + CSetOfObjectsPtr curr_map_obj = CSetOfObjects::Create(); + curr_gridmap->getAs3DObject(curr_map_obj); + curr_map_obj->setName(format("map_%s", + curr_neighbor->agent.topic_namespace.data.c_str())); + curr_map_obj->setPose(pose_out + CPose3D(0,0,off_z,0,0,0)); + off_z += off_z_step; + fused_scene->insert(curr_map_obj); + + m_fused_map_win_manager->win->unlockAccess3DScene(); + m_fused_map_win_manager->win->forceRepaint(); + } + + // TODO - merge current gridmap into the fused map by using the found + // transformation + + } // end for all gridmaps + + TColorManager traj_color_mngr; // different color to each trajectory + // Traverse and add all the trajectories to the fused map visualization + for (trajectories_t::const_iterator + it = mrpt_trajectories.begin(); + it != mrpt_trajectories.end(); + ++it) { + + TNeighborAgentMapProps* curr_neighbor = it->first; + + if (!neighbor_to_is_used.at(curr_neighbor)) { + m_logger->logFmt(LVL_WARN, "Skipping visualizing trajectory of agent %s in the fused map", + curr_neighbor->agent.topic_namespace.data.c_str()); + continue; + } + + CSetOfLinesPtr curr_traj = it->second; + m_logger->logFmt(LVL_WARN, "Adding #%lu lines...", + static_cast(curr_traj->getLineCount())); + + // set the pose of the trajectory + CPose3D rel_pose(neighbor_to_rel_pose.at(curr_neighbor)); + // elevate by the last used offset + rel_pose += CPose3D(0,0,off_z,0,0,0); + + curr_traj->setColor(traj_color_mngr.getNextTColorf()); + curr_traj->setPose(rel_pose); + curr_traj->setName(format("traj_%s", curr_neighbor->agent.topic_namespace.data.c_str())); + { // save 3D Scene + COpenGLScenePtr fused_scene = m_fused_map_win_manager->win->get3DSceneAndLock(); + fused_scene->insert(curr_traj); + + m_fused_map_win_manager->win->unlockAccess3DScene(); + m_fused_map_win_manager->win->forceRepaint(); + } + + } + + + { // save the COpenGLScene + + COpenGLScenePtr fused_scene = m_fused_map_win_manager->win->get3DSceneAndLock(); + std::string fname = output_dir_fname + "/" + "output_scene.3DScene"; + fused_scene->saveToFile(fname); + + m_fused_map_win_manager->win->unlockAccess3DScene(); + m_fused_map_win_manager->win->forceRepaint(); + } + + + // save final fused map + // TODO + + + } // end if gridmap.size() >= 2 + MRPT_END; +} + +CWindowManager* CMapMerger::initWindowVisuals() { + mrpt::graphslam::CWindowManager* win_manager = new CWindowManager(); + this->initWindowVisuals(win_manager); + return win_manager; +} + +void CMapMerger::initWindowVisuals( + mrpt::graphslam::CWindowManager* win_manager) { + using namespace mrpt::opengl; + using namespace mrpt::gui; + using namespace mrpt::utils; + using namespace mrpt::graphslam; + ASSERT_(win_manager); + + CWindowObserver* win_observer = new CWindowObserver(); + CDisplayWindow3D* win = new CDisplayWindow3D( + "GraphSlam building procedure", 800, 600); + win->setPos(400, 200); + win_observer->observeBegin(*win); + { + COpenGLScenePtr &scene = win->get3DSceneAndLock(); + COpenGLViewportPtr main_view = scene->getViewport("main"); + win_observer->observeBegin(*main_view); + win->unlockAccess3DScene(); + } + + // pass the window and the observer pointers to the CWindowManager instance + win_manager->setCDisplayWindow3DPtr(win); + win_manager->setWindowObserverPtr(win_observer); + + addSupWidgets(win_manager->win); + m_logger->logFmt(LVL_DEBUG, "Initialized CDisplayWindow3D..."); +} diff --git a/mrpt_graphslam_2d/src/TNeighborAgentMapProps.cpp b/mrpt_graphslam_2d/src/TNeighborAgentMapProps.cpp new file mode 100644 index 0000000..0d024a7 --- /dev/null +++ b/mrpt_graphslam_2d/src/TNeighborAgentMapProps.cpp @@ -0,0 +1,97 @@ +/* +---------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | http://www.mrpt.org/ | + | | + | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | + | See: http://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See details in http://www.mrpt.org/License | + +---------------------------------------------------------------------------+ */ + +#include "mrpt_graphslam_2d/TNeighborAgentMapProps.h" + +using namespace mrpt_msgs; +using namespace mrpt::maps; +using namespace mrpt::graphslam; +using namespace ros; +using namespace mrpt::utils; +using namespace nav_msgs; +using namespace std; + + +TNeighborAgentMapProps::TNeighborAgentMapProps( + mrpt::utils::COutputLogger* logger_in, + const mrpt_msgs::GraphSlamAgent& agent_in, + ros::NodeHandle* nh_in): + m_logger(logger_in), + nh(nh_in), + agent(agent_in), + queue_size(1), + has_init_class(false), + has_setup_comm(false) +{ + ASSERT_(nh); + ASSERT_(m_logger); + m_logger->logFmt(LVL_WARN, "In TNeighborAgentMapProps constructor"); + + this->map_topic = "/" + agent.topic_namespace.data + "/" + + "feedback" + "/" + "gridmap"; + this->robot_trajectory_topic = "/" + agent.topic_namespace.data + "/" + + "feedback" + "/" + "robot_trajectory"; + + cout << "Map topic: " << this->map_topic << endl; + cout << "Trajectory topic: " << this->robot_trajectory_topic << endl; + + has_init_class = true; + readROSParameters(); + +} + +void TNeighborAgentMapProps::readROSParameters() { + //std::map agent_pose; + //bool res = nh->getParam("/" + agent.topic_namespace.data + "/" + "global_pos", agent_pose); + + //if (res) { + //m_logger->logFmt(LVL_INFO, "%s", getMapAsString(agent_pose).c_str()); + + //global_init_pos = pose_t( + //atof(pos_x), + //atof(pos_y), + //atof(rot_z)); + //m_logger->logFmt(LVL_INFO, "global_init_pos: %s\n", global_init_pos); + //} + //else { + //cout << "parameter fetch was not successful" << endl; + //} + +} + +void TNeighborAgentMapProps::setupComm() { + ASSERT_(has_init_class); + m_logger->logFmt(LVL_WARN, "In TNeighborAgentMapProps::setupComm"); + this->setupSubs(); + has_setup_comm = true; +} + +void TNeighborAgentMapProps::setupSubs() { + m_logger->logFmt(LVL_WARN, "In TNeighborAgentMapProps::setupSubs"); + this->map_sub = nh->subscribe( + this->map_topic, + this->queue_size, + &TNeighborAgentMapProps::updateGridMap, this); + + this->robot_trajectory_sub = nh->subscribe( + this->robot_trajectory_topic, + this->queue_size, + &TNeighborAgentMapProps::updateRobotTrajectory, this); +} // end of setupSubs + +void TNeighborAgentMapProps::updateGridMap( + const nav_msgs::OccupancyGrid::ConstPtr& nav_gridmap) { + nav_map = nav_gridmap; +} // end of updateGridMap + +void TNeighborAgentMapProps::updateRobotTrajectory( + const nav_msgs::Path::ConstPtr& nav_robot_traj) { + nav_robot_trajectory = nav_robot_traj; +} + diff --git a/mrpt_graphslam_2d/src/common.cpp b/mrpt_graphslam_2d/src/common.cpp new file mode 100644 index 0000000..817cfa6 --- /dev/null +++ b/mrpt_graphslam_2d/src/common.cpp @@ -0,0 +1,50 @@ +/* +---------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | http://www.mrpt.org/ | + | | + | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | + | See: http://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See details in http://www.mrpt.org/License | + +---------------------------------------------------------------------------+ */ + +#include +#include "mrpt_graphslam_2d/misc/common.h" + +using namespace mrpt::poses; +using namespace mrpt::math; +using namespace std; +using namespace mrpt::math; +using namespace mrpt::utils; + +std::string mrpt::graphslam::detail::getGridMapAlignmentResultsAsString( + const mrpt::poses::CPosePDF& pdf, + const mrpt::slam::CGridMapAligner::TReturnInfo& ret_info) { + + CPosePDFSOGPtr pdf_out = CPosePDFSOG::Create(); + pdf_out->copyFrom(pdf); + CPose2D pose_out; CMatrixDouble33 cov_out; + pdf_out->getMostLikelyCovarianceAndMean(cov_out, pose_out); + + stringstream ss; + ss << "--------------------" << endl; + ss << "Results: " << endl + << "\tPDFPtr pose: " << pdf.getMeanVal() << endl + << "\t# Correspondences: " << ret_info.correspondences.size() << endl + << "\tAlignment goodness: " << ret_info.goodness << endl + << "\tModes size: " << pdf_out->size() << endl + << "\tModes: " << endl << getSTLContainerAsString(pdf_out->getSOGModes()) + << "\tMost likely pose: " << pose_out << endl + << "\tCorresponding covariance matrix: " << endl + << cov_out << endl; + ss << "--------------------" << endl; + return ss.str(); +} + +bool mrpt::graphslam::detail::isEssentiallyZero( + const mrpt::poses::CPose2D& p) { + double epsilon = 0.001; + return ( + approximatelyEqual(p.x(), 0.0, epsilon) && // all 0s + approximatelyEqual(p.y(), 0.0, epsilon) && + approximatelyEqual(p.phi(), 0.0, epsilon)); +} diff --git a/mrpt_graphslam_2d/src/map_merger_node.cpp b/mrpt_graphslam_2d/src/map_merger_node.cpp new file mode 100644 index 0000000..2943321 --- /dev/null +++ b/mrpt_graphslam_2d/src/map_merger_node.cpp @@ -0,0 +1,61 @@ +/* +---------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | http://www.mrpt.org/ | + | | + | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | + | See: http://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See details in http://www.mrpt.org/License | + +---------------------------------------------------------------------------+ */ + +#include +#include +#include +#include +#include +#include +#include "mrpt_graphslam_2d/CMapMerger.h" +#include "mrpt_graphslam_2d/TNeighborAgentMapProps.h" + +using namespace mrpt::graphslam; +using namespace mrpt::graphslam::detail; +using namespace mrpt_msgs; +using namespace mrpt::maps; +using namespace ros; +using namespace mrpt::utils; +using namespace nav_msgs; +using namespace std; + + + +/**\brief Node that fetches the local maps produced by the graphSLAM agents and + * joins them together using a RANSAC-based map-merging technique + * + * Node is to be used for inspecting the overall graphSLAM procedure and + * present the user with a final version of all the independent maps after + * merging. + */ +int main(int argc, char **argv) +{ + // init ROS Node + std::string node_name = "map_merger"; + ros::init(argc, argv, node_name); + ros::NodeHandle nh; + ros::Rate loop_rate(10); + + // initialize logger. + COutputLogger logger; + logger.setLoggerName(node_name); + logger.setMinLoggingLevel(LVL_DEBUG); + logger.logFmt(LVL_WARN, "Initialized %s node...\n", node_name.c_str()); + + CMapMerger map_merger(&logger, &nh); + + bool continue_exec = true; + while (ros::ok() && continue_exec) { + continue_exec = map_merger.updateState(); + ros::spinOnce(); + loop_rate.sleep(); + } + + return 0; +} diff --git a/mrpt_graphslam_2d/src/mrpt_graphslam_2d_mr_node.cpp b/mrpt_graphslam_2d/src/mrpt_graphslam_2d_mr_node.cpp new file mode 100644 index 0000000..49e9bad --- /dev/null +++ b/mrpt_graphslam_2d/src/mrpt_graphslam_2d_mr_node.cpp @@ -0,0 +1,90 @@ +/* +---------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | http://www.mrpt.org/ | + | | + | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | + | See: http://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See details in http://www.mrpt.org/License | + +---------------------------------------------------------------------------+ */ + +// MRPT headers +#include +#include +#include + +#include +#include + +// ROS headers +#include "mrpt_graphslam_2d/CGraphSlamHandler_ROS.h" + +using namespace mrpt; +using namespace mrpt::utils; +using namespace mrpt::poses; +using namespace mrpt::obs; +using namespace mrpt::system; +using namespace mrpt::graphs; +using namespace mrpt::math; +using namespace mrpt::opengl; +using namespace mrpt::utils; +using namespace mrpt::graphslam; +using namespace mrpt::graphslam::deciders; +using namespace mrpt::graphslam::optimizers; +using namespace mrpt::graphslam::apps; + +using namespace std; + +/** Main function of the mrpt_graphslam condensed-measurements _application */ +int main(int argc, char **argv) +{ + COutputLogger logger; + + try { + std::string node_name = "mrpt_graphslam_2d_mr"; + + ros::init(argc, argv, node_name); + ros::NodeHandle nh; + + node_name = node_name + nh.getNamespace(); + logger.setLoggerName(node_name); + logger.logFmt(LVL_WARN, "Initialized %s node...\n", node_name.c_str()); + + ros::Rate loop_rate(10); + + // Initialization + TUserOptionsChecker_ROS options_checker; + CGraphSlamHandler_ROS graphslam_handler( + &logger, &options_checker, &nh); + graphslam_handler.readParams(); + graphslam_handler.initEngine_MR(); + graphslam_handler.setupComm(); + + std::string ns = nh.getNamespace(); + // overwite default results directory due to the multi-robot nature + graphslam_handler.setResultsDirName(std::string(ns.begin()+2, ns.end())); + + // print the parameters just for verification + graphslam_handler.printParams(); + + bool cont_exec = true; + while (ros::ok() && cont_exec) { + cont_exec = graphslam_handler.usePublishersBroadcasters(); + + ros::spinOnce(); + loop_rate.sleep(); + + } + } + catch (exception& e) { + cout << "Known error!" << endl; + logger.logFmt(LVL_ERROR, "Caught exception: %s", e.what()); + mrpt::system::pause(); + return -1; + } + catch (...) { + cout << "Unknown error!" << endl; + logger.logFmt(LVL_ERROR, "Finished with unknown exception. Exiting\n."); + mrpt::system::pause(); + return -1; + } +} diff --git a/mrpt_graphslam_2d/src/mrpt_graphslam_2d_node.cpp b/mrpt_graphslam_2d/src/mrpt_graphslam_2d_node.cpp index 3069460..24a0485 100644 --- a/mrpt_graphslam_2d/src/mrpt_graphslam_2d_node.cpp +++ b/mrpt_graphslam_2d/src/mrpt_graphslam_2d_node.cpp @@ -16,7 +16,7 @@ #include // ROS headers -#include "mrpt_graphslam_2d/CGraphSlamResources.h" +#include "mrpt_graphslam_2d/CGraphSlamHandler_ROS.h" using namespace mrpt; using namespace mrpt::utils; @@ -30,18 +30,18 @@ using namespace mrpt::utils; using namespace mrpt::graphslam; using namespace mrpt::graphslam::deciders; using namespace mrpt::graphslam::optimizers; -using namespace mrpt::graphslam::supplementary; +using namespace mrpt::graphslam::apps; using namespace std; -/** Main function of the mrpt_graphslam_application */ +/** Main function of the mrpt_graphslam application */ int main(int argc, char **argv) { std::string node_name = "mrpt_graphslam_2d"; COutputLogger logger; logger.setLoggerName(node_name); - logger.logFmt(LVL_WARN, "Initializing %s node...\n", "mrpt_graphslam_2d"); + logger.logFmt(LVL_WARN, "Initializing %s node...\n", node_name.c_str()); ros::init(argc, argv, node_name); ros::NodeHandle nh; @@ -50,27 +50,25 @@ int main(int argc, char **argv) try { - // CGraphSlamResources initialization - CGraphSlamResources resources(&logger, &nh); - resources.readParams(); - resources.setupCommunication(); + + // Initialization + TUserOptionsChecker_ROS options_checker; + CGraphSlamHandler_ROS graphslam_handler( + &logger, &options_checker, &nh); + graphslam_handler.readParams(); + graphslam_handler.initEngine_ROS(); + graphslam_handler.setupComm(); + // print the parameters just for verification - resources.printParams(); + graphslam_handler.printParams(); bool cont_exec = true; while (ros::ok() && cont_exec) { - cont_exec = resources.usePublishersBroadcasters(); + cont_exec = graphslam_handler.usePublishersBroadcasters(); ros::spinOnce(); loop_rate.sleep(); } - - // - // Postprocessing - // - - resources.generateReport(); - } catch (exception& e) { ROS_ERROR_STREAM( @@ -88,7 +86,4 @@ int main(int argc, char **argv) mrpt::system::pause(); return -1; } - - } - diff --git a/mrpt_rbpf_slam/include/mrpt_rbpf_slam/mrpt_rbpf_slam.h b/mrpt_rbpf_slam/include/mrpt_rbpf_slam/mrpt_rbpf_slam.h index 486c358..65e0a3d 100644 --- a/mrpt_rbpf_slam/include/mrpt_rbpf_slam/mrpt_rbpf_slam.h +++ b/mrpt_rbpf_slam/include/mrpt_rbpf_slam/mrpt_rbpf_slam.h @@ -5,7 +5,7 @@ * */ -#ifndef MPRT_RBPF_SLAM_H +#ifndef MRPT_RBPF_SLAM_H #define MRPT_RBPF_SLAM_H #include