search

ros-navigation / navigation2

ROS 2 Navigation Framework and System
https://nav2.org/
Other
2.52k stars 1.28k forks source link

In the running of goToPose(), cancelTask() does not work #4559

Closed atinfinity closed 2 months ago

atinfinity commented 2 months ago

Bug report

Required Info:

Steps to reproduce issue

I used https://github.com/atinfinity/megarover_samples_ros2.

sudo apt install ros-humble-rmw-cyclonedds-cpp
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
mkdir -p ~/dev_ws/src
cd ~/dev_ws/src
git clone https://github.com/atinfinity/megarover_samples_ros2.git
cd ..
rosdep install -y -i --from-paths src/megarover_samples_ros2
colcon build --symlink-install
source ~/dev_ws/install/setup.bash

And, I launched Gazebo and navigation2.

ros2 launch megarover_samples_ros2 vmegarover_with_sample_world.launch.py
ros2 launch megarover_samples_ros2 vmegarover_navigation.launch.py

I launched the following node using BasicNavigator(https://docs.nav2.org/commander_api/index.html).

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped
from std_msgs.msg import Empty
from nav2_simple_commander.robot_navigator import BasicNavigator

class ExampleNavigationNode(Node):

    def __init__(self):
        super().__init__('example_navigation')
        self.navigation_cancel_subscriber = self.create_subscription(
            Empty,
            '/navigation/cancel',
            self.sub_navigation_cancel_callback,
            10)
        timer_period = 1.0
        self.timer = self.create_timer(timer_period, self.timer_callback)

        self.goal_pose = PoseStamped()
        self.goal_pose.header.frame_id = 'map'
        self.goal_pose.header.stamp = self.get_clock().now().to_msg()
        self.goal_pose.pose.position.x = 4.0
        self.goal_pose.pose.position.y = 0.0
        self.goal_pose.pose.orientation.w = 1.0
        self.goal_pose.pose.orientation.z = 0.0

        self.navigator = BasicNavigator()
        self.navigator.waitUntilNav2Active()
        self.navigator.goToPose(self.goal_pose)

    def sub_navigation_cancel_callback(self, msg):
        self.get_logger().info("sub_navigation_cancel_callback")
        self.navigator.cancelTask()

    def timer_callback(self):
        # get path to goal
        path = self.navigator.getPath(PoseStamped(), self.goal_pose, use_start=False)

def main():
    rclpy.init()
    node = ExampleNavigationNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == "__main__":
    main()

And, I run the following command.

ros2 topic pub --once /navigation/cancel std_msgs/msg/Empty "{}"

I called getPath() to debug in timer callback. In this case, cancelTask() does not work.

Expected behavior

In the running of goToPose(), cancelTask() works.

Actual behavior

When I called getPath() in timer callback. In this case, cancelTask() does not work.

SteveMacenski commented 2 months ago

getPath probably already completed, so there's no action to cancel when your cancel subscription is triggered. cancelTask cancels on-going tasks, but getting a path is a sub-200ms (usually) task so it doesn't do anything. goToPose is a long-running task the length of your navigation run so that would still be on-going in order to be cancelable.

I don't know what " In this case, cancelTask() does not work." could mean in that context since there is no task happening to cancel (unless you timed your cancel really well with a cancelable, long-duration planning action)

atinfinity commented 2 months ago

Thank you for your comment. In this case, I expected that the goToPose() cancell.

SteveMacenski commented 2 months ago

The simple navigator assumes one action is called at a time and stores the future for that to use in cancellations / getting feedback. It looks like your application calls multiple actions, so you may need to handle your own futures / cancelations. The Simple Navigator is not meant to be a catch all for advanced applications, but a simple to use and understand baseline for students, prototypers, researchers, or creating proof of concepts.

I wouldn't be opposed to a PR that would maintain a dict of different running actions statuses that you could cancelTask(taskid='my_curr_task') with the particular current running action you'd like to cancel, but that was explicitly a non-requirement for the simple navigator's original design. Most applications do not require calling multiple actions simultaneously from the python script.

If you're not interested in creating a generalized solution, you can most easily get around this by creating your own client / handling for getPath yourself and leave the goToPose to the navigator object so that you don't call multiple actions using the navigator to override the navigation task's processing state (getPath is also a trivial and short action, so that's easy enough). Let the navigator handle navigation and any other actions you call are handled by your script separately. If its just getPath, that's probably easiest. If you plan to call even more things, I suggest working on a PR to manage multiple futures/states for actions to contribute back to the simple navigator (and then it'll be less-simple!)

atinfinity commented 2 months ago

Thank you for your advice. I understood. I'll consider it. So, I closed this issue.