demo_inspection.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, TaskResult
import rclpy
 
"""
Basic stock inspection demo. In this demonstration, the expectation
is that there are cameras or RFID sensors mounted on the robots
collecting information about stock quantity and location.
"""
 
 
def main() -> None:
    rclpy.init()
 
    navigator = BasicNavigator()
 
    # Inspection route, probably read in from a file for a real application
    # from either a map or drive and repeat. (x, y, yaw)
    inspection_route = [
        [8.21, 1.3, 1.57],
        [8.7, 4.0, 1.57],
        [11.6, 4.0, -1.57],
        [11.6, 1.3, -1.57],
        [12.6, 1.3, 1.57],
        [12.6, 4.0, -1.57],
        [15.27, 1.3, -1.57],
        [16.4, 1.3, 1.57],
        [19.0, 1.3, 1.57],
        [19.0, 4.0, 1.57],
    ]
 
    # 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 = 0.0
    initial_pose.pose.position.y = 0.0
    initial_pose.pose.orientation.z = 0.0
    initial_pose.pose.orientation.w = 1.0
    navigator.setInitialPose(initial_pose)
 
    # Wait for navigation to fully activate
    navigator.waitUntilNav2Active()
 
    # Send our route
    inspection_points = []
    inspection_pose = PoseStamped()
    inspection_pose.header.frame_id = 'map'
    inspection_pose.header.stamp = navigator.get_clock().now().to_msg()
    for pt in inspection_route:
        inspection_pose.pose.position.x = pt[0]
        inspection_pose.pose.position.y = pt[1]
        # Simplification of angle handling for demonstration purposes
        if pt[2] > 0:
            inspection_pose.pose.orientation.z = 0.707
            inspection_pose.pose.orientation.w = 0.707
        else:
            inspection_pose.pose.orientation.z = -0.707
            inspection_pose.pose.orientation.w = 0.707
        inspection_points.append(deepcopy(inspection_pose))
 
    wpf_task = navigator.followWaypoints(inspection_points)
 
    # Do something during our route (e.x. AI to analyze stock information or upload to the cloud)
    # Simply the current waypoint ID for the demonstration
    i = 0
    while not navigator.isTaskComplete(task=wpf_task):
        i += 1
        feedback = navigator.getFeedback(task=wpf_task)
        if feedback and i % 5 == 0:
            print(
                'Executing current waypoint: '
                + str(feedback.current_waypoint + 1)
                + '/'
                + str(len(inspection_points))
            )
 
    result = navigator.getResult()
    if result == TaskResult.SUCCEEDED:
        print('Inspection of shelves complete! Returning to start...')
    elif result == TaskResult.CANCELED:
        print('Inspection of shelving was canceled. Returning to start...')
    elif result == TaskResult.FAILED:
        (error_code, error_msg) = navigator.getTaskError()
        print(f'Inspection of shelving failed!:{error_code}:{error_msg}')
        print('Returning to start...')
 
    # go back to start
    initial_pose.header.stamp = navigator.get_clock().now().to_msg()
    go_to_pose_task = navigator.goToPose(initial_pose)
    while not navigator.isTaskComplete(task=go_to_pose_task):
        pass
 
    exit(0)
 
 
if __name__ == '__main__':
    main()