Fails to move to the goal point

Hello guys,

I am trying to make the robot move on Construct as directed from ROS Navigation In 5 Days.
To make things simpler I am just trying to make the robot move to where you put the “publish point” via rviz2.

I get a very weird response, I can see the path on rviz, but the robot doesn’t move at all.

(It does move when I set the “2D Goal Pose” but I want to make it move on “publish point” as well)

Here is the python script:

#!/usr/bin/env python
from nav2_msgs.action import NavigateToPose
from action_msgs.msg import GoalStatus
from geometry_msgs.msg import PointStamped

import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node


class NavToPoseActionClient(Node):

    def __init__(self):
        super().__init__('Nav_To_Pose_Action_Client')
        self._action_client = ActionClient(
            self, NavigateToPose, '/navigate_to_pose')
        self.subscriber_ = self.create_subscription(
            PointStamped, 'clicked_point', self.callback, 1)

    def callback(self, pointStamped):
        self.get_logger().info('Recieved Data:\n X : %f \n Y : %f \n Z : %f' %
                               (pointStamped.point.x, pointStamped.point.y, pointStamped.point.z))

        self.send_goal(pointStamped)

    def send_goal(self, pointStamped):

        goal_pose = NavigateToPose.Goal()
        goal_pose.pose.header.frame_id = 'map'
        goal_pose.pose.pose.position.x = pointStamped.point.x
        goal_pose.pose.pose.position.y = pointStamped.point.y
        goal_pose.pose.pose.position.z = pointStamped.point.z

        self.get_logger().info('waiting for action server')
        self._action_client.wait_for_server()
        self.get_logger().info('action server detected')

        self._send_goal_future = self._action_client.send_goal_async(
            goal_pose,
            feedback_callback=self.feedback_callback)

        self._send_goal_future.add_done_callback(self.goal_response_callback)

    def goal_response_callback(self, future):
        goal_handle = future.result()

        if not goal_handle.accepted:
            self.get_logger().info('Goal rejected :(')
            return

        self.get_logger().info('Goal accepted :)')

        self._get_result_future = goal_handle.get_result_async()

        self._get_result_future.add_done_callback(self.get_result_callback)

    def get_result_callback(self, future):

        result = future.result().result
        status = future.result().status

        if status == GoalStatus.STATUS_SUCCEEDED:
            self.get_logger().info('Navigation succeeded! ')
        else:
            self.get_logger().info(
                'Navigation failed with status: {0}'.format(status))

    def feedback_callback(self, feedback_msg):
        feedback = feedback_msg.feedback


def main(args=None):
    rclpy.init(args=args)

    action_client = NavToPoseActionClient()

    rclpy.spin(action_client)


if __name__ == '__main__':
    main()

Here is the launch file:

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():
    map_file = os.path.join(
        get_package_share_directory('nav2_project'), 'config', 'turtlebot_area.yaml')
    nav2_yaml = os.path.join(
        get_package_share_directory('nav2_project'), 'config', 'amcl_config.yaml')
    controller_yaml = os.path.join(
        get_package_share_directory('nav2_project'), 'config', 'controller.yaml')
    bt_navigator_yaml = os.path.join(
        get_package_share_directory('nav2_project'), 'config', 'bt_navigator.yaml')
    planner_yaml = os.path.join(
        get_package_share_directory('nav2_project'), 'config', 'planner_server.yaml')
    recovery_yaml = os.path.join(
        get_package_share_directory('nav2_project'), 'config', 'recovery.yaml')

    return LaunchDescription([
        Node(
            package='nav2_planner',
            executable='planner_server',
            name='planner_server',
            output='screen',
            parameters=[planner_yaml]
        ),
        Node(
            package='nav2_controller',
            executable='controller_server',
            output='screen',
            parameters=[controller_yaml]
        ),
        Node(
            package='nav2_bt_navigator',
            executable='bt_navigator',
            name='bt_navigator',
            output='screen',
            parameters=[bt_navigator_yaml]
        ),
        Node(
            package='nav2_recoveries',
            executable='recoveries_server',
            name='recoveries_server',
            parameters=[recovery_yaml],
            output='screen'
        ),
        Node(
            package='nav2_map_server',
            executable='map_server',
            name='map_server',
            output='screen',
            parameters=[{'use_sim_time': True},
                        {'yaml_filename': map_file}
                        ]),
        Node(
            package='nav2_amcl',
            executable='amcl',
            name='amcl',
            output='screen',
            parameters=[nav2_yaml]
        ),
        Node(
            package='nav2_lifecycle_manager',
            executable='lifecycle_manager',
            name='lifecycle_manager_localization',
            output='screen',
            parameters=[{'autostart': True},
                        {'node_names': ['map_server',
                                        'amcl',
                                        'planner_server',
                                        'controller_server',
                                        'recoveries_server',
                                        'bt_navigator']}]
        ),
        Node(
            package='nav2_project',
            executable='nav2_to_pose',
            name='Nav_To_Pose_Action_Client',
            output='screen',
            parameters=[]
        )
    ])

