demo_security.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
from rclpy.duration import Duration
 
"""
Basic security route patrol demo. In this demonstration, the expectation
is that there are security cameras mounted on the robots recording or being
watched live by security staff.
"""
 
 
def main() -> None:
    rclpy.init()
 
    navigator = BasicNavigator()
 
    # Security route, probably read in from a file for a real application
    # from either a map or drive and repeat.
    security_route = [
        [10.15, -0.77],
        [17.86, -0.77],
        [21.58, -3.5],
        [19.4, -6.4],
        [-6.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 = 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()
 
    # Do security route until dead
    while rclpy.ok():  # type: ignore[attr-defined]
        # Send our route
        route_poses = []
        pose = PoseStamped()
        pose.header.frame_id = 'map'
        pose.header.stamp = navigator.get_clock().now().to_msg()
        pose.pose.orientation.w = 1.0
        for pt in security_route:
            pose.pose.position.x = pt[0]
            pose.pose.position.y = pt[1]
            route_poses.append(deepcopy(pose))
        go_through_poses_task = navigator.goThroughPoses(route_poses)
 
        # Do something during our route (e.x. AI detection on camera images for anomalies)
        # Simply print ETA for the demonstration
        i = 0
        while not navigator.isTaskComplete(task=go_through_poses_task):
            i += 1
            feedback = navigator.getFeedback(task=go_through_poses_task)
            if feedback and i % 5 == 0:
                print(
                    'Estimated time to complete current route: '
                    + '{:.0f}'.format(
                        Duration.from_msg(feedback.estimated_time_remaining).nanoseconds
                        / 1e9
                    )
                    + ' seconds.'
                )
 
                # Some failure mode, must stop since the robot is clearly stuck
                if Duration.from_msg(feedback.navigation_time) > Duration(
                    seconds=180.0
                ):
                    print('Navigation has exceeded timeout of 180s, canceling request.')
                    navigator.cancelTask()
 
        # If at end of route, reverse the route to restart
        security_route.reverse()
 
        result = navigator.getResult()
        if result == TaskResult.SUCCEEDED:
            print('Route complete! Restarting...')
        elif result == TaskResult.CANCELED:
            print('Security route was canceled, exiting.')
            exit(1)
        elif result == TaskResult.FAILED:
            (error_code, error_msg) = navigator.getTaskError()
            print(f'Security route failed!:{error_code}:{error_msg}')
            print('Restarting from other side...')
 
    exit(0)
 
 
if __name__ == '__main__':
    main()