Not following Plan

[ValkyrX]

[Problem description]

Server Platform(s):

• OS: [e.g. Ubuntu 22.04] 20.04
• Architecture: [e.g. x86_64] x86_64
• ROS Version: [e.g. Humble] Noetic
• Branch/Tag: [e.g. ros2] Ros1

Client Platform(s):

• OS: [e.g. Android 14] Windows
• Architecture: [e.g. aarch64] 64bit
• Browser: [e.g. Chrome] edge and chrome

So , if i use the 2d nav goal then the robot will go where i tell it, if i set a plan using a few waypoints and then ensure the correct topic is selected and press go , nothing happens ? is this featire working at present ? i am using an slamtec athena 2.0 robot , the topic selected is the path plan topic , i select the map as frame and the base_link as teh robot , in fact i have tried them all :-)

Thanks
K

[MoffKalast]

Ah yes, well the problem is that the waypoints publisher isn't actually a standard thing and more of a way to send your backend some points that can be processed further.

I can't find any good documentation on the Athena so I'll presume it uses move_base or at least something that mimics it in the interface part.

In ROS 1 move_base only accepts single goals so there needs to be a node that takes the path, sends each one of them to move_base sequentially and check for completion, and if the robot can't actually get there, perform some kind of failsafe, e.g. return to the first point, or stop and abort, or call for help, or something else entirely. It's hard to build a general solution for it since it depends on the application.

For autonomous boats I've mainly used the waypoints publisher with the line_planner which does navigation without move_base entirely and supports Paths natively. Most of the wiki demo videos were done using that one. You could also try that as a test, but I don't think it'll be a good fit for your use case.

Overall implementing this is relatively simple so here's an untested sample from chatgpt that looks about right but doesn't include any exception handling 😄:

#!/usr/bin/env python

import rospy
import actionlib
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal

class WaypointFollower:
    def __init__(self):
        rospy.init_node('waypoint_follower')

        # Subscribe to the /waypoints topic
        rospy.Subscriber('/waypoints', Path, self.waypoints_callback)

        # Create a SimpleActionClient for move_base
        self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)

        # Wait for the move_base action server to start
        rospy.loginfo("Waiting for move_base action server...")
        self.client.wait_for_server()
        rospy.loginfo("Connected to move_base action server")

        self.waypoints = []
        self.current_goal_index = 0

    def waypoints_callback(self, msg):
        rospy.loginfo("Received waypoints")
        self.waypoints = msg.poses
        self.current_goal_index = 0
        self.send_next_goal()

    def send_next_goal(self):
        if self.current_goal_index < len(self.waypoints):
            goal = MoveBaseGoal()
            goal.target_pose = self.waypoints[self.current_goal_index]
            goal.target_pose.header.stamp = rospy.Time.now()
            rospy.loginfo(f"Sending goal {self.current_goal_index + 1}/{len(self.waypoints)}: {goal.target_pose.pose}")
            self.client.send_goal(goal, done_cb=self.done_callback)
        else:
            rospy.loginfo("All waypoints reached")

    def done_callback(self, state, result):
        rospy.loginfo(f"Goal {self.current_goal_index + 1} reached")
        self.current_goal_index += 1
        self.send_next_goal()

if __name__ == '__main__':
    try:
        WaypointFollower()
        rospy.spin()
    except rospy.ROSInterruptException:
        rospy.loginfo("Waypoint follower node terminated.")

For obvious reasons, this shouldn't be implemetned in the browser client, since the robot will stop navigating if you ever drop the connection which may be uh... problematic.

In ROS 2 and nav2 there is the navigate through poses functionality and a short demo node on how to pass the Path message to it, which is conceptually very similar to the code above. I suppose we should add a similar demo to Noetic as well.

[ValkyrX]

ps if it help here is the docs on the athena 2.0 ros implimentation and topics it uses

https://wiki.slamtec.com/pages/viewpage.action?pageId=36208700

[MoffKalast]