As you can see, the point gets received with no problem, the Goal is also accepted but then it throws an error.

[nav2_to_pose-8] [INFO] [1673352152.027592317] [Nav_To_Pose_Action_Client]: Recieved Data:
[nav2_to_pose-8] X : 0.732929
[nav2_to_pose-8] Y : -0.247461
[nav2_to_pose-8] Z : 0.005278
[nav2_to_pose-8] [INFO] [1673352152.042409259] [Nav_To_Pose_Action_Client]: waiting for action server
[nav2_to_pose-8] [INFO] [1673352152.043535508] [Nav_To_Pose_Action_Client]: action server detected
[bt_navigator-3] [INFO] [1673352152.046623188] [bt_navigator]: Begin navigating from current location to (0.73, -0.25)
[nav2_to_pose-8] [INFO] [1673352152.080267036] [Nav_To_Pose_Action_Client]: Goal accepted :slight_smile:
[controller_server-2] [INFO] [1673352152.219180241] [controller_server]: Received a goal, begin computing control effort.
[ERROR] [controller_server-2]: process has died [pid 18113, exit code -11, cmd ‘/opt/ros/foxy/lib/nav2_controller/controller_server --ros-args --params-file /home/user/ros2_ws/install/nav2_project/share/nav2_project/config/controller.yaml’].
[bt_navigator-3] [ERROR] [1673352153.296289697] [bt_navigator]: Action server failed while executing action callback: “send_goal failed”
[bt_navigator-3] [WARN] [1673352153.296357049] [bt_navigator]: [navigate_to_pose] [ActionServer] Aborting handle.
[nav2_to_pose-8] [INFO] [1673352153.305228181] [Nav_To_Pose_Action_Client]: Navigation failed with status: 6

Do you have any ideas on why this happens?

Thank you for your time.

Hi @NoN33d ,

I have taken the liberty to change the “Course Support Category” from ROS Navigation in 5 Days to ROS2 Navigation.

Could you please provide more info?

  1. Which unit are you working on?
  2. What is the exercise number?
  3. Is this the course or rosject?
  4. If this is rosject, then which part of rosject?

Regards,
Girish

Hello @girishkumar.kannan ,

  1. ROS Navigation
  2. IV, part 3 “Create a script that moves the robot to a given spot”
  3. Rosject
  4. Part IV

I was on the course ROS Navigation in 5 Days, on chapter 5, on the end of the notebook it says “Finish the real robot project Section IV”. So I moved there and continued to Section IV.

To make things simpler I try to just move the robot with “publish point” like I did for that course (since this is more simple, if that works I can then start working on the exercise).

Thank you,
Nikos

Hi @NoN33d ,

I am confused with your answer.

Are you on ROS1 or ROS2?
The program code and launch file you posted belong to ROS2.
But you are indicating me that you are on ROS Navigation - Rosject Part 4.

ROS1 uses rospy
ROS2 uses rclpy

“publish point” is mentioned in ROS2 Navigation and not in ROS(1) Navigation.

I am assuming that you know ROS1 and ROS2 are different.
Are you programming a ROS1 rosject with ROS2? or the other way around?

– Girish

Sorry about that, I didn’t know ROS 1 is still an option, I am programming a ROS 2 project.

Hi @NoN33d ,

Looking at your error, I think you need to work around with controller.yaml configuration values.

Play around with controller frequency and tolerance values and see if you can get it working.

