Merge pull request #10 from lge-ros2/foxy-devel

Foxy devel

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;
 

nav2_regulated_pure_pursuit_controller/CMakeLists.txt

@@ -13,7 +13,6 @@ find_package(pluginlib REQUIRED)
 find_package(tf2 REQUIRED)
 
 nav2_package()
-set(CMAKE_CXX_STANDARD 17)
 
 include_directories(
   include

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

@@ -30,6 +30,12 @@ using nav2_util::declare_parameter_if_not_declared;
 using nav2_util::geometry_utils::euclidean_distance;
 using namespace nav2_costmap_2d;  // NOLINT
 
+double clamp(double value, double min, double max) {
+  if (value < min) return min;
+  if (value > max) return max;
+  return value;
+}
+
 namespace nav2_regulated_pure_pursuit_controller
 {
 
@@ -213,7 +219,7 @@ double RegulatedPurePursuitController::getLookAheadDistance(const geometry_msgs:
   double lookahead_dist = lookahead_dist_;
   if (use_velocity_scaled_lookahead_dist_) {
     lookahead_dist = speed.linear.x * lookahead_time_;
-    lookahead_dist = std::clamp(lookahead_dist, min_lookahead_dist_, max_lookahead_dist_);
+    lookahead_dist = clamp(lookahead_dist, min_lookahead_dist_, max_lookahead_dist_);
   }
 
   return lookahead_dist;
@@ -305,7 +311,7 @@ void RegulatedPurePursuitController::rotateToHeading(
   const double & dt = control_duration_;
   const double min_feasible_angular_speed = curr_speed.angular.z - max_angular_accel_ * dt;
   const double max_feasible_angular_speed = curr_speed.angular.z + max_angular_accel_ * dt;
-  angular_vel = std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed);
+  angular_vel = clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed);
 }
 
 geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoint(
@@ -346,7 +352,7 @@ bool RegulatedPurePursuitController::isCollisionImminent(
   pose_msg.header.frame_id = arc_pts_msg.header.frame_id;
   pose_msg.header.stamp = arc_pts_msg.header.stamp;
 
-  const double projection_time = costmap_->getResolution() / linear_vel;
+  const double projection_time = costmap_->getResolution() / fabs(linear_vel);
 
   geometry_msgs::msg::Pose2D curr_pose;
   curr_pose.x = robot_pose.pose.position.x;
@@ -461,8 +467,8 @@ void RegulatedPurePursuitController::applyConstraints(
   double & dt = control_duration_;
   const double max_feasible_linear_speed = curr_speed.linear.x + max_linear_accel_ * dt;
   const double min_feasible_linear_speed = curr_speed.linear.x - max_linear_decel_ * dt;
-  linear_vel = std::clamp(linear_vel, min_feasible_linear_speed, max_feasible_linear_speed);
-  linear_vel = std::clamp(linear_vel, 0.0, desired_linear_vel_);
+  linear_vel = clamp(linear_vel, min_feasible_linear_speed, max_feasible_linear_speed);
+  linear_vel = clamp(linear_vel, 0.0, desired_linear_vel_);
 }
 
 void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & path)

nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp

@@ -17,6 +17,7 @@
 
 #include <QtWidgets>
 #include <QBasicTimer>
+#undef NO_ERROR
 
 #include <memory>
 #include <string>

nav2_util/src/dump_params.cpp

@@ -14,7 +14,9 @@
 
 #include <iomanip>
 #include <iostream>
+#ifndef _WIN32
 #include <libgen.h>
+#endif
 #include <memory>
 #include <sstream>
 #include <string>

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 <windows.h>
+#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);

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)
 

smac_planner/include/smac_planner/smoother.hpp

@@ -105,8 +105,13 @@ class Smoother
   {
     _options.max_solver_time_in_seconds = params.max_time;
 
+#ifdef _MSC_VER
+    std::vector<double> parameters_vec(path.size() * 2);
+    double * parameters = parameters_vec.data();
+#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];
     }
@@ -123,7 +128,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];
     }

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;

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;

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;

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;