Update to Jazzy/Rolling with F2C v1.2.1

opennav_coverage/CMakeLists.txt

@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(opennav_coverage)
 
 find_package(ament_cmake REQUIRED)
+find_package(ament_index_cpp REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
@@ -29,6 +30,7 @@ set(executable_name opennav_coverage)
 set(library_name ${executable_name}_core)
 
 set(dependencies
+  ament_index_cpp
   rclcpp
   rclcpp_action
   rclcpp_lifecycle

opennav_coverage/package.xml

@@ -9,6 +9,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>ament_index_cpp</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_action</depend>
   <depend>rclcpp_lifecycle</depend>

opennav_coverage/src/coverage_server.cpp

@@ -46,7 +46,7 @@ CoverageServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
     node, "coordinates_in_cartesian_frame", rclcpp::ParameterValue(true));
   get_parameter("coordinates_in_cartesian_frame", cartesian_frame_);
 
-  double action_server_result_timeout;
+  double action_server_result_timeout = 10.0;
   nav2_util::declare_parameter_if_not_declared(
     node, "action_server_result_timeout", rclcpp::ParameterValue(10.0));
   get_parameter("action_server_result_timeout", action_server_result_timeout);

opennav_coverage_bt/CMakeLists.txt

@@ -12,7 +12,7 @@ find_package(nav2_behavior_tree REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(opennav_coverage_msgs REQUIRED)
-find_package(behaviortree_cpp_v3 REQUIRED)
+find_package(behaviortree_cpp REQUIRED)
 
 # potentially replace with nav2_common, nav2_package()
 set(CMAKE_CXX_STANDARD 17)
@@ -33,7 +33,7 @@ set(dependencies
   nav_msgs
   geometry_msgs
   opennav_coverage_msgs
-  behaviortree_cpp_v3
+  behaviortree_cpp
 )
 
 add_library(opennav_compute_complete_coverage_action_bt_node SHARED src/compute_complete_coverage_path.cpp)

opennav_coverage_bt/include/opennav_coverage_bt/utils.hpp

@@ -16,7 +16,7 @@
 #define OPENNAV_COVERAGE_BT__UTILS_HPP_
 
 #include <charconv>
-#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp/behavior_tree.h"
 
 namespace BT
 {

opennav_coverage_bt/package.xml

@@ -18,7 +18,7 @@
   <depend>nav_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>opennav_coverage_msgs</depend>
-  <depend>behaviortree_cpp_v3</depend>
+  <depend>behaviortree_cpp</depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>

opennav_coverage_bt/src/cancel_complete_coverage_path.cpp

@@ -31,7 +31,7 @@ CoverageCancel::CoverageCancel(
 
 }  // namespace opennav_coverage_bt
 
-#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp/bt_factory.h"
 BT_REGISTER_NODES(factory)
 {
   BT::NodeBuilder builder =

opennav_coverage_bt/src/compute_complete_coverage_path.cpp

@@ -99,7 +99,7 @@ void ComputeCoveragePathAction::halt()
 
 }  // namespace opennav_coverage_bt
 
-#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp/bt_factory.h"
 BT_REGISTER_NODES(factory)
 {
   BT::NodeBuilder builder =

opennav_coverage_bt/test/test_cancel_complete_coverage.cpp

@@ -17,7 +17,7 @@
 #include <set>
 #include <string>
 
-#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp/bt_factory.h"
 
 #include "nav2_behavior_tree/utils/test_action_server.hpp"
 #include "opennav_coverage_bt/cancel_complete_coverage_path.hpp"

opennav_coverage_bt/test/test_compute_coverage_path.cpp

@@ -20,7 +20,7 @@
 #include "nav_msgs/msg/path.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 
-#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp/bt_factory.h"
 
 #include "nav2_behavior_tree/utils/test_action_server.hpp"
 #include "opennav_coverage_bt/compute_complete_coverage_path.hpp"
@@ -141,13 +141,13 @@ TEST_F(ComputeCoveragePathActionTestFixture, test_tick)
 
   // check if returned path is correct
   nav_msgs::msg::Path path;
-  config_->blackboard->get<nav_msgs::msg::Path>("path", path);
+  [[maybe_unused]] auto res = config_->blackboard->get<nav_msgs::msg::Path>("path", path);
   EXPECT_EQ(path.poses.size(), 2u);
   EXPECT_EQ(path.poses[0].pose.position.x, 0.0);
   EXPECT_EQ(path.poses[1].pose.position.x, 1.0);
 
   // halt node so another goal can be sent
-  tree_->rootNode()->halt();
+  tree_->haltTree();
   EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE);
 }
 

opennav_coverage_demo/launch/coverage_demo_launch.py

@@ -13,11 +13,12 @@
 # limitations under the License.
 
 import os
+import tempfile
 
 from ament_index_python.packages import get_package_share_directory
-
 from launch import LaunchDescription
-from launch.actions import ExecuteProcess, IncludeLaunchDescription
+from launch.actions import ExecuteProcess, IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler
+from launch.event_handlers import OnShutdown
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
 
@@ -26,22 +27,21 @@ def generate_launch_description():
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
     coverage_demo_dir = get_package_share_directory('opennav_coverage_demo')
     rviz_config_file = os.path.join(coverage_demo_dir, 'rviz_config.rviz')
+    sim_dir = get_package_share_directory("nav2_minimal_tb3_sim")
 
     world = os.path.join(coverage_demo_dir, 'blank.world')
     param_file_path = os.path.join(coverage_demo_dir, 'demo_params.yaml')
-    sdf = os.path.join(nav2_bringup_dir, 'worlds', 'waffle.model')
+    robot_sdf = os.path.join(sim_dir, "urdf", "gz_waffle.sdf.xacro")
 
     # start the simulation
-    start_gazebo_server_cmd = ExecuteProcess(
-        cmd=['gzserver', '-s', 'libgazebo_ros_init.so',
-             '-s', 'libgazebo_ros_factory.so', world],
-        cwd=[coverage_demo_dir], output='screen')
-
-    # start_gazebo_client_cmd = ExecuteProcess(
-    #     cmd=['gzclient'],
-    #     cwd=[coverage_demo_dir], output='screen')
-
-    urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf')
+    world_sdf = tempfile.mktemp(prefix="nav2_", suffix=".sdf")
+    world_sdf_xacro = ExecuteProcess(cmd=["xacro", "-o", world_sdf, "headless:=false", world])
+    gazebo_server = ExecuteProcess(
+        cmd=["gz", "sim", "-r", "-s", world_sdf],
+        output="screen",
+    )
+
+    urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf')
     with open(urdf, 'r') as infp:
         robot_description = infp.read()
 
@@ -53,15 +53,29 @@ def generate_launch_description():
         parameters=[{'use_sim_time': True,
                      'robot_description': robot_description}])
 
-    start_gazebo_spawner_cmd = Node(
-        package='gazebo_ros',
-        executable='spawn_entity.py',
-        output='screen',
-        arguments=[
-            '-entity', 'tb3',
-            '-file', sdf,
-            '-x', '5.0', '-y', '5.0', '-z', '0.10',
-            '-R', '0.0', '-P', '0.0', '-Y', '0.0'])
+    remove_temp_sdf_file = RegisterEventHandler(
+        event_handler=OnShutdown(on_shutdown=[OpaqueFunction(function=lambda _: os.remove(world_sdf))])
+    )
+
+    gazebo_client = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(get_package_share_directory("ros_gz_sim"), "launch", "gz_sim.launch.py")
+        ),
+        launch_arguments={"gz_args": ["-v4 -g "]}.items(),
+    )
+
+    start_gazebo_spawner_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py')),
+        launch_arguments={'namespace': '',
+                          'robot_name': 'turtlebot3_waffle',
+                          'robot_sdf': robot_sdf,
+                          'x_pose': str(0.0),
+                          'y_pose': str(0.0),
+                          'z_pose': str(0.1),
+                          'roll': str(0.0),
+                          'pitch': str(0.0),
+                          'yaw': str(0.0)}.items())
 
     # start the visualization
     rviz_cmd = IncludeLaunchDescription(
@@ -90,10 +104,13 @@ def generate_launch_description():
         output='screen')
 
     ld = LaunchDescription()
