ros2 · dhood · Jun 16, 2017 · Mar 9, 2017 · Mar 9, 2017 · Mar 10, 2017
diff --git a/amcl/AMENT_IGNORE b/amcl/AMENT_IGNORE
diff --git a/amcl/CMakeLists.txt b/amcl/CMakeLists.txt
@@ -1,141 +1,142 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
 project(amcl)
 
-find_package(catkin REQUIRED
-        COMPONENTS
-            rosbag
-            roscpp
-            tf
-            dynamic_reconfigure
-            nav_msgs
-        )
-
-find_package(Boost REQUIRED)
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra)
+endif()
 
-# dynamic reconfigure
-generate_dynamic_reconfigure_options(
-    cfg/AMCL.cfg
+find_package(ament_cmake_auto REQUIRED)
+find_package(ament_cmake_ros REQUIRED)
+set(required_dependencies
+  "geometry_msgs"
+  "nav_msgs"
+  "rclcpp"
+  "rcutils"
+  "sensor_msgs"
+  "std_srvs"
+  "tf2"
+  "tf2_geometry_msgs"
+  "tf2_msgs"
+  "tf2_ros"
 )
+ament_auto_find_build_dependencies(REQUIRED ${required_dependencies})
 
-catkin_package(
-    CATKIN_DEPENDS
-        rosbag
-        roscpp
-        dynamic_reconfigure
-        tf
-  INCLUDE_DIRS include
-  LIBRARIES amcl_sensors amcl_map amcl_pf
-)
+# dynamic reconfigure
+# generate_dynamic_reconfigure_options(
+#     cfg/AMCL.cfg
+# )
 
 include_directories(include/amcl include/amcl/map include/amcl/sensors include/amcl/pf)
-include_directories(${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
 
-add_library(amcl_pf
+ament_auto_add_library(amcl_pf
                     src/amcl/pf/pf.c
                     src/amcl/pf/pf_kdtree.c
                     src/amcl/pf/pf_pdf.c
                     src/amcl/pf/pf_vector.c
                     src/amcl/pf/eig3.c
                     src/amcl/pf/pf_draw.c)
 
-add_library(amcl_map
+ament_auto_add_library(amcl_map
                     src/amcl/map/map.c
                     src/amcl/map/map_cspace.cpp
                     src/amcl/map/map_range.c
                     src/amcl/map/map_store.c
                     src/amcl/map/map_draw.c)
 
-add_library(amcl_sensors
+ament_target_dependencies(amcl_map)
+ament_auto_add_library(amcl_sensors
                     src/amcl/sensors/amcl_sensor.cpp
                     src/amcl/sensors/amcl_odom.cpp
                     src/amcl/sensors/amcl_laser.cpp)
-target_link_libraries(amcl_sensors amcl_map amcl_pf)
 
 
-add_executable(amcl
+ament_auto_add_executable(amcl
                        src/amcl_node.cpp)
-add_dependencies(amcl amcl_gencfg)
+# add_dependencies(amcl amcl_gencfg)
+
+ament_target_dependencies(amcl
+  "geometry_msgs"
+  "nav_msgs"
+  "rclcpp"
+  "sensor_msgs"
+  "std_srvs"
+  "tf2"
+  "tf2_geometry_msgs"
+  "tf2_msgs"
+  "tf2_ros")
 
 target_link_libraries(amcl
     amcl_sensors amcl_map amcl_pf
-    ${Boost_LIBRARIES}
-    ${catkin_LIBRARIES}
-)
-
-install( TARGETS
-    amcl amcl_sensors amcl_map amcl_pf
-    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-install(DIRECTORY include/amcl/
-  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
 )
 
 install(DIRECTORY examples/
-    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/examples
+    DESTINATION share/${PROJECT_NAME}/examples
 )
 
 ## Configure Tests
-if(CATKIN_ENABLE_TESTING)
-  find_package(rostest REQUIRED)
-
-  # Bags
-  catkin_download_test_data(${PROJECT_NAME}_basic_localization_stage_indexed.bag
-    http://download.ros.org/data/amcl/basic_localization_stage_indexed.bag
-    DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
-    MD5 41fe43af189ec71e5e48eb9ed661a655)
-  catkin_download_test_data(${PROJECT_NAME}_global_localization_stage_indexed.bag
-    http://download.ros.org/data/amcl/global_localization_stage_indexed.bag
-    DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
-    MD5 752f711cf4f6e8d1d660675e2da096b0)
-  catkin_download_test_data(${PROJECT_NAME}_small_loop_prf_indexed.bag
-    http://download.ros.org/data/amcl/small_loop_prf_indexed.bag
-    DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
-    MD5 e4ef0fc006872b43f12ed8a7ce7dcd81)
-  catkin_download_test_data(${PROJECT_NAME}_small_loop_crazy_driving_prg_indexed.bag
-    http://download.ros.org/data/amcl/small_loop_crazy_driving_prg_indexed.bag
-    DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
-    MD5 4a58d1a7962914009d99000d06e5939c)
-  catkin_download_test_data(${PROJECT_NAME}_texas_greenroom_loop_indexed.bag
-    http://download.ros.org/data/amcl/texas_greenroom_loop_indexed.bag
-    DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
-    MD5 6e3432115cccdca1247f6c807038e13d)
-  catkin_download_test_data(${PROJECT_NAME}_texas_willow_hallway_loop_indexed.bag
-    http://download.ros.org/data/amcl/texas_willow_hallway_loop_indexed.bag
-    DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
-    MD5 27deb742fdcd3af44cf446f39f2688a8)
-  catkin_download_test_data(${PROJECT_NAME}_rosie_localization_stage.bag
-    http://download.ros.org/data/amcl/rosie_localization_stage.bag
-    DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
-    MD5 3347bf3835724cfa45e958c5c1846066)
-
-  # Maps
-  catkin_download_test_data(${PROJECT_NAME}_willow-full.pgm
-    http://download.ros.org/data/amcl/willow-full.pgm
-    DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
-    MD5 b84465cdbbfe3e2fb9eb4579e0bcaf0e)
-  catkin_download_test_data(${PROJECT_NAME}_willow-full-0.05.pgm
-    http://download.ros.org/data/amcl/willow-full-0.05.pgm
-    DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
-    MD5 b61694296e08965096c5e78611fd9765)
-
-  # Tests
-  add_rostest(test/set_initial_pose.xml)
-  add_rostest(test/set_initial_pose_delayed.xml)
-  add_rostest(test/basic_localization_stage.xml)
-  add_rostest(test/small_loop_prf.xml)
-  add_rostest(test/small_loop_crazy_driving_prg.xml)
-  add_rostest(test/texas_greenroom_loop.xml)
-  add_rostest(test/rosie_multilaser.xml)
-  add_rostest(test/texas_willow_hallway_loop.xml)
-
-# Not sure when or if this actually passed.
+#if(CATKIN_ENABLE_TESTING)
+#  find_package(rostest REQUIRED)
 #
-# The point of this is that you start with an even probability
-# distribution over the whole map and the robot localizes itself after
-# some number of iterations of sensing and motion.
+#  # Bags
+#  catkin_download_test_data(${PROJECT_NAME}_basic_localization_stage_indexed.bag
+#    http://download.ros.org/data/amcl/basic_localization_stage_indexed.bag
+#    DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
+#    MD5 41fe43af189ec71e5e48eb9ed661a655)
+#  catkin_download_test_data(${PROJECT_NAME}_global_localization_stage_indexed.bag
+#    http://download.ros.org/data/amcl/global_localization_stage_indexed.bag
+#    DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
+#    MD5 752f711cf4f6e8d1d660675e2da096b0)
+#  catkin_download_test_data(${PROJECT_NAME}_small_loop_prf_indexed.bag
+#    http://download.ros.org/data/amcl/small_loop_prf_indexed.bag
+#    DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
+#    MD5 e4ef0fc006872b43f12ed8a7ce7dcd81)
+#  catkin_download_test_data(${PROJECT_NAME}_small_loop_crazy_driving_prg_indexed.bag
+#    http://download.ros.org/data/amcl/small_loop_crazy_driving_prg_indexed.bag
+#    DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
+#    MD5 4a58d1a7962914009d99000d06e5939c)
+#  catkin_download_test_data(${PROJECT_NAME}_texas_greenroom_loop_indexed.bag
+#    http://download.ros.org/data/amcl/texas_greenroom_loop_indexed.bag
+#    DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
+#    MD5 6e3432115cccdca1247f6c807038e13d)
+#  catkin_download_test_data(${PROJECT_NAME}_texas_willow_hallway_loop_indexed.bag
+#    http://download.ros.org/data/amcl/texas_willow_hallway_loop_indexed.bag
+#    DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
+#    MD5 27deb742fdcd3af44cf446f39f2688a8)
+#  catkin_download_test_data(${PROJECT_NAME}_rosie_localization_stage.bag
+#    http://download.ros.org/data/amcl/rosie_localization_stage.bag
+#    DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
+#    MD5 3347bf3835724cfa45e958c5c1846066)
 #
-#  add_rostest(test/global_localization_stage.xml)
-endif()
+#  # Maps
+#  catkin_download_test_data(${PROJECT_NAME}_willow-full.pgm
+#    http://download.ros.org/data/amcl/willow-full.pgm
+#    DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
+#    MD5 b84465cdbbfe3e2fb9eb4579e0bcaf0e)
+#  catkin_download_test_data(${PROJECT_NAME}_willow-full-0.05.pgm
+#    http://download.ros.org/data/amcl/willow-full-0.05.pgm
+#    DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
+#    MD5 b61694296e08965096c5e78611fd9765)
+#
+#  # Tests
+#  add_rostest(test/set_initial_pose.xml)
+#  add_rostest(test/set_initial_pose_delayed.xml)
+#  add_rostest(test/basic_localization_stage.xml)
+#  add_rostest(test/small_loop_prf.xml)
+#  add_rostest(test/small_loop_crazy_driving_prg.xml)
+#  add_rostest(test/texas_greenroom_loop.xml)
+#  add_rostest(test/rosie_multilaser.xml)
+#  add_rostest(test/texas_willow_hallway_loop.xml)
+#
+## Not sure when or if this actually passed.
+##
+## The point of this is that you start with an even probability
+## distribution over the whole map and the robot localizes itself after
+## some number of iterations of sensing and motion.
+##
+##  add_rostest(test/global_localization_stage.xml)
+#endif()
+ament_auto_package()
diff --git a/amcl/package.xml b/amcl/package.xml
@@ -1,4 +1,4 @@
-<package>
+<package format="2">
     <name>amcl</name>
     <version>1.14.0</version>
     <description>
@@ -20,22 +20,46 @@
     <maintainer email="mferguson@fetchrobotics.com">Michael Ferguson</maintainer>
     <license>LGPL</license>
 
-    <buildtool_depend>catkin</buildtool_depend>
+    <buildtool_depend>ament_cmake_auto</buildtool_depend>
+    <buildtool_depend>ament_cmake_ros</buildtool_depend>
 
+    <!--
     <build_depend>rosbag</build_depend>
     <build_depend>dynamic_reconfigure</build_depend>
     <build_depend>message_filters</build_depend>
+    <build_depend>rostest</build_depend>
+    -->
+    <build_depend>geometry_msgs</build_depend>
     <build_depend>nav_msgs</build_depend>
-    <build_depend>roscpp</build_depend>
+    <build_depend>rclcpp</build_depend>
+    <build_depend>rcutils</build_depend>
     <build_depend>rostest</build_depend>
+    <build_depend>sensor_msgs</build_depend>
     <build_depend>std_srvs</build_depend>
-    <build_depend>tf</build_depend>
+    <build_depend>tf2</build_depend>
+    <build_depend>tf2_geometry_msgs</build_depend>
+    <build_depend>tf2_msgs</build_depend>
+    <build_depend>tf2_ros</build_depend>
 
-    <run_depend>rosbag</run_depend>
-    <run_depend>roscpp</run_depend>
-    <run_depend>dynamic_reconfigure</run_depend>
-    <run_depend>tf</run_depend>
-    <run_depend>nav_msgs</run_depend>
+    <!--
+    <exec_depend>rosbag</exec_depend>
+    <exec_depend>dynamic_reconfigure</exec_depend>
+    -->
+    <exec_depend>geometry_msgs</exec_depend>
+    <exec_depend>nav_msgs</exec_depend>
+    <exec_depend>nav_msgs</exec_depend>
+    <exec_depend>rclcpp</exec_depend>
+    <exec_depend>rcutils</exec_depend>
+    <exec_depend>sensor_msgs</exec_depend>
+    <exec_depend>std_srvs</exec_depend>
+    <exec_depend>tf2</exec_depend>
+    <exec_depend>tf2_geometry_msgs</exec_depend>
+    <exec_depend>tf2_msgs</exec_depend>
+    <exec_depend>tf2_ros</exec_depend>
 
     <test_depend>map_server</test_depend>
+
+    <export>
+        <build_type>ament_cmake</build_type>
+    </export>
 </package>
diff --git a/amcl/src/amcl/map/map.c b/amcl/src/amcl/map/map.c
@@ -69,6 +69,7 @@ void map_free(map_t *map)
 // Get the cell at the given point
 map_cell_t *map_get_cell(map_t *map, double ox, double oy, double oa)
 {
+  (void)oa;
   int i, j;
   map_cell_t *cell;
 

diff --git a/amcl/src/amcl/pf/pf.c b/amcl/src/amcl/pf/pf.c
@@ -218,7 +218,6 @@ int pf_update_converged(pf_t *pf)
   int i;
   pf_sample_set_t *set;
   pf_sample_t *sample;
-  double total;
 
   set = pf->sets + pf->current_set;
   double mean_x = 0, mean_y = 0;
@@ -472,8 +471,9 @@ int pf_resample_limit(pf_t *pf, int k)
 
 
 // Re-compute the cluster statistics for a sample set
-void pf_cluster_stats(pf_t *pf, pf_sample_set_t *set)
+void pf_cluster_stats(pf_t * pf, pf_sample_set_t *set)
 {
+  (void)pf;
   int i, j, k, cidx;
   pf_sample_t *sample;
   pf_cluster_t *cluster;

diff --git a/amcl/src/amcl/pf/pf_kdtree.c b/amcl/src/amcl/pf/pf_kdtree.c
@@ -185,6 +185,7 @@ int pf_kdtree_get_cluster(pf_kdtree_t *self, pf_vector_t pose)
 // Compare keys to see if they are equal
 int pf_kdtree_equal(pf_kdtree_t *self, int key_a[], int key_b[])
 {
+  (void)self;
   //double a, b;
 
   if (key_a[0] != key_b[0])

diff --git a/amcl/src/amcl/sensors/amcl_sensor.cpp b/amcl/src/amcl/sensors/amcl_sensor.cpp
@@ -47,6 +47,8 @@ AMCLSensor::~AMCLSensor()
 // Apply the action model
 bool AMCLSensor::UpdateAction(pf_t *pf, AMCLSensorData *data)
 {
+  (void)pf;
+  (void)data;
   return false;
 }
 
@@ -55,6 +57,8 @@ bool AMCLSensor::UpdateAction(pf_t *pf, AMCLSensorData *data)
 // Initialize the filter
 bool AMCLSensor::InitSensor(pf_t *pf, AMCLSensorData *data)
 {
+  (void)pf;
+  (void)data;
   return false;
 }
 
@@ -63,6 +67,8 @@ bool AMCLSensor::InitSensor(pf_t *pf, AMCLSensorData *data)
 // Apply the sensor model
 bool AMCLSensor::UpdateSensor(pf_t *pf, AMCLSensorData *data)
 {
+  (void)pf;
+  (void)data;
   return false;
 }