diff --git a/.env.dev b/.env.dev index baed5be..54a8182 100755 --- a/.env.dev +++ b/.env.dev @@ -1,5 +1,8 @@ +# NOTE: this file is not being used in docker compose correctly +# SET ALL ENVIRONMENT VARIABLES IN THE YAML GPS_PORT=/dev/null WEBCAM_PORT=/dev/null RLSENSE_PORT=/dev/null TEENSY_PORT=/dev/null FEATHER_PORT=/dev/null + diff --git a/Dockerfile b/Dockerfile index 4ffeb8d..9b0305e 100644 --- a/Dockerfile +++ b/Dockerfile @@ -24,7 +24,8 @@ 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 && \ - echo 'chmod -R +x src/buggy/scripts/' >> ~/.bashrc + echo 'chmod -R +x src/buggy/scripts/' >> ~/.bashrc && \ + echo 'source environments/docker_env.bash' >> ~/.bashrc # add mouse to tmux RUN echo 'set -g mouse on' >> ~/.tmux.conf diff --git a/docker-dev.yml b/docker-dev.yml index 65470a1..6dd0db3 100755 --- a/docker-dev.yml +++ b/docker-dev.yml @@ -12,6 +12,7 @@ services: - "${FEATHER_PORT:-/dev/null}:/dev/ttyACM1" stdin_open: true # docker run -i tty: true # docker run -t + env_file: .env.dev environment: - DISPLAY=host.docker.internal:0 hostname: main diff --git a/rb_ws/environments/docker_env.bash b/rb_ws/environments/docker_env.bash new file mode 100755 index 0000000..f580c4d --- /dev/null +++ b/rb_ws/environments/docker_env.bash @@ -0,0 +1,4 @@ +#!/bin/sh +export RBROOT=/rb_ws +export PYTHONPATH=$PYTHONPATH:$RBROOT/src/buggy/scripts +export TRAJPATH=$RBROOT/src/buggy/paths/ \ No newline at end of file diff --git a/rb_ws/src/buggy/scripts/controller/controller_node.py b/rb_ws/src/buggy/scripts/controller/controller_node.py index 1461a33..eb7c87e 100755 --- a/rb_ws/src/buggy/scripts/controller/controller_node.py +++ b/rb_ws/src/buggy/scripts/controller/controller_node.py @@ -1,10 +1,8 @@ #!/usr/bin/env python3 +import os import threading -import sys - import numpy as np - import rclpy from rclpy.node import Node @@ -12,7 +10,6 @@ from nav_msgs.msg import Odometry from buggy.msg import TrajectoryMsg -sys.path.append("/rb_ws/src/buggy/scripts") from util.trajectory import Trajectory from controller.stanley_controller import StanleyController @@ -24,7 +21,7 @@ def __init__(self): Creates a ROS node with a publisher that periodically sends a message indicating whether the node is still alive. - + """ super().__init__('controller') self.get_logger().info('INITIALIZED.') @@ -36,7 +33,7 @@ def __init__(self): self.declare_parameter("traj_name", "buggycourse_safe.json") traj_name = self.get_parameter("traj_name").value - self.cur_traj = Trajectory(json_filepath="/rb_ws/src/buggy/paths/" + traj_name) #TODO: Fixed filepath, not good + self.cur_traj = Trajectory(json_filepath=os.environ["TRAJPATH"] + traj_name) start_index = self.cur_traj.get_index_from_distance(start_dist) diff --git a/rb_ws/src/buggy/scripts/controller/controller_superclass.py b/rb_ws/src/buggy/scripts/controller/controller_superclass.py index 731c96e..905d606 100755 --- a/rb_ws/src/buggy/scripts/controller/controller_superclass.py +++ b/rb_ws/src/buggy/scripts/controller/controller_superclass.py @@ -1,10 +1,8 @@ from abc import ABC, abstractmethod -import sys from nav_msgs.msg import Odometry -sys.path.append("/rb_ws/src/buggy/scripts") from util.trajectory import Trajectory class Controller(ABC): diff --git a/rb_ws/src/buggy/scripts/controller/stanley_controller.py b/rb_ws/src/buggy/scripts/controller/stanley_controller.py index 8c3ac1c..625f817 100755 --- a/rb_ws/src/buggy/scripts/controller/stanley_controller.py +++ b/rb_ws/src/buggy/scripts/controller/stanley_controller.py @@ -1,11 +1,9 @@ -import sys import numpy as np from sensor_msgs.msg import NavSatFix from geometry_msgs.msg import Pose as ROSPose from nav_msgs.msg import Odometry -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/scripts/path_planner/path_planner.py b/rb_ws/src/buggy/scripts/path_planner/path_planner.py index a42e6a1..2f27706 100755 --- a/rb_ws/src/buggy/scripts/path_planner/path_planner.py +++ b/rb_ws/src/buggy/scripts/path_planner/path_planner.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -import sys +import os from threading import Lock import numpy as np @@ -12,7 +12,6 @@ from geometry_msgs.msg import Pose from buggy.msg import TrajectoryMsg -sys.path.append("/rb_ws/src/buggy/scripts") from util.trajectory import Trajectory class PathPlanner(Node): @@ -53,7 +52,7 @@ def __init__(self) -> None: #Parameters self.declare_parameter("traj_name", "buggycourse_safe.json") traj_name = self.get_parameter("traj_name").value - self.nominal_traj = Trajectory(json_filepath="/rb_ws/src/buggy/paths/" + traj_name) #TODO: Fixed filepath, not good + self.nominal_traj = Trajectory(json_filepath=os.environ["TRAJPATH"] + traj_name) self.declare_parameter("curb_name", "") curb_name = self.get_parameter("curb_name").value @@ -61,7 +60,7 @@ def __init__(self) -> None: if curb_name is None: self.left_curb = None else: - self.left_curb = Trajectory(json_filepath="/rb_ws/src/buggy/paths/" + curb_name) #TODO: Fixed filepath, not good + self.left_curb = Trajectory(json_filepath=os.environ["TRAJPATH"] + curb_name) #Publishers self.other_buggy_xtrack_publisher = self.create_publisher(Float64, "debug/other_buggy_xtrack", 10) diff --git a/rb_ws/src/buggy/scripts/simulator/engine.py b/rb_ws/src/buggy/scripts/simulator/engine.py index af2bdc5..6c89591 100755 --- a/rb_ws/src/buggy/scripts/simulator/engine.py +++ b/rb_ws/src/buggy/scripts/simulator/engine.py @@ -1,6 +1,5 @@ #! /usr/bin/env python3 import threading -import sys import time import rclpy from rclpy.node import Node @@ -10,8 +9,6 @@ from nav_msgs.msg import Odometry import numpy as np import utm - -sys.path.append("/rb_ws/src/buggy/scripts") from util.constants import Constants class Simulator(Node): @@ -21,7 +18,6 @@ def __init__(self): super().__init__('sim_single') self.get_logger().info('INITIALIZED.') - self.starting_poses = { "Hill1_NAND": (Constants.UTM_EAST_ZERO + 0, Constants.UTM_NORTH_ZERO + 0, -110), "Hill1_SC": (Constants.UTM_EAST_ZERO + 20, Constants.UTM_NORTH_ZERO + 30, -110),