Waypoint follower

If you forked the rosject (clicking on the link we provided to you earlier), you should have a package named follow_waypoints on your ros2_ws/src folder already, but for documentation purposes, and in case you want to know the baby steps, here is how we created that package.

First, in a fourth terminal we created that package:

cd ~/ros2_ws/src

ros2 pkg create --build-type ament_python follow_waypoints

By listing the content of that ~/ros2_ws/src folder, we see that the package has been created:

ls

# follow_waypoints  neo_local_planner2  neo_simulation2  neobotix_mp_400_navigation

That follow_waypoints package has a folder with the same name on it. On that folder, we created a file named follow_waypoints.py

cd ~/ros2_ws/src/follow_waypoints/follow_waypoints

touch follow_waypoints.py

chmod +x follow_waypoints.py

The touch command was used to create the file, and the chmod +x command was used to give execution permissions to that file (make it executable, basically)

We then pasted some content on the follow_waypoints.py file. You can see the content by opening that file using the Code Editor.

The content we pasted is basically a modified version of https://github.com/ros-planning/navigation2/blob/main/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py

Inspection Route – To navigation to – How to follow waypoints using nav2

 

On lines 33 to 36 we define an inspection_route variable, which essentially is an array that defines the waypoints (positions in the map) that the robot has to go when patrolling.

#! /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.

import time
from copy import deepcopy

from geometry_msgs.msg import PoseStamped
from rclpy.duration import Duration
import rclpy

from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult


def main():
    rclpy.init()

    navigator = BasicNavigator()

    # Inspection route, probably read in from a file for a real application
    # from either a map or drive and repeat.
    inspection_route = [ # simulation points
        [5.0, 0.0],
        [-5.0, -5.0],
        [-5.0, 5.0]]


    # 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)

    # Wait for navigation to fully activate
    navigator.waitUntilNav2Active()

    while rclpy.ok():

        # Send our route
        inspection_points = []
        inspection_pose = PoseStamped()
        inspection_pose.header.frame_id = 'map'
        inspection_pose.header.stamp = navigator.get_clock().now().to_msg()
        inspection_pose.pose.orientation.z = 1.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_points.append(deepcopy(inspection_pose))
        nav_start = navigator.get_clock().now()
        navigator.followWaypoints(inspection_points)

        # Do something during our route (e.x. AI to analyze stock information or upload to the cloud)
        # Simply print the current waypoint ID for the demonstation
        i = 0
        while not navigator.isNavComplete():
            i = i + 1
            feedback = navigator.getFeedback()
            if feedback and i % 5 == 0:
                print('Executing current waypoint: ' +
                    str(feedback.current_waypoint + 1) + '/' + str(len(inspection_points)))

        result = navigator.getResult()
        if result == NavigationResult.SUCCEEDED:
            print('Inspection of shelves complete! Returning to start...')
        elif result == NavigationResult.CANCELED:
            print('Inspection of shelving was canceled. Returning to start...')
            exit(1)
        elif result == NavigationResult.FAILED:
            print('Inspection of shelving failed! Returning to start...')

        # go back to start
        # initial_pose.header.stamp = navigator.get_clock().now().to_msg()
        # navigator.goToPose(initial_pose)
        while not navigator.isNavComplete:
            pass


if __name__ == '__main__':
    main()

cd ~/ros2_ws

colcon build; source install/setup.bash

cd ~/ros2_ws

source install/setup.bash

ros2 run follow_waypoints follow_waypoints_exe