Regards,
Girish

Hello,

But why does it work with 2D Goal Pose? Using that the robot is moving with no issues.

I will give a try though.

Thank you,
Nikos

After tweaking more found some values that works! It moves! Thanks @girishkumar.kannan !

So sadly it seems like it worked like once and then stopped working again…
I don’t get why this happen, is there a way to find out why the robot is not moving?

Here are the controller.yaml parameters, it worked once with this setup:

controller_server:
  ros__parameters:
    use_sim_time: True
    controller_frequency: 20.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    failure_tolerance: 0.3
    progress_checker_plugin: "progress_checker"
    goal_checker_plugins: ["general_goal_checker"] 
    controller_plugins: ["FollowPath"]

    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0
    # Goal checker parameters
    general_goal_checker:
      stateful: True
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.25
      yaw_goal_tolerance: 0.25
    # DWB parameters
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      debug_trajectory_details: True
      min_vel_x: 0.0
      min_vel_y: 0.0
      max_vel_x: 0.26
      max_vel_y: 0.0
      max_vel_theta: 1.0
      min_speed_xy: 0.0
      max_speed_xy: 0.26
      min_speed_theta: 0.0
      acc_lim_x: 2.5
      acc_lim_y: 0.0
      acc_lim_theta: 3.2
      decel_lim_x: -2.5
      decel_lim_y: 0.0
      decel_lim_theta: -3.2
      vx_samples: 20
      vy_samples: 5
      vtheta_samples: 20
      sim_time: 1.7
      linear_granularity: 0.05
      angular_granularity: 0.025
      transform_tolerance: 0.2
      xy_goal_tolerance: 0.25
      trans_stopped_velocity: 0.25
      short_circuit_trajectory_evaluation: True
      stateful: True
      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
      BaseObstacle.scale: 0.02
      PathAlign.scale: 32.0
      PathAlign.forward_point_distance: 0.1
      GoalAlign.scale: 24.0
      GoalAlign.forward_point_distance: 0.1
      PathDist.scale: 32.0
      GoalDist.scale: 24.0
      RotateToGoal.scale: 32.0
      RotateToGoal.slowing_factor: 5.0
      RotateToGoal.lookahead_time: -1.0

Hi @NoN33d ,

I just read through your controller.yaml file. I can’t find robot_radius or footprint defined. maybe that is one of the reasons that your robot is not moving.

Another common reason would be not terminating your teleop program, if you have it running.
If you moved your robot with teleop and stopped the robot later with teleop, and you have not terminated teleop, then robot will not move if you give the goal poses.

Check these two issues and let me know if you can start your robot to move!

Regards,
Girish

Hello and thank you for the help.

I did add the robot_radius but still it doesn’t work, nothing changed, I publish the point on the map, the goal gets received, the path is drawn correctly but then nothing happens, robot is stopped and I just get that same error message as before.

[bt_navigator-3] [ERROR] [1673533077.058700696] [bt_navigator]: Action server failed while executing action callback: "send_goal failed"
[bt_navigator-3] [WARN] [1673533077.058899262] [bt_navigator]: [navigate_to_pose] [ActionServer] Aborting handle.
[nav2_to_pose-8] [INFO] [1673533077.062720798] [Nav_To_Pose_Action_Client]: Navigation failed with status: 6
[ERROR] [controller_server-2]: process has died [pid 7708, exit code -11, cmd '/opt/ros/foxy/lib/nav2_controller/controller_server --ros-args --params-file /home/user/ros2_ws/install/nav2_project/share/nav2_project/config/controller.yaml'].

Also I don’t use the teleop program at all.

Thank you,
Nikos

Hello again,

Is there maybe any rosject that I can run on the constructsim (ROS2 Foxy) that uses ‘/navigate_to_pose’ to take a quick look and compare what I am doing wrong?

If I have a working example I could find out what is wrong with it.

Thank you,
Nikos

Hi @NoN33d ,

Honestly, I have just finished the ROS2 Navigation course and ROS1 Navigation Rosject, meaning, I have not started my ROS2 Navigation rosject yet. So I guess, you should play around a bit with the parameters for the nav2 stack. Refer Foxy documentation page for nav2 stack parameters that are available. Parameters may be less for Foxy version compared to Galactic.

