Merge pull request #7 from umdlife/feat/upgrade_nav2

UP-350 Upgrade nav2 to latest foxy version and add nav2_controllers

debian/create_debians.sh

@@ -52,7 +52,8 @@ PACKAGE_LIST=(
             navigation2/nav2_rviz_plugins \
             navigation2/nav2_amcl \
             navigation2/nav2_planner \
-            navigation2/nav2_navfn_planner
+            navigation2/nav2_navfn_planner \
+            navigation2/nav2_controller
 )
 
 for PACKAGE in ${PACKAGE_LIST[@]}; do

doc/parameters/param_list.md

@@ -5,7 +5,10 @@
 | Parameter | Default | Description |
 | ----------| --------| ------------|
 | default_bt_xml_filename | N/A | path to the default behavior tree XML description |
-| plugin_lib_names | ["nav2_compute_path_to_pose_action_bt_node", "nav2_follow_path_action_bt_node", "nav2_back_up_action_bt_node", "nav2_spin_action_bt_node", "nav2_wait_action_bt_node", "nav2_clear_costmap_service_bt_node", "nav2_is_stuck_condition_bt_node", "nav2_goal_reached_condition_bt_node", "nav2_initial_pose_received_condition_bt_node", "nav2_goal_updated_condition_bt_node", "nav2_reinitialize_global_localization_service_bt_node", "nav2_rate_controller_bt_node", "nav2_distance_controller_bt_node", "nav2_recovery_node_bt_node", "nav2_pipeline_sequence_bt_node", "nav2_round_robin_node_bt_node", "nav2_transform_available_condition_bt_node"] | list of behavior tree node shared libraries |
+| enable_groot_monitoring | True | enable Groot live monitoring of the behavior tree |
+| groot_zmq_publisher_port | 1666 | change port of the zmq publisher needed for groot |
+| groot_zmq_server_port | 1667 | change port of the zmq server needed for groot |
+| plugin_lib_names | ["nav2_compute_path_to_pose_action_bt_node", "nav2_follow_path_action_bt_node", "nav2_back_up_action_bt_node", "nav2_spin_action_bt_node", "nav2_wait_action_bt_node", "nav2_clear_costmap_service_bt_node", "nav2_is_stuck_condition_bt_node", "nav2_goal_reached_condition_bt_node", "nav2_initial_pose_received_condition_bt_node", "nav2_goal_updated_condition_bt_node", "nav2_reinitialize_global_localization_service_bt_node", "nav2_rate_controller_bt_node", "nav2_distance_controller_bt_node", "nav2_speed_controller_bt_node", "nav2_truncate_path_action_bt_node", "nav2_recovery_node_bt_node", "nav2_pipeline_sequence_bt_node", "nav2_round_robin_node_bt_node", "nav2_transform_available_condition_bt_node", "nav2_time_expired_condition_bt_node", "nav2_distance_traveled_condition_bt_node", "nav2_rotate_action_bt_node", "nav2_translate_action_bt_node", "nav2_is_battery_low_condition_bt_node", "nav2_goal_updater_node_bt_node", "nav2_navigate_to_pose_action_bt_node"] | list of behavior tree node shared libraries |
 | transform_tolerance | 0.1 | TF transform tolerance |
 | global_frame | "map" | Reference frame |
 | robot_base_frame | "base_link" | Robot base frame |
@@ -166,7 +169,7 @@ When `plugins` parameter is not overridden, the following default plugins are lo
 | `<data source>`.marking | true | Whether source should mark in costmap |
 | `<data source>`.clearing | false | Whether source should raytrace clear in costmap |
 | `<data source>`.obstacle_range | 2.5 | Maximum range to mark obstacles in costmap |
-| `<data source>`.raytrace_range | 3.0 | Maximum range to raytrace clear obstacles from costmap | 
+| `<data source>`.raytrace_range | 3.0 | Maximum range to raytrace clear obstacles from costmap |
 
 # controller_server
 
@@ -237,6 +240,7 @@ When `controller_plugins`\`progress_checker_plugin`\`goal_checker_plugin` parame
 | `<dwb plugin>`.critics | N/A | List of critic plugins to use |
 | `<dwb plugin>`.default_critic_namespaces | ["dwb_critics"] | Namespaces to load critics in |
 | `<dwb plugin>`.prune_plan | true | Whether to prune the path of old, passed points |
