Path planner

.gitignore

@@ -1,10 +1,15 @@
 .DS_Store
-rb_ws/bags
 .docker-compose.yml.un~
 .python-requirements.txt.un~
 docker-compose.yml~
 *TEMP_DO_NOT_EDIT.txt
-rb_ws/src/buggy/bags/                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                   
+
+# BAGS
+rb_ws/bags
+rb_ws/src/buggy/bags/*                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                  
 *.bag
+rb_ws/rosbag2/
+
+# VISION
 .vision/
-vision/data/
+vision/data/

Dockerfile

@@ -23,16 +23,8 @@ RUN pip3 install -r python-requirements.txt
 RUN echo 'source "/opt/ros/humble/setup.bash" --' >> ~/.bashrc && \
     echo 'cd rb_ws' >> ~/.bashrc && \
     echo 'colcon build --symlink-install' >> ~/.bashrc && \
-    echo 'source install/local_setup.bash' >> ~/.bashrc
-# RUN 	echo 'source "/opt/ros/humble/setup.bash" --' >> ~/.bashrc && \
-#     echo 'cd rb_ws' >> ~/.bashrc && \
-#     echo 'catkin_make >/dev/null' >> ~/.bashrc && \
-#     echo 'source devel/setup.bash' >> ~/.bashrc
-
-
-
-# RUN echo "exec firefox" > ~/.xinitrc && chmod +x ~/.xinitrc
-# CMD ["x11vnc", "-create", "-forever"]
+    echo 'source install/local_setup.bash' >> ~/.bashrc && \
+    echo 'chmod -R +x src/buggy/scripts/' >> ~/.bashrc
 
 # add mouse to tmux
 RUN echo 'set -g mouse on' >> ~/.tmux.conf

rb_ws/src/buggy/CMakeLists.txt

@@ -0,0 +1,48 @@
+cmake_minimum_required(VERSION 3.8)
+project(buggy)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclpy REQUIRED)
+
+# Install Launch Files
+install(DIRECTORY
+  launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+# Install Python modules
+# ament_python_install_package(${PROJECT_NAME})
+
+# Install Python executables
+install(PROGRAMS
+  scripts/hello_world.py
+  scripts/controller/controller_node.py
+  scripts/path_planner/path_planner.py
+  scripts/simulator/engine.py
+  scripts/watchdog/watchdog_node.py
+  scripts/buggy_state_converter.py
+  scripts/serial/ros_to_bnyahaj.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/TrajectoryMsg.msg"
+)
+ament_export_dependencies(rosidl_default_runtime)
+
+ament_package()

rb_ws/src/buggy/launch/sim_2d_double.xml

@@ -0,0 +1,31 @@
+<launch>
+
+    <node pkg="foxglove_bridge" exec="foxglove_bridge" name="foxglove" output = "screen" namespace="SC"/>
+
+    <node pkg="buggy" exec="buggy_state_converter.py" name="NAND_state_converter" output = "screen" namespace="NAND"/>
+    <node pkg="buggy" exec="controller_node.py" name="NAND_controller" output = "screen" namespace="NAND">
+        <param name="dist" value="0.0"/>
+        <param name="traj_name" value="buggycourse_safe.json"/>
+        <param name="controller" value="stanley"/>
+    </node>
+    <node pkg="buggy" exec="engine.py" name="NAND_sim_single" output = "screen" namespace="NAND">
+        <param name="velocity" value="8"/>
+        <param name="pose" value="Hill1_NAND"/>
+    </node>
+
+    <node pkg="buggy" exec="buggy_state_converter.py" name="SC_state_converter" output = "screen" namespace="SC"/>
+    <node pkg="buggy" exec="controller_node.py" name="SC_controller" output = "screen" namespace="SC">
+        <param name="dist" value="0.0"/>
+        <param name="traj_name" value="buggycourse_safe.json"/>
+        <param name="controller" value="stanley"/>
+    </node>
+    <node pkg="buggy" exec="engine.py" name="SC_sim_single" output = "screen" namespace="SC">
+        <param name="velocity" value="12"/>
+        <param name="pose" value="Hill1_SC"/>
+    </node>
+    <node pkg="buggy" exec="path_planner.py" name="SC_path_planner" namespace="SC" output = "screen">
+        <param name="traj_name" value="buggycourse_safe.json"/>
+        <remap from="other/state" to="/NAND/self/state"/>
+    </node>
+
+</launch>

rb_ws/src/buggy/launch/sim_2d_single.xml

@@ -2,23 +2,14 @@
 
     <node pkg="foxglove_bridge" exec="foxglove_bridge" name="foxglove" namespace="SC"/>
 
-    <node pkg="buggy" exec="sim_single" name="SC_sim_single" namespace="SC">
+    <node pkg="buggy" exec="engine.py" name="SC_sim_single" namespace="SC">
         <param name="velocity" value="12"/>
         <param name="pose" value="Hill1_SC"/>
     </node>
-    <node pkg="buggy" exec="buggy_state_converter" name="SC_state_converter" namespace="SC"/>
-    <node pkg="buggy" exec="controller" name="SC_controller" namespace="SC">
+    <node pkg="buggy" exec="buggy_state_converter.py" name="SC_state_converter" namespace="SC"/>
+    <node pkg="buggy" exec="controller_node.py" name="SC_controller" namespace="SC">
         <param name="dist" value="0.0"/>
         <param name="traj_name" value="buggycourse_safe.json"/>
         <param name="controller" value="stanley"/>
     </node>
-
-    <!-- <node pkg="buggy" exec="buggy_state_converter" name="NAND_state_converter" namespace="NAND"/>
-    <node pkg="buggy" exec="sim_single" name="NAND_sim_single" namespace="NAND">
-        <param name="velocity" value="12"/>
-        <param name="pose" value="Hill1_NAND"/>
-    </node> -->
-
-    <!-- <node name="foxglove" pkg="foxglove_bridge" type="foxglove_bridge" />
-    <node name="hello_world" pkg="buggy" type="hello_world" /> -->
 </launch>

rb_ws/src/buggy/launch/watchdog.xml

@@ -1,4 +1,4 @@
 <launch>
     <node pkg="foxglove_bridge" exec="foxglove_bridge" name="foxglove" namespace="SC"/>
-    <node pkg="buggy" exec="watchdog" name="SC_watchdog" namespace="SC"/>
+    <node pkg="buggy" exec="watchdog_node.py" name="SC_watchdog" namespace="SC"/>
 </launch>

rb_ws/src/buggy/package.xml

@@ -2,18 +2,30 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>buggy</name>
-  <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="[email protected]">root</maintainer>
-  <license>TODO: License declaration</license>
+  <version>2.0.0</version>
+  <description>CMU's First Robotic Buggy</description>
+  <maintainer email="[email protected]">CMU's Robotic Club Officers</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+
+
+  <depend>rclcpp</depend>
+  <depend>rclpy</depend>
+
 
-  <test_depend>ament_copyright</test_depend>
-  <test_depend>ament_flake8</test_depend>
-  <test_depend>ament_pep257</test_depend>
-  <test_depend>python3-pytest</test_depend>
   <exec_depend>ros2launch</exec_depend>
 
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+
   <export>
-    <build_type>ament_python</build_type>
+    <build_type>ament_cmake</build_type>
   </export>
-</package>
+</package>

rb_ws/src/buggy/buggy/controller/controller_node.py → rb_ws/src/buggy/scripts/controller/controller_node.py

@@ -1,3 +1,5 @@
+#!/usr/bin/env python3
+
 import threading
 import sys
 
@@ -8,8 +10,9 @@
 
 from std_msgs.msg import Float32, Float64, Bool
 from nav_msgs.msg import Odometry
+from buggy.msg import TrajectoryMsg
 
-sys.path.append("/rb_ws/src/buggy/buggy")
+sys.path.append("/rb_ws/src/buggy/scripts")
 from util.trajectory import Trajectory
 from controller.stanley_controller import StanleyController
 
@@ -55,12 +58,12 @@ def __init__(self):
             Float64, "input/steering", 1
         )
         self.heading_publisher = self.create_publisher(
-            Float32, "auton/debug/heading", 1
+            Float32, "debug/heading", 1
         )
 
         # Subscribers
         self.odom_subscriber = self.create_subscription(Odometry, 'self/state', self.odom_listener, 1)
-        self.traj_subscriber = self.create_subscription(Odometry, 'self/cur_traj', self.traj_listener, 1)
+        self.traj_subscriber = self.create_subscription(TrajectoryMsg, 'self/cur_traj', self.traj_listener, 1)
 
         self.lock = threading.Lock()
 
@@ -139,7 +142,7 @@ def loop(self):
         self.heading_publisher.publish(Float32(data=np.rad2deg(self.odom.pose.pose.orientation.z)))
         steering_angle = self.controller.compute_control(self.odom, self.cur_traj)
         steering_angle_deg = np.rad2deg(steering_angle)
-        self.steer_publisher.publish(Float64(data=float(steering_angle_deg)))
+        self.steer_publisher.publish(Float64(data=float(steering_angle_deg.item())))
 
 
 

rb_ws/src/buggy/buggy/controller/controller_superclass.py → rb_ws/src/buggy/scripts/controller/controller_superclass.py

@@ -4,7 +4,7 @@
 from nav_msgs.msg import Odometry
 
 
-sys.path.append("/rb_ws/src/buggy/buggy")
+sys.path.append("/rb_ws/src/buggy/scripts")
 from util.trajectory import Trajectory
 
 class Controller(ABC):

rb_ws/src/buggy/buggy/controller/stanley_controller.py → rb_ws/src/buggy/scripts/controller/stanley_controller.py

@@ -5,7 +5,7 @@
 from geometry_msgs.msg import Pose as ROSPose
 from nav_msgs.msg import Odometry
 
-sys.path.append("/rb_ws/src/buggy/buggy")
+sys.path.append("/rb_ws/src/buggy/scripts")
 from util.trajectory import Trajectory
 from controller.controller_superclass import Controller
 from util.pose import Pose
@@ -30,10 +30,10 @@ class StanleyController(Controller):
     def __init__(self, start_index, namespace, node):
         super(StanleyController, self).__init__(start_index, namespace, node)
         self.debug_reference_pos_publisher = self.node.create_publisher(
-            NavSatFix, "auton/debug/reference_navsat", 1
+            NavSatFix, "controller/debug/reference_navsat", 1
         )
         self.debug_error_publisher = self.node.create_publisher(
-            ROSPose, "auton/debug/error", 1
+            ROSPose, "controller/debug/stanley_error", 1
         )
 
     def compute_control(self, state_msg : Odometry, trajectory : Trajectory):

rb_ws/src/buggy/buggy/hello_world.py → rb_ws/src/buggy/scripts/hello_world.py

@@ -1,5 +1,7 @@
+#!/usr/bin/env python3
+
 def main():
-    print('Hi from buggy. Hello again')
+    print('Hi from buggy.')
 
 
 if __name__ == '__main__':