From 9c07fb8ce6fda403fac6f8cdc616aadaa8b0b841 Mon Sep 17 00:00:00 2001 From: seanyen Date: Fri, 8 May 2020 00:07:58 -0700 Subject: [PATCH 1/3] Windows bringup. --- nav2_amcl/CMakeLists.txt | 14 +++++++- nav2_amcl/src/amcl_node.cpp | 2 ++ nav2_amcl/src/include/portable_utils.h | 28 +++++++++++++++ nav2_amcl/src/map/CMakeLists.txt | 2 +- nav2_amcl/src/motion_model/CMakeLists.txt | 2 +- nav2_amcl/src/pf/CMakeLists.txt | 7 +++- nav2_amcl/src/pf/eig3.c | 7 ++-- nav2_amcl/src/pf/pf.c | 2 ++ nav2_amcl/src/pf/pf_pdf.c | 2 ++ nav2_amcl/src/pf/pf_vector.c | 4 +-- nav2_amcl/src/sensors/CMakeLists.txt | 5 +-- nav2_amcl/src/sensors/laser/laser.cpp | 1 - nav2_behavior_tree/CMakeLists.txt | 2 +- nav2_bt_navigator/CMakeLists.txt | 6 +++- nav2_bt_navigator/src/ros_topic_logger.cpp | 4 +++ nav2_common/cmake/nav2_package.cmake | 12 +++++++ nav2_controller/CMakeLists.txt | 6 +++- nav2_controller/src/nav2_controller.cpp | 4 +-- nav2_costmap_2d/CMakeLists.txt | 10 ++++-- .../costmap_queue/CMakeLists.txt | 2 +- nav2_dwb_controller/dwb_core/CMakeLists.txt | 2 +- .../dwb_critics/CMakeLists.txt | 2 +- .../dwb_plugins/CMakeLists.txt | 2 +- .../nav_2d_utils/CMakeLists.txt | 2 +- nav2_lifecycle_manager/CMakeLists.txt | 6 +++- nav2_map_server/CMakeLists.txt | 18 ++++++++-- .../cmake_modules/FindGRAPHICSMAGICKCPP.cmake | 4 +-- nav2_map_server/src/map_io.cpp | 36 +++++++++++++++++++ nav2_navfn_planner/CMakeLists.txt | 2 +- nav2_planner/CMakeLists.txt | 6 +++- nav2_planner/src/planner_server.cpp | 4 +-- nav2_recoveries/CMakeLists.txt | 8 +++-- nav2_recoveries/src/recovery_server.cpp | 2 +- .../src/planning/planner_tester.hpp | 2 +- nav2_util/src/CMakeLists.txt | 8 +++-- nav2_util/src/dump_params.cpp | 8 +++++ nav2_voxel_grid/CMakeLists.txt | 2 +- nav2_voxel_grid/src/voxel_grid.cpp | 1 - nav2_waypoint_follower/CMakeLists.txt | 6 +++- .../src/waypoint_follower.cpp | 2 +- 40 files changed, 202 insertions(+), 43 deletions(-) create mode 100644 nav2_amcl/src/include/portable_utils.h diff --git a/nav2_amcl/CMakeLists.txt b/nav2_amcl/CMakeLists.txt index 916e524088..be83000bd4 100644 --- a/nav2_amcl/CMakeLists.txt +++ b/nav2_amcl/CMakeLists.txt @@ -22,6 +22,9 @@ 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) @@ -39,6 +42,11 @@ 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 @@ -70,9 +78,13 @@ target_link_libraries(${library_name} map_lib motions_lib sensors_lib ) -install(TARGETS ${executable_name} ${library_name} +install(TARGETS ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS ${executable_name} RUNTIME DESTINATION lib/${PROJECT_NAME} ) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index cc99e323ff..fd17347dc0 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -48,6 +48,8 @@ #include "tf2/utils.h" #pragma GCC diagnostic pop +#include "portable_utils.h" + using namespace std::placeholders; using namespace std::chrono_literals; diff --git a/nav2_amcl/src/include/portable_utils.h b/nav2_amcl/src/include/portable_utils.h new file mode 100644 index 0000000000..1577e6875b --- /dev/null +++ b/nav2_amcl/src/include/portable_utils.h @@ -0,0 +1,28 @@ +#ifndef PORTABLE_UTILS_H +#define PORTABLE_UTILS_H + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef HAVE_DRAND48 +// Some system (e.g., Windows) doesn't come with drand48(), srand48(). +// Use rand, and srand for such system. +static double drand48(void) +{ + return ((double)rand()) / RAND_MAX; +} + +static void srand48(long int seedval) +{ + srand(seedval); +} +#endif + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/nav2_amcl/src/map/CMakeLists.txt b/nav2_amcl/src/map/CMakeLists.txt index 7017eb494a..602629f9fd 100644 --- a/nav2_amcl/src/map/CMakeLists.txt +++ b/nav2_amcl/src/map/CMakeLists.txt @@ -10,5 +10,5 @@ install(TARGETS map_lib ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin ) diff --git a/nav2_amcl/src/motion_model/CMakeLists.txt b/nav2_amcl/src/motion_model/CMakeLists.txt index 88ed7c70e5..6322ad9870 100644 --- a/nav2_amcl/src/motion_model/CMakeLists.txt +++ b/nav2_amcl/src/motion_model/CMakeLists.txt @@ -12,5 +12,5 @@ install(TARGETS motions_lib ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin ) diff --git a/nav2_amcl/src/pf/CMakeLists.txt b/nav2_amcl/src/pf/CMakeLists.txt index b277cc9378..c6f16e6a7d 100644 --- a/nav2_amcl/src/pf/CMakeLists.txt +++ b/nav2_amcl/src/pf/CMakeLists.txt @@ -11,9 +11,14 @@ add_library(pf_lib SHARED pf_draw.c ) +target_include_directories(pf_lib PRIVATE ../include) +if(HAVE_DRAND48) + target_compile_definitions(pf_lib PRIVATE "HAVE_DRAND48") +endif() + install(TARGETS pf_lib ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin ) diff --git a/nav2_amcl/src/pf/eig3.c b/nav2_amcl/src/pf/eig3.c index 8b290c8abc..a87354dbc8 100644 --- a/nav2_amcl/src/pf/eig3.c +++ b/nav2_amcl/src/pf/eig3.c @@ -7,8 +7,11 @@ #define MAX(a, b) ((a) > (b) ? (a) : (b)) #endif -// #define n 3 -static const int n = 3; +#ifdef _MSC_VER +#define n 3 +#else +static int n = 3; +#endif static double hypot2(double x, double y) { diff --git a/nav2_amcl/src/pf/pf.c b/nav2_amcl/src/pf/pf.c index 664fd44a13..851e63b64e 100644 --- a/nav2_amcl/src/pf/pf.c +++ b/nav2_amcl/src/pf/pf.c @@ -35,6 +35,8 @@ #include "nav2_amcl/pf/pf_pdf.hpp" #include "nav2_amcl/pf/pf_kdtree.hpp" +#include "portable_utils.h" + // Compute the required number of samples, given that there are k bins // with samples in them. diff --git a/nav2_amcl/src/pf/pf_pdf.c b/nav2_amcl/src/pf/pf_pdf.c index b6a1d13cda..858bea7df8 100644 --- a/nav2_amcl/src/pf/pf_pdf.c +++ b/nav2_amcl/src/pf/pf_pdf.c @@ -34,6 +34,8 @@ #include "nav2_amcl/pf/pf_pdf.hpp" +#include "portable_utils.h" + // Random number generator seed value static unsigned int pf_pdf_seed; diff --git a/nav2_amcl/src/pf/pf_vector.c b/nav2_amcl/src/pf/pf_vector.c index a38c4e9994..29e183749e 100644 --- a/nav2_amcl/src/pf/pf_vector.c +++ b/nav2_amcl/src/pf/pf_vector.c @@ -53,7 +53,7 @@ int pf_vector_finite(pf_vector_t a) int i; for (i = 0; i < 3; i++) { - if (!finite(a.v[i])) { + if (!isfinite(a.v[i])) { return 0; } } @@ -152,7 +152,7 @@ int pf_matrix_finite(pf_matrix_t a) for (i = 0; i < 3; i++) { for (j = 0; j < 3; j++) { - if (!finite(a.m[i][j])) { + if (!isfinite(a.m[i][j])) { return 0; } } diff --git a/nav2_amcl/src/sensors/CMakeLists.txt b/nav2_amcl/src/sensors/CMakeLists.txt index 56c3a382bf..e105675cbf 100644 --- a/nav2_amcl/src/sensors/CMakeLists.txt +++ b/nav2_amcl/src/sensors/CMakeLists.txt @@ -4,11 +4,12 @@ add_library(sensors_lib SHARED laser/likelihood_field_model.cpp laser/likelihood_field_model_prob.cpp ) -target_link_libraries(sensors_lib pf_lib) +# map_update_cspace +target_link_libraries(sensors_lib pf_lib map_lib) install(TARGETS sensors_lib ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin ) diff --git a/nav2_amcl/src/sensors/laser/laser.cpp b/nav2_amcl/src/sensors/laser/laser.cpp index b19e83607d..9059179874 100644 --- a/nav2_amcl/src/sensors/laser/laser.cpp +++ b/nav2_amcl/src/sensors/laser/laser.cpp @@ -23,7 +23,6 @@ #include #include #include -#include #include "nav2_amcl/sensors/laser/laser.hpp" diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 117c2629d5..a2d70feb28 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -110,7 +110,7 @@ install(TARGETS ${library_name} ${plugin_libs} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin ) install(DIRECTORY include/ diff --git a/nav2_bt_navigator/CMakeLists.txt b/nav2_bt_navigator/CMakeLists.txt index 3f56c116a1..da362c9106 100644 --- a/nav2_bt_navigator/CMakeLists.txt +++ b/nav2_bt_navigator/CMakeLists.txt @@ -60,9 +60,13 @@ ament_target_dependencies(${library_name} ${dependencies} ) -install(TARGETS ${executable_name} ${library_name} +install(TARGETS ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS ${executable_name} RUNTIME DESTINATION lib/${PROJECT_NAME} ) diff --git a/nav2_bt_navigator/src/ros_topic_logger.cpp b/nav2_bt_navigator/src/ros_topic_logger.cpp index 192318d2f7..472b30d323 100644 --- a/nav2_bt_navigator/src/ros_topic_logger.cpp +++ b/nav2_bt_navigator/src/ros_topic_logger.cpp @@ -39,8 +39,12 @@ void RosTopicLogger::callback( // BT timestamps are a duration since the epoch. Need to convert to a time_point // before converting to a msg. +#ifndef _WIN32 event.timestamp = tf2_ros::toMsg(std::chrono::time_point(timestamp)); +#else + event.timestamp = tf2_ros::toMsg(timestamp); +#endif event.node_name = node.name(); event.previous_status = toStr(prev_status, false); event.current_status = toStr(status, false); diff --git a/nav2_common/cmake/nav2_package.cmake b/nav2_common/cmake/nav2_package.cmake index 61dd187cfe..ee8555f799 100644 --- a/nav2_common/cmake/nav2_package.cmake +++ b/nav2_common/cmake/nav2_package.cmake @@ -42,4 +42,16 @@ macro(nav2_package) set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} --coverage") set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} --coverage") endif() + + # Defaults for Microsoft C++ compiler + if(MSVC) + # https://blog.kitware.com/create-dlls-on-windows-without-declspec-using-new-cmake-export-all-feature/ + set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + + # Enable Math Constants + # https://docs.microsoft.com/en-us/cpp/c-runtime-library/math-constants?view=vs-2019 + add_compile_definitions( + _USE_MATH_DEFINES + ) + endif() endmacro() diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index 90dc9fd172..b4cb1b50d0 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -59,9 +59,13 @@ ament_target_dependencies(${executable_name} target_link_libraries(${executable_name} ${library_name}) -install(TARGETS ${executable_name} ${library_name} +install(TARGETS ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS ${executable_name} RUNTIME DESTINATION lib/${PROJECT_NAME} ) diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index ef7798137e..11dc15e670 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -88,7 +88,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) exit(-1); } - for (uint i = 0; i != controller_types_.size(); i++) { + for (size_t i = 0; i != controller_types_.size(); i++) { try { nav2_core::Controller::Ptr controller = lp_loader_.createUniqueInstance(controller_types_[i]); @@ -105,7 +105,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) } } - for (uint i = 0; i != controller_ids_.size(); i++) { + for (size_t i = 0; i != controller_ids_.size(); i++) { controller_ids_concat_ += controller_ids_[i] + std::string(" "); } diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index abd70ac461..8f3262b8a6 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -131,13 +131,17 @@ target_link_libraries(nav2_costmap_2d install(TARGETS nav2_costmap_2d_core - nav2_costmap_2d layers nav2_costmap_2d_client - nav2_costmap_2d_markers - nav2_costmap_2d_cloud ARCHIVE DESTINATION lib LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS + nav2_costmap_2d + nav2_costmap_2d_markers + nav2_costmap_2d_cloud RUNTIME DESTINATION lib/${PROJECT_NAME} ) diff --git a/nav2_dwb_controller/costmap_queue/CMakeLists.txt b/nav2_dwb_controller/costmap_queue/CMakeLists.txt index 489e73fa7b..4641d0b547 100644 --- a/nav2_dwb_controller/costmap_queue/CMakeLists.txt +++ b/nav2_dwb_controller/costmap_queue/CMakeLists.txt @@ -45,7 +45,7 @@ endif() install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin ) install(DIRECTORY include/ DESTINATION include/ diff --git a/nav2_dwb_controller/dwb_core/CMakeLists.txt b/nav2_dwb_controller/dwb_core/CMakeLists.txt index cec51d5f9b..3b58ccfeb3 100644 --- a/nav2_dwb_controller/dwb_core/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_core/CMakeLists.txt @@ -58,7 +58,7 @@ ament_target_dependencies(dwb_core install(TARGETS dwb_core ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin ) install(DIRECTORY include/ diff --git a/nav2_dwb_controller/dwb_critics/CMakeLists.txt b/nav2_dwb_controller/dwb_critics/CMakeLists.txt index b892eb7d28..7b7cb8c70d 100644 --- a/nav2_dwb_controller/dwb_critics/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_critics/CMakeLists.txt @@ -60,7 +60,7 @@ ament_target_dependencies(${PROJECT_NAME} install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin ) install(DIRECTORY include/ DESTINATION include/ diff --git a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt index 64348944b5..ea4d7868ea 100644 --- a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt @@ -63,7 +63,7 @@ endif() install(TARGETS simple_goal_checker stopped_goal_checker standard_traj_generator ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin ) install(DIRECTORY include/ DESTINATION include/ diff --git a/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt index 6dbe3d3019..06bb2a5a19 100644 --- a/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt +++ b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt @@ -46,7 +46,7 @@ ament_target_dependencies(path_ops install(TARGETS conversions path_ops ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin ) install(DIRECTORY include/ DESTINATION include/ diff --git a/nav2_lifecycle_manager/CMakeLists.txt b/nav2_lifecycle_manager/CMakeLists.txt index c68e636f29..b349a921b9 100644 --- a/nav2_lifecycle_manager/CMakeLists.txt +++ b/nav2_lifecycle_manager/CMakeLists.txt @@ -57,10 +57,14 @@ ament_target_dependencies(lifecycle_manager ) install(TARGETS - lifecycle_manager ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS + lifecycle_manager RUNTIME DESTINATION lib/${PROJECT_NAME} ) diff --git a/nav2_map_server/CMakeLists.txt b/nav2_map_server/CMakeLists.txt index a8ba50e6c0..9243742486 100644 --- a/nav2_map_server/CMakeLists.txt +++ b/nav2_map_server/CMakeLists.txt @@ -88,6 +88,11 @@ target_link_libraries(${library_name} target_link_libraries(${map_server_executable} ${library_name}) +if(WIN32) + target_compile_definitions(${map_server_executable} PRIVATE + YAML_CPP_DLL) +endif() + target_link_libraries(${map_saver_cli_executable} ${library_name}) @@ -100,10 +105,19 @@ target_include_directories(${map_io_library_name} SYSTEM PRIVATE target_link_libraries(${map_io_library_name} ${GRAPHICSMAGICKCPP_LIBRARIES}) -install(TARGETS ${map_server_executable} ${library_name} ${map_io_library_name} - ${map_saver_cli_executable} ${map_saver_server_executable} +if(WIN32) + target_compile_definitions(${map_io_library_name} PRIVATE + YAML_CPP_DLL) +endif() + +install(TARGETS + ${library_name} ${map_io_library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +install(TARGETS + ${map_server_executable} ${map_saver_cli_executable} ${map_saver_server_executable} RUNTIME DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY include/ diff --git a/nav2_map_server/cmake_modules/FindGRAPHICSMAGICKCPP.cmake b/nav2_map_server/cmake_modules/FindGRAPHICSMAGICKCPP.cmake index 37a2fbfb74..c04356b4aa 100644 --- a/nav2_map_server/cmake_modules/FindGRAPHICSMAGICKCPP.cmake +++ b/nav2_map_server/cmake_modules/FindGRAPHICSMAGICKCPP.cmake @@ -26,9 +26,9 @@ find_path(GRAPHICSMAGICKCPP_INCLUDE_DIRS PATH_SUFFIXES GraphicsMagick) find_library(GRAPHICSMAGICKCPP_LIBRARIES - NAMES "GraphicsMagick++") + NAMES "GraphicsMagick++" "graphicsmagick") find_package_handle_standard_args( GRAPHICSMAGICKCPP - GRAPHICSMAGICKCPP__LIBRARIES + GRAPHICSMAGICKCPP_LIBRARIES GRAPHICSMAGICKCPP_INCLUDE_DIRS) \ No newline at end of file diff --git a/nav2_map_server/src/map_io.cpp b/nav2_map_server/src/map_io.cpp index 3179a46df6..3f31704948 100644 --- a/nav2_map_server/src/map_io.cpp +++ b/nav2_map_server/src/map_io.cpp @@ -31,7 +31,9 @@ #include "nav2_map_server/map_io.hpp" +#ifndef _WIN32 #include +#endif #include #include #include @@ -45,6 +47,40 @@ #include "tf2/LinearMath/Matrix3x3.h" #include "tf2/LinearMath/Quaternion.h" +#ifdef _WIN32 +// https://github.com/rtv/Stage/blob/master/replace/dirname.c +static +char * dirname(char * path) +{ + static const char dot[] = "."; + char * last_slash; + + /* Find last '/'. */ + last_slash = path != NULL ? strrchr(path, '/') : NULL; + + if (last_slash == path) { + /* The last slash is the first character in the string. We have to + return "/". */ + ++last_slash; + } else if (last_slash != NULL && last_slash[1] == '\0') { + /* The '/' is the last character, we have to look further. */ + last_slash = reinterpret_cast(memchr(path, last_slash - path, '/')); + } + + if (last_slash != NULL) { + /* Terminate the path. */ + last_slash[0] = '\0'; + } else { + /* This assignment is ill-designed but the XPG specs require to + return a string containing "." in any case no directory part is + found and so a static and constant string is required. */ + path = reinterpret_cast(dot); + } + + return path; +} +#endif + namespace nav2_map_server { using nav2_util::geometry_utils::orientationAroundZAxis; diff --git a/nav2_navfn_planner/CMakeLists.txt b/nav2_navfn_planner/CMakeLists.txt index 1315b65ed5..ab4815b016 100644 --- a/nav2_navfn_planner/CMakeLists.txt +++ b/nav2_navfn_planner/CMakeLists.txt @@ -60,7 +60,7 @@ pluginlib_export_plugin_description_file(nav2_core global_planner_plugin.xml) install(TARGETS ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin ) install(DIRECTORY include/ diff --git a/nav2_planner/CMakeLists.txt b/nav2_planner/CMakeLists.txt index 4012f40b9d..69f4d57156 100644 --- a/nav2_planner/CMakeLists.txt +++ b/nav2_planner/CMakeLists.txt @@ -67,9 +67,13 @@ ament_target_dependencies(${executable_name} # prevent pluginlib from using boost target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") -install(TARGETS ${executable_name} ${library_name} +install(TARGETS ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS ${executable_name} RUNTIME DESTINATION lib/${PROJECT_NAME} ) diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 97d33b33fe..dbb159d730 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -90,7 +90,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state) exit(-1); } - for (uint i = 0; i != plugin_types_.size(); i++) { + for (size_t i = 0; i != plugin_types_.size(); i++) { try { nav2_core::GlobalPlanner::Ptr planner = gp_loader_.createUniqueInstance(plugin_types_[i]); @@ -107,7 +107,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state) } } - for (uint i = 0; i != plugin_types_.size(); i++) { + for (size_t i = 0; i != plugin_types_.size(); i++) { planner_ids_concat_ += plugin_ids_[i] + std::string(" "); } diff --git a/nav2_recoveries/CMakeLists.txt b/nav2_recoveries/CMakeLists.txt index 6b0def011e..01849b8c11 100644 --- a/nav2_recoveries/CMakeLists.txt +++ b/nav2_recoveries/CMakeLists.txt @@ -103,13 +103,17 @@ ament_target_dependencies(${executable_name} ${dependencies} ) -install(TARGETS ${executable_name} - ${library_name} + +install(TARGETS ${library_name} nav2_backup_recovery nav2_spin_recovery nav2_wait_recovery ARCHIVE DESTINATION lib LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS ${executable_name} RUNTIME DESTINATION lib/${PROJECT_NAME} ) diff --git a/nav2_recoveries/src/recovery_server.cpp b/nav2_recoveries/src/recovery_server.cpp index a4caa363d8..ecd23d4481 100644 --- a/nav2_recoveries/src/recovery_server.cpp +++ b/nav2_recoveries/src/recovery_server.cpp @@ -88,7 +88,7 @@ RecoveryServer::loadRecoveryPlugins() { auto node = shared_from_this(); - for (uint i = 0; i != plugin_names_.size(); i++) { + for (size_t i = 0; i != plugin_names_.size(); i++) { try { RCLCPP_INFO( get_logger(), "Creating recovery plugin %s of type %s", diff --git a/nav2_system_tests/src/planning/planner_tester.hpp b/nav2_system_tests/src/planning/planner_tester.hpp index 6a411050e7..2e47a84be5 100644 --- a/nav2_system_tests/src/planning/planner_tester.hpp +++ b/nav2_system_tests/src/planning/planner_tester.hpp @@ -50,7 +50,7 @@ class NavFnPlannerTester : public nav2_planner::PlannerServer void printCostmap() { // print costmap for debug - for (uint i = 0; i != costmap_->getSizeInCellsX() * costmap_->getSizeInCellsY(); i++) { + for (size_t i = 0; i != costmap_->getSizeInCellsX() * costmap_->getSizeInCellsY(); i++) { if (i % costmap_->getSizeInCellsX() == 0) { std::cout << "" << std::endl; } diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt index ec62f85567..bd903ef6d1 100644 --- a/nav2_util/src/CMakeLists.txt +++ b/nav2_util/src/CMakeLists.txt @@ -37,9 +37,13 @@ target_link_libraries(dump_params ${Boost_PROGRAM_OPTIONS_LIBRARY}) install(TARGETS ${library_name} - lifecycle_bringup - dump_params ARCHIVE DESTINATION lib LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS + lifecycle_bringup + dump_params RUNTIME DESTINATION lib/${PROJECT_NAME} ) diff --git a/nav2_util/src/dump_params.cpp b/nav2_util/src/dump_params.cpp index b47c6815b7..34672f2b21 100644 --- a/nav2_util/src/dump_params.cpp +++ b/nav2_util/src/dump_params.cpp @@ -388,6 +388,14 @@ validate( boost::any_cast &>(v).values.swap(result); } +#ifdef _WIN32 +static const char * basename(const char * filepath) +{ + const char * base = std::strrchr(filepath, '/'); + return base ? (base + 1) : filepath; +} +#endif + int main(int argc, char * argv[]) { rclcpp::init(argc, argv); diff --git a/nav2_voxel_grid/CMakeLists.txt b/nav2_voxel_grid/CMakeLists.txt index fe5d7fe753..4b88a87d86 100644 --- a/nav2_voxel_grid/CMakeLists.txt +++ b/nav2_voxel_grid/CMakeLists.txt @@ -25,7 +25,7 @@ ament_target_dependencies(voxel_grid install(TARGETS voxel_grid ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin ) install(DIRECTORY include/ diff --git a/nav2_voxel_grid/src/voxel_grid.cpp b/nav2_voxel_grid/src/voxel_grid.cpp index 4eca35f317..c41bc40533 100644 --- a/nav2_voxel_grid/src/voxel_grid.cpp +++ b/nav2_voxel_grid/src/voxel_grid.cpp @@ -35,7 +35,6 @@ * Author: Eitan Marder-Eppstein *********************************************************************/ #include -#include namespace nav2_voxel_grid { diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt index d7b5460261..71925e74b5 100644 --- a/nav2_waypoint_follower/CMakeLists.txt +++ b/nav2_waypoint_follower/CMakeLists.txt @@ -49,9 +49,13 @@ ament_target_dependencies(${library_name} ${dependencies} ) -install(TARGETS ${executable_name} ${library_name} +install(TARGETS ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS ${executable_name} RUNTIME DESTINATION lib/${PROJECT_NAME} ) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 978ecb834c..dd08a89d59 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -126,7 +126,7 @@ WaypointFollower::followWaypoints() static_cast(goal->poses.size())); rclcpp::Rate r(loop_rate_); - uint goal_index = 0; + uint32_t goal_index = 0; bool new_goal = true; while (rclcpp::ok()) { From 5d16bc5b520fea91a4032c96453ac79a7d369e5c Mon Sep 17 00:00:00 2001 From: seanyen Date: Tue, 12 May 2020 11:32:37 -0700 Subject: [PATCH 2/3] nullity check. --- nav2_map_server/src/map_io.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/nav2_map_server/src/map_io.cpp b/nav2_map_server/src/map_io.cpp index 3f31704948..e6465b7b81 100644 --- a/nav2_map_server/src/map_io.cpp +++ b/nav2_map_server/src/map_io.cpp @@ -55,6 +55,10 @@ char * dirname(char * path) static const char dot[] = "."; char * last_slash; + if (path == NULL) { + return path; + } + /* Find last '/'. */ last_slash = path != NULL ? strrchr(path, '/') : NULL; From 81805709fd32cc2619ae39c2fbd6c3facaf4f01f Mon Sep 17 00:00:00 2001 From: seanyen Date: Tue, 12 May 2020 11:57:30 -0700 Subject: [PATCH 3/3] nullity check. --- nav2_map_server/src/map_io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_map_server/src/map_io.cpp b/nav2_map_server/src/map_io.cpp index e6465b7b81..6941b354fe 100644 --- a/nav2_map_server/src/map_io.cpp +++ b/nav2_map_server/src/map_io.cpp @@ -62,7 +62,7 @@ char * dirname(char * path) /* Find last '/'. */ last_slash = path != NULL ? strrchr(path, '/') : NULL; - if (last_slash == path) { + if (last_slash != NULL && last_slash == path) { /* The last slash is the first character in the string. We have to return "/". */ ++last_slash;