Merge pull request #22 from bergercookie/graphslam-devel

Add support for 2D multi-robot SLAM
mrpt-ros-pkg · May 26, 2017 · 2feed28 · 2feed28
2 parents 0df706e + d0c8273
commit 2feed28
Show file tree

Hide file tree

Showing 59 changed files with 9,491 additions and 1,346 deletions.
diff --git a/mrpt_graphslam_2d/.gitignore 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
@@ -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
@@ -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.