AVAI-Controller-Game

commit 0ee065f899a694c9f4542d1cacc492b65ae1f062 Author: Nicolas Kuhl Date: Thu Jun 22 09:05:00 2023 +0200 Initial Version diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..d18e8c4 --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +build/ +log/ +install/ diff --git a/README.md b/README.md new file mode 100644 index 0000000..6fd8d9d --- /dev/null +++ b/README.md @@ -0,0 +1,9 @@ +sudo apt-get update && sudo apt-get install python3-pip -y && pip3 install websockets + +colcon build +ros2 run simulator sim + +firefox src/simulator/simulator/avai.html + +-> Distance published as Float64 on /distance +-> inputPower subscribed as Float64 on /inputPower (0-100) diff --git a/src/simulator/package.xml b/src/simulator/package.xml new file mode 100644 index 0000000..41b6e4e --- /dev/null +++ b/src/simulator/package.xml @@ -0,0 +1,18 @@ + + + + simulator + 0.0.0 + Basic simulator for testing a controller + Nicolas Kuhl + Apache License 2.0 + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/simulator/resource/simulator b/src/simulator/resource/simulator new file mode 100644 index 0000000..e69de29 diff --git a/src/simulator/setup.cfg b/src/simulator/setup.cfg new file mode 100644 index 0000000..36db212 --- /dev/null +++ b/src/simulator/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/simulator +[install] +install-scripts=$base/lib/simulator diff --git a/src/simulator/setup.py b/src/simulator/setup.py new file mode 100644 index 0000000..1e80c7c --- /dev/null +++ b/src/simulator/setup.py @@ -0,0 +1,26 @@ +from setuptools import setup + +package_name = 'simulator' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='ubuntu', + maintainer_email='avai@unconfigured.de', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'sim = simulator.sim:main' + ], + }, +) diff --git a/src/simulator/simulator/__init__.py b/src/simulator/simulator/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/simulator/simulator/avai.html b/src/simulator/simulator/avai.html new file mode 100644 index 0000000..9b4473c --- /dev/null +++ b/src/simulator/simulator/avai.html @@ -0,0 +1,260 @@ + +AVAI-Controller-Game + +

+ +

+ViewPortWidth: 960 +
+ +
+PixelsPerMeter: 10 +
+ +
+Offset: 75m +
+ +
+Leading acceleration: 0 +
+ +
+Ideal distance: -10m +
+ +
+Allowed Variation: 10m +
+ +
+ +Ideal distance based on speed of follower +
+ +Enable hardfail mode +
+ + + + + +

