from copy import deepcopy

from geometry_msgs.msg import PoseStamped

from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult

import rclpy

import math

"""

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.

From Ros2_learners repo

This version uses a text file to input waypoints. Robot heading is in degrees

North = 0, E = 90, S = 180, W = 270, matching nautical.

Simply put in x and y positions and rotation, separated by one space, on each line.

Line 38 has data file name in home folder. Position data must be float. Rotation is integer.

Do not end data file with blank line

"""

def lineToData(line):

"""Converts a raw line list into an

appropriate data format."""

return (float(line[0]), float(line[1]), int(line[2]))

def readData(fileName):

"""Generic data reading function:

reads lines in a text file and

splits them into lists."""

data = []

with open(fileName) as f:

for line in f.readlines():

data.append(lineToData(line.split()))

return data

#pairs = readData('upper_playground_data.txt')

#for x, y in pairs:

# print("pose position.", x)

# print("Pose position.", y)

def main():

rclpy.init()

navigator = BasicNavigator()

triples = readData('security_route_data.txt')

# Security route, read in from text file

# Set robot's initial pose in Rviz

# Wait for navigation to fully activate

navigator.waitUntilNav2Active()

# Do security route once

while rclpy.ok():

# Send our route

poses = []

pose = PoseStamped()

pose.header.frame_id = 'map'

pose.header.stamp = navigator.get_clock().now().to_msg()

for x, y, rotation in triples:

rotrot = 360 - rotation

# print(pairs)

pose.pose.position.x = x

pose.pose.position.y = y

# Convert Z rotation to quaternion

pose.pose.orientation.z = math.sin(math.radians(rotrot) / 2)

pose.pose.orientation.w = math.cos(math.radians(rotrot) / 2)

poses.append(deepcopy(pose))

# print(pose)

nav_start = navigator.get_clock().now()

navigator.followWaypoints(poses)

# Do something during our route (e.x. AI detection on camera images for anomalies)

# Simply print number of waypoint for the demonstation

i = 0

while not navigator.isTaskComplete():

i += 1

feedback = navigator.getFeedback()

if feedback and i % 5 == 0:

print('Executing current waypoint: : {0}/{1: <5}'.format(

str(feedback.current_waypoint + 1), str(len(poses))), end='\r')

# my_file.close()

exit(0)

if __name__ == '__main__':

main()