diff --git a/nav2_amcl/CMakeLists.txt b/nav2_amcl/CMakeLists.txt index 3a5836ab2db..3cb30254fa3 100644 --- a/nav2_amcl/CMakeLists.txt +++ b/nav2_amcl/CMakeLists.txt @@ -2,90 +2,136 @@ cmake_minimum_required(VERSION 3.5) project(nav2_amcl) find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(message_filters REQUIRED) +find_package(nav_msgs REQUIRED) find_package(nav2_common REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(nav2_util REQUIRED) +find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) find_package(rclcpp_components REQUIRED) -find_package(message_filters REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(nav_msgs REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_srvs REQUIRED) -find_package(tf2_ros REQUIRED) find_package(tf2 REQUIRED) -find_package(nav2_util REQUIRED) -find_package(nav2_msgs REQUIRED) -find_package(pluginlib REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) nav2_package() -include_directories( - include -) - include(CheckSymbolExists) check_symbol_exists(drand48 stdlib.h HAVE_DRAND48) -add_subdirectory(src/pf) -add_subdirectory(src/map) -add_subdirectory(src/motion_model) -add_subdirectory(src/sensors) +if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wno-gnu-folding-constant) +endif() -set(executable_name amcl) +add_library(pf_lib SHARED + src/pf/pf.c + src/pf/pf_kdtree.c + src/pf/pf_pdf.c + src/pf/pf_vector.c + src/pf/eig3.c + src/pf/pf_draw.c +) +target_include_directories(pf_lib + PUBLIC + "$" + "$") +if(HAVE_DRAND48) + target_compile_definitions(pf_lib PRIVATE "HAVE_DRAND48") +endif() -add_executable(${executable_name} - src/main.cpp +add_library(map_lib SHARED + src/map/map.c + src/map/map_range.c + src/map/map_draw.c + src/map/map_cspace.cpp +) +target_include_directories(map_lib + PUBLIC + "$" + "$") + +add_library(motions_lib SHARED + src/motion_model/omni_motion_model.cpp + src/motion_model/differential_motion_model.cpp +) +target_include_directories(motions_lib + PUBLIC + "$" + "$") +target_link_libraries(motions_lib PUBLIC + pf_lib + pluginlib::pluginlib + nav2_util::nav2_util_core +) + +add_library(sensors_lib SHARED + src/sensors/laser/laser.cpp + src/sensors/laser/beam_model.cpp + src/sensors/laser/likelihood_field_model.cpp + src/sensors/laser/likelihood_field_model_prob.cpp +) +target_include_directories(sensors_lib + PUBLIC + "$" + "$") +target_link_libraries(sensors_lib PUBLIC + pf_lib + map_lib ) +set(executable_name amcl) + set(library_name ${executable_name}_core) add_library(${library_name} SHARED src/amcl_node.cpp ) - -target_include_directories(${library_name} PRIVATE src/include) - if(HAVE_DRAND48) target_compile_definitions(${library_name} PRIVATE "HAVE_DRAND48") endif() - -set(dependencies - rclcpp - rclcpp_lifecycle - rclcpp_components - message_filters - tf2_geometry_msgs - geometry_msgs - nav_msgs - sensor_msgs - std_srvs - tf2_ros - tf2 - nav2_util - nav2_msgs - pluginlib +target_include_directories(${library_name} + PUBLIC + "$" + "$") +target_link_libraries(${library_name} PUBLIC + ${geometry_msgs_TARGETS} + message_filters::message_filters + nav2_util::nav2_util_core + ${sensor_msgs_TARGETS} + ${std_srvs_TARGETS} + pluginlib::pluginlib + sensors_lib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${nav_msgs_TARGETS} + tf2_ros::tf2_ros + tf2::tf2 + ${nav2_msgs_TARGETS} ) - -ament_target_dependencies(${executable_name} - ${dependencies} +target_link_libraries(${library_name} PRIVATE + rclcpp_components::component + tf2_geometry_msgs::tf2_geometry_msgs ) -target_link_libraries(${executable_name} - ${library_name} -) - -ament_target_dependencies(${library_name} - ${dependencies} +add_executable(${executable_name} + src/main.cpp ) - -target_link_libraries(${library_name} - map_lib pf_lib sensors_lib +target_include_directories(${executable_name} + PUBLIC + "$" + "$") +target_link_libraries(${executable_name} PRIVATE + ${library_name} ) rclcpp_components_register_nodes(${library_name} "nav2_amcl::AmclNode") -install(TARGETS ${library_name} +install(TARGETS ${library_name} pf_lib map_lib motions_lib sensors_lib + EXPORT ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -96,7 +142,7 @@ install(TARGETS ${executable_name} ) install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) if(BUILD_TESTING) @@ -104,11 +150,26 @@ if(BUILD_TESTING) # the following line skips the linter which checks for copyrights set(ament_cmake_copyright_FOUND TRUE) set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() endif() -ament_export_include_directories(include) +ament_export_include_directories("include/${PROJECT_NAME}") ament_export_libraries(${library_name} pf_lib sensors_lib motions_lib map_lib) -ament_export_dependencies(${dependencies}) +ament_export_dependencies( + geometry_msgs + message_filters + nav_msgs + nav2_msgs + nav2_util + pluginlib + rclcpp + rclcpp_lifecycle + sensor_msgs + std_srvs + tf2 + tf2_ros +) +ament_export_targets(${library_name}) pluginlib_export_plugin_description_file(nav2_amcl plugins.xml) ament_package() diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 514bba4c557..d8f35b5c7f4 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -37,17 +37,13 @@ #include "nav2_msgs/msg/particle_cloud.hpp" #include "nav2_msgs/srv/set_initial_pose.hpp" #include "nav_msgs/srv/set_map.hpp" +#include "pluginlib/class_loader.hpp" +#include "rclcpp/node_options.hpp" #include "sensor_msgs/msg/laser_scan.hpp" #include "std_srvs/srv/empty.hpp" +#include "tf2_ros/message_filter.h" #include "tf2_ros/transform_broadcaster.h" #include "tf2_ros/transform_listener.h" -#include "pluginlib/class_loader.hpp" - -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-parameter" -#pragma GCC diagnostic ignored "-Wreorder" -#include "tf2_ros/message_filter.h" -#pragma GCC diagnostic pop #define NEW_UNIFORM_SAMPLING 1 diff --git a/nav2_amcl/include/nav2_amcl/angleutils.hpp b/nav2_amcl/include/nav2_amcl/angleutils.hpp index 84de30f78db..a2f489952ae 100644 --- a/nav2_amcl/include/nav2_amcl/angleutils.hpp +++ b/nav2_amcl/include/nav2_amcl/angleutils.hpp @@ -22,7 +22,7 @@ #ifndef NAV2_AMCL__ANGLEUTILS_HPP_ #define NAV2_AMCL__ANGLEUTILS_HPP_ -#include +#include namespace nav2_amcl { diff --git a/nav2_amcl/include/nav2_amcl/motion_model/differential_motion_model.hpp b/nav2_amcl/include/nav2_amcl/motion_model/differential_motion_model.hpp index 2d173602042..5780c84b207 100644 --- a/nav2_amcl/include/nav2_amcl/motion_model/differential_motion_model.hpp +++ b/nav2_amcl/include/nav2_amcl/motion_model/differential_motion_model.hpp @@ -22,11 +22,9 @@ #ifndef NAV2_AMCL__MOTION_MODEL__DIFFERENTIAL_MOTION_MODEL_HPP_ #define NAV2_AMCL__MOTION_MODEL__DIFFERENTIAL_MOTION_MODEL_HPP_ -#include -#include -#include #include "nav2_amcl/motion_model/motion_model.hpp" -#include "nav2_amcl/angleutils.hpp" +#include "nav2_amcl/pf/pf.hpp" +#include "nav2_amcl/pf/pf_vector.hpp" namespace nav2_amcl diff --git a/nav2_amcl/include/nav2_amcl/motion_model/motion_model.hpp b/nav2_amcl/include/nav2_amcl/motion_model/motion_model.hpp index 4cd1c6a8798..55a8ac2fcd5 100644 --- a/nav2_amcl/include/nav2_amcl/motion_model/motion_model.hpp +++ b/nav2_amcl/include/nav2_amcl/motion_model/motion_model.hpp @@ -17,11 +17,8 @@ #ifndef NAV2_AMCL__MOTION_MODEL__MOTION_MODEL_HPP_ #define NAV2_AMCL__MOTION_MODEL__MOTION_MODEL_HPP_ -#include -#include - #include "nav2_amcl/pf/pf.hpp" -#include "nav2_amcl/pf/pf_pdf.hpp" +#include "nav2_amcl/pf/pf_vector.hpp" namespace nav2_amcl { diff --git a/nav2_amcl/include/nav2_amcl/motion_model/omni_motion_model.hpp b/nav2_amcl/include/nav2_amcl/motion_model/omni_motion_model.hpp index ae586d4d6c5..e57259a3e19 100644 --- a/nav2_amcl/include/nav2_amcl/motion_model/omni_motion_model.hpp +++ b/nav2_amcl/include/nav2_amcl/motion_model/omni_motion_model.hpp @@ -22,12 +22,9 @@ #ifndef NAV2_AMCL__MOTION_MODEL__OMNI_MOTION_MODEL_HPP_ #define NAV2_AMCL__MOTION_MODEL__OMNI_MOTION_MODEL_HPP_ -#include -#include -#include #include "nav2_amcl/motion_model/motion_model.hpp" -#include "nav2_amcl/angleutils.hpp" - +#include "nav2_amcl/pf/pf.hpp" +#include "nav2_amcl/pf/pf_vector.hpp" namespace nav2_amcl { diff --git a/nav2_amcl/include/nav2_amcl/pf/pf_pdf.hpp b/nav2_amcl/include/nav2_amcl/pf/pf_pdf.hpp index a3d620e8d3a..e85b3a6e202 100644 --- a/nav2_amcl/include/nav2_amcl/pf/pf_pdf.hpp +++ b/nav2_amcl/include/nav2_amcl/pf/pf_pdf.hpp @@ -30,9 +30,6 @@ #include "nav2_amcl/pf/pf_vector.hpp" -// #include -// #include - #ifdef __cplusplus extern "C" { #endif @@ -65,9 +62,6 @@ pf_pdf_gaussian_t * pf_pdf_gaussian_alloc(pf_vector_t x, pf_matrix_t cx); // Destroy the pdf void pf_pdf_gaussian_free(pf_pdf_gaussian_t * pdf); -// Compute the value of the pdf at some point [z]. -// double pf_pdf_gaussian_value(pf_pdf_gaussian_t *pdf, pf_vector_t z); - // Draw randomly from a zero-mean Gaussian distribution, with standard // deviation sigma. // We use the polar form of the Box-Muller transformation, explained here: diff --git a/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp b/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp index 713478d24d3..1ab9755b2bc 100644 --- a/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp +++ b/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp @@ -51,38 +51,15 @@ typedef struct // Return a zero vector pf_vector_t pf_vector_zero(void); -// Check for NAN or INF in any component -// int pf_vector_finite(pf_vector_t a); - -// Print a vector -// void pf_vector_fprintf(pf_vector_t s, FILE * file, const char * fmt); - -// Simple vector addition -// pf_vector_t pf_vector_add(pf_vector_t a, pf_vector_t b); - // Simple vector subtraction pf_vector_t pf_vector_sub(pf_vector_t a, pf_vector_t b); // Transform from local to global coords (a + b) pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b); -// Transform from global to local coords (a - b) -// pf_vector_t pf_vector_coord_sub(pf_vector_t a, pf_vector_t b); - - // Return a zero matrix pf_matrix_t pf_matrix_zero(void); -// Check for NAN or INF in any component -// int pf_matrix_finite(pf_matrix_t a); - -// Print a matrix -// void pf_matrix_fprintf(pf_matrix_t s, FILE * file, const char * fmt); - -// Compute the matrix inverse. Will also return the determinant, -// which should be checked for underflow (indicated singular matrix). -// pf_matrix_t pf_matrix_inverse(pf_matrix_t a, double *det); - // Decompose a covariance matrix [a] into a rotation matrix [r] and a // diagonal matrix [d] such that a = r * d * r^T. void pf_matrix_unitary(pf_matrix_t * r, pf_matrix_t * d, pf_matrix_t a); diff --git a/nav2_amcl/include/nav2_amcl/sensors/laser/laser.hpp b/nav2_amcl/include/nav2_amcl/sensors/laser/laser.hpp index edc41241284..8381e9b54b5 100644 --- a/nav2_amcl/include/nav2_amcl/sensors/laser/laser.hpp +++ b/nav2_amcl/include/nav2_amcl/sensors/laser/laser.hpp @@ -18,10 +18,10 @@ #ifndef NAV2_AMCL__SENSORS__LASER__LASER_HPP_ #define NAV2_AMCL__SENSORS__LASER__LASER_HPP_ -#include +#include "nav2_amcl/map/map.hpp" #include "nav2_amcl/pf/pf.hpp" #include "nav2_amcl/pf/pf_pdf.hpp" -#include "nav2_amcl/map/map.hpp" +#include "nav2_amcl/pf/pf_vector.hpp" namespace nav2_amcl { diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index 8fac0f1774d..38da836d086 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -15,27 +15,30 @@ 'amcl' Player driver.

- Mohammad Haghighipanah LGPL-2.1-or-later + Brian P. Gerkey + contradict@gmail.com + ament_cmake nav2_common - rclcpp - tf2_geometry_msgs + geometry_msgs message_filters nav_msgs + nav2_msgs + nav2_util + pluginlib + rclcpp + rclcpp_components + rclcpp_lifecycle sensor_msgs std_srvs - tf2_ros tf2 - nav2_util - nav2_msgs - launch_ros - launch_testing - pluginlib + tf2_geometry_msgs + tf2_ros + ament_lint_common ament_lint_auto diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index b5e512ba946..5c6b165d594 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -34,7 +34,9 @@ #include "nav2_amcl/pf/pf.hpp" #include "nav2_util/string_utils.hpp" #include "nav2_amcl/sensors/laser/laser.hpp" +#include "rclcpp/node_options.hpp" #include "tf2/convert.h" +#include "tf2/utils.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2/LinearMath/Transform.h" #include "tf2_ros/buffer.h" @@ -43,15 +45,9 @@ #include "tf2_ros/transform_listener.h" #include "tf2_ros/create_timer_ros.h" -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wpedantic" -#include "tf2/utils.h" -#pragma GCC diagnostic pop - #include "nav2_amcl/portable_utils.hpp" #include "nav2_util/validate_messages.hpp" -using namespace std::placeholders; using rcl_interfaces::msg::ParameterType; using namespace std::chrono_literals; @@ -1571,15 +1567,18 @@ AmclNode::initServices() { global_loc_srv_ = create_service( "reinitialize_global_localization", - std::bind(&AmclNode::globalLocalizationCallback, this, _1, _2, _3)); + std::bind(&AmclNode::globalLocalizationCallback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); initial_guess_srv_ = create_service( "set_initial_pose", - std::bind(&AmclNode::initialPoseReceivedSrv, this, _1, _2, _3)); + std::bind(&AmclNode::initialPoseReceivedSrv, this, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3)); nomotion_update_srv_ = create_service( "request_nomotion_update", - std::bind(&AmclNode::nomotionUpdateCallback, this, _1, _2, _3)); + std::bind(&AmclNode::nomotionUpdateCallback, this, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3)); } void diff --git a/nav2_amcl/src/map/CMakeLists.txt b/nav2_amcl/src/map/CMakeLists.txt deleted file mode 100644 index 9e965f6695d..00000000000 --- a/nav2_amcl/src/map/CMakeLists.txt +++ /dev/null @@ -1,13 +0,0 @@ -add_library(map_lib SHARED - map.c - map_range.c - map_draw.c - map_cspace.cpp -) - -install(TARGETS - map_lib - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) diff --git a/nav2_amcl/src/map/map.c b/nav2_amcl/src/map/map.c index a4528cc663c..c96c25772a2 100644 --- a/nav2_amcl/src/map/map.c +++ b/nav2_amcl/src/map/map.c @@ -25,11 +25,7 @@ * CVS: $Id: map.c 1713 2003-08-23 04:03:43Z inspectorg $ **************************************************************************/ -#include -#include #include -#include -#include #include "nav2_amcl/map/map.hpp" diff --git a/nav2_amcl/src/map/map_cspace.cpp b/nav2_amcl/src/map/map_cspace.cpp index ce66baf1a56..d6c55879508 100644 --- a/nav2_amcl/src/map/map_cspace.cpp +++ b/nav2_amcl/src/map/map_cspace.cpp @@ -19,9 +19,8 @@ * */ -#include -#include -#include +#include +#include #include #include "nav2_amcl/map/map.hpp" diff --git a/nav2_amcl/src/map/map_range.c b/nav2_amcl/src/map/map_range.c index 761f216d39c..5041a21e699 100644 --- a/nav2_amcl/src/map/map_range.c +++ b/nav2_amcl/src/map/map_range.c @@ -25,7 +25,6 @@ * CVS: $Id: map_range.c 1347 2003-05-05 06:24:33Z inspectorg $ **************************************************************************/ -#include #include #include #include diff --git a/nav2_amcl/src/motion_model/CMakeLists.txt b/nav2_amcl/src/motion_model/CMakeLists.txt deleted file mode 100644 index f1c2e354136..00000000000 --- a/nav2_amcl/src/motion_model/CMakeLists.txt +++ /dev/null @@ -1,16 +0,0 @@ -add_library(motions_lib SHARED - omni_motion_model.cpp - differential_motion_model.cpp -) -target_link_libraries(motions_lib pf_lib) -ament_target_dependencies(motions_lib - pluginlib - nav2_util -) - -install(TARGETS - motions_lib - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) diff --git a/nav2_amcl/src/motion_model/differential_motion_model.cpp b/nav2_amcl/src/motion_model/differential_motion_model.cpp index 228b66057a0..afac92bb894 100644 --- a/nav2_amcl/src/motion_model/differential_motion_model.cpp +++ b/nav2_amcl/src/motion_model/differential_motion_model.cpp @@ -19,7 +19,11 @@ * */ +#include + +#include "nav2_amcl/angleutils.hpp" #include "nav2_amcl/motion_model/differential_motion_model.hpp" +#include "nav2_amcl/pf/pf_pdf.hpp" namespace nav2_amcl { diff --git a/nav2_amcl/src/motion_model/omni_motion_model.cpp b/nav2_amcl/src/motion_model/omni_motion_model.cpp index 969fb7f0d6c..6190273aa32 100644 --- a/nav2_amcl/src/motion_model/omni_motion_model.cpp +++ b/nav2_amcl/src/motion_model/omni_motion_model.cpp @@ -19,7 +19,12 @@ * */ +#include + +#include "nav2_amcl/angleutils.hpp" #include "nav2_amcl/motion_model/omni_motion_model.hpp" +#include "nav2_amcl/pf/pf_pdf.hpp" +#include "nav2_amcl/pf/pf_vector.hpp" namespace nav2_amcl { diff --git a/nav2_amcl/src/pf/CMakeLists.txt b/nav2_amcl/src/pf/CMakeLists.txt deleted file mode 100644 index 3b4b2fa5ca1..00000000000 --- a/nav2_amcl/src/pf/CMakeLists.txt +++ /dev/null @@ -1,25 +0,0 @@ -if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wno-gnu-folding-constant) -endif() - -add_library(pf_lib SHARED - pf.c - pf_kdtree.c - pf_pdf.c - pf_vector.c - eig3.c - pf_draw.c -) - -target_include_directories(pf_lib PRIVATE ../include) -if(HAVE_DRAND48) - target_compile_definitions(pf_lib PRIVATE "HAVE_DRAND48") -endif() -target_link_libraries(pf_lib m) - -install(TARGETS - pf_lib - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) diff --git a/nav2_amcl/src/pf/pf.c b/nav2_amcl/src/pf/pf.c index 9d1f5a82892..38731ff4df2 100644 --- a/nav2_amcl/src/pf/pf.c +++ b/nav2_amcl/src/pf/pf.c @@ -236,16 +236,6 @@ int pf_update_converged(pf_t * pf) return 1; } -// Update the filter with some new action -// void pf_update_action(pf_t * pf, pf_action_model_fn_t action_fn, void * action_data) -// { -// pf_sample_set_t * set; - -// set = pf->sets + pf->current_set; - -// (*action_fn)(action_data, set); -// } - // Update the filter with some new sensor observation void pf_update_sensor(pf_t * pf, pf_sensor_model_fn_t sensor_fn, void * sensor_data) { @@ -504,8 +494,6 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set) for (i = 0; i < set->sample_count; i++) { sample = set->samples + i; - // printf("%d %f %f %f\n", i, sample->pose.v[0], sample->pose.v[1], sample->pose.v[2]); - // Get the cluster label for this sample cidx = pf_kdtree_get_cluster(set->kdtree, sample->pose); assert(cidx >= 0); @@ -566,10 +554,6 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set) sqrt( cluster->m[2] * cluster->m[2] + cluster->m[3] * cluster->m[3])); - - // printf("cluster %d %d %f (%f %f %f)\n", i, cluster->count, cluster->weight, - // cluster->mean.v[0], cluster->mean.v[1], cluster->mean.v[2]); - // pf_matrix_fprintf(cluster->cov, stdout, "%e"); } // Compute overall filter stats @@ -589,40 +573,6 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set) set->cov.m[2][2] = -2 * log(sqrt(m[2] * m[2] + m[3] * m[3])); } - -// Compute the CEP statistics (mean and variance). -// void pf_get_cep_stats(pf_t * pf, pf_vector_t * mean, double * var) -// { -// int i; -// double mn, mx, my, mrr; -// pf_sample_set_t * set; -// pf_sample_t * sample; - -// set = pf->sets + pf->current_set; - -// mn = 0.0; -// mx = 0.0; -// my = 0.0; -// mrr = 0.0; - -// for (i = 0; i < set->sample_count; i++) { -// sample = set->samples + i; - -// mn += sample->weight; -// mx += sample->weight * sample->pose.v[0]; -// my += sample->weight * sample->pose.v[1]; -// mrr += sample->weight * sample->pose.v[0] * sample->pose.v[0]; -// mrr += sample->weight * sample->pose.v[1] * sample->pose.v[1]; -// } - -// mean->v[0] = mx / mn; -// mean->v[1] = my / mn; -// mean->v[2] = 0.0; - -// *var = mrr / mn - (mx * mx / (mn * mn) + my * my / (mn * mn)); -// } - - // Get the statistics for a particular cluster. int pf_get_cluster_stats( pf_t * pf, int clabel, double * weight, diff --git a/nav2_amcl/src/pf/pf_kdtree.c b/nav2_amcl/src/pf/pf_kdtree.c index 7b27f2f0f59..870122cbbe3 100644 --- a/nav2_amcl/src/pf/pf_kdtree.c +++ b/nav2_amcl/src/pf/pf_kdtree.c @@ -51,10 +51,6 @@ static pf_kdtree_node_t * pf_kdtree_find_node( // Recursively label nodes in this cluster static void pf_kdtree_cluster_node(pf_kdtree_t * self, pf_kdtree_node_t * node, int depth); -// Recursive node printing -// static void pf_kdtree_print_node(pf_kdtree_t *self, pf_kdtree_node_t *node); - - #ifdef INCLUDE_RTKGUI // Recursively draw nodes @@ -117,50 +113,9 @@ void pf_kdtree_insert(pf_kdtree_t * self, pf_vector_t pose, double value) key[2] = floor(pose.v[2] / self->size[2]); self->root = pf_kdtree_insert_node(self, NULL, self->root, key, value); - - // Test code - /* - printf("find %d %d %d\n", key[0], key[1], key[2]); - assert(pf_kdtree_find_node(self, self->root, key) != NULL); - - pf_kdtree_print_node(self, self->root); - - printf("\n"); - - for (i = 0; i < self->node_count; i++) - { - node = self->nodes + i; - if (node->leaf) - { - printf("find %d %d %d\n", node->key[0], node->key[1], node->key[2]); - assert(pf_kdtree_find_node(self, self->root, node->key) == node); - } - } - printf("\n\n"); - */ } -//////////////////////////////////////////////////////////////////////////////// -// Determine the probability estimate for the given pose. TODO: this -// should do a kernel density estimate rather than a simple histogram. -// double pf_kdtree_get_prob(pf_kdtree_t * self, pf_vector_t pose) -// { -// int key[3]; -// pf_kdtree_node_t * node; - -// key[0] = floor(pose.v[0] / self->size[0]); -// key[1] = floor(pose.v[1] / self->size[1]); -// key[2] = floor(pose.v[2] / self->size[2]); - -// node = pf_kdtree_find_node(self, self->root, key); -// if (node == NULL) { -// return 0.0; -// } -// return node->value; -// } - - //////////////////////////////////////////////////////////////////////////////// // Determine the cluster label for the given pose int pf_kdtree_get_cluster(pf_kdtree_t * self, pf_vector_t pose) @@ -293,8 +248,6 @@ pf_kdtree_node_t * pf_kdtree_insert_node( pf_kdtree_node_t * pf_kdtree_find_node(pf_kdtree_t * self, pf_kdtree_node_t * node, int key[]) { if (node->leaf) { - // printf("find : leaf %p %d %d %d\n", node, node->key[0], node->key[1], node->key[2]); - // If the keys are the same... if (pf_kdtree_equal(self, key, node->key)) { return node; @@ -302,8 +255,6 @@ pf_kdtree_node_t * pf_kdtree_find_node(pf_kdtree_t * self, pf_kdtree_node_t * no return NULL; } } else { - // printf("find : brch %p %d %f\n", node, node->pivot_dim, node->pivot_value); - assert(node->children[0] != NULL); assert(node->children[1] != NULL); @@ -319,27 +270,6 @@ pf_kdtree_node_t * pf_kdtree_find_node(pf_kdtree_t * self, pf_kdtree_node_t * no } -//////////////////////////////////////////////////////////////////////////////// -// Recursive node printing -/* -void pf_kdtree_print_node(pf_kdtree_t *self, pf_kdtree_node_t *node) -{ - if (node->leaf) - { - printf("(%+02d %+02d %+02d)\n", node->key[0], node->key[1], node->key[2]); - printf("%*s", node->depth * 11, ""); - } - else - { - printf("(%+02d %+02d %+02d) ", node->key[0], node->key[1], node->key[2]); - pf_kdtree_print_node(self, node->children[0]); - pf_kdtree_print_node(self, node->children[1]); - } - return; -} -*/ - - //////////////////////////////////////////////////////////////////////////////// // Cluster the leaves in the tree void pf_kdtree_cluster(pf_kdtree_t * self) @@ -446,9 +376,6 @@ void pf_kdtree_draw_node(pf_kdtree_t * self, pf_kdtree_node_t * node, rtk_fig_t rtk_fig_rectangle(fig, ox, oy, 0.0, self->size[0], self->size[1], 0); - // snprintf(text, sizeof(text), "%0.3f", node->value); - // rtk_fig_text(fig, ox, oy, 0.0, text); - snprintf(text, sizeof(text), "%d", node->cluster); rtk_fig_text(fig, ox, oy, 0.0, text); } else { diff --git a/nav2_amcl/src/pf/pf_pdf.c b/nav2_amcl/src/pf/pf_pdf.c index 98a7748ffac..d580e826f7c 100644 --- a/nav2_amcl/src/pf/pf_pdf.c +++ b/nav2_amcl/src/pf/pf_pdf.c @@ -29,8 +29,6 @@ #include #include #include -// #include -// #include #include "nav2_amcl/pf/pf_pdf.hpp" @@ -54,7 +52,6 @@ pf_pdf_gaussian_t * pf_pdf_gaussian_alloc(pf_vector_t x, pf_matrix_t cx) pdf->x = x; pdf->cx = cx; - // pdf->cxi = pf_matrix_inverse(cx, &pdf->cxdet); // Decompose the convariance matrix into a rotation // matrix and a diagonal matrix. @@ -64,8 +61,6 @@ pf_pdf_gaussian_t * pf_pdf_gaussian_alloc(pf_vector_t x, pf_matrix_t cx) pdf->cd.v[2] = sqrt(cd.m[2][2]); // Initialize the random number generator - // pdf->rng = gsl_rng_alloc(gsl_rng_taus); - // gsl_rng_set(pdf->rng, ++pf_pdf_seed); srand48(++pf_pdf_seed); return pdf; @@ -80,28 +75,6 @@ void pf_pdf_gaussian_free(pf_pdf_gaussian_t * pdf) } -/* -// Compute the value of the pdf at some point [x]. -double pf_pdf_gaussian_value(pf_pdf_gaussian_t *pdf, pf_vector_t x) -{ - int i, j; - pf_vector_t z; - double zz, p; - - z = pf_vector_sub(x, pdf->x); - - zz = 0; - for (i = 0; i < 3; i++) - for (j = 0; j < 3; j++) - zz += z.v[i] * pdf->cxi.m[i][j] * z.v[j]; - - p = 1 / (2 * M_PI * pdf->cxdet) * exp(-zz / 2); - - return p; -} -*/ - - // Generate a sample from the pdf. pf_vector_t pf_pdf_gaussian_sample(pf_pdf_gaussian_t * pdf) { diff --git a/nav2_amcl/src/pf/pf_vector.c b/nav2_amcl/src/pf/pf_vector.c index 00fa3840607..ccd82b48f6d 100644 --- a/nav2_amcl/src/pf/pf_vector.c +++ b/nav2_amcl/src/pf/pf_vector.c @@ -26,9 +26,6 @@ *************************************************************************/ #include -// #include -// #include -// #include #include "nav2_amcl/pf/pf_vector.hpp" #include "nav2_amcl/pf/eig3.hpp" @@ -47,47 +44,6 @@ pf_vector_t pf_vector_zero(void) } -// // Check for NAN or INF in any component -// int pf_vector_finite(pf_vector_t a) -// { -// int i; - -// for (i = 0; i < 3; i++) { -// if (!isfinite(a.v[i])) { -// return 0; -// } -// } - -// return 1; -// } - - -// Print a vector -// void pf_vector_fprintf(pf_vector_t a, FILE * file, const char * fmt) -// { -// int i; - -// for (i = 0; i < 3; i++) { -// fprintf(file, fmt, a.v[i]); -// fprintf(file, " "); -// } -// fprintf(file, "\n"); -// } - - -// // Simple vector addition -// pf_vector_t pf_vector_add(pf_vector_t a, pf_vector_t b) -// { -// pf_vector_t c; - -// c.v[0] = a.v[0] + b.v[0]; -// c.v[1] = a.v[1] + b.v[1]; -// c.v[2] = a.v[2] + b.v[2]; - -// return c; -// } - - // Simple vector subtraction pf_vector_t pf_vector_sub(pf_vector_t a, pf_vector_t b) { @@ -115,20 +71,6 @@ pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b) } -// // Transform from global to local coords (a - b) -// pf_vector_t pf_vector_coord_sub(pf_vector_t a, pf_vector_t b) -// { -// pf_vector_t c; - -// c.v[0] = +(a.v[0] - b.v[0]) * cos(b.v[2]) + (a.v[1] - b.v[1]) * sin(b.v[2]); -// c.v[1] = -(a.v[0] - b.v[0]) * sin(b.v[2]) + (a.v[1] - b.v[1]) * cos(b.v[2]); -// c.v[2] = a.v[2] - b.v[2]; -// c.v[2] = atan2(sin(c.v[2]), cos(c.v[2])); - -// return c; -// } - - // Return a zero matrix pf_matrix_t pf_matrix_zero(void) { @@ -145,95 +87,11 @@ pf_matrix_t pf_matrix_zero(void) } -// // Check for NAN or INF in any component -// int pf_matrix_finite(pf_matrix_t a) -// { -// int i, j; - -// for (i = 0; i < 3; i++) { -// for (j = 0; j < 3; j++) { -// if (!isfinite(a.m[i][j])) { -// return 0; -// } -// } -// } - -// return 1; -// } - - -// Print a matrix -// void pf_matrix_fprintf(pf_matrix_t a, FILE * file, const char * fmt) -// { -// int i, j; - -// for (i = 0; i < 3; i++) { -// for (j = 0; j < 3; j++) { -// fprintf(file, fmt, a.m[i][j]); -// fprintf(file, " "); -// } -// fprintf(file, "\n"); -// } -// } - - -/* -// Compute the matrix inverse -pf_matrix_t pf_matrix_inverse(pf_matrix_t a, double *det) -{ - double lndet; - int signum; - gsl_permutation *p; - gsl_matrix_view A, Ai; - - pf_matrix_t ai; - - A = gsl_matrix_view_array((double*) a.m, 3, 3); - Ai = gsl_matrix_view_array((double*) ai.m, 3, 3); - - // Do LU decomposition - p = gsl_permutation_alloc(3); - gsl_linalg_LU_decomp(&A.matrix, p, &signum); - - // Check for underflow - lndet = gsl_linalg_LU_lndet(&A.matrix); - if (lndet < -1000) - { - //printf("underflow in matrix inverse lndet = %f", lndet); - gsl_matrix_set_zero(&Ai.matrix); - } - else - { - // Compute inverse - gsl_linalg_LU_invert(&A.matrix, p, &Ai.matrix); - } - - gsl_permutation_free(p); - - if (det) - *det = exp(lndet); - - return ai; -} -*/ - - // Decompose a covariance matrix [a] into a rotation matrix [r] and a diagonal // matrix [d] such that a = r d r^T. void pf_matrix_unitary(pf_matrix_t * r, pf_matrix_t * d, pf_matrix_t a) { int i, j; - /* - gsl_matrix *aa; - gsl_vector *eval; - gsl_matrix *evec; - gsl_eigen_symmv_workspace *w; - - aa = gsl_matrix_alloc(3, 3); - eval = gsl_vector_alloc(3); - evec = gsl_matrix_alloc(3, 3); - */ - double aa[3][3]; double eval[3]; double evec[3][3]; @@ -246,25 +104,13 @@ void pf_matrix_unitary(pf_matrix_t * r, pf_matrix_t * d, pf_matrix_t a) } // Compute eigenvectors/values - /* - w = gsl_eigen_symmv_alloc(3); - gsl_eigen_symmv(aa, eval, evec, w); - gsl_eigen_symmv_free(w); - */ - eigen_decomposition(aa, evec, eval); *d = pf_matrix_zero(); for (i = 0; i < 3; i++) { - // d->m[i][i] = gsl_vector_get(eval, i); d->m[i][i] = eval[i]; for (j = 0; j < 3; j++) { - // r->m[i][j] = gsl_matrix_get(evec, i, j); r->m[i][j] = evec[i][j]; } } - - // gsl_matrix_free(evec); - // gsl_vector_free(eval); - // gsl_matrix_free(aa); } diff --git a/nav2_amcl/src/sensors/CMakeLists.txt b/nav2_amcl/src/sensors/CMakeLists.txt deleted file mode 100644 index e105675cbf4..00000000000 --- a/nav2_amcl/src/sensors/CMakeLists.txt +++ /dev/null @@ -1,15 +0,0 @@ -add_library(sensors_lib SHARED - laser/laser.cpp - laser/beam_model.cpp - laser/likelihood_field_model.cpp - laser/likelihood_field_model_prob.cpp -) -# map_update_cspace -target_link_libraries(sensors_lib pf_lib map_lib) - -install(TARGETS - sensors_lib - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) diff --git a/nav2_amcl/src/sensors/laser/beam_model.cpp b/nav2_amcl/src/sensors/laser/beam_model.cpp index b6f281cbf78..299da673e21 100644 --- a/nav2_amcl/src/sensors/laser/beam_model.cpp +++ b/nav2_amcl/src/sensors/laser/beam_model.cpp @@ -109,7 +109,7 @@ BeamModel::sensorFunction(LaserData * data, pf_sample_set_t * set) assert(pz <= 1.0); assert(pz >= 0.0); - // p *= pz; + // here we have an ad-hoc weighting scheme for combining beam probs // works well, though... p += pz * pz * pz; diff --git a/nav2_amcl/src/sensors/laser/laser.cpp b/nav2_amcl/src/sensors/laser/laser.cpp index 9059179874d..2f7050efad7 100644 --- a/nav2_amcl/src/sensors/laser/laser.cpp +++ b/nav2_amcl/src/sensors/laser/laser.cpp @@ -19,10 +19,7 @@ * */ -#include -#include -#include -#include +#include #include "nav2_amcl/sensors/laser/laser.hpp" diff --git a/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp b/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp index 1148c2828db..a8e3b7a1f63 100644 --- a/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp +++ b/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp @@ -19,8 +19,8 @@ * */ -#include -#include +#include +#include #include "nav2_amcl/sensors/laser/laser.hpp" diff --git a/nav2_amcl/src/sensors/laser/likelihood_field_model_prob.cpp b/nav2_amcl/src/sensors/laser/likelihood_field_model_prob.cpp index 99efa793f87..ce9dc0331e7 100644 --- a/nav2_amcl/src/sensors/laser/likelihood_field_model_prob.cpp +++ b/nav2_amcl/src/sensors/laser/likelihood_field_model_prob.cpp @@ -19,9 +19,8 @@ * */ - -#include -#include +#include +#include #include "nav2_amcl/sensors/laser/laser.hpp" diff --git a/nav2_map_server/CMakeLists.txt b/nav2_map_server/CMakeLists.txt index 3c2ce2eef8c..c193685363b 100644 --- a/nav2_map_server/CMakeLists.txt +++ b/nav2_map_server/CMakeLists.txt @@ -166,7 +166,7 @@ if(BUILD_TESTING) add_subdirectory(test) endif() -ament_export_include_directories(include) +ament_export_include_directories("include/${PROJECT_NAME}") ament_export_libraries( ${library_name} ${map_io_library_name} diff --git a/nav2_voxel_grid/CMakeLists.txt b/nav2_voxel_grid/CMakeLists.txt index afd87f59aff..e204367ada8 100644 --- a/nav2_voxel_grid/CMakeLists.txt +++ b/nav2_voxel_grid/CMakeLists.txt @@ -43,7 +43,7 @@ if(BUILD_TESTING) endif() ament_export_dependencies(rclcpp) -ament_export_include_directories(include) +ament_export_include_directories("include/${PROJECT_NAME}") ament_export_libraries(voxel_grid) ament_export_targets(voxel_grid)