+| `<dwb plugin>`.shorten_transformed_plan | true | Determines whether we will pass the full plan on to the critics |
 | `<dwb plugin>`.prune_distance | 1.0 | Distance (m) to prune backward until |
 | `<dwb plugin>`.debug_trajectory_details | false | Publish debug information |
 | `<dwb plugin>`.trajectory_generator_name | "dwb_plugins::StandardTrajectoryGenerator" | Trajectory generator plugin name |
@@ -276,7 +280,7 @@ When `controller_plugins`\`progress_checker_plugin`\`goal_checker_plugin` parame
 | `<dwb plugin>`.`<name>`.x_only_threshold | 0.05 | Threshold to check in the X velocity direction |
 | `<dwb plugin>`.`<name>`.scale | 1.0 | Weighed scale for critic |
 
-## kinematic_parameters 
+## kinematic_parameters
 
 | Parameter | Default | Description |
 | ----------| --------| ------------|
@@ -295,7 +299,7 @@ When `controller_plugins`\`progress_checker_plugin`\`goal_checker_plugin` parame
 | `<dwb plugin>`.decel_lim_y | 0.0 | Maximum deceleration Y (m/s^2) |
 | `<dwb plugin>`.decel_lim_theta | 0.0 | Maximum deceleration rotation (rad/s^2) |
 
-## xy_theta_iterator 
+## xy_theta_iterator
 
 | Parameter | Default | Description |
 | ----------| --------| ------------|
@@ -473,6 +477,46 @@ When `planner_plugins` parameter is not overridden, the following default plugin
 | `<name>`.use_astar | false | Whether to use A*, if false, uses Dijstra's expansion |
 | `<name>`.allow_unknown | true | Whether to allow planning in unknown space |
 
+# smac_planner
+
+* `<name>`: Corresponding planner plugin ID for this type
+
+| Parameter | Default | Description |
+| ----------| --------| ------------|
+| `<name>`.tolerance  | 0.5 | Tolerance in meters between requested goal pose and end of path |
+| `<name>`.downsample_costmap | false | Whether to downsample costmap |
+| `<name>`.downsampling_factor | 1 | Factor to downsample costmap by |
+| `<name>`.allow_unknown | true | whether to allow traversing in unknown space |
+| `<name>`.max_iterations | -1 | Number of iterations before failing, disabled by -1 |
+| `<name>`.max_on_approach_iterations | 1000 | Iterations after within threshold before returning approximate path with best heuristic |
+| `<name>`.max_planning_time_ms | 5000 | Maximum planning time in ms |
+| `<name>`.smooth_path | false | Whether to smooth path with CG smoother |
+| `<name>`.motion_model_for_search | DUBIN | Motion model to search with. Options for SE2: DUBIN, REEDS_SHEPP. 2D: MOORE, VON_NEUMANN |
+| `<name>`.angle_quantization_bins | 1 | Number of angle quantization bins for SE2 node |
+| `<name>`.minimum_turning_radius | 0.20 | Minimum turning radius in m of vehicle or desired path |
+| `<name>`.reverse_penalty | 2.0 | Penalty to apply to SE2 node if in reverse direction |
+| `<name>`.change_penalty | 0.5 | Penalty to apply to SE2 node if changing direction |
+| `<name>`.non_straight_penalty | 1.1 | Penalty to apply to SE2 node if non-straight direction |
+| `<name>`.cost_penalty | 1.2 | Penalty to apply to SE2 node for cost at pose |
+| `<name>`.analytic_expansion_ratio | 2.0 | For SE2 nodes the planner will attempt an analytic path expansion every N iterations, where N = closest_distance_to_goal / analytic_expansion_ratio. Higher ratios result in more frequent expansions |
+| `<name>`.smoother.smoother.w_curve | 1.5 | Smoother weight on curvature of path |
+| `<name>`.smoother.smoother.w_dist | 0.0 | Smoother weight on distance from original path |
+| `<name>`.smoother.smoother.w_smooth | 15000 | Smoother weight on distance between nodes |
+| `<name>`.smoother.smoother.w_cost | 1.5 | Smoother weight on costmap cost |
+| `<name>`.smoother.smoother.cost_scaling_factor | 10.0 | Inflation layer's scale factor |
+| `<name>`.smoother.optimizer.max_time | 0.10 | Maximum time to spend smoothing, in seconds |
+| `<name>`.smoother.optimizer.max_iterations | 500 | Maximum number of iterations to spend smoothing |
+| `<name>`.smoother.optimizer.debug_optimizer | false | Whether to print debug info from Ceres |
+| `<name>`.smoother.optimizer.gradient_tol | 1e-10 | Minimum change in gradient to terminate smoothing |
+| `<name>`.smoother.optimizer.fn_tol | 1e-7 | Minimum change in function to terminate smoothing |
+| `<name>`.smoother.optimizer.param_tol | 1e-15 | Minimum change in parameter block to terminate smoothing |
+
+| `<name>`.smoother.optimizer.advanced.min_line_search_step_size | 1e-20 | Terminate smoothing iteration if change in parameter block less than this |
+| `<name>`.smoother.optimizer.advanced.max_num_line_search_step_size_iterations | 50 | Maximum iterations for line search |
+| `<name>`.smoother.optimizer.advanced.line_search_sufficient_function_decrease | 1e-20 | Function decrease amount to terminate current line search iteration |
+| `<name>`.smoother.optimizer.advanced.max_num_line_search_direction_restarts | 10 | Maximum umber of restarts of line search when over-estimating |
+| `<name>`.smoother.optimizer.advanced.max_line_search_step_expansion | 50 | Step size multiplier at each iteration of line search |
+
 # waypoint_follower
 
 | Parameter | Default | Description |
