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 
[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.