You could probably find some public rosject. You can search for those rosjects on your homepage (https://app.theconstructsim.com/Home). Click on the button that says “Public Rosjects”.

Regards,
Girish

@NoN33d
Thank you for the details given so far. One of our experts will have a look today.

@girishkumar.kannan
Thank you for assisting with this!

Hi @NoN33d,

Is your Nav2 setup working without your script for published points? What I mean is, are you able to launch the navigation system every time and navigate through the 2D Goal Pose button on Rviz? You need to be sure that this works reliably before adding a script further down the pipeline.

If so, then it must be something in your script that is causing the controller server node to die. I see that there is a message “Navigation failed with status: 6.” I couldn’t find it with a quick search, but a look in your system would be a good idea to see what that status: 6 means.

As for the rosject, let me see if I can get one to you. However, you can directly use navigate_to_pose in the terminal in the meantime to test (Tip: ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "p.. - type quotations and the p for pose and then autocomplete with tab). The whole command:

ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "pose:
  header:
    stamp:
      sec: 0
      nanosec: 0
    frame_id: ''
  pose:
    position:
      x: 0.0
      y: 0.0
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0

This is the same as sending a goal through Rviz2

Hi @NoN33d ,

I think I got the solution for this problem.

Refer to this post here: Path planning error - ROS2 Navigation course - #3 by rtellez

What you should do to get the robot move (and reach) the goal pose:

  1. Replace your behavior.xml file with the file contents provided in the above post.
  2. Replace your bt_navigator.yaml file with the file contents provided in the above post.
  3. Very Important: Make sure you change the path to the behavior.xml file in the bt_navigator.yaml file. You must provide the path to “your” behavior.xml file.

Once you do follow these steps correctly, you can send a goal pose to your robot either with 2d Goal Pose button on Rviz2 or use the action server NavigateToPose.

After following these steps, when you send a goal pose, you will still see the following error message:

[bt_navigator-3] [ERROR] [<timestamp>] [bt_navigator]: Action server failed while executing action callback: "send_goal failed"

Just ignore this message!
Can this be solved?
Short answer: No, not in Foxy.
Long answer: Yes, you need to update Foxy to Galactic, this problem is fixed in Galactic. Problem is that our rosject environment uses Foxy and we cannot update it to Galactic by ourselves unless TheConstruct team updates. So don’t worry about this!

Let me know if this worked for you. [This worked for me, so it should work for you too!]

Regards,
Girish

Hello @roalgoal and @girishkumar.kannan and thank you both for the help.

Yesterday I made it to work, I will write here what I did, maybe it will help someone else as well.

First of all @roalgoal 2D Goal Pose used to work, but when I tested it yesterday it didn’t for some reason.

So what I did was to delete the package and re-create it without any custom scripts just the default localization and navigation, but that didn’t help. I was having the same errors when trying to set a 2D Goal Pose (I don’t know how that worked before).

What made it work was splitting the localization and navigation into different packages.
Using exact the same setup but split into two different packages (one with amcl and map server, and the other one with controller, bt navigator, planner server and recovery) made it work.

The other thing that I was doing wrong, and was causing the robot not to work reliably was how I handled the simulation and the execution of the programs.

After a lot of trial and error, the workflow that works 100% for that rosject is:

  1. Kill the bridge (on the terminal).
  2. Reset the environment from the UI.
  3. Start the bridge.
  4. Run the programs.

Then I put my own custom logic and that works as well :slight_smile:

(@girishkumar.kannan I didn’t have the time to test your solution but I sure will, maybe it will be able to handle both localization and navigation with one package)

Thank you for the support!

Hi @NoN33d ,

I experienced something similar and did something too!
I loaded map_server and amcl with one lifecycle_manager and used another lifecycle_manager for the other 4 path planning nodes. I could still load it from one single launch file instead of two files like you did.

I experienced the exact same thing also! This is also exactly my solution to fix.

Maybe it will, but my solution is the link to correct versions of behavior.xml and bt_navigator.yaml files for Foxy version. What you have is for Galactic version (I believe), which might give you some problems.

Regards,
Girish

This topic was automatically closed 5 days after the last reply. New replies are no longer allowed.