From bc127616ae563ff08f4f298758276b200ed2baed Mon Sep 17 00:00:00 2001 From: Akash Munagala Date: Mon, 2 Aug 2021 21:25:16 -0700 Subject: [PATCH 1/6] Fix nav2_util Replace sleep with Sleep on Windows --- nav2_util/src/dump_params.cpp | 2 ++ nav2_util/test/test_lifecycle_cli_node.cpp | 12 ++++++++++++ 2 files changed, 14 insertions(+) diff --git a/nav2_util/src/dump_params.cpp b/nav2_util/src/dump_params.cpp index 259479f1f6..247a09d936 100644 --- a/nav2_util/src/dump_params.cpp +++ b/nav2_util/src/dump_params.cpp @@ -14,7 +14,9 @@ #include #include +#ifndef _WIN32 #include +#endif #include #include #include diff --git a/nav2_util/test/test_lifecycle_cli_node.cpp b/nav2_util/test/test_lifecycle_cli_node.cpp index 2177412e7d..50a458cb1f 100644 --- a/nav2_util/test/test_lifecycle_cli_node.cpp +++ b/nav2_util/test/test_lifecycle_cli_node.cpp @@ -23,6 +23,10 @@ #include "nav2_util/node_thread.hpp" #include "rclcpp/rclcpp.hpp" +#ifdef _WIN32 +#include +#endif + class DummyNode : public nav2_util::LifecycleNode { public: @@ -80,7 +84,11 @@ TEST(LifeycleCLI, fails_no_node_name) Handle handle; auto rc = system("ros2 run nav2_util lifecycle_bringup"); (void)rc; +#ifdef _WIN32 + Sleep(1000); +#else sleep(1); +#endif // check node didn't mode EXPECT_EQ(handle.node->activated, false); SUCCEED(); @@ -90,7 +98,11 @@ TEST(LifeycleCLI, succeeds_node_name) { Handle handle; auto rc = system("ros2 run nav2_util lifecycle_bringup nav2_test_cli"); +#ifdef _WIN32 + Sleep(3000); +#else sleep(3); +#endif // check node moved (void)rc; EXPECT_EQ(handle.node->activated, true); From 026089957d56e6e8c424470d4a140c21d81073dd Mon Sep 17 00:00:00 2001 From: Akash Munagala Date: Tue, 3 Aug 2021 19:57:31 -0700 Subject: [PATCH 2/6] Fix rviz plugin Undef NO_ERROR from windows headers --- nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp index 772759aa47..a3a1c8d5ea 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp @@ -17,6 +17,7 @@ #include #include +#undef NO_ERROR #include #include From a0026c9205583dd00f0c1623abf2edac3da51722 Mon Sep 17 00:00:00 2001 From: Akash Munagala Date: Tue, 3 Aug 2021 20:21:41 -0700 Subject: [PATCH 3/6] Fix smac_planner * Define _USE_MATH_DEFINES for M_PI * Replace uint with unsigned int * Don't add compiler flags that aren't supported on MSVC * Use vector instead of VLA when building with MSVC --- smac_planner/CMakeLists.txt | 7 ++++++- smac_planner/include/smac_planner/smoother.hpp | 12 ++++++++++-- .../include/smac_planner/smoother_cost_function.hpp | 2 +- smac_planner/src/costmap_downsampler.cpp | 8 ++++---- smac_planner/src/smac_planner.cpp | 2 +- smac_planner/src/smac_planner_2d.cpp | 2 +- 6 files changed, 23 insertions(+), 10 deletions(-) diff --git a/smac_planner/CMakeLists.txt b/smac_planner/CMakeLists.txt index 68850c8176..ea4eeac022 100644 --- a/smac_planner/CMakeLists.txt +++ b/smac_planner/CMakeLists.txt @@ -31,7 +31,9 @@ endif() set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") -add_compile_options(-O3 -Wextra -Wdeprecated -fPIC) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-O3 -Wextra -Wdeprecated -fPIC) +endif() include_directories( include @@ -41,6 +43,9 @@ include_directories( ) set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") +if(MSVC) + add_compile_definitions(_USE_MATH_DEFINES) +endif() set(library_name smac_planner) diff --git a/smac_planner/include/smac_planner/smoother.hpp b/smac_planner/include/smac_planner/smoother.hpp index 756e6b5793..b1dd73442d 100644 --- a/smac_planner/include/smac_planner/smoother.hpp +++ b/smac_planner/include/smac_planner/smoother.hpp @@ -105,15 +105,23 @@ class Smoother { _options.max_solver_time_in_seconds = params.max_time; +#ifdef _MSC_VER + std::vector parameters(path.size() * 2); +#else double parameters[path.size() * 2]; // NOLINT - for (uint i = 0; i != path.size(); i++) { +#endif + for (unsigned int i = 0; i != path.size(); i++) { parameters[2 * i] = path[i][0]; parameters[2 * i + 1] = path[i][1]; } ceres::GradientProblemSolver::Summary summary; ceres::GradientProblem problem(new UnconstrainedSmootherCostFunction(&path, costmap, params)); +#ifdef _MSC_VER + ceres::Solve(_options, problem, parameters.data(), &summary); +#else ceres::Solve(_options, problem, parameters, &summary); +#endif if (_debug) { std::cout << summary.FullReport() << '\n'; @@ -123,7 +131,7 @@ class Smoother return false; } - for (uint i = 0; i != path.size(); i++) { + for (unsigned int i = 0; i != path.size(); i++) { path[i][0] = parameters[2 * i]; path[i][1] = parameters[2 * i + 1]; } diff --git a/smac_planner/include/smac_planner/smoother_cost_function.hpp b/smac_planner/include/smac_planner/smoother_cost_function.hpp index 82d1c88a6d..dc58f13fa8 100644 --- a/smac_planner/include/smac_planner/smoother_cost_function.hpp +++ b/smac_planner/include/smac_planner/smoother_cost_function.hpp @@ -106,7 +106,7 @@ class UnconstrainedSmootherCostFunction : public ceres::FirstOrderFunction Eigen::Vector2d xi; Eigen::Vector2d xi_p1; Eigen::Vector2d xi_m1; - uint x_index, y_index; + unsigned int x_index, y_index; cost[0] = 0.0; double cost_raw = 0.0; double grad_x_raw = 0.0; diff --git a/smac_planner/src/costmap_downsampler.cpp b/smac_planner/src/costmap_downsampler.cpp index 025a30b351..ba3d282bb1 100644 --- a/smac_planner/src/costmap_downsampler.cpp +++ b/smac_planner/src/costmap_downsampler.cpp @@ -84,8 +84,8 @@ nav2_costmap_2d::Costmap2D * CostmapDownsampler::downsample( } // Assign costs - for (uint i = 0; i < _downsampled_size_x; ++i) { - for (uint j = 0; j < _downsampled_size_y; ++j) { + for (unsigned int i = 0; i < _downsampled_size_x; ++i) { + for (unsigned int j = 0; j < _downsampled_size_y; ++j) { setCostOfCell(i, j); } } @@ -122,12 +122,12 @@ void CostmapDownsampler::setCostOfCell( unsigned int x_offset = new_mx * _downsampling_factor; unsigned int y_offset = new_my * _downsampling_factor; - for (uint i = 0; i < _downsampling_factor; ++i) { + for (unsigned int i = 0; i < _downsampling_factor; ++i) { mx = x_offset + i; if (mx >= _size_x) { continue; } - for (uint j = 0; j < _downsampling_factor; ++j) { + for (unsigned int j = 0; j < _downsampling_factor; ++j) { my = y_offset + j; if (my >= _size_y) { continue; diff --git a/smac_planner/src/smac_planner.cpp b/smac_planner/src/smac_planner.cpp index 71ba1d3333..7d7c6879ab 100644 --- a/smac_planner/src/smac_planner.cpp +++ b/smac_planner/src/smac_planner.cpp @@ -340,7 +340,7 @@ nav_msgs::msg::Path SmacPlanner::createPlan( // populate final path // TODO(stevemacenski): set orientation to tangent of path - for (uint i = 0; i != path_world.size(); i++) { + for (unsigned int i = 0; i != path_world.size(); i++) { pose.pose.position.x = path_world[i][0]; pose.pose.position.y = path_world[i][1]; plan.poses[i] = pose; diff --git a/smac_planner/src/smac_planner_2d.cpp b/smac_planner/src/smac_planner_2d.cpp index 643ffe41a6..cc3f70aa2c 100644 --- a/smac_planner/src/smac_planner_2d.cpp +++ b/smac_planner/src/smac_planner_2d.cpp @@ -303,7 +303,7 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( removeHook(path_world); // populate final path - for (uint i = 0; i != path_world.size(); i++) { + for (unsigned int i = 0; i != path_world.size(); i++) { pose.pose.position.x = path_world[i][0]; pose.pose.position.y = path_world[i][1]; plan.poses[i] = pose; From 8086a0a0b471580234b015db5537c98600f365c5 Mon Sep 17 00:00:00 2001 From: Akash Munagala Date: Sun, 15 Aug 2021 10:40:54 -0700 Subject: [PATCH 4/6] Fix dirname for \ paths --- nav2_map_server/src/map_io.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/nav2_map_server/src/map_io.cpp b/nav2_map_server/src/map_io.cpp index 7375c64da8..10e84ca9b6 100644 --- a/nav2_map_server/src/map_io.cpp +++ b/nav2_map_server/src/map_io.cpp @@ -59,6 +59,13 @@ char * dirname(char * path) return path; } + /* Replace all "\" with "/" */ + char * c = path; + while (*c != '\0') { + if (*c == '\\') *c = '/'; + ++c; + } + /* Find last '/'. */ last_slash = path != NULL ? strrchr(path, '/') : NULL; From 8614968fee11f6414fe35a87517ca935a36f33ee Mon Sep 17 00:00:00 2001 From: Akash Munagala Date: Wed, 13 Oct 2021 20:12:00 -0500 Subject: [PATCH 5/6] Address feedback --- smac_planner/include/smac_planner/smoother.hpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/smac_planner/include/smac_planner/smoother.hpp b/smac_planner/include/smac_planner/smoother.hpp index b1dd73442d..f81d94720c 100644 --- a/smac_planner/include/smac_planner/smoother.hpp +++ b/smac_planner/include/smac_planner/smoother.hpp @@ -106,7 +106,8 @@ class Smoother _options.max_solver_time_in_seconds = params.max_time; #ifdef _MSC_VER - std::vector parameters(path.size() * 2); + std::vector parametersVec(path.size() * 2); + double* parameters = parametersVec.data(); #else double parameters[path.size() * 2]; // NOLINT #endif @@ -117,11 +118,7 @@ class Smoother ceres::GradientProblemSolver::Summary summary; ceres::GradientProblem problem(new UnconstrainedSmootherCostFunction(&path, costmap, params)); -#ifdef _MSC_VER - ceres::Solve(_options, problem, parameters.data(), &summary); -#else ceres::Solve(_options, problem, parameters, &summary); -#endif if (_debug) { std::cout << summary.FullReport() << '\n'; From 6db5be2e0c954090eb6589ab4134e675552f8f0b Mon Sep 17 00:00:00 2001 From: Akash Munagala Date: Thu, 14 Oct 2021 15:07:42 -0500 Subject: [PATCH 6/6] Fix linter --- nav2_map_server/src/map_io.cpp | 2 +- smac_planner/include/smac_planner/smoother.hpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/nav2_map_server/src/map_io.cpp b/nav2_map_server/src/map_io.cpp index 10e84ca9b6..73aa121fff 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) /* Replace all "\" with "/" */ char * c = path; while (*c != '\0') { - if (*c == '\\') *c = '/'; + if (*c == '\\') {*c = '/';} ++c; } diff --git a/smac_planner/include/smac_planner/smoother.hpp b/smac_planner/include/smac_planner/smoother.hpp index f81d94720c..d72d2b2a11 100644 --- a/smac_planner/include/smac_planner/smoother.hpp +++ b/smac_planner/include/smac_planner/smoother.hpp @@ -106,8 +106,8 @@ class Smoother _options.max_solver_time_in_seconds = params.max_time; #ifdef _MSC_VER - std::vector parametersVec(path.size() * 2); - double* parameters = parametersVec.data(); + std::vector parameters_vec(path.size() * 2); + double * parameters = parameters_vec.data(); #else double parameters[path.size() * 2]; // NOLINT #endif