+ + + + diff --git a/src/simulator/simulator/sim.py b/src/simulator/simulator/sim.py new file mode 100644 index 0000000..399a17e --- /dev/null +++ b/src/simulator/simulator/sim.py @@ -0,0 +1,273 @@ +#!/bin/python3 +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float64 + +import threading +from websockets.sync import server +import time +import math +import json + +controllerInput = 0 +speedSlider = 0 + +clearZoneCenter = 0.0 +clearZoneVariation = 0.0 +autoCenter = False + +enableSim = False +resetSim = False +inBounds = True +hardFail = False + +leadingPosMeter = 0.0 +followingPosMeter = -10.0 + +leadingVelocityMpS = 0.0 +followingVelocityMpS = 0.0 + +leadingTimeToMaxSpeedMS = 8000 +leadingSlowDownFactor = 1.50 +leadingMaxSpeedMpS = 30 + +followingTimeToMaxSpeedMS = 8000 +followingSlowDownFactor = 1.50 +followingMaxSpeedMpS = 30 + + +webSock = None +def handler(websocket): + global webSock + while True: + data = websocket.recv() + handleInboundData(data) + webSock = websocket +# import pdb; pdb.set_trace() + +def runWebSock(): + serv = server.serve(handler, "localhost", 6123) + serv.serve_forever() + +def handleInboundData(data): + js = json.loads(data) +# print(js) + global speedSlider + speedSlider = int(js["leadAccSlid"]) + global clearZoneCenter + if not js["autocenter"]: + clearZoneCenter = int(js["clearZoneCenter"]) + global clearZoneVariation + clearZoneVariation = int(js["clearZoneVariation"]) + global autoCenter + autoCenter = js["autocenter"] + global enableSim + enableSim = js["runS"] + global resetSim + resetSim = js["reset"] + global hardFail + hardFail = js["hardfail"] + +def sendUpdate(): + if webSock is None: + return + js = {"leadingVelocityMpS": leadingVelocityMpS, "followingVelocityMpS": followingVelocityMpS, "leadingPosMeter": leadingPosMeter, "followingPosMeter": followingPosMeter, "clearZoneCenter": clearZoneCenter, "inBounds": inBounds} + res = json.dumps(js) + webSock.send(res) + + +leaderlastLinearComponent = 0 +leaderlastLinearComponentTS = -1 + +def processLeaderCurve(): + global leaderlastLinearComponent + global leaderlastLinearComponentTS + # using standard sigmoid curve limited to range 0 - 1 + # https://www.wolframalpha.com/input?i=1%2F%281%2Be%5E-%28%28x*12%29-6%29%29+from+0+to+1 + max = leadingTimeToMaxSpeedMS * speedSlider/100 + timedelta = (time.time()*1000) - leaderlastLinearComponentTS + leaderlastLinearComponentTS = time.time()*1000 + + if timedelta > leadingTimeToMaxSpeedMS: + leaderlastLinearComponent = 0 + return 0 + if leaderlastLinearComponent < max: + leaderlastLinearComponent += timedelta + if leaderlastLinearComponent > max: + leaderlastLinearComponent = max + + if leaderlastLinearComponent > max: + leaderlastLinearComponent -= timedelta * leadingSlowDownFactor + (leaderlastLinearComponent/leadingTimeToMaxSpeedMS) + if leaderlastLinearComponent < 0: + leaderlastLinearComponent = 0 + + x = leaderlastLinearComponent / leadingTimeToMaxSpeedMS + exponent = -((x*12)-6) + base = 1 + math.exp(exponent) + res = (1 / base) * leadingMaxSpeedMpS + + return 0 if res < 0.08 else res + +followerlastLinearComponent = 0 +followerlastLinearComponentTS = -1 + +def processFollowerCurve(): + global followerlastLinearComponent + global followerlastLinearComponentTS + # using standard sigmoid curve limited to range 0 - 1 + # https://www.wolframalpha.com/input?i=1%2F%281%2Be%5E-%28%28x*12%29-6%29%29+from+0+to+1 + max = followingTimeToMaxSpeedMS * controllerInput/100 + timedelta = (time.time()*1000) - followerlastLinearComponentTS + followerlastLinearComponentTS = time.time()*1000 + + if timedelta > followingTimeToMaxSpeedMS: + followerlastLinearComponent = 0 + return 0 + if followerlastLinearComponent < max: + followerlastLinearComponent += timedelta + if followerlastLinearComponent > max: + followerlastLinearComponent = max + + if followerlastLinearComponent > max: + followerlastLinearComponent -= timedelta * followingSlowDownFactor + (followerlastLinearComponent/followingTimeToMaxSpeedMS) + if followerlastLinearComponent < 0: + followerlastLinearComponent = 0 + + x = followerlastLinearComponent / followingTimeToMaxSpeedMS + exponent = -((x*12)-6) + base = 1 + math.exp(exponent) + res = (1 / base) * followingMaxSpeedMpS + + return 0 if res < 0.08 else res + +def tickDistance(): + global inBounds + global enableSim + if leadingPosMeter + clearZoneCenter - clearZoneVariation > followingPosMeter: + inBounds = False + if hardFail: + enableSim = False + # print("FAIL") + return + if leadingPosMeter + clearZoneCenter + clearZoneVariation < followingPosMeter: + inBounds = False + if hardFail: + enableSim = False + # print("FAIL") + return + if leadingPosMeter < followingPosMeter: + inBounds = False + if hardFail: + enableSim = False + # print("FAIL") + return + inBounds = True + + +lastRunTimeStamp = time.time() * 1000 + +def tickPhysics(): + print("Starting physics tick") + + runCtr = 0 + speedArray = [] + global leadingPosMeter + global followingPosMeter + global resetSim + while True: + if resetSim: + global enableSim + enableSim = False + global inBounds + inBounds = True + global leadingPosMeter + leadingPosMeter = 0 + global followingPosMeter + followingPosMeter = -10 + global leaderlastLinearComponent + leaderlastLinearComponent = 0 + global leaderlastLinearComponentTS + leaderlastLinearComponent = -1 + global followerlastLinearComponent + followerlastLinearComponent = 0 + global followerlastLinearComponentTS + followerlastLinearComponent = -1 + global leadingVelocityMpS + leadingVelocityMpS = 0 + global followingVelocityMpS + followingVelocityMpS = 0 + speedArray.clear() + runCtr = 0 + resetSim = False + continue + + if autoCenter: + global clearZoneCenter + clearZoneCenter = -1 * followingVelocityMpS * 3.6 / 2 + + if not enableSim and len(speedArray) > 0: + print(speedArray) + speedArray.clear() + + if not enableSim: + time.sleep(.100) + continue + else: + time.sleep(.01) + runCtr += 1 + + if runCtr % 20 == 0: + speedArray.append(speedSlider) + + leadingVelocityMpS = processLeaderCurve() + + followingVelocityMpS = processFollowerCurve() + + global lastRunTimeStamp + timedelta = (time.time()*1000) - lastRunTimeStamp + lastRunTimeStamp = time.time() * 1000 + + portion = timedelta * leadingVelocityMpS/1000 + leadingPosMeter += portion + + portion = timedelta * followingVelocityMpS/1000 + followingPosMeter += portion + + tickDistance() + sendUpdate() + + +class Sim(Node): + + def __init__(self): + super().__init__('sim') + self.publisher_ = self.create_publisher(Float64, 'distance', 10) + self.timer = self.create_timer(0.05, self.callback) + self.subscription = self.create_subscription(Float64,'inputPower', self.call, 10) + + def call(self, msg): + global controllerInput + controllerInput = max(min(msg.data,100),0) + + def callback(self): + msg = Float64() + msg.data = (leadingPosMeter - followingPosMeter) * 1.00 + self.publisher_.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + thr = threading.Thread(target=runWebSock) + thr.start() + + thr2 = threading.Thread(target=tickPhysics) + thr2.start() + + sim = Sim() + rclpy.spin(sim) + + pub.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/simulator/test/test_copyright.py b/src/simulator/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/src/simulator/test/test_copyright.py @@ -0,0 +1,23 @@ +# 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 + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/simulator/test/test_flake8.py b/src/simulator/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/src/simulator/test/test_flake8.py @@ -0,0 +1,25 @@ +# 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/src/simulator/test/test_pep257.py b/src/simulator/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/src/simulator/test/test_pep257.py @@ -0,0 +1,23 @@ +# 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'