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

@@ -12,12 +12,14 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+from datetime import datetime
 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, TimerAction
+from launch.event_handlers import OnShutdown
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
 
@@ -26,42 +28,100 @@ 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')
+    # world = os.path.join(sim_dir, "worlds", "tb3_empty_world.sdf.xacro")
     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")
+
+    # declare_simulator_cmd = DeclareLaunchArgument(
+    #     'headless', default_value='False', description='Whether to execute gzclient)'
+    # )
 
     # 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_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')
+    urdf = os.path.join(sim_dir, "urdf", "turtlebot3_waffle.urdf")
     with open(urdf, 'r') as infp:
         robot_description = infp.read()
 
+    remappings = [("/tf", "tf"), ("/tf_static", "tf_static")]
+
     start_robot_state_publisher_cmd = Node(
-        package='robot_state_publisher',
-        executable='robot_state_publisher',
-        name='robot_state_publisher',
-        output='screen',
-        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'])
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        namespace="",
+        output="screen",
+        parameters=[{"use_sim_time": True, "robot_description": robot_description}],
+        # remappings=remappings,
+    )
+
+    world_sdf = tempfile.mktemp(prefix="nav2_", suffix=".sdf")
+    # world_sdf_xacro = ExecuteProcess(
+    #     cmd=['xacro', '-o', world_sdf, ['headless:=', 'False'], world])
+    world_sdf_xacro = ExecuteProcess(cmd=["xacro", "-o", world_sdf, "headless:=false", world])
+    gazebo_server = ExecuteProcess(
+        cmd=["gz", "sim", "-r", "-s", world_sdf],
+        output="screen",
+    )
+
+    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(),
+    )
+
+    pose = {
+        # "x": 5.00,
+        # "y": 5.00,
+        "x": 0.00,
+        "y": 0.00,
+        "z": 0.10,
+        "R": 0.00,
+        "P": 0.00,
+        "Y": 0.00,
+    }
+
+    gz_robot = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(os.path.join(sim_dir, "launch", "spawn_tb3.launch.py")),
+        launch_arguments={
+            "namespace": "",
+            # "use_sim_time": "true",
+            "robot_name": "turtlebot3_waffle",
+            "robot_sdf": robot_sdf,
+            "x_pose": str(pose["x"]),
+            "y_pose": str(pose["y"]),
+            "z_pose": str(pose["z"]),
+            "roll": str(pose["R"]),
+            "pitch": str(pose["P"]),
+            "yaw": str(pose["Y"]),
+        }.items(),
+    )
+
+    # 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'])
 
     # start the visualization
     rviz_cmd = IncludeLaunchDescription(
@@ -89,13 +149,39 @@ def generate_launch_description():
         emulate_tty=True,
         output='screen')
 
+    # publish initial pose
+    # current_time = datetime.now()
+    # sec = int(current_time.timestamp())
+    # nanosec = int(current_time.microsecond * 1000)  # Convert microseconds to nanoseconds
+
+    message = (
+        "{ header: { stamp: { sec: 0, nanosec: 0 }, frame_id: 'map' }, "
+        "pose: { pose: { position: { x: 0.0, y: 0.0, z: 0.01 }, orientation: { x: 0.0, y: 0.0, z: 0.0, w: 1.0 } }, "
+        "covariance: [0.25, 0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, "
+        "0.0685, 0, 0, 0, 0, 0, 0, 0.0685, 0, 0, 0, 0, 0, 0.0685] } }"
+    )
+
+    # Define the command to publish to the topic
+    publish_cmd_1 = ExecuteProcess(
+        cmd=["ros2", "topic", "pub", "/initialpose", "geometry_msgs/msg/PoseWithCovarianceStamped", message, "--once"],
+        output="screen",
+    )
+
     ld = LaunchDescription()
-    ld.add_action(start_gazebo_server_cmd)
+
+    ld.add_action(world_sdf_xacro)
+    ld.add_action(remove_temp_sdf_file)
+    ld.add_action(gz_robot)
+    ld.add_action(gazebo_server)
+    ld.add_action(gazebo_client)
+
+    # 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(start_gazebo_spawner_cmd)
+    # ld.add_action(start_gazebo_spawner_cmd)
     ld.add_action(rviz_cmd)
     ld.add_action(bringup_cmd)
     ld.add_action(fake_localization_cmd)
     ld.add_action(demo_cmd)
+    # ld.add_action(TimerAction(period=1.0, actions=[publish_cmd_1]))
     return ld

opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py

@@ -146,7 +146,8 @@ 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 = [[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/world/blank.world

@@ -3,14 +3,46 @@
   <!-- 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);