@@ -667,6 +711,14 @@ When `recovery_plugins` parameter is not overridden, the following default plugi
 
 ## Conditions
 
+### BT Node DistanceTraveled
+
+| Input Port | Default | Description |
+| ---------- | ------- | ----------- |
+| distance | 1.0 | Distance in meters after which the node should return success |
+| global_frame | "map" | Reference frame |
+| robot_base_frame | "base_link" | Robot base frame |
+
 ### BT Node GoalReached
 
 | Input Port | Default | Description |
@@ -675,7 +727,21 @@ When `recovery_plugins` parameter is not overridden, the following default plugi
 | global_frame | "map" | Reference frame |
 | robot_base_frame | "base_link" | Robot base frame |
 
-### BT Node TransformAvailable (condition)
+### BT Node IsBatteryLow
+
+| Input Port | Default | Description |
+| ---------- | ------- | ----------- |
+| min_battery | N/A | Minimum battery percentage/voltage |
+| battery_topic | "/battery_status" | Battery topic |
+| is_voltage | false | If true voltage will be used to check for low battery |
+
+### BT Node TimeExpired
+
+| Input Port | Default | Description |
+| ---------- | ------- | ----------- |
+| seconds | 1.0 | Number of seconds after which node returns success |
+
+### BT Node TransformAvailable
 
 | Input Port | Default | Description |
 | ---------- | ------- | ----------- |

minimal_repos.txt

@@ -12,4 +12,5 @@ nav2_map_server
 nav2_amcl
 nav2_rviz_plugins
 nav2_navfn_planner
-geographic_msgs
+geographic_msgs
+nav2_controller

nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -160,6 +160,7 @@ class AmclNode : public nav2_util::LifecycleNode
   // Pose-generating function used to uniformly distribute particles over the map
   static pf_vector_t uniformPoseGenerator(void * arg);
   pf_t * pf_{nullptr};
+  std::mutex pf_mutex_;
   bool pf_init_;
   pf_vector_t pf_odom_pose_;
   int resample_count_{0};

nav2_amcl/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_amcl</name>
-  <version>0.4.3</version>
+  <version>0.4.7</version>
   <description>
     <p>
       amcl is a probabilistic localization system for a robot moving in

nav2_amcl/src/amcl_node.cpp

@@ -401,7 +401,7 @@ AmclNode::checkLaserReceived()
     RCLCPP_WARN(
       get_logger(), "Laser scan has not been received"
       " (and thus no pose updates have been published)."
-      " Verify that data is being published on the %s topic.", scan_topic_);
+      " Verify that data is being published on the %s topic.", scan_topic_.c_str());
     return;
   }
 
@@ -410,7 +410,7 @@ AmclNode::checkLaserReceived()
     RCLCPP_WARN(
       get_logger(), "No laser scan received (and thus no pose updates have been published) for %f"
       " seconds.  Verify that data is being published on the %s topic.",
-      d.nanoseconds() * 1e-9, scan_topic_);
+      d.nanoseconds() * 1e-9, scan_topic_.c_str());
   }
 }
 
