Rviz UI

[FrGe2016]

I have used this ros package https://github.com/ethz-asl/polygon_coverage_planning. It is interesting because, It can handles obstacle polygons inside an external polygon that define the terrain limits. The ui is also interesting allowing for polygon drawing in rviz and capability to draw over a satellite image, row overlap etc. But has a few drawbacks :

• not ros 2
• not integrated in the navigation stack
• since it was conceive for drone, points in the path do not have orientation.

[SteveMacenski]

Sounds like good potential inhancements! This has the ability to process internal voids with the new F2C release and update in #53. The rviz drawing UI not so much, but that would be relatively straight forward to add. I actually did some of my testing by using the clicked_point topic in rviz to set points for tests, so its easy to subscribe to that to populate polygons (or create a whole rviz panel thing for additional options).

An effort from yourself or others interested in this capability would be greatly appreciated and I'd be happy to review / merge it to have that available for the community!

I hope you don't mind, I renamed the ticket to be a bit more descriptive

[Chuckwisc]

The opennav_coverage_demo is working for me, but I wanted to integrate opennav_coverage
into my project and use a rviz /clicked_point defined field_polygon.

I made a package, points_to_polygon, which takes /clicked_point, publishes a Polygon topic and an [x,y] topic.
Feel free to use the points_to_polygon package. I am new to ros2 and not totally sure of how I did things.

My project (jazzy) is based on linorobot2 (huumble), using linorobot2's rviz, slam and navigation launch.
The navigation.yaml is mainly the current Nav2 website navigation.yaml, with coverage server added.
To launch coverage server I use a simplified version of opennav_coverage_demo/launch
coverage_demo_launch.py modified to coverage.launch.py
bringup_launch.py modified to coverage_bringup.launch.py
since the linorobot2 launch already bring up the controller_server, bt_navigator and velocity_smoother.

The node in coverage.launch.py is points2polygon_coverage.py,
modified from opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py.
In the callback for the subscription to the [x,y] topic ( easier to use than the Polygon topic)
I feed the [x,y] list to navigateCoverage() instead of the demo_coverage.py field.
The [x,y] list from /clicked_point looks to be working, but the navigateCoverage() errors.

[points2polygon_coverage-1] [[-3.3, -4.5], [-2.1, 1.3], [1.5, 1.1], [0.4, -4.4], [-3.3, -4.5]]
[points2polygon_coverage-1] Waiting for bt_navigator to become active..
[points2polygon_coverage-1] Getting bt_navigator state...
[points2polygon_coverage-1] Result of get_state: active
[points2polygon_coverage-1] Waiting for 'NavigateCompleteCoverage' action server
[points2polygon_coverage-1] Navigating to with field of size: 5...
[points2polygon_coverage-1] Task with failed with status code: 6
[points2polygon_coverage-1] Goal failed!

I get a similar error if the demo_coverage.py field is fed to navigateCoverage() within points2polygon_coverage.py,
so think the issue is with my opennav_coverage launch (coverage.launch.py,coverage_bringup.launch.py).

The opennav_coverage README mentions an eventual tutorial about using the coverage server, which would be very helpful for integrating the coverage server into a project.

Here is the points_to_polygon -PointsToPolygonNode.py

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, HistoryPolicy
from geometry_msgs.msg import Point, Polygon, Point32
from geometry_msgs.msg import PointStamped
from std_msgs.msg import Float64MultiArray

class PointsToPolygonNode(Node):
def init(self):
super().init('points_to_polygon_node')

    # Parameters
            # Declare the parameter and get its value
    self.declare_parameter('default_max_points', 5)  # Default value if not provided
    self.default_max_points = self.get_parameter('default_max_points').get_parameter_value().integer_value
    self.max_points = self.default_max_points

    # Subscriber to Point messages - changed Point tp PointStamped
    #polygon_coverage_ros had clickPointCallback(    const geometry_msgs::PointStampedConstPtr& msg) 

    qos_profile = QoSProfile(
        depth=self.max_points,  # Store only the last self.max_points messages
        history=HistoryPolicy.KEEP_LAST  # Keep only the last N messages
    )
    self.point_subscriber = self.create_subscription(
        PointStamped,
        '/clicked_point',
        self.point_callback,
        qos_profile
    )
    print('made point sub')
    # only want last msg 
    qos_profile = QoSProfile(depth=1)
    # Publisher for Polygon messages
    self.polygon_publisher = self.create_publisher(
        Polygon,
        'points2polygon',
        qos_profile
    )
    # Publisher for xy list messages
    self.xy_publisher = self.create_publisher(
        Float64MultiArray,
        'points2xy',
        qos_profile
    )
  
    print('made field_boundry pub')
    # List to store points
    self.points_list = []
    self.xy_list = []

    self.get_logger().info('Node is ready to create polygons!')
    # changed Point to PointStamped , msg.x to msg.point.x etc
