[WIP][TEST]Mppi eigen

.github/ISSUE_TEMPLATE.md

@@ -11,6 +11,8 @@ For Bug report or feature requests, please fill out the relevant category below
 
 - Operating System:
   - <!-- OS and version (e.g. Windows 10, Ubuntu 16.04...) -->
+- Computer:
+  - <!-- Nvidia Jetson Orin, 13th Gen Intel NUC, Ryzen 9 7940-HS -->
 - ROS2 Version:
   - <!-- ROS2 distribution and install method (e.g. Foxy binaries, Dashing source...) -->
 - Version or commit hash:
@@ -34,7 +36,7 @@ For Bug report or feature requests, please fill out the relevant category below
 #### Additional information
 
 <!-- If you are reporting a bug delete everything below
-     If you are requesting a feature deleted everything above this line -->
+     If you are requesting a feature deleted everything above this line & Configuration & Tuning Help -->
 ----
 ## Feature request
 
@@ -43,3 +45,30 @@ For Bug report or feature requests, please fill out the relevant category below
 
 #### Implementation considerations
 <!-- Relevant information on how the feature could be implemented and pros and cons of the different solutions -->
+
+<!-- If you are reporting a bug or feature delete everything above this line -->
+----
+## Configuration or Tuning Help
+
+**Required Info:**
+
+- Operating System:
+  - <!-- OS and version (e.g. Windows 10, Ubuntu 16.04...) -->
+- Computer:
+  - <!-- Nvidia Jetson Orin, 13th Gen Intel NUC, Ryzen 9 7940-HS -->
+- ROS2 Version:
+  - <!-- ROS2 distribution and install method (e.g. Foxy binaries, Dashing source...) -->
+- Version or commit hash:
+  - <!-- from source: output of `git -C navigation2 rev-parse HEAD
+         apt binaries: output of: dpkg-query --show "ros-$ROS_DISTRO-navigation2"
+                              or: dpkg-query --show "ros-$ROS_DISTRO-nav2-*" -->
+- DDS implementation:
+  - <!-- rmw_implementation used (e.g. Fast-RTPS, RTI Connext, etc.) -->
+- Nav2 Package:
+  - <!-- MPPI, DWB, Smac, etc-->
+
+#### Tuning / Configuration Goal
+<!-- Description in a few sentences what the aim is that you're trying to tune / configure -->
+
+#### What I've Tried
+<!-- Description **in detail** what you've tried, methodology, graphs or figures, and progress -->

.github/ISSUE_TEMPLATE/bug_report.md

@@ -0,0 +1,47 @@
+---
+name: Bug report
+about: Create a report to help us improve
+title: ''
+labels: ''
+assignees: ''
+
+---
+
+<!--
+For general questions, please ask on Robotics Stack Exchange: https://robotics.stackexchange.com/, make sure to include at least the `ros2` and `nav2` tags and the rosdistro version you are running, e.g. `ardent`.
+For general design discussions, please post on discourse: https://discourse.ros.org/c/ng-ros
+Not sure if this is the right repository? Open an issue on https://github.com/ros-navigation/navigation2
+For Bug report or feature requests, please fill out the relevant category below
+-->
+
+## Bug report
+
+**Required Info:**
+
+- Operating System:
+  - <!-- OS and version (e.g. Windows 10, Ubuntu 16.04...) -->
+- Computer:
+  - <!-- Nvidia Jetson Orin, 13th Gen Intel NUC, Ryzen 9 7940-HS -->
+- ROS2 Version:
+  - <!-- ROS2 distribution and install method (e.g. Foxy binaries, Dashing source...) -->
+- Version or commit hash:
+  - <!-- from source: output of `git -C navigation2 rev-parse HEAD
+         apt binaries: output of: dpkg-query --show "ros-$ROS_DISTRO-navigation2"
+                              or: dpkg-query --show "ros-$ROS_DISTRO-nav2-*" -->
+- DDS implementation:
+  - <!-- rmw_implementation used (e.g. Fast-RTPS, RTI Connext, etc.) -->
+
+#### Steps to reproduce issue
+<!-- Detailed instructions on how to reliably reproduce this issue http://sscce.org/
+``` code that can be copy-pasted is preferred ``` -->
+```
+
+```
+
+#### Expected behavior
+
+#### Actual behavior
+
+#### Reproduction instructions
+
+#### Additional information