-    ld.add_action(start_gazebo_server_cmd)
-    # ld.add_action(start_gazebo_client_cmd)
-    ld.add_action(start_robot_state_publisher_cmd)
+    ld.add_action(world_sdf_xacro)
+    ld.add_action(remove_temp_sdf_file)
+    ld.add_action(gazebo_server)
+    ld.add_action(gazebo_client)
     ld.add_action(start_gazebo_spawner_cmd)
+
+    ld.add_action(start_robot_state_publisher_cmd)
     ld.add_action(rviz_cmd)
     ld.add_action(bringup_cmd)
     ld.add_action(fake_localization_cmd)

opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py

@@ -146,7 +146,7 @@ def main():
     navigator.startup()
 
     # Some example field
-    field = [[5.0, 5.0], [5.0, 15.0], [15.0, 15.0], [10.0, 5.0], [5.0, 5.0]]
+    field = [[0.0, 0.0], [0.0, 15.0], [15.0, 15.0], [10.0, 0.0], [0.0, 0.0]]
     navigator.navigateCoverage(field)
 
     i = 0

opennav_coverage_demo/package.xml

@@ -17,6 +17,8 @@
   <depend>opennav_row_coverage</depend>
   <depend>opennav_coverage_navigator</depend>
 
+  <exec_depend>nav2_minimal_tb3_sim</exec_depend>
+
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
 

