0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

path to trajectory c++ -> python

Last updated at Posted at 2024-08-21

#!/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()

0
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?