ROS2 nav2 複数のwaypointを経由して動かすノードの実装（実装の備忘録）

参考

環境

ROS2、galactic

実装

ワークスペース（私のワークスペースはros2)のsrcに移動

cd
cd ros2/src

パッケージの作成

ros2 pkg create --build-type ament_python follow_waypoints

pythonの作成

cd follow_waypoints/follow_waypoints
sudo nano follow_waypoints.py

下記の内容をコピー&ペースト

#! /usr/bin/env python3
# Copyright 2021 Samsung Research America
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

from copy import deepcopy
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult
import rclpy
from rclpy.duration import Duration

"""
Basic navigation demo to go to poses.
"""
def main():
    rclpy.init()

    navigator = BasicNavigator()

    inspection_route = [
        #ここの座標を自分の設定したい座標に変更
        [-0.68568,0.48128],
        [1.57747,0.64474],
        [3.73398,0.785842]]


    # Set our demo's initial pose
    #initial_pose = PoseStamped()
    #initial_pose.header.frame_id = 'map'
    #initial_pose.header.stamp = navigator.get_clock().now().to_msg()
    #initial_pose.pose.position.x = 3.45
    #initial_pose.pose.position.y = 2.15
    #initial_pose.pose.orientation.z = 1.0
    #initial_pose.pose.orientation.w = 0.0
    #navigator.setInitialPose(initial_pose)

    # Activate navigation, if not autostarted. This should be called after setInitialPose()
    # or this will initialize at the origin of the map and update the costmap with bogus readings.
    # If autostart, you should `waitUntilNav2Active()` instead.
    # navigator.lifecycleStartup()

    # Wait for navigation to fully activate, since autostarting nav2
    navigator.waitUntilNav2Active()
    while rclpy.ok():
        inspection_poinsts = []
        inspection_pose = PoseStamped()
        inspection_pose.header.frame_id = 'map'
        inspection_pose.header.stamp = navigator.get_clock().now().to_msg()
        inspection_pose.pose.orientation.z = 0.0
        inspection_pose.pose.orientation.w = 0.0
        for pt in inspection_route:
            inspection_pose.pose.position.x = pt[0]
            inspection_pose.pose.position.y = pt[1]
            inspection_poinsts.append(deepcopy(inspection_pose))
        nav_start = navigator.get_clock().now()
        navigator.followWaypoints(inspection_poinsts)

        i = 0
        while not navigator.isNavComplete():
        ################################################
        #
        # Implement some code here for your application!
        #
        ################################################

        # Do something with the feedback
            i = i + 1
            feedback = navigator.getFeedback()
            if feedback and i % 5 == 0:
                print('Executing current waypoint: ' +
                      str(feedback.current_waypoint + 1) + '/' + str(len(inspection_poinsts)))

        # Do something depending on the return code
        result = navigator.getResult()
        if result == NavigationResult.SUCCEEDED:
            print('Goal succeeded!')
        elif result == NavigationResult.CANCELED:
            print('Goal was canceled!')
            exit(1)
        elif result == NavigationResult.FAILED:
            print('Goal failed!')
        else:
            print('Goal has an invalid return status!')
        
        while not navigator.isNavComplete:
            pass

        navigator.lifecycleShutdown()
        exit(0)

if __name__ == '__main__':
    main()

上記pythonファイルの下記の部分を自分の設定したいwaypointに変更する

[-0.68568,0.48128],
[1.57747,0.64474],
[3.73398,0.785842]]

権限の変更

chomod +x follow_waypoints.py

configファイルの作成

cd
cd ros2/src/follow_waypoints
mkdir config
cd config
sudo nano follow_waypoint.yaml

以下内容をコピー&ペースト

waypoint_follower:
  ros__parameters:
    loop_rate: 20
    stop_on_failure: false
    waypoint_task_executor_plugin: "wait_at_waypoint"
      wait_at_waypoint:
        plugin: "nav2_waypoint_follower::WaitAtWaypoint"
        enabled: True
        waypoint_pause_duration: 200

setup.pyの編集。
下記内容を参考に'follow_waypoints = follow_waypoints.follow_waypoints:main'を追記

    entry_points={
        'console_scripts': [
            'follow_waypoints = follow_waypoints.follow_waypoints:main'
        ],
    },
)

ビルド

cd 
cd ros2
colcon build
source install/setup.bash

実行

ros2 run follow_waypoints follow_waypoints