Pose list node

launchers/default.sh

@@ -13,7 +13,8 @@ dt-launchfile-init
 # NOTE: Use `dt-exec COMMAND` to run the main process (blocking process)
 
 # launching app
-roslaunch perception perception.launch veh:=$VEHICLE_NAME
+roslaunch planning planning.launch veh:=$VEHICLE_NAME &
+roslaunch perception perception.launch veh:=$VEHICLE_NAME &
 # rosrun controls controls.launch
 
 

packages/planning/CMakeLists.txt

@@ -0,0 +1,9 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(planning)
+
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+  std_msgs
+)
+
+catkin_package()

packages/planning/launch/planning.launch

@@ -0,0 +1,11 @@
+<launch>
+    <group ns="$(arg veh)">
+
+        <node pkg="planning" type="pose_list_node.py"
+              name="pose_list" output="screen">
+            <remap from="~/pose_list"
+                     to="/$(arg veh)/pose_list"/>
+        </node>
+    </group>
+
+</launch>

packages/planning/package.xml

@@ -0,0 +1,11 @@
+<package>
+  <name>planning</name>
+  <version>0.1.0</version>
+  <description>
+    Template planning module for Duckietown
+  </description>
+  <maintainer email="YOUR_EMAIL@EXAMPLE.COM">YOUR_FULL_NAME</maintainer>
+  <license>None</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+</package>

packages/planning/src/pose_list_node.py

@@ -0,0 +1,40 @@
+#!/usr/bin/env python3
+
+import os
+import rospy
+import numpy as np
+import math
+from typing import Tuple
+from duckietown.dtros import DTROS, NodeType
+from std_msgs.msg import String, Header
+from as_msgs.msg import Pose
+
+
+class PoseListNode(DTROS):
+    def __init__(self, node_name) -> None:
+        super(PoseListNode, self).__init__(node_name=node_name, node_type=NodeType.GENERIC)
+        self.pose_list = []
+
+        example_pose = Pose()
+        example_pose.ID = 0
+        example_pose.x = 0
+        example_pose.y = 0
+        example_pose.heading = 0
+        self.pose_list.append(example_pose)
+        self.pub = rospy.Publisher('pose_list', String, queue_size=10)
+
+    def add_pose(self, pose: Pose):
+        self.pose_list.append(pose)
+
+    def run(self):
+        rate = rospy.Rate(0.1)
+        while not rospy.is_shutdown():
+            message = f"Nodes: {self.pose_list}"
+            # rospy.loginfo("pose list: '%s'" % message)
+            self.pub.publish(message)
+
+
+if __name__ == "__main__":
+    node = PoseListNode(node_name="pose_list")
+    node.run()
+    rospy.spin()