Virtual obstacles in move base

[gglaspell]

Time permitting, would it be possible to have the virtual fence in vizanti output in well-known text representation of geometry?

As an example of wkt: - POLYGON ((-12 30, 9 30, 9 -2, -12 -2))

The reason I ask is I often use this virtual obstacle package to create virtual fences in move_base for my UGV: https://github.com/GMahmoud/virtual_costmap_layer/tree/feature-big-refactoring

Note that the main branch works with Melodic, but the feature-big-refactoring branch works in Noetic.

The idea is to draw the virtual fence in vizanti and the virtual obstacles package would add the virtual fence to the virtual obstacle layer in move_base.

Note there is also a service call to clear the virtual obstacles which I think could be assigned to a custom button in vizanti.

Please let me know what you think. In my head, it is another way to tie vizanti's features into move_base. This is mainly for ROS Noetic. I think NAV2 has something built in to deal with no-go zones, but I haven't looked into it yet.

[MoffKalast]

Hmm given that it's Noetic specific and not entirely a part of the ROS convention I'm not entirely sure if it would make sense to add it natively.

I think a more practical option would be to instead have a helper node that does the conversion. One that subscribes to a Path/PoseArray topic and calls those services with the correct formatting, then you also have a lot more control over what gets called when, and you can also convert path data from other sources than just Vizanti. On the ros2 branch there is a demo node that does a similar thing for Nav2's NavigateThroughPoses.

Here's an example from gpt-4o that looks roughly correct at first glance but might needs some tweaking (to give you a general idea anyway):

#!/usr/bin/env python

import rospy
from nav_msgs.msg import Path
from virtual_costmap_layer.srv import AddElement, AddElementRequest, AddElementResponse
from virtual_costmap_layer.msg import Form
from geometry_msgs.msg import PoseStamped
import time

def path_to_wkt(path):
    """
    Converts a ROS Path message into Well-Known Text (WKT) POLYGON representation.
    """
    # Extract points from the Path message
    points = []
    for pose_stamped in path.poses:
        x = pose_stamped.pose.position.x
        y = pose_stamped.pose.position.y
        points.append((x, y))

    # Close the polygon by repeating the first point at the end
    if points[0] != points[-1]:
        points.append(points[0])

    # Create WKT POLYGON string
    wkt_polygon = "POLYGON ((" + ", ".join(f"{x} {y}" for x, y in points) + "))"
    return wkt_polygon

def path_callback(path_msg):
    """
    Callback function that is called when a new Path message is received.
    """
    rospy.loginfo("Received a new Path message. Converting to WKT...")

    # Convert the Path message to WKT format (Polygon)
    wkt_polygon = path_to_wkt(path_msg)

    # Prepare the AddElementRequest message for the service call
    add_element_req = AddElementRequest()
    form = Form()
    form.type = Form.TYPE_POLYGON  # Set the type as POLYGON
    form.data = wkt_polygon
    form.lifetime = rospy.Duration(0)  # Set lifetime to 0 (forever)

    add_element_req.form = form

    # Call the service to add the virtual costmap layer element
    try:
        rospy.loginfo("Calling the virtual_costmap_layer/add service...")
        add_element_service = rospy.ServiceProxy('virtual_costmap_layer/add', AddElement)
        response = add_element_service(add_element_req)

        if response.success:
            rospy.loginfo(f"Successfully added element to virtual costmap layer. UUID: {response.uuid}")
        else:
            rospy.logwarn(f"Failed to add element: {response.message}")
    except rospy.ServiceException as e:
        rospy.logerr(f"Service call failed: {e}")

def main():
    """
    Main function to initialize the ROS node and start the Path subscriber.
    """
    rospy.init_node('path_to_wkt_node')

    # Wait for the virtual_costmap_layer/add service to become available
    rospy.wait_for_service('virtual_costmap_layer/add')

    # Subscribe to the /fence topic that provides Path messages
    rospy.Subscriber('/fence', Path, path_callback)

    # Keep the node running
    rospy.loginfo("path_to_wkt_node is running and waiting for Path messages...")
    rospy.spin()

if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass

Note there is also a service call to clear the virtual obstacles which I think could be assigned to a custom button in vizanti.

Well it seems to be Trigger service call, and the button currently only works with Empty/Bool topic messages. It would be very useful to add that sometime though. Or just some general service manager where you can trigger any service similar to the reconfigure widget.

[gglaspell]

I am going to mark this as closed. I plan on trying this soon. I like the fact that your suggestion uses area planner to select the no go zones.