@@ -505,6 +505,8 @@ AmclNode::globalLocalizationCallback(
   const std::shared_ptr<std_srvs::srv::Empty::Request>/*req*/,
   std::shared_ptr<std_srvs::srv::Empty::Response>/*res*/)
 {
+  std::lock_guard<std::mutex> lock(pf_mutex_);
+
   RCLCPP_INFO(get_logger(), "Initializing with uniform distribution");
 
   pf_init_model(
@@ -529,6 +531,8 @@ AmclNode::nomotionUpdateCallback(
 void
 AmclNode::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
 {
+  std::lock_guard<std::mutex> lock(pf_mutex_);
+
   RCLCPP_INFO(get_logger(), "initialPoseReceived");
 
   if (msg->header.frame_id == "") {
@@ -626,6 +630,8 @@ AmclNode::handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped & msg)
 void
 AmclNode::laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan)
 {
+  std::lock_guard<std::mutex> lock(pf_mutex_);
+
   // Since the sensor data is continually being published by the simulator or robot,
   // we don't want our callbacks to fire until we're in the active state
   if (!active_) {return;}
@@ -1246,7 +1252,7 @@ AmclNode::initMessageFilters()
     rclcpp_node_.get(), scan_topic_, rmw_qos_profile_sensor_data);
 
   laser_scan_filter_ = std::make_unique<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>>(
-    *laser_scan_sub_, *tf_buffer_, odom_frame_id_, 10, rclcpp_node_);
+    *laser_scan_sub_, *tf_buffer_, odom_frame_id_, 10, rclcpp_node_, transform_tolerance_);
 
   laser_scan_connection_ = laser_scan_filter_->registerCallback(
     std::bind(

nav2_behavior_tree/CMakeLists.txt

@@ -6,9 +6,9 @@ find_package(nav2_common REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
-find_package(std_msgs REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
 find_package(nav2_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(behaviortree_cpp_v3 REQUIRED)
@@ -31,6 +31,7 @@ set(dependencies
   rclcpp_action
   rclcpp_lifecycle
   geometry_msgs
+  sensor_msgs
   nav2_msgs
   nav_msgs
   behaviortree_cpp_v3
@@ -89,6 +90,9 @@ list(APPEND plugin_libs nav2_distance_traveled_condition_bt_node)
 add_library(nav2_initial_pose_received_condition_bt_node SHARED plugins/condition/initial_pose_received_condition.cpp)
 list(APPEND plugin_libs nav2_initial_pose_received_condition_bt_node)
 
+add_library(nav2_is_battery_low_condition_bt_node SHARED plugins/condition/is_battery_low_condition.cpp)
+list(APPEND plugin_libs nav2_is_battery_low_condition_bt_node)
+
 add_library(nav2_reinitialize_global_localization_service_bt_node SHARED plugins/action/reinitialize_global_localization_service.cpp)
 list(APPEND plugin_libs nav2_reinitialize_global_localization_service_bt_node)
 

nav2_behavior_tree/README.md

@@ -72,6 +72,7 @@ The nav2_behavior_tree package provides several navigation-specific nodes that a
 | IsStuck | Condition | Determines if the robot is not progressing towards the goal. If the robot is stuck and not progressing, the condition returns SUCCESS, otherwise it returns FAILURE. |
 | TransformAvailable | Condition | Checks if a TF transform is available. Returns failure if it cannot be found. Once found, it will always return success. Useful for initial condition checks. |
 | GoalUpdated | Condition | Checks if the global navigation goal has changed in the blackboard. Returns failure if the goal is the same, if it changes, it returns success. |
+| IsBatteryLow | Condition | Checks if battery is low by subscribing to a sensor_msgs/BatteryState topic and checking if battery voltage/percentage is below a specified minimum value. |
 | NavigateToPose | Action | Invokes the NavigateToPose ROS2 action server, which is implemented by the bt_navigator module. |
 | RateController | Decorator | A node that throttles the tick rate for its child. The tick rate can be supplied to the node as a parameter. The node returns RUNNING when it is not ticking its child. Currently, in the navigation stack, the `RateController` is used to adjust the rate at which the `ComputePathToPose` and `GoalReached` nodes are ticked. |
 | DistanceController | Decorator | A node that controls the tick rate for its child based on the distance traveled. The distance to be traveled before replanning can be supplied to the node as a parameter. The node returns RUNNING when it is not ticking its child. Currently, in the navigation stack, the `DistanceController` is used to adjust the rate at which the `ComputePathToPose` and `GoalReached` nodes are ticked. |

nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp

@@ -1,4 +1,5 @@
 // Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Florian Gramss
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -22,6 +23,8 @@
 #include "behaviortree_cpp_v3/behavior_tree.h"
 #include "behaviortree_cpp_v3/bt_factory.h"
 #include "behaviortree_cpp_v3/xml_parsing.h"
+#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h"
+
 
 namespace nav2_behavior_tree
 {
@@ -40,28 +43,29 @@ class BehaviorTreeEngine
     std::function<bool()> cancelRequested,
     std::chrono::milliseconds loopTimeout = std::chrono::milliseconds(10));
 
-  BT::Tree buildTreeFromText(
+  BT::Tree createTreeFromText(
     const std::string & xml_string,
     BT::Blackboard::Ptr blackboard);
 
+  BT::Tree createTreeFromFile(
+    const std::string & file_path,
+    BT::Blackboard::Ptr blackboard);
+
+  void addGrootMonitoring(
+    BT::Tree * tree,
+    uint16_t publisher_port,
+    uint16_t server_port,
+    uint16_t max_msg_per_second = 25);
+
+  void resetGrootMonitor();
+
   // In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state
-  void haltAllActions(BT::TreeNode * root_node)
-  {
-    // this halt signal should propagate through the entire tree.
-    root_node->halt();
-
-    // but, just in case...
-    auto visitor = [](BT::TreeNode * node) {
-        if (node->status() == BT::NodeStatus::RUNNING) {
-          node->halt();
-        }
-      };
-    BT::applyRecursiveVisitor(root_node, visitor);
-  }
+  void haltAllActions(BT::TreeNode * root_node);
 
 protected:
   // The factory that will be used to dynamically construct the behavior tree
   BT::BehaviorTreeFactory factory_;
+  std::unique_ptr<BT::PublisherZMQ> groot_monitor_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

@@ -40,6 +40,10 @@ class BtActionNode : public BT::ActionNodeBase
     std::string node_namespace;
     node_namespace = node_->get_namespace();
 
+    // Get the required items from the blackboard
+    server_timeout_ =
+      config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
+    getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
     // Initialize the input and output messages
     goal_ = typename ActionT::Goal();
     result_ = typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult();
@@ -185,7 +189,7 @@ class BtActionNode : public BT::ActionNodeBase
   {
     if (should_cancel_goal()) {
       auto future_cancel = action_client_->async_cancel_goal(goal_handle_);
-      if (rclcpp::spin_until_future_complete(node_, future_cancel) !=
+      if (rclcpp::spin_until_future_complete(node_, future_cancel, server_timeout_) !=
         rclcpp::FutureReturnCode::SUCCESS)
       {
         RCLCPP_ERROR(
@@ -231,8 +235,8 @@ class BtActionNode : public BT::ActionNodeBase
 
     auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options);
 
-    if (rclcpp::spin_until_future_complete(node_, future_goal_handle) !=
-      rclcpp::executor::FutureReturnCode::SUCCESS)
+    if (rclcpp::spin_until_future_complete(node_, future_goal_handle, server_timeout_) !=
+      rclcpp::FutureReturnCode::SUCCESS)
     {
       throw std::runtime_error("send_goal failed");
     }
@@ -246,9 +250,9 @@ class BtActionNode : public BT::ActionNodeBase
   void increment_recovery_count()
   {
     int recovery_count = 0;
-    config().blackboard->get<int>("number_recoveries", recovery_count);  // NOLINT
+    config().blackboard->template get<int>("number_recoveries", recovery_count);  // NOLINT
     recovery_count += 1;
-    config().blackboard->set<int>("number_recoveries", recovery_count);  // NOLINT
+    config().blackboard->template set<int>("number_recoveries", recovery_count);  // NOLINT
   }
 
   std::string action_name_;
@@ -263,6 +267,10 @@ class BtActionNode : public BT::ActionNodeBase
 
   // The node that will be used for any ROS operations
   rclcpp::Node::SharedPtr node_;
+
+  // The timeout value while waiting for response from a server when a
+  // new action goal is sent or canceled
+  std::chrono::milliseconds server_timeout_;
 };
 
 }  // namespace nav2_behavior_tree