diff --git a/launchers/default.sh b/launchers/default.sh old mode 100755 new mode 100644 index 7732486..d6bf4ff --- a/launchers/default.sh +++ b/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 diff --git a/packages/planning/CMakeLists.txt b/packages/planning/CMakeLists.txt new file mode 100644 index 0000000..bfb25cc --- /dev/null +++ b/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() \ No newline at end of file diff --git a/packages/planning/launch/planning.launch b/packages/planning/launch/planning.launch new file mode 100644 index 0000000..ce7113d --- /dev/null +++ b/packages/planning/launch/planning.launch @@ -0,0 +1,11 @@ + + + + + + + + + \ No newline at end of file diff --git a/packages/planning/package.xml b/packages/planning/package.xml new file mode 100644 index 0000000..88b377e --- /dev/null +++ b/packages/planning/package.xml @@ -0,0 +1,11 @@ + + planning + 0.1.0 + + Template planning module for Duckietown + + YOUR_FULL_NAME + None + + catkin + diff --git a/packages/planning/src/pose_list_node.py b/packages/planning/src/pose_list_node.py new file mode 100755 index 0000000..732cdd4 --- /dev/null +++ b/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()