Oh interesting, that seems to be something else indeed. They've got this move_to_locations topic that seems to be conceptually similar.

So to use that you'd just need a node similar to the one above that takes the Poses given by the Path and only keeps the positions, shoves them into a Point array and publishes that with that message type. Then presumably it would do the whole path and handle all the goal completions internally.

But since they also implement the standard /move_base_simple/goal topic, the code above should also work fine I think. Unless they don't implement the action server, in which case it would need some slight modifications to work with simple goals instead.

[ValkyrX]

So i added the script above and ran it, however it doesn't move the bot so I guess I'm missing something else, like how-to pass that to the actual robot itself

[MoffKalast]

Yeah I guess the action server isn't implemented on their end. Maybe using the sdk message would work, though not sure:

#!/usr/bin/env python

import rospy
from nav_msgs.msg import Path
from geometry_msgs.msg import Point
from slamware_ros_sdk.msg import MoveToLocationsRequest, MoveOptions

class PathToMoveToLocations:
    def __init__(self):
        rospy.init_node('path_to_move_to_locations')
        
        self.publisher = rospy.Publisher('/move_to_locations', MoveToLocationsRequest, queue_size=10)
        
        rospy.Subscriber('/path', Path, self.path_callback)
        
        rospy.loginfo("Path to MoveToLocations node initialized.")
        
    def path_callback(self, path_msg):
        move_request = MoveToLocationsRequest()
        
        # Convert the Path waypoints to Point[]
        move_request.locations = [pose.pose.position for pose in path_msg.poses]
        
        # Set default MoveOptions
        move_request.options = MoveOptions()
        
        # Set end yaw
        move_request.yaw = 0.0
        
        # Publish the MoveToLocationsRequest message
        self.publisher.publish(move_request)

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

[gglaspell]

@MoffKalast I was thinking of using vizanti (ROS Noetic) with move_base_sequence (https://github.com/MarkNaeem/move_base_sequence). That should allow us to set multiple waypoints in vizanti for the UGV. I think the key is to set the 2D nav goal to /move_base_sequence/corner_pose. Furthermore, we should be able to set /move_base_sequence/wayposes for vizanti's waypoint mission planner. Lastly, the I think we can set the custom button to do a ros servicecall /move_base_sequence/toggle_state to pause/unpause the navigation stack.

It would be nice to incorporate vizanti's area planner with move_base_sequence somehow, but I am less clear on how that would happen.

[MoffKalast]

Hmm interesting, they went with a PoseArray instead of a Path (for the move_base_sequence). It's a minimal difference, the Poses just aren't Stamped. I suppose it might be worthwhile to add that type too, in case there's other infrastructure already using that same thing. A few visualizers are multi-type already so it's a reasonably well supported thing on the client side.

I think you can use the regular 2D Nav Goal publisher to feed new goals into corner_pose already, though it's unclear when it actually starts the route in that case.

[MoffKalast]

Hey @gglaspell it's been a while, but support for sending PoseArrays is now merged for both Noetic and Humble in case it helps.

[gglaspell]

@MoffKalast Thank you very much!! I have a demo in November with a UGV that I plan on using Vizanti as my mission planner. Being able to use the pose arrays with move base sequence is just in time. I will definitely pull it in the morning. I appreciate you taking the time to add that in.

I am also working up a technical report on the VRX simulator where I am using vizanti there too, but for a USV.

[MoffKalast]

Cool, I hope it helps 👍

I'll close this one for now and we can open a new issue if there's anything more.

I am also working up a technical report on the VRX simulator where I am using vizanti there too, but for a USV.

Ah yes I've tried that one out for testing the satelite view on ROS 2 initially. The WAM-V has a ball launcher that works with the button widget which is pretty neat, haha.

[gglaspell]

I just wanted to add that PoseArrays is working flawlessly, but you probably already knew that. Also, I am using the button feature to toggle pause/resume the navigation stack. Also, I have another button that clears the waypoints, which is pretty handy. Thanks again for adding PoseArrays.