Merge pull request ros-navigation#3 from botsandus/AUTO-408_add_mppic

AUTO-408 Add mppic
doisyg · Jan 9, 2023 · bf708be · bf708be
2 parents 4fe52c8 + 2946804
commit bf708be
Show file tree

Hide file tree

Showing 117 changed files with 9,520 additions and 375 deletions.
diff --git a/.circleci/config.yml b/.circleci/config.yml
@@ -33,12 +33,12 @@ _commands:
         - restore_cache:
             name: Restore Cache << parameters.key >>
             keys:
-              - "<< parameters.key >>-v12\
+              - "<< parameters.key >>-v13\
                 -{{ arch }}\
                 -{{ .Branch }}\
                 -{{ .Environment.CIRCLE_PR_NUMBER }}\
                 -{{ checksum  \"<< parameters.workspace >>/lockfile.txt\" }}"
-              - "<< parameters.key >>-v12\
+              - "<< parameters.key >>-v13\
                 -{{ arch }}\
                 -main\
                 -<no value>\
@@ -58,7 +58,7 @@ _commands:
       steps:
         - save_cache:
             name: Save Cache << parameters.key >>
-            key: "<< parameters.key >>-v12\
+            key: "<< parameters.key >>-v13\
               -{{ arch }}\
               -{{ .Branch }}\
               -{{ .Environment.CIRCLE_PR_NUMBER }}\
@@ -442,7 +442,7 @@ _environments:
     UNDERLAY_MIXINS: "release ccache lld"
     CCACHE_LOGFILE: "/tmp/ccache.log"
     CCACHE_MAXSIZE: "200M"
-    MAKEFLAGS: "-j 2 -l 2 "
+    MAKEFLAGS: "-j 4 -l 4 "
     COLCON_DEFAULTS_FILE: "/tmp/defaults.yaml"
     RCUTILS_LOGGING_BUFFERED_STREAM: "0"
     RCUTILS_LOGGING_USE_STDOUT: "0"
@@ -453,6 +453,7 @@ executors:
   release_exec:
     docker:
       - image: ghcr.io/ros-planning/navigation2:main
+    resource_class: large
     working_directory: /opt/overlay_ws
     environment:
       <<: *common_environment

diff --git a/.circleci/defaults.yaml b/.circleci/defaults.yaml
@@ -6,7 +6,7 @@ _common: &common
 "build":
   <<: *common
   "executor": "parallel"
-  "parallel-workers": 2
+  "parallel-workers": 4
   "symlink-install": true
 "test":
   <<: *common

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
@@ -82,6 +82,9 @@ bool BtActionServer<ActionT>::on_configure()
       "-r",
       std::string("__node:=") +
       std::string(node->get_name()) + "_" + client_node_name + "_rclcpp_node",
+      "-p",
+      "use_sim_time:=" +
+      std::string(node->get_parameter("use_sim_time").as_bool() ? "true" : "false"),
       "--"});
 
   // Support for handling the topic-based goal pose from rviz

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp
@@ -29,6 +29,10 @@ namespace nav2_behavior_tree
  */
 class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
 {
+  using Action = nav2_msgs::action::FollowPath;
+  using ActionResult = Action::Result;
+  using ActionGoal = Action::Goal;
+
 public:
   /**
    * @brief A constructor for nav2_behavior_tree::FollowPathAction
@@ -46,13 +50,28 @@ class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
    */
   void on_tick() override;
 
+  /**
+   * @brief Function to perform some user-defined operation upon successful completion of the action
+   */
+  BT::NodeStatus on_success() override;
+
+  /**
+   * @brief Function to perform some user-defined operation upon abortion of the action
+   */
+  BT::NodeStatus on_aborted() override;
+
+  /**
+   * @brief Function to perform some user-defined operation upon cancellation of the action
+   */
+  BT::NodeStatus on_cancelled() override;
+
   /**
    * @brief Function to perform some user-defined operation after a timeout
    * waiting for a result that hasn't been received yet
    * @param feedback shared_ptr to latest feedback message
    */
   void on_wait_for_result(
-    std::shared_ptr<const nav2_msgs::action::FollowPath::Feedback> feedback) override;
+    std::shared_ptr<const Action::Feedback> feedback) override;
 
   /**
    * @brief Creates list of BT ports
@@ -65,6 +84,8 @@ class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
         BT::InputPort<nav_msgs::msg::Path>("path", "Path to follow"),
         BT::InputPort<std::string>("controller_id", ""),
         BT::InputPort<std::string>("goal_checker_id", ""),
+        BT::OutputPort<ActionResult::_error_code_type>(
+          "follow_path_error_code", "The follow path error code"),
       });
   }
 };

diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml
@@ -112,6 +112,7 @@
       <input_port name="goal_checker_id" default="GoalChecker">Goal checker</input_port>
       <input_port name="service_name">Service name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
+      <output_port name="follow_path_error_code">Follow Path error code</output_port>
     </Action>
 
     <Action ID="NavigateToPose">

diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp
@@ -24,7 +24,7 @@ FollowPathAction::FollowPathAction(
   const std::string & xml_tag_name,
   const std::string & action_name,
   const BT::NodeConfiguration & conf)
-: BtActionNode<nav2_msgs::action::FollowPath>(xml_tag_name, action_name, conf)
+: BtActionNode<Action>(xml_tag_name, action_name, conf)
 {
 }
 
@@ -35,8 +35,27 @@ void FollowPathAction::on_tick()
   getInput("goal_checker_id", goal_.goal_checker_id);
 }
 
+BT::NodeStatus FollowPathAction::on_success()
+{
+  setOutput("follow_path_error_code", ActionGoal::NONE);
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus FollowPathAction::on_aborted()
+{
+  setOutput("follow_path_error_code", result_.result->error_code);
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus FollowPathAction::on_cancelled()
+{
+  // Set empty error code, action was cancelled
+  setOutput("compute_path_to_pose_error_code", ActionGoal::NONE);
+  return BT::NodeStatus::SUCCESS;
+}
+
 void FollowPathAction::on_wait_for_result(
-  std::shared_ptr<const nav2_msgs::action::FollowPath::Feedback>/*feedback*/)
+  std::shared_ptr<const Action::Feedback>/*feedback*/)
 {
   // Grab the new path
   nav_msgs::msg::Path new_path;

diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py
@@ -55,7 +55,6 @@ def generate_launch_description():
 
     # Create our own temporary YAML files that include substitutions
     param_substitutions = {
-        'use_sim_time': use_sim_time,
         'yaml_filename': map_yaml_file}
 
     configured_params = RewrittenYaml(

diff --git a/nav2_bringup/launch/localization_launch.py b/nav2_bringup/launch/localization_launch.py
@@ -20,7 +20,7 @@
 from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable
 from launch.conditions import IfCondition
 from launch.substitutions import LaunchConfiguration, PythonExpression
-from launch_ros.actions import LoadComposableNodes
+from launch_ros.actions import LoadComposableNodes, SetParameter
 from launch_ros.actions import Node
 from launch_ros.descriptions import ComposableNode
 from nav2_common.launch import RewrittenYaml
@@ -53,7 +53,6 @@ def generate_launch_description():
 
     # Create our own temporary YAML files that include substitutions
     param_substitutions = {
-        'use_sim_time': use_sim_time,
         'yaml_filename': map_yaml_file}
 
     configured_params = RewrittenYaml(
@@ -107,6 +106,7 @@ def generate_launch_description():
     load_nodes = GroupAction(
         condition=IfCondition(PythonExpression(['not ', use_composition])),
         actions=[
+            SetParameter("use_sim_time", use_sim_time),
             Node(
                 package='nav2_map_server',
                 executable='map_server',
@@ -133,36 +133,39 @@ def generate_launch_description():
                 name='lifecycle_manager_localization',
                 output='screen',
                 arguments=['--ros-args', '--log-level', log_level],
-                parameters=[{'use_sim_time': use_sim_time},
-                            {'autostart': autostart},
+                parameters=[{'autostart': autostart},
                             {'node_names': lifecycle_nodes}])
         ]
     )
 
-    load_composable_nodes = LoadComposableNodes(
+    load_composable_nodes = GroupAction(
         condition=IfCondition(use_composition),
-        target_container=container_name,
-        composable_node_descriptions=[
-            ComposableNode(
-                package='nav2_map_server',
-                plugin='nav2_map_server::MapServer',
-                name='map_server',
-                parameters=[configured_params],
-                remappings=remappings),
-            ComposableNode(
-                package='nav2_amcl',
-                plugin='nav2_amcl::AmclNode',
-                name='amcl',
-                parameters=[configured_params],
-                remappings=remappings),
-            ComposableNode(
-                package='nav2_lifecycle_manager',
-                plugin='nav2_lifecycle_manager::LifecycleManager',
-                name='lifecycle_manager_localization',
-                parameters=[{'use_sim_time': use_sim_time,
-                             'autostart': autostart,
-                             'node_names': lifecycle_nodes}]),
-        ],
+        actions=[
+            SetParameter("use_sim_time", use_sim_time),
+            LoadComposableNodes(
+                target_container=container_name,
+                composable_node_descriptions=[
+                    ComposableNode(
+                        package='nav2_map_server',
+                        plugin='nav2_map_server::MapServer',
+                        name='map_server',
+                        parameters=[configured_params],
+                        remappings=remappings),
+                    ComposableNode(
+                        package='nav2_amcl',
+                        plugin='nav2_amcl::AmclNode',
+                        name='amcl',
+                        parameters=[configured_params],
+                        remappings=remappings),
+                    ComposableNode(
+                        package='nav2_lifecycle_manager',
+                        plugin='nav2_lifecycle_manager::LifecycleManager',
+                        name='lifecycle_manager_localization',
+                        parameters=[{'autostart': autostart,
+                                    'node_names': lifecycle_nodes}]),
+                ],
+            )
+        ]
     )
 
     # Create the launch description and populate

diff --git a/nav2_bringup/launch/multi_tb3_simulation_launch.py b/nav2_bringup/launch/multi_tb3_simulation_launch.py
@@ -116,10 +116,9 @@ def generate_launch_description():
         group = GroupAction([
             IncludeLaunchDescription(
                 PythonLaunchDescriptionSource(
-                        os.path.join(launch_dir, 'rviz_launch.py')),
+                    os.path.join(launch_dir, 'rviz_launch.py')),
                 condition=IfCondition(use_rviz),
-                launch_arguments={
-                                  'namespace': TextSubstitution(text=robot['name']),
+                launch_arguments={'namespace': TextSubstitution(text=robot['name']),
                                   'use_namespace': 'True',
                                   'rviz_config': rviz_config_file}.items()),