initial demo prototype working!

Author: SteveMacenski

README.md

@@ -2,7 +2,7 @@
 
 This package contains the Complete Coverage Task Server & auxiliary tools utilizing the [Fields2Cover](https://github.com/Fields2Cover/Fields2Cover) complete coverage planning system which includes a great deal of options in headland, swath, route, and final path planning. You can find more information about Fields2Cover (F2C) in its [ReadTheDocs Documentation](https://fields2cover.github.io/index.html). It can accept both GPS and Cartesian coordinates and publishes the field, headland, swaths, and route as separate topics in cartesian coordinates for debugging and visualization.
 
-This capability was created by [Open Navigation LLC](https://www.opennav.org/) in partnership with [Bonsai Robotics](https://www.bonsairobotics.ai/). Bonsai Robotics funded the development of this work for their own product and has graciously allowed Open Navigation to open-source it for the community to leverage in their own systems. Please thank Bonsai Robotics for their commendable donation to the ROS community!
+This capability was created by [Open Navigation LLC](https://www.opennav.org/) in partnership with [Bonsai Robotics](https://www.bonsairobotics.ai/). Bonsai Robotics builds autonomous software for machines in adverse and GPS degraded conditions utilizing vision. Bonsai Robotics funded the development of this work for their own product and has graciously allowed Open Navigation to open-source it for the community to leverage in their own systems. Please thank Bonsai Robotics for their commendable donation to the ROS community! Bonsai is hiring [here](https://www.bonsairobotics.ai/jobs).
 
 ![BonsaixOpenNavigation](https://github.com/ros-planning/navigation2/assets/14944147/b5c23851-0694-4b87-b5ab-fb7c957413f4)
 
@@ -146,7 +146,3 @@ If you use this work, please make sure to cite both Nav2 and Fields2Cover:
   doi={10.1109/LRA.2023.3248439}
 }
 ```
-
-## Current Work
-
-  - Use setup with BT nodes / XML / Navigator. Simulator demo altogether. Demo video (in readme).

opennav_coverage/include/opennav_coverage/path_generator.hpp

@@ -94,6 +94,8 @@ class PathGenerator
    */
   void setTurnPointDistance(const double & setting) {default_turn_point_distance_ = setting;}
 
+  float getTurnPointDistance() {return default_turn_point_distance_;}
+
 protected:
   /**
    * @brief Creates generator pointer of a requested type

opennav_coverage/include/opennav_coverage/utils.hpp

@@ -194,7 +194,8 @@ inline opennav_coverage_msgs::msg::PathComponents toCoveragePathMsg(
 
 /**
  * @brief Converts full path to nav_msgs/path message for action client, visualization
- * and use in direct-sending to a controller to replace the planner server
+ * and use in direct-sending to a controller to replace the planner server. Interpolates
+ * the F2C path to make it dense for following semantics.
  * @param path Full path to convert
  * @param Field Field to use for conversion from UTM if necessary
  * @param header header
@@ -203,7 +204,8 @@ inline opennav_coverage_msgs::msg::PathComponents toCoveragePathMsg(
  */
 inline nav_msgs::msg::Path toNavPathMsg(
   const Path & raw_path, const F2CField & field,
-  const std_msgs::msg::Header & header, const bool is_cartesian)
+  const std_msgs::msg::Header & header, const bool is_cartesian,
+  const float & pt_dist)
 {
   using f2c::types::PathSectionType;
   nav_msgs::msg::Path msg;
@@ -218,9 +220,38 @@ inline nav_msgs::msg::Path toNavPathMsg(
     path = f2c::Transform::transformToPrevCRS(raw_path, field);
   }
 
-  msg.poses.resize(path.states.size());
   for (unsigned int i = 0; i != path.states.size(); i++) {
-    msg.poses[i] = toMsg(path.states[i]);
+    // Swaths come in pairs of start-end sequentially
+    if (i > 0 && path.states[i].type == PathSectionType::SWATH &&
+      path.states[i - 1].type == PathSectionType::SWATH)
+    {
+      const float & x0 = path.states[i - 1].point.getX();
+      const float & y0 = path.states[i - 1].point.getY();
+      const float & x1 = path.states[i].point.getX();
+      const float & y1 = path.states[i].point.getY();
+
+      const float dist = hypotf(x1 - x0, y1 - y0);
+      const float ux = (x1 - x0) / dist;
+      const float uy = (y1 - y0) / dist;
+      float curr_dist = pt_dist;
+
+      geometry_msgs::msg::PoseStamped pose;
+      pose.pose.orientation =
+        nav2_util::geometry_utils::orientationAroundZAxis(path.states[i].angle);
+      pose.pose.position.x = x0;
+      pose.pose.position.y = y0;
+      pose.pose.position.z = path.states[i].point.getZ();
+
+      while (curr_dist < dist) {
+        pose.pose.position.x += pt_dist * ux;
+        pose.pose.position.y += pt_dist * uy;
+        msg.poses.push_back(pose);
+        curr_dist += pt_dist;
+      }
+    } else {
+      // Turns are already dense paths
+      msg.poses.push_back(toMsg(path.states[i]));
+    }
   }
 
   return msg;

opennav_coverage/src/coverage_server.cpp

@@ -210,7 +210,8 @@ void CoverageServer::computeCoveragePath()
         Path path = path_gen_->generatePath(route, goal->path_mode);
         result->coverage_path =
           util::toCoveragePathMsg(path, master_field, header, cartesian_frame_);
-        result->nav_path = util::toNavPathMsg(path, master_field, header, cartesian_frame_);
+        result->nav_path = util::toNavPathMsg(
+          path, master_field, header, cartesian_frame_, path_gen_->getTurnPointDistance());
       } else {
         result->coverage_path =
           util::toCoveragePathMsg(route, master_field, true, header, cartesian_frame_);
@@ -223,7 +224,7 @@ void CoverageServer::computeCoveragePath()
     auto cycle_duration = this->now() - start_time;
     result->planning_time = cycle_duration;
 
-    // Visualization always uses cartesian coordinates for convenience
+    // Visualize in native coordinates
     visualizer_->visualize(field, field_no_headland, result, header);
     action_server_->succeeded_current(result);
   } catch (CoverageException & e) {

opennav_coverage/test/test_utils.cpp

@@ -107,7 +107,7 @@ TEST(UtilsTests, TesttoNavPathMsg)
   path_in.states.resize(10);
   F2CField field;
 
-  auto msg = util::toNavPathMsg(path_in, field, header_in, true);
+  auto msg = util::toNavPathMsg(path_in, field, header_in, true, 0.1);
   EXPECT_EQ(msg.header.frame_id, "test");
   EXPECT_EQ(msg.poses.size(), 10u);
 }

opennav_coverage_bt/behavior_trees/navigate_w_basic_complete_coverage.xml

@@ -4,15 +4,21 @@
   for changes in operational modes (e.g. raise tool, turn off payload in turns not in field).
 
   Field filepath is set by the Navigator plugin in the action request, but also may be hardcoded
-  or set using the polygons input port instead for dynamically computed values instead of from file
+  or set using the polygons input port instead for dynamically computed values instead of from file.
+
+  It might be wise to still have the planner server on hand to connect to the coverage path in realistic uses.
 -->
 
 <root main_tree_to_execute="MainTree">
   <BehaviorTree ID="MainTree">
-    <PipelineSequence name="NavigateWithoutReplanning">
-      <!-- May also use 'polygons={field_polygon} polygons_frame_id={polygon_frame_id}' if set polygon via NavigateCompleteCoverage instead of filepath -->
-      <ComputeCoveragePath nav_path="{path}" file_field="{field_filepath}" coords_cartesian="false" error_code_id="{compute_coverage_error_code}"/>
-      <FollowPath path="{path}" controller_id="FollowPath" error_code_id="{follow_path_error_code}"/>
-    </PipelineSequence>
+    <RateController hz="0.0000001"> <!-- once, for demo -->
+      <Sequence name="NavigateWithoutReplanning">
+        <!-- May use:
+         'polygons="{field_polygon}" polygons_frame_id="{polygon_frame_id}"' if set polygon via NavigateCompleteCoverage
+         or file_field="{field_filepath}" if setting polygon file via NavigateCompleteCoverage -->
+        <ComputeCoveragePath nav_path="{path}" polygons="{field_polygon}" polygons_frame_id="{polygon_frame_id}" error_code_id="{compute_coverage_error_code}"/>
+        <FollowPath path="{path}" controller_id="FollowPath" error_code_id="{follow_path_error_code}"/>
+      </Sequence>
+    </RateController>
   </BehaviorTree>
 </root>

opennav_coverage_bt/src/compute_complete_coverage_path.cpp

@@ -49,7 +49,7 @@ void ComputeCoveragePathAction::on_tick()
     getInput("polygons", polys);
     goal_.polygons.resize(polys.size());
     for (unsigned int i = 0; i != polys.size(); i++) {
-      for (unsigned int j; j != polys[i].points.size(); j++) {
+      for (unsigned int j = 0; j != polys[i].points.size(); j++) {
         opennav_coverage_msgs::msg::Coordinate coord;
         coord.axis1 = polys[i].points[j].x;
         coord.axis2 = polys[i].points[j].y;

opennav_coverage_demo/README.md

@@ -0,0 +1,3 @@
+# Open Navigation's Nav2 Complete Coverage Demo
+
+This package contains the coverage navigator demo, using the Navigator, BT XML, BT Nodes, and Coverage Server to perform a simple coverage task.

opennav_coverage_demo/launch/bringup_launch.py

@@ -0,0 +1,107 @@
+# Copyright (c) 2023 Open Navigation LLC
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import LoadComposableNodes
+from launch_ros.actions import Node
+from launch_ros.descriptions import ComposableNode, ParameterFile
+from nav2_common.launch import RewrittenYaml
+
+
+def generate_launch_description():
+    params_file = LaunchConfiguration('params_file')
+
+    lifecycle_nodes = ['controller_server',
+                       'bt_navigator',
+                       'velocity_smoother',
+                       'coverage_server']
+
+    remappings = [('/tf', 'tf'),
+                  ('/tf_static', 'tf_static')]
+
+    # Create our own temporary YAML files that include substitutions
+    autostart = True
+    use_sim_time = True
+    param_substitutions = {
+        'use_sim_time': str(use_sim_time),
+        'autostart': str(autostart)}
+
+    configured_params = ParameterFile(
+        RewrittenYaml(
+            source_file=params_file,
+            root_key='',
+            param_rewrites=param_substitutions,
+            convert_types=True),
+        allow_substs=True)
+
+    stdout_linebuf_envvar = SetEnvironmentVariable(
+        'RCUTILS_LOGGING_BUFFERED_STREAM', '1')
+
+    declare_params_file_cmd = DeclareLaunchArgument('params_file')
+
+    create_container = Node(
+        name='nav2_container',
+        package='rclcpp_components',
+        executable='component_container_isolated',
+        parameters=[configured_params, {'autostart': autostart}],
+        remappings=remappings,
+        output='screen')
+
+    load_composable_nodes = LoadComposableNodes(
+        target_container='nav2_container',
+        composable_node_descriptions=[
+            ComposableNode(
+                package='nav2_controller',
+                plugin='nav2_controller::ControllerServer',
+                name='controller_server',
+                parameters=[configured_params],
+                remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]),
+            ComposableNode(
+                package='opennav_coverage',
+                plugin='opennav_coverage::CoverageServer',
+                name='coverage_server',
+                parameters=[configured_params],
+                remappings=remappings),
+            ComposableNode(
+                package='nav2_bt_navigator',
+                plugin='nav2_bt_navigator::BtNavigator',
+                name='bt_navigator',
+                parameters=[configured_params],
+                remappings=remappings),
+            ComposableNode(
+                package='nav2_velocity_smoother',
+                plugin='nav2_velocity_smoother::VelocitySmoother',
+                name='velocity_smoother',
+                parameters=[configured_params],
+                remappings=remappings +
+                           [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]),
+            ComposableNode(
+                package='nav2_lifecycle_manager',
+                plugin='nav2_lifecycle_manager::LifecycleManager',
+                name='lifecycle_manager_navigation',
+                parameters=[{'use_sim_time': use_sim_time,
+                             'autostart': autostart,
+                             'node_names': lifecycle_nodes}]),
+        ],
+    )
+
+    # # Create the launch description and populate
+    ld = LaunchDescription()
+    ld.add_action(stdout_linebuf_envvar)
+    ld.add_action(declare_params_file_cmd)
+    ld.add_action(create_container)
+    ld.add_action(load_composable_nodes)
+    return ld

opennav_coverage_demo/launch/coverage_demo_launch.py

@@ -0,0 +1,100 @@
+# Copyright (c) 2023 Open Navigation LLC
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
+    coverage_demo_dir = get_package_share_directory('opennav_coverage_demo')
+
+    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')
+
+    # 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')
+    with open(urdf, 'r') as infp:
+        robot_description = infp.read()
+
+    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', '0.0', '-y', '0.0', '-z', '0.10',
+            '-R', '0.0', '-P', '0.0', '-Y', '0.0'])
+
+    # start the visualization
+    rviz_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')),
+        launch_arguments={'namespace': ''}.items())
+
+    # start navigation
+    bringup_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(coverage_demo_dir, 'bringup_launch.py')),
+        launch_arguments={'params_file': param_file_path}.items())
+
+    # world->odom transform, no localization. Mostly just for visualization.
+    fake_localization_cmd = Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'])
+
+    # start the demo task
+    demo_cmd = Node(
+        package='opennav_coverage_demo',
+        executable='demo_coverage',
+        emulate_tty=True,
+        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(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)
+    return ld