diff --git a/.gitignore b/.gitignore
index e8edffcc..2af114f4 100644
--- a/.gitignore
+++ b/.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*/*
\ No newline at end of file
diff --git a/Dockerfile b/Dockerfile
index 6725e778..b855e3c5 100644
--- a/Dockerfile
+++ b/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
diff --git a/README_ROBOBUGGY2.md b/README_ROBOBUGGY2.md
index 6e48fe55..d92d1421 100755
--- a/README_ROBOBUGGY2.md
+++ b/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)
diff --git a/rb_ws/src/buggy/CMakeLists.txt b/rb_ws/src/buggy/CMakeLists.txt
new file mode 100644
index 00000000..3fc99baa
--- /dev/null
+++ b/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()
\ No newline at end of file
diff --git a/rb_ws/src/buggy/launch/controller.xml b/rb_ws/src/buggy/launch/controller.xml
deleted file mode 100644
index 52f0ac7f..00000000
--- a/rb_ws/src/buggy/launch/controller.xml
+++ /dev/null
@@ -1,4 +0,0 @@
-
-
-
-
\ No newline at end of file
diff --git a/rb_ws/src/buggy/launch/sim_2d_double.xml b/rb_ws/src/buggy/launch/sim_2d_double.xml
index c256114e..1fdaecd6 100644
--- a/rb_ws/src/buggy/launch/sim_2d_double.xml
+++ b/rb_ws/src/buggy/launch/sim_2d_double.xml
@@ -2,24 +2,29 @@
-
+
-
-
+
+
-
+
-
-
-
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/rb_ws/src/buggy/launch/sim_2d_single.xml b/rb_ws/src/buggy/launch/sim_2d_single.xml
index 2c62b1b1..f0e72042 100755
--- a/rb_ws/src/buggy/launch/sim_2d_single.xml
+++ b/rb_ws/src/buggy/launch/sim_2d_single.xml
@@ -2,23 +2,14 @@
-
+
-
-
+
+
-
-
-
-
\ No newline at end of file
diff --git a/rb_ws/src/buggy/launch/watchdog.xml b/rb_ws/src/buggy/launch/watchdog.xml
index 28c784df..5c74690e 100644
--- a/rb_ws/src/buggy/launch/watchdog.xml
+++ b/rb_ws/src/buggy/launch/watchdog.xml
@@ -1,4 +1,4 @@
-
+
\ No newline at end of file
diff --git a/rb_ws/src/buggy/msg/TrajectoryMsg.msg b/rb_ws/src/buggy/msg/TrajectoryMsg.msg
deleted file mode 100644
index 9c952ccf..00000000
--- a/rb_ws/src/buggy/msg/TrajectoryMsg.msg
+++ /dev/null
@@ -1,4 +0,0 @@
-float64 time
-float64 cur_idx
-float64[] easting
-float64[] northing
\ No newline at end of file
diff --git a/rb_ws/src/buggy/package.xml b/rb_ws/src/buggy/package.xml
index e1471e2c..68bf6c04 100644
--- a/rb_ws/src/buggy/package.xml
+++ b/rb_ws/src/buggy/package.xml
@@ -2,18 +2,23 @@
buggy
- 0.0.0
- TODO: Package description
- root
- TODO: License declaration
+ 2.0.0
+ CMU's First Robotic Buggy
+ CMU's Robotic Club Officers
+ MIT
- ament_copyright
- ament_flake8
- ament_pep257
- python3-pytest
+ ament_cmake
+ ament_cmake_python
+
+ rclcpp
+ rclpy
+
ros2launch
+ ament_lint_auto
+ ament_lint_common
+
- ament_python
+ ament_cmake
-
+
\ No newline at end of file
diff --git a/rb_ws/src/buggy/resource/buggy b/rb_ws/src/buggy/resource/buggy
deleted file mode 100644
index e69de29b..00000000
diff --git a/rb_ws/src/buggy/buggy/buggy_state_converter.py b/rb_ws/src/buggy/scripts/buggy_state_converter.py
old mode 100644
new mode 100755
similarity index 100%
rename from rb_ws/src/buggy/buggy/buggy_state_converter.py
rename to rb_ws/src/buggy/scripts/buggy_state_converter.py
diff --git a/rb_ws/src/buggy/buggy/controller/controller_node.py b/rb_ws/src/buggy/scripts/controller/controller_node.py
old mode 100644
new mode 100755
similarity index 98%
rename from rb_ws/src/buggy/buggy/controller/controller_node.py
rename to rb_ws/src/buggy/scripts/controller/controller_node.py
index 117cf715..7d312133
--- a/rb_ws/src/buggy/buggy/controller/controller_node.py
+++ b/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
diff --git a/rb_ws/src/buggy/buggy/controller/controller_superclass.py b/rb_ws/src/buggy/scripts/controller/controller_superclass.py
old mode 100644
new mode 100755
similarity index 96%
rename from rb_ws/src/buggy/buggy/controller/controller_superclass.py
rename to rb_ws/src/buggy/scripts/controller/controller_superclass.py
index f960de75..731c96ee
--- a/rb_ws/src/buggy/buggy/controller/controller_superclass.py
+++ b/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):
diff --git a/rb_ws/src/buggy/buggy/controller/stanley_controller.py b/rb_ws/src/buggy/scripts/controller/stanley_controller.py
old mode 100644
new mode 100755
similarity index 99%
rename from rb_ws/src/buggy/buggy/controller/stanley_controller.py
rename to rb_ws/src/buggy/scripts/controller/stanley_controller.py
index 34076535..4c5b10fd
--- a/rb_ws/src/buggy/buggy/controller/stanley_controller.py
+++ b/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
diff --git a/rb_ws/src/buggy/buggy/hello_world.py b/rb_ws/src/buggy/scripts/hello_world.py
old mode 100644
new mode 100755
similarity index 50%
rename from rb_ws/src/buggy/buggy/hello_world.py
rename to rb_ws/src/buggy/scripts/hello_world.py
index 0a119aa0..0a2d9ba7
--- a/rb_ws/src/buggy/buggy/hello_world.py
+++ b/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__':
diff --git a/rb_ws/src/buggy/buggy/path_planner/path_planner.py b/rb_ws/src/buggy/scripts/path_planner/path_planner.py
old mode 100644
new mode 100755
similarity index 99%
rename from rb_ws/src/buggy/buggy/path_planner/path_planner.py
rename to rb_ws/src/buggy/scripts/path_planner/path_planner.py
index c7742907..3357eee5
--- a/rb_ws/src/buggy/buggy/path_planner/path_planner.py
+++ b/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
diff --git a/rb_ws/src/buggy/buggy/serial/host_comm.py b/rb_ws/src/buggy/scripts/serial/host_comm.py
old mode 100644
new mode 100755
similarity index 100%
rename from rb_ws/src/buggy/buggy/serial/host_comm.py
rename to rb_ws/src/buggy/scripts/serial/host_comm.py
diff --git a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py
old mode 100644
new mode 100755
similarity index 98%
rename from rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py
rename to rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py
index edb77ce8..2f8810fb
--- a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py
+++ b/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 *
diff --git a/rb_ws/src/buggy/buggy/simulator/engine.py b/rb_ws/src/buggy/scripts/simulator/engine.py
old mode 100644
new mode 100755
similarity index 96%
rename from rb_ws/src/buggy/buggy/simulator/engine.py
rename to rb_ws/src/buggy/scripts/simulator/engine.py
index 84c9e393..26d82c67
--- a/rb_ws/src/buggy/buggy/simulator/engine.py
+++ b/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):
diff --git a/rb_ws/src/buggy/buggy/util/constants.py b/rb_ws/src/buggy/scripts/util/constants.py
old mode 100644
new mode 100755
similarity index 100%
rename from rb_ws/src/buggy/buggy/util/constants.py
rename to rb_ws/src/buggy/scripts/util/constants.py
diff --git a/rb_ws/src/buggy/buggy/util/pose.py b/rb_ws/src/buggy/scripts/util/pose.py
old mode 100644
new mode 100755
similarity index 100%
rename from rb_ws/src/buggy/buggy/util/pose.py
rename to rb_ws/src/buggy/scripts/util/pose.py
diff --git a/rb_ws/src/buggy/buggy/util/trajectory.py b/rb_ws/src/buggy/scripts/util/trajectory.py
old mode 100644
new mode 100755
similarity index 100%
rename from rb_ws/src/buggy/buggy/util/trajectory.py
rename to rb_ws/src/buggy/scripts/util/trajectory.py
diff --git a/rb_ws/src/buggy/buggy/watchdog/watchdog_node.py b/rb_ws/src/buggy/scripts/watchdog/watchdog_node.py
old mode 100644
new mode 100755
similarity index 96%
rename from rb_ws/src/buggy/buggy/watchdog/watchdog_node.py
rename to rb_ws/src/buggy/scripts/watchdog/watchdog_node.py
index 7a044f09..7c155e32
--- a/rb_ws/src/buggy/buggy/watchdog/watchdog_node.py
+++ b/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):
diff --git a/rb_ws/src/buggy/setup.cfg b/rb_ws/src/buggy/setup.cfg
deleted file mode 100644
index c2b78e07..00000000
--- a/rb_ws/src/buggy/setup.cfg
+++ /dev/null
@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/buggy
-[install]
-install_scripts=$base/lib/buggy
diff --git a/rb_ws/src/buggy/setup.py b/rb_ws/src/buggy/setup.py
deleted file mode 100644
index 03752caf..00000000
--- a/rb_ws/src/buggy/setup.py
+++ /dev/null
@@ -1,35 +0,0 @@
-from glob import glob
-import os
-
-from setuptools import find_packages, setup
-
-package_name = 'buggy'
-
-setup(
- name=package_name,
- version='0.0.0',
- packages=find_packages(exclude=['test']),
- data_files=[
- ('share/ament_index/resource_index/packages',
- ['resource/' + package_name]),
- ('share/' + package_name, ['package.xml']),
- (os.path.join("share", package_name), glob("launch/*.xml"))
- ],
- install_requires=['setuptools'],
- zip_safe=True,
- maintainer='root',
- maintainer_email='root@todo.todo',
- description='TODO: Package description',
- license='TODO: License declaration',
- tests_require=['pytest'],
- entry_points={
- 'console_scripts': [
- 'hello_world = buggy.hello_world:main',
- 'sim_single = buggy.simulator.engine:main',
- 'controller = buggy.controller.controller_node:main',
- 'path_planner = buggy.path_planner.path_planner:main',
- 'buggy_state_converter = buggy.buggy_state_converter:main',
- 'watchdog = buggy.watchdog.watchdog_node:main',
- ],
- },
-)
\ No newline at end of file
diff --git a/rb_ws/src/buggy/test/test_copyright.py b/rb_ws/src/buggy/test/test_copyright.py
deleted file mode 100644
index 97a39196..00000000
--- a/rb_ws/src/buggy/test/test_copyright.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_copyright.main import main
-import pytest
-
-
-# Remove the `skip` decorator once the source file(s) have a copyright header
-@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
-@pytest.mark.copyright
-@pytest.mark.linter
-def test_copyright():
- rc = main(argv=['.', 'test'])
- assert rc == 0, 'Found errors'
diff --git a/rb_ws/src/buggy/test/test_flake8.py b/rb_ws/src/buggy/test/test_flake8.py
deleted file mode 100644
index 27ee1078..00000000
--- a/rb_ws/src/buggy/test/test_flake8.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# Copyright 2017 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_flake8.main import main_with_errors
-import pytest
-
-
-@pytest.mark.flake8
-@pytest.mark.linter
-def test_flake8():
- rc, errors = main_with_errors(argv=[])
- assert rc == 0, \
- 'Found %d code style errors / warnings:\n' % len(errors) + \
- '\n'.join(errors)
diff --git a/rb_ws/src/buggy/test/test_pep257.py b/rb_ws/src/buggy/test/test_pep257.py
deleted file mode 100644
index b234a384..00000000
--- a/rb_ws/src/buggy/test/test_pep257.py
+++ /dev/null
@@ -1,23 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_pep257.main import main
-import pytest
-
-
-@pytest.mark.linter
-@pytest.mark.pep257
-def test_pep257():
- rc = main(argv=['.', 'test'])
- assert rc == 0, 'Found code style errors / warnings'