.github/ISSUE_TEMPLATE/configuration-or-tuning-help.md

@@ -0,0 +1,42 @@
+---
+name: Configuration or Tuning Help
+about: Help in Setting Up Nav2 for your Robot (non-bug, non-feature)
+title: ''
+labels: question
+assignees: ''
+
+---
+
+<!--
+For general questions, please ask on Robotics Stack Exchange: https://robotics.stackexchange.com/, make sure to include at least the `ros2` and `nav2` tags and the rosdistro version you are running, e.g. `ardent`.
+For general design discussions, please post on discourse: https://discourse.ros.org/c/ng-ros
+Not sure if this is the right repository? Open an issue on https://github.com/ros-navigation/navigation2
+For Bug report or feature requests, please fill out the relevant category below
+-->
+
+**Required Info:**
+
+- Operating System:
+  - <!-- OS and version (e.g. Windows 10, Ubuntu 16.04...) -->
+- Computer:
+  - <!-- Nvidia Jetson Orin, 13th Gen Intel NUC, Ryzen 9 7940-HS -->
+- ROS2 Version:
+  - <!-- ROS2 distribution and install method (e.g. Foxy binaries, Dashing source...) -->
+- Version or commit hash:
+  - <!-- from source: output of `git -C navigation2 rev-parse HEAD
+         apt binaries: output of: dpkg-query --show "ros-$ROS_DISTRO-navigation2"
+                              or: dpkg-query --show "ros-$ROS_DISTRO-nav2-*" -->
+- DDS implementation:
+  - <!-- rmw_implementation used (e.g. Fast-RTPS, RTI Connext, etc.) -->
+- Nav2 Package:
+  - <!-- MPPI, DWB, Smac, etc-->
+- Have you asked this on Robotics Stack Exchange?
+  - <!-- If yes, provide a link -->
+
+#### Tuning / Configuration Goal
+<!-- Description in a few sentences what the aim is that you're trying to tune / configure -->
+
+#### What I've Tried
+<!-- Description **in detail** what you've tried, methodology, graphs or figures, and progress -->
+<!-- Note: Maintainer time is limited, understand that you may be directed to community resources like docs.nav2.org, Robotics Stack Exchange, or other help resources. -->
+<!-- Note: If this is a commercial application and you would like professional hands-on support, please contact [email protected] instead. -->

.github/ISSUE_TEMPLATE/feature_request.md

@@ -0,0 +1,16 @@
+---
+name: Feature request
+about: Suggest an idea for this project
+title: ''
+labels: ''
+assignees: ''
+
+---
+
+## Feature request
+
+#### Feature description
+<!-- Description in a few sentences what the feature consists of and what problem it will solve -->
+
+#### Implementation considerations
+<!-- Relevant information on how the feature could be implemented and pros and cons of the different solutions -->

.github/PULL_REQUEST_TEMPLATE.md

@@ -27,6 +27,13 @@
 * I added some capabilities, need to document them
 -->
 
+## Description of how this change was tested
+
+<!--
+* I wrote unit tests that cover 90%+ of changes and extensively tested on my physical robot platform in production for 1 week
+* I wrote unit tests and tested in simulation for 10 minutes
+-->
+
 ---
 
 ## Future work that may be required in bullet points

nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp

@@ -59,8 +59,17 @@ inline BT::NodeStatus GoalUpdater::tick()
 
   callback_group_executor_.spin_some();
 
-  if (rclcpp::Time(last_goal_received_.header.stamp) > rclcpp::Time(goal.header.stamp)) {
-    goal = last_goal_received_;
+  if (last_goal_received_.header.stamp != rclcpp::Time(0)) {
+    auto last_goal_received_time = rclcpp::Time(last_goal_received_.header.stamp);
+    auto goal_time = rclcpp::Time(goal.header.stamp);
+    if (last_goal_received_time > goal_time) {
+      goal = last_goal_received_;
+    } else {
+      RCLCPP_WARN(
+        node_->get_logger(), "The timestamp of the received goal (%f) is older than the "
+        "current goal (%f). Ignoring the received goal.",
+        last_goal_received_time.seconds(), goal_time.seconds());
+    }
   }
 
   setOutput("output_goal", goal);

nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp

@@ -104,7 +104,7 @@ class DriveOnHeading : public TimedBehavior<ActionT>
       RCLCPP_WARN(
         this->logger_,
         "Exceeded time allowance before reaching the DriveOnHeading goal - Exiting DriveOnHeading");
-      return ResultStatus{Status::FAILED, ActionT::Result::NONE};
+      return ResultStatus{Status::FAILED, ActionT::Result::TIMEOUT};
     }
 
     geometry_msgs::msg::PoseStamped current_pose;

nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py

@@ -37,6 +37,69 @@
 from nav2_common.launch import ParseMultiRobotPose
 
 
+def generate_robot_actions(context, *args, **kwargs):
+    bringup_dir = get_package_share_directory('nav2_bringup')
+    launch_dir = os.path.join(bringup_dir, 'launch')
+    use_rviz = LaunchConfiguration('use_rviz')
+    params_file = LaunchConfiguration('params_file')
+    autostart = LaunchConfiguration('autostart')
+    rviz_config_file = LaunchConfiguration('rviz_config')
+    map_yaml_file = LaunchConfiguration('map')
+    use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
+
+    robots_substitution = ParseMultiRobotPose(LaunchConfiguration('robots'))
+    robots_list = robots_substitution.perform(context)
+
+    # Define commands for launching the navigation instances
+    bringup_cmd_group = []
+    for robot_name in robots_list:
+        init_pose = robots_list[robot_name]
+        group = GroupAction(
+            [
+                LogInfo(
+                    msg=['Launching namespace=', robot_name, ' init_pose=', str(init_pose),]
+                ),
+                IncludeLaunchDescription(
+                    PythonLaunchDescriptionSource(
+                        os.path.join(launch_dir, 'rviz_launch.py')
+                    ),
+                    condition=IfCondition(use_rviz),
+                    launch_arguments={
+                        'namespace': TextSubstitution(text=robot_name),
+                        'rviz_config': rviz_config_file,
+                    }.items(),
+                ),
+                IncludeLaunchDescription(
+                    PythonLaunchDescriptionSource(
+                        os.path.join(bringup_dir, 'launch', 'tb3_simulation_launch.py')
+                    ),
+                    launch_arguments={
+                        'namespace': robot_name,
+                        'map': map_yaml_file,
+                        'use_sim_time': 'True',
+                        'params_file': params_file,
+                        'autostart': autostart,
+                        'use_rviz': 'False',
+                        'use_simulator': 'False',
+                        'headless': 'False',
+                        'use_robot_state_pub': use_robot_state_pub,
+                        'x_pose': TextSubstitution(text=str(init_pose['x'])),
+                        'y_pose': TextSubstitution(text=str(init_pose['y'])),
+                        'z_pose': TextSubstitution(text=str(init_pose['z'])),
+                        'roll': TextSubstitution(text=str(init_pose['roll'])),
+                        'pitch': TextSubstitution(text=str(init_pose['pitch'])),
+                        'yaw': TextSubstitution(text=str(init_pose['yaw'])),
+                        'robot_name': TextSubstitution(text=robot_name),
+                    }.items(),
+                ),
+            ]
+        )
+
+        bringup_cmd_group.append(group)
+    bringup_cmd_group.append(LogInfo(msg=['number_of_robots=', str(len(robots_list))]))
+    return bringup_cmd_group
+
+
 def generate_launch_description():
     """
     Bring up the multi-robots with given launch arguments.
@@ -49,7 +112,6 @@ def generate_launch_description():
     """
     # Get the launch directory
     bringup_dir = get_package_share_directory('nav2_bringup')
-    launch_dir = os.path.join(bringup_dir, 'launch')
     sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')
 
     # Simulation settings
@@ -61,7 +123,6 @@ def generate_launch_description():
     autostart = LaunchConfiguration('autostart')
     rviz_config_file = LaunchConfiguration('rviz_config')
     use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
-    use_rviz = LaunchConfiguration('use_rviz')
     log_settings = LaunchConfiguration('log_settings', default='true')
 
     # Declare the launch arguments
@@ -71,6 +132,13 @@ def generate_launch_description():
         description='Full path to world file to load',
     )
 
+    declare_robots_cmd = DeclareLaunchArgument(
+        'robots',
+        default_value="""robot1={x: 0.5, y: 0.5, yaw: 0};
+                   robot2={x: -0.5, y: -0.5, z: 0, roll: 0, pitch: 0, yaw: 1.5707}""",
+        description='Robots and their initialization poses in YAML format',
+    )
+
     declare_map_yaml_cmd = DeclareLaunchArgument(
         'map',
         default_value=os.path.join(bringup_dir, 'maps', 'tb3_sandbox.yaml'),
@@ -93,7 +161,8 @@ def generate_launch_description():
 
     declare_rviz_config_file_cmd = DeclareLaunchArgument(
         'rviz_config',
-        default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'),
+        default_value=os.path.join(
+            bringup_dir, 'rviz', 'nav2_default_view.rviz'),
         description='Full path to the RVIZ config file to use.',
     )
 
@@ -121,60 +190,6 @@ def generate_launch_description():
             OpaqueFunction(function=lambda _: os.remove(world_sdf))
         ]))
 
-    robots_list = ParseMultiRobotPose('robots').value()
-
-    # Define commands for launching the navigation instances
-    bringup_cmd_group = []
-    for robot_name in robots_list:
-        init_pose = robots_list[robot_name]
-        group = GroupAction(
-            [
-                LogInfo(
-                    msg=[
-                        'Launching namespace=',
-                        robot_name,
-                        ' init_pose=',
-                        str(init_pose),
-                    ]
-                ),
-                IncludeLaunchDescription(
-                    PythonLaunchDescriptionSource(
-                        os.path.join(launch_dir, 'rviz_launch.py')
-                    ),
-                    condition=IfCondition(use_rviz),
-                    launch_arguments={
-                        'namespace': TextSubstitution(text=robot_name),
-                        'rviz_config': rviz_config_file,
-                    }.items(),
-                ),
-                IncludeLaunchDescription(
-                    PythonLaunchDescriptionSource(
-                        os.path.join(bringup_dir, 'launch', 'tb3_simulation_launch.py')
-                    ),
-                    launch_arguments={
-                        'namespace': robot_name,
-                        'map': map_yaml_file,
-                        'use_sim_time': 'True',
-                        'params_file': params_file,
-                        'autostart': autostart,
-                        'use_rviz': 'False',
-                        'use_simulator': 'False',
-                        'headless': 'False',
-                        'use_robot_state_pub': use_robot_state_pub,
-                        'x_pose': TextSubstitution(text=str(init_pose['x'])),
-                        'y_pose': TextSubstitution(text=str(init_pose['y'])),
-                        'z_pose': TextSubstitution(text=str(init_pose['z'])),
-                        'roll': TextSubstitution(text=str(init_pose['roll'])),
-                        'pitch': TextSubstitution(text=str(init_pose['pitch'])),
-                        'yaw': TextSubstitution(text=str(init_pose['yaw'])),
-                        'robot_name': TextSubstitution(text=robot_name),
-                    }.items(),
-                ),
-            ]
-        )
-
-        bringup_cmd_group.append(group)
-
     set_env_vars_resources = AppendEnvironmentVariable(
         'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models'))
     set_env_vars_resources2 = AppendEnvironmentVariable(
@@ -188,6 +203,7 @@ def generate_launch_description():
 
     # Declare the launch options
     ld.add_action(declare_world_cmd)
+    ld.add_action(declare_robots_cmd)
     ld.add_action(declare_map_yaml_cmd)
     ld.add_action(declare_params_file_cmd)
     ld.add_action(declare_use_rviz_cmd)
@@ -200,8 +216,6 @@ def generate_launch_description():
     ld.add_action(start_gazebo_cmd)
     ld.add_action(remove_temp_sdf_file)
 
-    ld.add_action(LogInfo(msg=['number_of_robots=', str(len(robots_list))]))
-
     ld.add_action(
         LogInfo(condition=IfCondition(log_settings), msg=['map yaml: ', map_yaml_file])
     )
@@ -223,8 +237,6 @@ def generate_launch_description():
     ld.add_action(
         LogInfo(condition=IfCondition(log_settings), msg=['autostart: ', autostart])
     )
-
-    for cmd in bringup_cmd_group:
-        ld.add_action(cmd)
+    ld.add_action(OpaqueFunction(function=generate_robot_actions))
 
     return ld