def point_callback(self, msg: PointStamped):
    print("start of point callback")
    print(msg.point.x)
    print(msg.point.y)
    # Convert Point to Point32
    point32 = Point32()
    point32.x = msg.point.x
    point32.y = msg.point.y
    point32.z = msg.point.z
    
    pair_to_check = [round(msg.point.x,1),round(msg.point.y,1)]
    if pair_to_check in self.xy_list:
        self.max_points = len(self.xy_list)
    else:
        self.xy_list.append([round(msg.point.x,1),round(msg.point.y,1)]) 

    # Add the point to the list
    self.points_list.append(point32)
    print('point appended')
    self.get_logger().info(f'Received Point: ({msg.point.x}, {msg.point.y}, {msg.point.z})')

    # Check if we have enough points to form a polygon
    if len(self.points_list) >= self.max_points:
        print('grt than max_points')
        # take first point and add as last point 
        self.points_list.append(self.points_list[0])
        self.publish_polygon()
        self.xy_list.append(self.xy_list[0])
        print(self.xy_list)
        self.publish_xy()
        self.xy_list = []

def publish_polygon(self):
    polygon = Polygon()
    polygon.points = self.points_list

    # Publish the Polygon message
    self.polygon_publisher.publish(polygon)
    self.get_logger().info(f'Published Polygon with {len(self.points_list)} points.')
    print('published polygon')
    # Clear the points list after publishing
    self.points_list = []

def publish_xy(self):
    # Prepare the message
    msg = Float64MultiArray()
    msg.data = [item for sublist in self.xy_list for item in sublist]  # Flatten the list

    # Publish the XY message
    self.xy_publisher.publish(msg)
    self.get_logger().info(f'Published XY with {len(self.xy_list)} points.')
    print('published xy')
    # Clear the points list after publishing
    self.points_list = []

def main(args=None):
rclpy.init(args=args)
print('Hi from points_to_polygon.')
node = PointsToPolygonNode()
rclpy.spin(node)

# Shutdown
node.destroy_node()
rclpy.shutdown()

if name == 'main':
main()

Here is the points2polygon_coverage.py ( modified from opennav_coverage_demo/demo_coverage.py).

#! /usr/bin/env python3

modified from

Copyright 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 enum import Enum
import time

from action_msgs.msg import GoalStatus
from geometry_msgs.msg import Point32, Polygon
from lifecycle_msgs.srv import GetState
from opennav_coverage_msgs.action import NavigateCompleteCoverage
import rclpy
from rclpy.action import ActionClient
from rclpy.duration import Duration
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray

class TaskResult(Enum):
UNKNOWN = 0
SUCCEEDED = 1
CANCELED = 2
FAILED = 3

class CoverageNavigatorTester(Node):

def __init__(self):
    super().__init__(node_name='coverage_navigator_tester')
    self.goal_handle = None
    self.result_future = None
    self.status = None
    self.feedback = None

    self.coverage_client = ActionClient(self, NavigateCompleteCoverage,
                                        'navigate_complete_coverage')
    self.node = Node('list_subscriber')
# Create the subscription within main
    self.subscription_xy = self.node.create_subscription(
        Float64MultiArray,
        '/points2xy',
        self.callback_function,
        1
    )
    self.subscription_xy  # prevent unused variable warning
    self.node.get_logger().info('List Subscriber Node started.')
    rclpy.spin(self.node)



def destroy_node(self):
    self.coverage_client.destroy()
    super().destroy_node()

def toPolygon(self, field):
    poly = Polygon()
    for coord in field:
        pt = Point32()
        pt.x = coord[0]
        pt.y = coord[1]
        poly.points.append(pt)
    return poly

def navigateCoverage(self, field):
    """Send a `NavToPose` action request."""
    print("Waiting for 'NavigateCompleteCoverage' action server")
    while not self.coverage_client.wait_for_server(timeout_sec=1.0):
        print('"NavigateCompleteCoverage" action server not available, waiting...')

    goal_msg = NavigateCompleteCoverage.Goal()
    goal_msg.frame_id = 'map'
    goal_msg.polygons.append(self.toPolygon(field))

    print('Navigating to with field of size: ' + str(len(field)) + '...')
    send_goal_future = self.coverage_client.send_goal_async(goal_msg,
                                                            self._feedbackCallback)
    rclpy.spin_until_future_complete(self, send_goal_future)
    self.goal_handle = send_goal_future.result()
    if not self.goal_handle.accepted:
        print('Navigate Coverage request was rejected!')
        return False
    self.result_future = self.goal_handle.get_result_async()
    return True