opennav_coverage_demo/world/blank.world

@@ -3,14 +3,45 @@
   <!-- We use a custom world for the pioneer so that the camera angle is launched correctly -->
 
   <world name="default">
-    <include>
-      <uri>model://ground_plane</uri>
-    </include>
-
-    <!-- Global light source -->
-    <include>
-      <uri>model://sun</uri>
-    </include>
-
+    <light name='sun' type='directional'>
+        <cast_shadows>0</cast_shadows>
+        <pose>0 0 10 0 0 0</pose>
+        <diffuse>0.8 0.8 0.8 1</diffuse>
+        <specular>0.8 0.8 0.8 1</specular>
+        <attenuation>
+            <range>1000</range>
+            <constant>0.9</constant>
+            <linear>0.01</linear>
+            <quadratic>0.001</quadratic>
+        </attenuation>
+        <direction>-0.5 0.1 -0.9</direction>
+    </light>
+    <model name='ground_plane'>
+        <static>1</static>
+        <link name='link'>
+            <collision name='collision'>
+                <geometry>
+                    <plane>
+                        <normal>0 0 1</normal>
+                        <size>100 100</size>
+                    </plane>
+                </geometry>
+                <max_contacts>10</max_contacts>
+            </collision>
+            <visual name='visual'>
+                <geometry>
+                    <plane>
+                        <normal>0 0 1</normal>
+                        <size>100 100</size>
+                    </plane>
+                </geometry>
+                <material>
+                    <ambient>0.8 0.8 0.8 1</ambient>
+                    <diffuse>0.8 0.8 0.8 1</diffuse>
+                    <specular>0.8 0.8 0.8 1</specular>
+                </material>
+            </visual>
+        </link>
+    </model>
   </world>
 </sdf>

opennav_coverage_navigator/src/coverage_navigator.cpp

@@ -122,7 +122,7 @@ CoverageNavigator::onLoop()
   try {
     // Get current path points
     nav_msgs::msg::Path current_path;
-    blackboard->get<nav_msgs::msg::Path>(path_blackboard_id_, current_path);
+    [[maybe_unused]] auto res = blackboard->get(path_blackboard_id_, current_path);
 
     // Find the closest pose to current pose on global path
     auto find_closest_pose_idx =
@@ -165,7 +165,7 @@ CoverageNavigator::onLoop()
   }
 
   int recovery_count = 0;
-  blackboard->get<int>("number_recoveries", recovery_count);
+  [[maybe_unused]] auto result = blackboard->get("number_recoveries", recovery_count);
   feedback_msg->number_of_recoveries = recovery_count;
   feedback_msg->current_pose = current_pose;
   feedback_msg->navigation_time = clock_->now() - start_time_;

opennav_row_coverage/src/row_coverage_server.cpp

@@ -50,7 +50,7 @@ RowCoverageServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
     node, "order_ids", rclcpp::ParameterValue(true));
   get_parameter("order_ids", order_ids_);
 
-  double action_server_result_timeout;
+  double action_server_result_timeout = 10.0;
   nav2_util::declare_parameter_if_not_declared(
     node, "action_server_result_timeout", rclcpp::ParameterValue(10.0));
   get_parameter("action_server_result_timeout", action_server_result_timeout);