pathological_test.py

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from rclpy.duration import Duration
from rmf_fleet_msgs.msg import PathRequest, Location

rclpy.init(args=None)
node = Node("test_slotcar")

path = PathRequest()
path.fleet_name = "tinyRobot"
path.robot_name = "tinyRobot2"
path.task_id = "hello2"

curr_t = node.get_clock().now()
print(curr_t)
duration = Duration(seconds = 1)
location1 = Location()
#location1.t = (curr_t + duration).to_msg()
location1.x = 11.651602745056152
location1.y = -6.966331958770752
location1.yaw = -3.129042387008667
#location1.level_name = "L1"
path.path.append(location1)

location2 = Location()
#location2.t = (curr_t + duration +duration).to_msg()
location2.x = 18.739524841308594
location2.y = -6.873981952667236
location2.yaw = -3.129042387008667
location2.level_name = "L1"

num_split = 100
for i in range(1,num_split):
    dx = location2.x - location1.x
    dy = location2.y - location1.y
    location = Location()
    location.x = i*dx/num_split + location1.x
    location.y = i*dy/num_split + location1.y
    location.yaw = location2.yaw
    path.path.append(location)

path.path.append(location2)
    
location3 = Location()
#location3.t = (curr_t + duration +duration +duration).to_msg()
location3.x = 18.739524841308594
location3.y = -6.873981952667236
location3.yaw = -1.563750982284546
location3.level_name = "L1"
path.path.append(location3)

location4 = Location()
#location4.t = (curr_t + duration +duration +duration).to_msg()
location4.x = 18.75492286682129
location4.y = -9.059619903564453
location4.yaw = -1.563750982284546
location4.level_name = "L1"
path.path.append(location4)


pub = node.create_publisher(PathRequest, 'robot_path_requests', 1)
pub.publish(path)

This script isn't able to work since rclpy.spin(node) is never called.