Merge branch 'master' of https://github.com/ros-planning/navigation2 into bump_013

SteveMacenski · SteveMacenski · commit 1c845b9761a2 · 2018-12-10T14:04:42.000-08:00
diff --git a/README.md b/README.md
@@ -2,7 +2,9 @@
 
 ROS2 Navigation System
 
-[![Build Status](https://travis-ci.org/ros-planning/navigation2.svg?branch=master)](https://travis-ci.org/ros-planning/navigation2)
+Internal Travis CI: [![Build Status](https://travis-ci.org/ros-planning/navigation2.svg?branch=master)](https://travis-ci.org/ros-planning/navigation2)
+
+ROS Build Farm PR Builder: [![Build Status](http://build.ros2.org/job/Cdev__navigation2__ubuntu_bionic_amd64/badge/icon)](http://build.ros2.org/job/Cdev__navigation2__ubuntu_bionic_amd64/)
 
 # Overview
 The ROS 2 Navigation System is the control system that enables a robot to autonomously reach a goal state, such as a specific position and orientation relative to a specific map. Given a current pose, a map, and a goal, such as a destination pose, the navigation system generates a plan to reach the goal, and outputs commands to autonomously drive the robot, respecting any safety constraints and avoiding obstacles encountered along the way.
diff --git a/nav2_bringup/launch/nav2_params.yaml b/nav2_bringup/launch/nav2_params.yaml
@@ -21,8 +21,22 @@ DwbController:
 local_costmap:
   local_costmap:
     ros__parameters:
+      obstacle_layer:
+        enabled: False
       always_send_full_costmap: True
+      observation_sources: scan
+      scan:
+        topic: /scan
+        max_obstacle_height: 2.0
+        clearing: True
 global_costmap:
   global_costmap:
     ros__parameters:
+      obstacle_layer:
+        enabled: False
       always_send_full_costmap: True
+      observation_sources: scan
+      scan:
+        topic: /scan
+        max_obstacle_height: 2.0
+        clearing: True
diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp
@@ -110,7 +110,7 @@ void ObstacleLayer::onInitialize()
     node_->get_parameter_or(source + "." + "min_obstacle_height", min_obstacle_height, 0.0);
     node_->get_parameter_or(source + "." + "max_obstacle_height", max_obstacle_height, 0.0);
     node_->get_parameter_or(source + "." + "inf_is_valid", inf_is_valid, false);
-    node_->get_parameter_or(source + "." + "marking", marking, false);
+    node_->get_parameter_or(source + "." + "marking", marking, true);
     node_->get_parameter_or(source + "." + "clearing", clearing, false);
 
     if (!(data_type == "PointCloud2" || data_type == "LaserScan")) {
@@ -120,15 +120,13 @@ void ObstacleLayer::onInitialize()
               "Only topics that use point cloud2s or laser scans are currently supported");
     }
 
-    std::string raytrace_range_param_name, obstacle_range_param_name;
-
     // get the obstacle range for the sensor
     double obstacle_range;
     node_->get_parameter_or(source + "." + "obstacle_range", obstacle_range, 2.5);
 
     // get the raytrace range for the sensor
     double raytrace_range;
-    node_->get_parameter_or(source + "." + "raytrace_range", obstacle_range, 3.0);
+    node_->get_parameter_or(source + "." + "raytrace_range", raytrace_range, 3.0);
 
     RCLCPP_DEBUG(node_->get_logger(),
       "Creating an observation buffer for source %s, topic %s, frame %s",
diff --git a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt
@@ -18,6 +18,7 @@ find_package(nav_2d_utils REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(nav2_util REQUIRED)
+find_package(nav2_dynamic_params REQUIRED)
 
 set(dependencies
   angles
@@ -27,6 +28,7 @@ set(dependencies
   pluginlib
   rclcpp
   nav2_util
+  nav2_dynamic_params
 )
 
 include_directories(
diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp
@@ -37,6 +37,7 @@
 
 #include <memory>
 #include "rclcpp/rclcpp.hpp"
+#include "nav2_dynamic_params/dynamic_params_client.hpp"
 
 namespace dwb_plugins
 {
@@ -104,7 +105,7 @@ class KinematicParameters
   double min_speed_xy_sq_, max_speed_xy_sq_;
 
   void reconfigureCB();
-  // std::shared_ptr<dynamic_reconfigure::Server<KinematicParamsConfig> > dsrv_;
+  std::unique_ptr<nav2_dynamic_params::DynamicParamsClient> dsrv_;
 };
 
 }  // namespace dwb_plugins
diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml
@@ -20,6 +20,7 @@
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>nav2_util</depend>
+  <depend>nav2_dynamic_params</depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
diff --git a/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp b/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp
@@ -85,46 +85,47 @@ void KinematicParameters::initialize(const std::shared_ptr<rclcpp::Node> & nh)
   setDecelerationAsNeeded(nh, "y");
   setDecelerationAsNeeded(nh, "theta");
 
-  // TODO(crdelsey): Handle dynamic params
-  // the rest of the initial values are loaded through the dynamic reconfigure mechanisms
-  // dsrv_ = std::make_shared<dynamic_reconfigure::Server<KinematicParamsConfig> >(nh);
-  // dynamic_reconfigure::Server<KinematicParamsConfig>::CallbackType cb =
-  //   boost::bind(&KinematicParameters::reconfigureCB, this, _1, _2);
-  reconfigureCB();
+  dsrv_ = std::make_unique<nav2_dynamic_params::DynamicParamsClient>(nh);
+  dsrv_->add_parameters({
+      "min_vel_x",
+      "min_vel_y",
+      "max_vel_x",
+      "max_vel_y",
+      "max_vel_theta",
+      "min_speed_xy",
+      "max_speed_xy",
+      "min_speed_theta",
+      "acc_lim_x",
+      "acc_lim_y",
+      "acc_lim_theta",
+      "decel_lim_x",
+      "decel_lim_y",
+      "decel_lim_theta"
+    });
+  dsrv_->set_callback([this]() {reconfigureCB();});
 }
 
+#define UPDATE_PARAMETER(name) dsrv_->get_event_param_or(#name, name ## _, 0.0)
 void KinematicParameters::reconfigureCB()
 {
-  // TODO(crdelsey): Remove hard coded parameters when dynamic_reconfigure is in
-  min_vel_x_ = -0.26;
-  max_vel_x_ = 0.26;
-  max_vel_theta_ = 1.0;
-
-  max_speed_xy_ = max_vel_x_;
-
-  acc_lim_x_ = 2.5;
-  acc_lim_theta_ = 3.2;
-  decel_lim_x_ = -acc_lim_x_;
-  decel_lim_theta_ = -acc_lim_theta_;
-
-  // min_vel_x_ = config.min_vel_x;
-  // min_vel_y_ = config.min_vel_y;
-  // max_vel_x_ = config.max_vel_x;
-  // max_vel_y_ = config.max_vel_y;
-  // max_vel_theta_ = config.max_vel_theta;
-  //
-  // min_speed_xy_ = config.min_speed_xy;
-  // max_speed_xy_ = config.max_speed_xy;
+  UPDATE_PARAMETER(min_vel_x);
+  UPDATE_PARAMETER(min_vel_y);
+  UPDATE_PARAMETER(max_vel_x);
+  UPDATE_PARAMETER(max_vel_y);
+  UPDATE_PARAMETER(max_vel_theta);
+
+  UPDATE_PARAMETER(min_speed_xy);
+  UPDATE_PARAMETER(max_speed_xy);
   min_speed_xy_sq_ = min_speed_xy_ * min_speed_xy_;
   max_speed_xy_sq_ = max_speed_xy_ * max_speed_xy_;
-  // min_speed_theta_ = config.min_speed_theta;
-  //
-  // acc_lim_x_ = config.acc_lim_x;
-  // acc_lim_y_ = config.acc_lim_y;
-  // acc_lim_theta_ = config.acc_lim_theta;
-  // decel_lim_x_ = config.decel_lim_x;
-  // decel_lim_y_ = config.decel_lim_y;
-  // decel_lim_theta_ = config.decel_lim_theta;
+  UPDATE_PARAMETER(min_speed_theta);
+
+  UPDATE_PARAMETER(acc_lim_x);
+  UPDATE_PARAMETER(acc_lim_y);
+  UPDATE_PARAMETER(acc_lim_theta);
+  UPDATE_PARAMETER(decel_lim_x);
+  UPDATE_PARAMETER(decel_lim_y);
+  UPDATE_PARAMETER(decel_lim_theta);
 }
 
 bool KinematicParameters::isValidSpeed(double x, double y, double theta)
diff --git a/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt b/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt
@@ -3,6 +3,5 @@ ament_add_gtest(vtest velocity_iterator_test.cpp)
 ament_add_gtest(goal_checker goal_checker.cpp)
 target_link_libraries(goal_checker simple_goal_checker stopped_goal_checker)
 
-# Re-enable when DWB reads kinematic parameters
-# ament_add_gtest(twist_gen_test twist_gen.cpp)
-# target_link_libraries(twist_gen_test standard_traj_generator)
+ament_add_gtest(twist_gen_test twist_gen.cpp)
+target_link_libraries(twist_gen_test standard_traj_generator)
diff --git a/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp b/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp
@@ -152,7 +152,7 @@ TEST(VelocityIterator, max_xy)
 TEST(VelocityIterator, min_xy)
 {
   auto nh = makeTestNode("min_xy");
-  nh->set_parameters({rclcpp::Parameter("min_speed_xy", -1)});
+  nh->set_parameters({rclcpp::Parameter("min_speed_xy", -1.0)});
   StandardTrajectoryGenerator gen;
   gen.initialize(nh);
   std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(zero);
@@ -164,7 +164,7 @@ TEST(VelocityIterator, min_xy)
 TEST(VelocityIterator, min_theta)
 {
   auto nh = makeTestNode("min_theta");
-  nh->set_parameters({rclcpp::Parameter("min_speed_theta", -1)});
+  nh->set_parameters({rclcpp::Parameter("min_speed_theta", -1.0)});
   StandardTrajectoryGenerator gen;
   gen.initialize(nh);
   std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(zero);
@@ -177,8 +177,8 @@ TEST(VelocityIterator, no_limits)
 {
   auto nh = makeTestNode("no_limits");
   nh->set_parameters({rclcpp::Parameter("max_speed_xy", -1.0)});
-  nh->set_parameters({rclcpp::Parameter("min_speed_xy", -1)});
-  nh->set_parameters({rclcpp::Parameter("min_speed_theta", -1)});
+  nh->set_parameters({rclcpp::Parameter("min_speed_xy", -1.0)});
+  nh->set_parameters({rclcpp::Parameter("min_speed_theta", -1.0)});
   StandardTrajectoryGenerator gen;
   gen.initialize(nh);
   std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(zero);
@@ -191,8 +191,8 @@ TEST(VelocityIterator, no_limits_samples)
 {
   auto nh = makeTestNode("no_limits_samples");
   nh->set_parameters({rclcpp::Parameter("max_speed_xy", -1.0)});
-  nh->set_parameters({rclcpp::Parameter("min_speed_xy", -1)});
-  nh->set_parameters({rclcpp::Parameter("min_speed_theta", -1)});
+  nh->set_parameters({rclcpp::Parameter("min_speed_xy", -1.0)});
+  nh->set_parameters({rclcpp::Parameter("min_speed_theta", -1.0)});
   int x_samples = 10, y_samples = 3, theta_samples = 5;
   nh->set_parameters({rclcpp::Parameter("vx_samples", x_samples)});
   nh->set_parameters({rclcpp::Parameter("vy_samples", y_samples)});
@@ -224,7 +224,7 @@ TEST(VelocityIterator, dwa_gen)
 {
   auto nh = makeTestNode("dwa_gen");
   nh->set_parameters({rclcpp::Parameter("use_dwa", true)});
-  nh->set_parameters({rclcpp::Parameter("min_speed_theta", -1)});
+  nh->set_parameters({rclcpp::Parameter("min_speed_theta", -1.0)});
   dwb_plugins::LimitedAccelGenerator gen;
   gen.initialize(nh);
   std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(zero);
@@ -236,7 +236,7 @@ TEST(VelocityIterator, dwa_gen)
 TEST(VelocityIterator, dwa_gen_no_param)
 {
   auto nh = makeTestNode("dwa_gen_no_param");
-  nh->set_parameters({rclcpp::Parameter("min_speed_theta", -1)});
+  nh->set_parameters({rclcpp::Parameter("min_speed_theta", -1.0)});
   dwb_plugins::LimitedAccelGenerator gen;
   gen.initialize(nh);
   std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(zero);
@@ -248,7 +248,7 @@ TEST(VelocityIterator, nonzero)
 {
   auto nh = makeTestNode("nonzero");
   nh->set_parameters({rclcpp::Parameter("use_dwa", true)});
-  nh->set_parameters({rclcpp::Parameter("min_speed_theta", -1)});
+  nh->set_parameters({rclcpp::Parameter("min_speed_theta", -1.0)});
   dwb_plugins::LimitedAccelGenerator gen;
   gen.initialize(nh);
   nav_2d_msgs::msg::Twist2D initial;
@@ -431,7 +431,9 @@ TEST(TrajectoryGenerator, dwa)
 int main(int argc, char ** argv)
 {
   forward.x = 0.3;
-  rclcpp::init(argc, argv);
+  rclcpp::init(0, nullptr);
   testing::InitGoogleTest(&argc, argv);
-  return RUN_ALL_TESTS();
+  int ret = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+  return ret;
 }
diff --git a/nav2_map_server/CMakeLists.txt b/nav2_map_server/CMakeLists.txt
@@ -18,6 +18,7 @@ find_package(SDL REQUIRED)
 find_package(SDL_image REQUIRED)
 find_package(yaml_cpp_vendor REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(tf2 REQUIRED)
 
 include_directories(
   include
@@ -26,35 +27,51 @@ include_directories(
   ${SDL_IMAGE_INCLUDE_DIRS}
 )
 
-set(executable_name map_server)
+set(map_server_executable map_server)
 
-add_executable(${executable_name}
+set(map_saver_executable map_saver)
+
+add_executable(${map_server_executable}
   src/main.cpp
 )
 
-set(library_name ${executable_name}_core)
+add_executable(${map_saver_executable}
+  src/map_saver.cpp
+)
+
+set(library_name ${map_server_executable}_core)
 
 add_library(${library_name} SHARED
   src/occ_grid_loader.cpp
   src/map_server.cpp
 )
 
-set(dependencies
+set(map_server_dependencies
   rclcpp
   nav_msgs
   yaml_cpp_vendor
   std_msgs
 )
 
-ament_target_dependencies(${executable_name}
-  ${dependencies}
+set(map_saver_dependencies
+  rclcpp
+  nav_msgs
+  tf2
+)
+
+ament_target_dependencies(${map_server_executable}
+  ${map_server_dependencies}
+)
+
+ament_target_dependencies(${map_saver_executable}
+  ${map_saver_dependencies}
 )
 
 ament_target_dependencies(${library_name}
-  ${dependencies}
+  ${map_server_dependencies}
 )
 
-target_link_libraries(${executable_name}
+target_link_libraries(${map_server_executable}
   ${library_name}
 )
 
@@ -63,7 +80,8 @@ target_link_libraries(${library_name}
   ${SDL_LIBRARY}
   ${SDL_IMAGE_LIBRARIES}
 )
-install(TARGETS ${executable_name} ${library_name}
+
+install(TARGETS ${map_server_executable} ${library_name} ${map_saver_executable}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PROJECT_NAME}
diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml
@@ -20,6 +20,7 @@
   <depend>yaml_cpp_vendor</depend>
   <depend>launch_ros</depend>
   <depend>launch_testing</depend>
+  <depend>tf2</depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
diff --git a/nav2_map_server/src/map_saver.cpp b/nav2_map_server/src/map_saver.cpp
diff --git a/nav2_tasks/CMakeLists.txt b/nav2_tasks/CMakeLists.txt
diff --git a/nav2_tasks/include/nav2_tasks/is_stuck_condition.hpp b/nav2_tasks/include/nav2_tasks/is_stuck_condition.hpp
diff --git a/nav2_tasks/package.xml b/nav2_tasks/package.xml