def isTaskComplete(self):
    """Check if the task request of any type is complete yet."""
    if not self.result_future:
        # task was cancelled or completed
        return True
    rclpy.spin_until_future_complete(self, self.result_future, timeout_sec=0.10)
    if self.result_future.result():
        self.status = self.result_future.result().status
        if self.status != GoalStatus.STATUS_SUCCEEDED:
            print(f'Task with failed with status code: {self.status}')
            return True
    else:
        # Timed out, still processing, not complete yet
        return False

    print('Task succeeded!')
    return True

def _feedbackCallback(self, msg):
    self.feedback = msg.feedback
    return

def getFeedback(self):
    """Get the pending action feedback message."""
    return self.feedback

def getResult(self):
    """Get the pending action result message."""
    if self.status == GoalStatus.STATUS_SUCCEEDED:
        return TaskResult.SUCCEEDED
    elif self.status == GoalStatus.STATUS_ABORTED:
        return TaskResult.FAILED
    elif self.status == GoalStatus.STATUS_CANCELED:
        return TaskResult.CANCELED
    else:
        return TaskResult.UNKNOWN

def startup(self, node_name='bt_navigator'):
    # Waits for the node within the tester namespace to become active
    print(f'Waiting for {node_name} to become active..')
    node_service = f'{node_name}/get_state'
    state_client = self.create_client(GetState, node_service)
    while not state_client.wait_for_service(timeout_sec=1.0):
        print(f'{node_service} service not available, waiting...')


    req = GetState.Request()
    state = 'unknown'
    while state != 'active':
        print(f'Getting {node_name} state...')
        future = state_client.call_async(req)
        rclpy.spin_until_future_complete(self, future)
        if future.result() is not None:
            state = future.result().current_state.label
            print(f'Result of get_state: {state}')
        time.sleep(2)
    return


def callback_function(self, msg):
    # Convert the flat list back into a nested list
    flat_list = msg.data
    nested_list = self.convert_to_nested_list(flat_list)
    print(nested_list)
    self.latest_message = msg.data
    self.latest_nested_list = nested_list
    # Pass the nested list to a function
    #navigator = CoverageNavigatorTester()

    self.startup()
    self.navigateCoverage(nested_list)

    i = 0
    while not self.isTaskComplete():
    # Do something with the feedback
        i = i + 1
        feedback = self.getFeedback()
        if feedback and i % 5 == 0:
            print('Estimated time of arrival: ' + '{0:.0f}'.format(
                  Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9)
                + ' seconds.')
        time.sleep(1)
        # Do something depending on the return code
    result = self.getResult()
    if result == TaskResult.SUCCEEDED:
        print('Goal succeeded!')
    elif result == TaskResult.CANCELED:
        print('Goal was canceled!')
    elif result == TaskResult.FAILED:
        print('Goal failed!')
    else:
        print('Goal has an invalid return status!')


@staticmethod
def convert_to_nested_list(flat_list):
    # Assuming each pair has 2 elements
    return [[flat_list[i], flat_list[i + 1]] for i in range(0, len(flat_list), 2)]

def main():
rclpy.init()

self.navigator = CoverageNavigatorTester()
#self.navigator.startup()

if name == 'main':
main()

Here is the coverage.launch.py.

modified from

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
import tempfile

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
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

def generate_launch_description():

linorobot2_navigation_dir = get_package_share_directory('linorobot2_navigation')


param_file_path = os.path.join(linorobot2_navigation_dir,'config','navigation.yaml')


    # start the demo task
node_cmd = Node(
    package='opennav_coverage_demo',
    executable='points2polygon_coverage',
    emulate_tty=True,
    output='screen',
)


# start navigation
bringup_cmd = IncludeLaunchDescription(
    PythonLaunchDescriptionSource(os.path.join(linorobot2_navigation_dir, 'launch','coverage_bringup.launch.py')),
    launch_arguments={'params_file': param_file_path}.items(),
)


ld = LaunchDescription()
ld.add_action(node_cmd)
ld.add_action(bringup_cmd)

return ld

Here is the coverage_bringup.launch.py

modified from

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 = ['coverage_server']

remappings = [('/tf', 'tf'),
              ('/tf_static', 'tf_static')]

# Create our own temporary YAML files that include substitutions
autostart = True
use_sim_time = False #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')



load_composable_nodes = LoadComposableNodes(
    target_container='nav2_container',
    composable_node_descriptions=[
        ComposableNode(
            package='opennav_coverage',
            plugin='opennav_coverage::CoverageServer',
            name='coverage_server',
            parameters=[configured_params],
            remappings=remappings),
        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(load_composable_nodes)
return ld