moved packages

.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
+
+# BAGS
+rb_ws/bags
 rb_ws/src/buggy/bags/*                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                   
 *.bag
+rb_ws/rosbag2*/*
+
+# VISION
 .vision*/*
 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 'cchmod -R +x  src/buggy/scripts/'
 
 # add mouse to tmux
 RUN echo 'set -g mouse on' >> ~/.tmux.conf

README_ROBOBUGGY2.md

@@ -137,8 +137,8 @@ Ask Software Lead (WIP)
 - UTM coordinates (assume we're in Zone 17N): `/sim_2d/utm` (geometry_msgs/Pose - position.x = Easting meters , position.y = Northing meters, position.z = heading in degrees from East axis + is CCW)
 - INS Simulation: `/nav/odom` (nsg_msgs/Odometry) (**Noise** is implemented to vary ~1cm)
 Commands:
-- Steering angle: `/buggy/steering` in degrees (example_interfaces/Float64)
-- Velocity: `/buggy/velocity` in m/s (example_interfaces/Float64)
+- Steering angle: `/buggy/steering` in degrees (std_msgs/Float64)
+- Velocity: `/buggy/velocity` in m/s (std_msgs/Float64)
 
 ### Auton Logic
 Ask someone with experience (WIP)

rb_ws/src/buggy/CMakeLists.txt

@@ -0,0 +1,40 @@
+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}
+)
+
+ament_package()

rb_ws/src/buggy/launch/sim_2d_double.xml

@@ -2,24 +2,29 @@
 
     <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="path_planner" name="SC_path_planner" namespace="SC">
+    <!-- <node pkg="buggy" exec="path_planner" name="SC_path_planner" namespace="SC">
         <param name="traj_name" value="buggycourse_safe.json"/>
-    </node>
+    </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"/>
+    <node pkg="buggy" exec="buggy_state_converter.py" name="NAND_state_converter" namespace="NAND"/>
+    <node pkg="buggy" exec="engine.py" name="NAND_sim_single" namespace="NAND">
+        <param name="velocity" value="10"/>
         <param name="pose" value="Hill1_NAND"/>
     </node>
+    <node pkg="buggy" exec="controller_node.py" name="NAND_controller" namespace="NAND">
+        <param name="dist" value="0.0"/>
+        <param name="traj_name" value="buggycourse_safe.json"/>
+        <param name="controller" value="stanley"/>
+    </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,23 @@
 <?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="root@todo.todo">root</maintainer>
-  <license>TODO: License declaration</license>
+  <version>2.0.0</version>
+  <description>CMU's First Robotic Buggy</description>
+  <maintainer email="officers@roboclub.com">CMU's Robotic Club Officers</maintainer>
+  <license>MIT</license>
 
-  <test_depend>ament_copyright</test_depend>
-  <test_depend>ament_flake8</test_depend>
-  <test_depend>ament_pep257</test_depend>
-  <test_depend>python3-pytest</test_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclpy</depend>
+
   <exec_depend>ros2launch</exec_depend>
 
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
   <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
 
@@ -6,10 +8,10 @@
 import rclpy
 from rclpy.node import Node
 
-from example_interfaces.msg import Float32, Float64, Bool
+from std_msgs.msg import Float32, Float64, Bool
 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.stanley_controller import StanleyController
 

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

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__':

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

@@ -1,3 +1,5 @@
+#!/usr/bin/env python3
+
 import sys
 from threading import Lock
 
@@ -6,10 +8,10 @@
 from rclpy.node import Node
 
 from nav_msgs.msg import Odometry
-from example_interfaces.msg import Float64
+from std_msgs.msg import Float64
 from TrajectoryMsg.msg import TrajectoryMsg
 
-sys.path.append("/rb_ws/src/buggy/buggy")
+sys.path.append("/rb_ws/src/buggy/scripts")
 from util.pose import Pose
 from util.trajectory import Trajectory
 

rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py → rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py

@@ -1,13 +1,15 @@
+#!/usr/bin/env python3
+
 import argparse
 from threading import Lock
 import threading
 
 import rclpy
-from rclpy import Node
+from rclpy.node import Node
 from scipy.spatial.transform import Rotation
 import utm
 
-from example_interfaces.msg import Float64, Int8, UInt8, Bool
+from std_msgs.msg import Float64, Int8, UInt8, Bool
 
 from host_comm import *
 

rb_ws/src/buggy/buggy/simulator/engine.py → rb_ws/src/buggy/scripts/simulator/engine.py

@@ -4,13 +4,13 @@
 import rclpy
 from rclpy.node import Node
 from geometry_msgs.msg import Pose, Twist, PoseWithCovariance, TwistWithCovariance
-from example_interfaces.msg import Float64
+from std_msgs.msg import Float64
 from sensor_msgs.msg import NavSatFix
 from nav_msgs.msg import Odometry
 import numpy as np
 import utm
-sys.path.append("/rb_ws/src/buggy/buggy")
-from rb_ws.src.buggy.buggy.util.constants import Constants
+sys.path.append("/rb_ws/src/buggy/scripts")
+from util.constants import Constants
 
 class Simulator(Node):
 
@@ -176,6 +176,7 @@ def loop(self):
         if self.tick_count % self.interval == 0:
             self.publish()
         self.tick_count += 1
+        self.get_logger().debug("SIMULATED UTM: ({}, {}), HEADING: {}".format(self.e_utm, self.n_utm, self.heading))
 
 
 def main(args=None):

rb_ws/src/buggy/buggy/watchdog/watchdog_node.py → rb_ws/src/buggy/scripts/watchdog/watchdog_node.py

@@ -1,7 +1,9 @@
+#!/usr/bin/env python3
+
 import rclpy
 from rclpy.node import Node
 
-from example_interfaces.msg import Bool
+from std_msgs.msg import Bool
 
 class Watchdog(Node):