#!/usr/bin/env python3
import rclpy
import rclpy.node
import rclpy.qos
import rclpy.executors
from autoware_auto_planning_msgs.msg import PathWithLaneId
from autoware_auto_planning_msgs.msg import Trajectory
from autoware_auto_planning_msgs.msg import TrajectoryPoint
from std_msgs.msg import Float64MultiArray
class PyPTTNode(rclpy.node.Node):
def __init__(self):
super().__init__("py_ptt")
self.point_counter = 0
self.counter = 0
self.objectlist = []
self.sub = self.create_subscription(PathWithLaneId, "input", self.path_to_trajactory, 1)
self.pub = self.create_publisher(Trajectory, "output", 1)
self.sub2 = self.create_subscription(Float64MultiArray, "/aichallenge/objects", self.object_listener, 1)
def path_to_trajactory(self, msg):
#print(msg.data)
trajectory = Trajectory()
trajectory.header = msg.header
for path_point_with_lane_id in msg.points:
trajectory_point = TrajectoryPoint()
trajectory_point.pose = path_point_with_lane_id.point.pose
self.get_logger().info(f"{self.point_counter}th pose {trajectory_point.pose}")
self.point_counter += 1
trajectory_point.longitudinal_velocity_mps = path_point_with_lane_id.point.longitudinal_velocity_mps
trajectory.points.append(trajectory_point)
self.pub.publish(trajectory)
def object_listener(self, msg):
# when received first data
if len(msg.data)>0:
#self.get_logger().info(f"{len(msg.data)}")
# when add first objects position
if len(msg.data) == 4 and self.counter == 0:
self.objectlist = [[msg.data[0],msg.data[1],msg.data[2],msg.data[3]]]
self.get_logger().info(f"{self.counter}th position x : {self.objectlist[0][0]}")
self.get_logger().info(f"{self.counter}th position y : {self.objectlist[0][1]}")
self.get_logger().info(f"{self.counter}th position z : {self.objectlist[0][2]}")
self.get_logger().info(f"{self.counter}th scale x : {self.objectlist[0][3]*2}")
self.get_logger().info(f"{self.counter}th scale y : {self.objectlist[0][3]*2}")
self.counter += 1
# from second to end of object data
elif(len(msg.data)//4 > self.counter):
count = len(msg.data)//4 -1
self.objectlist.append([msg.data[count*4 + 0],msg.data[count*4 + 1],msg.data[count*4 + 2],msg.data[count*4 + 3]])
self.get_logger().info(f"{self.counter}th position x : {self.objectlist[count][0]}")
self.get_logger().info(f"{self.counter}th position y : {self.objectlist[count][1]}")
self.get_logger().info(f"{self.counter}th position z : {self.objectlist[count][2]}")
self.get_logger().info(f"{self.counter}th scale x : {self.objectlist[count][3]*2}")
self.get_logger().info(f"{self.counter}th scale y : {self.objectlist[count][3]*2}")
self.counter += 1
else:
self.get_logger().info("No Data In Object listener")
def main(args=None):
rclpy.init(args=args)
lon = PyPTTNode()
rclpy.spin(lon)
rclpy.shutdown()
if name == "main":
main()