Merge branch 'main' into simulation-velocity-port

.gitignore

@@ -1,8 +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/*                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                   
-*.bag
+
+# 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 'chmod -R +x src/buggy/scripts/' >> ~/.bashrc
 
 # add mouse to tmux
 RUN echo 'set -g mouse on' >> ~/.tmux.conf

python-requirements.txt

@@ -5,6 +5,8 @@ numpy
 osqp
 pandas
 pymap3d
+pyproj
+pyserial
 scipy
 setuptools==58.2.0
 trimesh

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/buggy/visualization/telematics.py

@@ -0,0 +1,63 @@
+#! /usr/bin/env python3
+# Runs the conversion script for all telematics data
+
+import rclpy
+import utm
+from nav_msgs.msg import Odometry
+from rclpy.node import Node
+from sensor_msgs.msg import NavSatFix
+
+
+class Telematics(Node):
+    """
+    Converts subscribers and publishers that need to be reformated, so that they are readible.
+    """
+
+    def __init__(self):
+        """Generate all the subscribers and publishers that need to be reformatted.
+        """
+        super().__init__('telematics')
+
+        # Implements behavior of callback_args from rospy.Subscriber
+        def wrap_args(callback, callback_args):
+            return lambda msg: callback(msg, callback_args)
+
+        self.self_publisher = self.create_publisher(NavSatFix, "/self/state_navsatfix", 10)
+        self.self_subscriber = self.create_subscription(Odometry, "/self/state", wrap_args(self.convert_buggystate, self.self_publisher), 1)
+
+        self.other_publisher = self.create_publisher(NavSatFix, "/other/state_navsatfix", 10)
+        self.other_subscriber = self.create_subscription(Odometry, "/other/state", wrap_args(self.convert_buggystate, self.other_publisher), 1)
+
+    # TODO Make this a static method?
+    def convert_buggystate(self, msg, publisher):
+        """Converts BuggyState/Odometry message to NavSatFix and publishes to specified publisher
+        
+        Args:
+            msg (Odometry): Buggy state to convert
+            publisher (Publisher): Publisher to send NavSatFix message to
+        """
+        try:
+            y = msg.pose.pose.position.y
+            x = msg.pose.pose.position.x
+            lat, long = utm.to_latlon(x, y, 17, "T")
+            down = msg.pose.pose.position.z
+            new_msg = NavSatFix()
+            new_msg.header = msg.header
+            new_msg.latitude = lat
+            new_msg.longitude = long
+            new_msg.altitude = down
+            publisher.publish(new_msg)
+
+        except Exception as e:
+            self.get_logger().warn(
+                "Unable to convert other buggy position to lon lat" + str(e)
+            )
+
+
+if __name__ == "__main__":
+    rclpy.init()
+    telem = Telematics()
+    rclpy.spin(telem)
+
+    telem.destroy_node()
+    rclpy.shutdown()

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

@@ -1,16 +1,15 @@
 <launch>
 
     <node pkg="foxglove_bridge" exec="foxglove_bridge" name="foxglove" namespace="SC"/>
-    <node pkg="buggy" exec="hello_world" name="hello_world" 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="sim_single" name="NAND_sim_single" namespace="NAND">
-        <param name="velocity" value="12"/>
-        <param name="pose" value="Hill1_NAND"/>
+    <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 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>