diff --git a/.github/workflows/python-app.yml b/.github/workflows/python-app.yml new file mode 100644 index 000000000..3de575d8e --- /dev/null +++ b/.github/workflows/python-app.yml @@ -0,0 +1,62 @@ +# This workflow will install Python dependencies, run tests and lint with a single version of Python +# For more information see: https://docs.github.com/en/actions/automating-builds-and-tests/building-and-testing-python + +name: Python application + +on: + push: + branches: + - '**' + + +permissions: + contents: read + +jobs: + PEP-Guidelines: + + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v3 + - name: Set up Python 3.10 + uses: actions/setup-python@v3 + with: + python-version: "3.10" + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install flake8 flake8-docstrings pep8-naming + - name: Lint with flake8 + run: | + # stop the build if there are Python syntax errors or undefined names + flake8 ./GEMstack --count --select=E9,F63,F7,F82 --show-source --statistics --exclude=__init__.py || exit 1 + # to enable more advanced checks on the repo, uncomment the lines below (There are around 3000 violations) + # flake8 ./GEMstack --ignore=D,C901,E402,E231 --count --max-complexity=10 --max-line-length=127 --statistics --exclude=__init__.py || exit 1 + # if we want to enable documentation checks, uncomment the line below + # flake8 ./GEMstack --ignore=E128,E402,E501,F401 --docstring-convention pep257 --max-line-length=120 --exclude=__init__.py || exit 1 + continue-on-error: false + + Documentation: + + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v3 + - name: Set up Python 3.10 + uses: actions/setup-python@v3 + with: + python-version: "3.10" + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install sphinx sphinx-rtd-theme + - name: Generate Documentation + run: | + # stop the build if there are Python syntax errors or undefined names + sphinx-build -b html docs docs/build + - name: Save Documentation as Artifact + uses: actions/upload-artifact@v4 + with: + name: documentation + path: docs/build diff --git a/GEMstack/knowledge/defaults/current.yaml b/GEMstack/knowledge/defaults/current.yaml index c886288be..bc14fcae0 100644 --- a/GEMstack/knowledge/defaults/current.yaml +++ b/GEMstack/knowledge/defaults/current.yaml @@ -1,34 +1,48 @@ # ********* Main settings entry point for behavior stack *********** - -# Configure settings for the vehicle / vehicle model -vehicle: !include ../vehicle/gem_e4.yaml - -#arguments for algorithm components here -model_predictive_controller: - dt: 0.1 - lookahead: 20 -control: - recovery: - brake_amount : 0.5 - brake_speed : 2.0 - pure_pursuit: - lookahead: 2.0 - lookahead_scale: 3.0 - crosstrack_gain: 1.0 - desired_speed: trajectory - longitudinal_control: - pid_p: 1.0 - pid_i: 0.1 - pid_d: 0.0 - -#configure the simulator, if using -simulator: - dt: 0.01 - real_time_multiplier: 1.0 # make the simulator run faster than real time by making this > 1 - gnss_emulator: - dt: 0.1 #10Hz - #position_noise: 0.1 #10cm noise - #orientation_noise: 0.04 #2.3 degrees noise - #velocity_noise: - # constant: 0.04 #4cm/s noise - # linear: 0.02 #2% noise \ No newline at end of file + + # Configure settings for the vehicle / vehicle model + vehicle: !include ../vehicle/gem_e4.yaml + + #arguments for algorithm components here + model_predictive_controller: + dt: 0.1 + lookahead: 20 + control: + recovery: + brake_amount : 0.5 + brake_speed : 2.0 + pure_pursuit: + lookahead: 2.0 + lookahead_scale: 2.0 + crosstrack_gain: 0.3 + desired_speed: trajectory #racing + launch_control: + enabled: false + stage_duration: 0.5 + longitudinal_control: + pid_p: 0.8 + pid_i: 0.03 + pid_d: 0.0 + planning: + longitudinal_plan: + mode: 'real' # 'real' or 'sim' + yielder: 'expert' # 'expert', 'analytic', or 'simulation' + planner: 'dt' # 'milestone', 'dt', or 'dx' + desired_speed: 1.0 + acceleration: 0.5 + max_deceleration: 6.0 + deceleration: 2.0 + min_deceleration: 0.5 + yield_deceleration: 0.5 + + #configure the simulator, if using + simulator: + dt: 0.01 + real_time_multiplier: 1.0 # make the simulator run faster than real time by making this > 1 + gnss_emulator: + dt: 0.05 #10Hz + #position_noise: 0.1 #10cm noise + #orientation_noise: 0.04 #2.3 degrees noise + #velocity_noise: + # constant: 0.04 #4cm/s noise + # linear: 0.02 #2% noise diff --git a/GEMstack/onboard/interface/gem_hardware.py b/GEMstack/onboard/interface/gem_hardware.py index 836d7ef71..e8496809f 100644 --- a/GEMstack/onboard/interface/gem_hardware.py +++ b/GEMstack/onboard/interface/gem_hardware.py @@ -316,6 +316,19 @@ def send_command(self, command : GEMVehicleCommand): self.accel_cmd.enable = True self.accel_cmd.clear = False self.accel_cmd.ignore = False + + # Launch control + currTime = rospy.get_time() - self.start_time + if currTime < 5: + self.brake_cmd.f64_cmd = maxbrake + self.accel_cmd.f64_cmd = 0 + elif currTime < 6: + self.brake_cmd.f64_cmd = maxbrake + self.accel_cmd.f64_cmd = maxacc + elif currTime < 10: + self.brake_cmd.f64_cmd = 0 + self.accel_cmd.f64_cmd = maxacc + self.gear_cmd.ui16_cmd = PacmodCmd.SHIFT_FORWARD self.gear_cmd.enable = True diff --git a/GEMstack/onboard/planning/pure_pursuit.py b/GEMstack/onboard/planning/pure_pursuit.py index e4b6f4f4e..de0ccbfed 100644 --- a/GEMstack/onboard/planning/pure_pursuit.py +++ b/GEMstack/onboard/planning/pure_pursuit.py @@ -168,6 +168,7 @@ def compute(self, state : VehicleState, component : Component = None): if desired_speed > self.speed_limit: desired_speed = self.speed_limit output_accel = self.pid_speed.advance(e = desired_speed - speed, t = t, feedforward_term=feedforward_accel) + if component is not None: component.debug('curr pt',(curr_x,curr_y)) component.debug('curr param',self.current_path_parameter) diff --git a/GEMstack/utils/klampt_visualization.py b/GEMstack/utils/klampt_visualization.py index 0e9219e76..063f690c7 100644 --- a/GEMstack/utils/klampt_visualization.py +++ b/GEMstack/utils/klampt_visualization.py @@ -6,6 +6,10 @@ from . import settings from ..state import ObjectFrameEnum,ObjectPose,PhysicalObject,VehicleState,VehicleGearEnum,Path,Obstacle,AgentState,AgentEnum,Roadgraph,RoadgraphLane,RoadgraphLaneEnum,RoadgraphCurve,RoadgraphCurveEnum,RoadgraphRegion,RoadgraphRegionEnum,RoadgraphSurfaceEnum,Trajectory,Route,SceneState,AllState +#KH: there is a bug on some system where the visualization crashes with an OpenGL error when drawing curves +#this is a workaround. We really should find the source of the bug! +MAX_POINTS_IN_CURVE = 50 + OBJECT_COLORS = { AgentEnum.CAR : (1,1,0,1), AgentEnum.PEDESTRIAN : (0,1,0,1), @@ -198,7 +202,10 @@ def plot_vehicle(vehicle : VehicleState, vehicle_model=None, axis_len=1.0): vehicle_model.link('rear_left_stop_light_link').appearance().setColor(0.3,0,0,1) def plot_path(name : str, path : Path, color=(0,0,0), width=1): - vis.add(name,[list(p) for p in path.points],color=color,width=width) + if len(path.points) > MAX_POINTS_IN_CURVE: # downsample due to OpenGL error? + vis.add(name,[list(p) for p in path.points[::len(path.points)//MAX_POINTS_IN_CURVE]],color=color,width=width) + else: + vis.add(name,[list(p) for p in path.points],color=color,width=width) def plot_curve(name : str, curve : RoadgraphCurve, color=None, width=None): style = CURVE_TO_STYLE.get(curve.type,CURVE_TO_STYLE[None]) @@ -211,7 +218,10 @@ def plot_curve(name : str, curve : RoadgraphCurve, color=None, width=None): if width is not None: style['width'] = width for i,seg in enumerate(curve.segments): - vis.add(name+"_%d" % i,seg,**style) + if len(seg) > MAX_POINTS_IN_CURVE: # downsample due to OpenGL error? + vis.add(name+"_%d" % i,seg[::len(seg)//MAX_POINTS_IN_CURVE],**style) + else: + vis.add(name+"_%d" % i,seg,**style) def plot_lane(name : str, lane : RoadgraphLane, on_route=False): if lane.surface != RoadgraphSurfaceEnum.PAVEMENT: @@ -284,6 +294,6 @@ def plot_scene(scene : SceneState, ground_truth_vehicle=None, vehicle_model = No def plot(state : AllState, ground_truth_vehicle = None, vehicle_model=None, title=None, show=True): plot_scene(state, ground_truth_vehicle=ground_truth_vehicle, vehicle_model=vehicle_model, title=title, show=show) if state.route is not None: - plot_path("route",state.route,color=(1,0.5,0,1)) + plot_path("route",state.route,color=(1,0.5,0,1),width=2) if state.trajectory is not None: - plot_path("trajectory",state.trajectory,color=(1,0,0,1),width=2) + plot_path("trajectory",state.trajectory,color=(1,0,0,1),width=3)