From 5eb9af1e3f45a46e7918ccdddb5f7de9ae16b891 Mon Sep 17 00:00:00 2001 From: Ben Przybyszewski Date: Tue, 28 Feb 2023 14:38:18 -0600 Subject: [PATCH 1/7] add new package for planning template node --- packages/planning/CMakeLists.txt | 8 ++++++++ packages/planning/launch/controls.launch | 6 ++++++ packages/planning/package.xml | 11 +++++++++++ 3 files changed, 25 insertions(+) create mode 100644 packages/planning/CMakeLists.txt create mode 100644 packages/planning/launch/controls.launch create mode 100644 packages/planning/package.xml diff --git a/packages/planning/CMakeLists.txt b/packages/planning/CMakeLists.txt new file mode 100644 index 0000000..96cb423 --- /dev/null +++ b/packages/planning/CMakeLists.txt @@ -0,0 +1,8 @@ +cmake_minimum_required(VERSION 2.8.3) +project(controls) + +find_package(catkin REQUIRED COMPONENTS + rospy +) + +catkin_package() \ No newline at end of file diff --git a/packages/planning/launch/controls.launch b/packages/planning/launch/controls.launch new file mode 100644 index 0000000..da372ee --- /dev/null +++ b/packages/planning/launch/controls.launch @@ -0,0 +1,6 @@ + + + + + + \ 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 + From 1b676d0defaa0214f1ea3b68c2d38e87dbff79c0 Mon Sep 17 00:00:00 2001 From: Ben Przybyszewski Date: Tue, 28 Feb 2023 14:50:01 -0600 Subject: [PATCH 2/7] add start of PoseListNode template --- packages/planning/src/pose_list_node.py | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) create mode 100644 packages/planning/src/pose_list_node.py diff --git a/packages/planning/src/pose_list_node.py b/packages/planning/src/pose_list_node.py new file mode 100644 index 0000000..c1f495b --- /dev/null +++ b/packages/planning/src/pose_list_node.py @@ -0,0 +1,24 @@ +#!/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 = [] + + def add_pose(self, pose: Pose): + self.pose_list.append(pose) + + +if __name__ == "__main__": + node = PoseListNode(node_name="pose_list") + rospy.spin() From 2f2e846ecbefe8661095221d870ea8d8d6971274 Mon Sep 17 00:00:00 2001 From: Ben Przybyszewski Date: Tue, 28 Feb 2023 14:53:29 -0600 Subject: [PATCH 3/7] update launch script --- packages/planning/launch/controls.launch | 6 ------ packages/planning/launch/planning.launch | 9 +++++++++ 2 files changed, 9 insertions(+), 6 deletions(-) delete mode 100644 packages/planning/launch/controls.launch create mode 100644 packages/planning/launch/planning.launch diff --git a/packages/planning/launch/controls.launch b/packages/planning/launch/controls.launch deleted file mode 100644 index da372ee..0000000 --- a/packages/planning/launch/controls.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - \ 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..171cdb7 --- /dev/null +++ b/packages/planning/launch/planning.launch @@ -0,0 +1,9 @@ + + + + + + + + \ No newline at end of file From 8ea49b60983d3a36d44fbe9d927622387598bb13 Mon Sep 17 00:00:00 2001 From: Ben Przybyszewski Date: Tue, 28 Feb 2023 16:18:08 -0600 Subject: [PATCH 4/7] add publisher node for list of poses --- packages/planning/src/pose_list_node.py | 9 +++++++++ 1 file changed, 9 insertions(+) mode change 100644 => 100755 packages/planning/src/pose_list_node.py diff --git a/packages/planning/src/pose_list_node.py b/packages/planning/src/pose_list_node.py old mode 100644 new mode 100755 index c1f495b..3a74460 --- a/packages/planning/src/pose_list_node.py +++ b/packages/planning/src/pose_list_node.py @@ -14,11 +14,20 @@ class PoseListNode(DTROS): def __init__(self, node_name) -> None: super(PoseListNode, self).__init__(node_name=node_name, node_type=NodeType.GENERIC) self.pose_list = [] + 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() From a10fd64c20e6622d39e2eda5400bc0183f5a8a64 Mon Sep 17 00:00:00 2001 From: Ben Przybyszewski Date: Tue, 28 Feb 2023 16:53:05 -0600 Subject: [PATCH 5/7] fix issue with launching --- packages/planning/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/packages/planning/CMakeLists.txt b/packages/planning/CMakeLists.txt index 96cb423..bfb25cc 100644 --- a/packages/planning/CMakeLists.txt +++ b/packages/planning/CMakeLists.txt @@ -1,8 +1,9 @@ cmake_minimum_required(VERSION 2.8.3) -project(controls) +project(planning) find_package(catkin REQUIRED COMPONENTS rospy + std_msgs ) catkin_package() \ No newline at end of file From 15f1231d8d8d3a0b1ee3425c225dad7f8b32485e Mon Sep 17 00:00:00 2001 From: Ben Przybyszewski Date: Tue, 28 Feb 2023 16:54:04 -0600 Subject: [PATCH 6/7] configure launching with perception and planning package --- launchers/default.sh | 3 ++- packages/planning/launch/planning.launch | 2 ++ 2 files changed, 4 insertions(+), 1 deletion(-) mode change 100755 => 100644 launchers/default.sh 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/launch/planning.launch b/packages/planning/launch/planning.launch index 171cdb7..ce7113d 100644 --- a/packages/planning/launch/planning.launch +++ b/packages/planning/launch/planning.launch @@ -3,6 +3,8 @@ + From 2210c1db2631b7aeb7c74df18cd9a4e4687eb65c Mon Sep 17 00:00:00 2001 From: Ben Przybyszewski Date: Thu, 2 Mar 2023 14:41:04 -0600 Subject: [PATCH 7/7] add example starting pose --- packages/planning/src/pose_list_node.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/packages/planning/src/pose_list_node.py b/packages/planning/src/pose_list_node.py index 3a74460..732cdd4 100755 --- a/packages/planning/src/pose_list_node.py +++ b/packages/planning/src/pose_list_node.py @@ -14,6 +14,13 @@ 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):