Initial Version
This commit is contained in:
Nicolas Kuhl
commit
0ee065f899
3
.gitignore
vendored
Normal file
3
.gitignore
vendored
Normal file
@ -0,0 +1,3 @@
|
|||||||
|
build/
|
||||||
|
log/
|
||||||
|
install/
|
||||||
9
README.md
Normal file
9
README.md
Normal file
@ -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)
|
||||||
18
src/simulator/package.xml
Normal file
18
src/simulator/package.xml
Normal file
@ -0,0 +1,18 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||||
|
<package format="3">
|
||||||
|
<name>simulator</name>
|
||||||
|
<version>0.0.0</version>
|
||||||
|
<description>Basic simulator for testing a controller</description>
|
||||||
|
<maintainer email="nicolas.kuhl@rub.de">Nicolas Kuhl</maintainer>
|
||||||
|
<license>Apache License 2.0</license>
|
||||||
|
|
||||||
|
<test_depend>ament_copyright</test_depend>
|
||||||
|
<test_depend>ament_flake8</test_depend>
|
||||||
|
<test_depend>ament_pep257</test_depend>
|
||||||
|
<test_depend>python3-pytest</test_depend>
|
||||||
|
|
||||||
|
<export>
|
||||||
|
<build_type>ament_python</build_type>
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
0
src/simulator/resource/simulator
Normal file
0
src/simulator/resource/simulator
Normal file
4
src/simulator/setup.cfg
Normal file
4
src/simulator/setup.cfg
Normal file
@ -0,0 +1,4 @@
|
|||||||
|
[develop]
|
||||||
|
script-dir=$base/lib/simulator
|
||||||
|
[install]
|
||||||
|
install-scripts=$base/lib/simulator
|
||||||
26
src/simulator/setup.py
Normal file
26
src/simulator/setup.py
Normal file
@ -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'
|
||||||
|
],
|
||||||
|
},
|
||||||
|
)
|
||||||
0
src/simulator/simulator/__init__.py
Normal file
0
src/simulator/simulator/__init__.py
Normal file
260
src/simulator/simulator/avai.html
Normal file
260
src/simulator/simulator/avai.html
Normal file
@ -0,0 +1,260 @@
|
|||||||
|
<html>
|
||||||
|
<title>AVAI-Controller-Game</title>
|
||||||
|
<body>
|
||||||
|
<canvas id="main" width="960px" height="540px"></canvas>
|
||||||
|
|
||||||
|
<div>
|
||||||
|
<label for="vp">ViewPortWidth: <span id="vpv">960</span></label>
|
||||||
|
<br/>
|
||||||
|
<input type="range" id="vp" name="vp" min="960" max="3000">
|
||||||
|
<br/>
|
||||||
|
<label for="ppm">PixelsPerMeter: <span id="ppmv">10</span></label>
|
||||||
|
<br/>
|
||||||
|
<input type="range" id="ppm" name="ppm" min="1" max="25">
|
||||||
|
<br/>
|
||||||
|
<label for="off">Offset: <span id="offv">75</span>m</label>
|
||||||
|
<br/>
|
||||||
|
<input type="range" id="off" name="off" min="0" max="200">
|
||||||
|
<br/>
|
||||||
|
<label for="leadAcc">Leading acceleration: <span id="leadAccv">0</span></label>
|
||||||
|
<br/>
|
||||||
|
<input type="range" id="leadAcc" name="leadAcc" min="0" max="100">
|
||||||
|
<br/>
|
||||||
|
<label for="meanDist">Ideal distance: <span id="meanDistv">-10</span>m</label>
|
||||||
|
<br/>
|
||||||
|
<input type="range" id="meanDist" name="meanDist" min="-250" max="0">
|
||||||
|
<br/>
|
||||||
|
<label for="variation">Allowed Variation: <span id="variationv">10</span>m</label>
|
||||||
|
<br/>
|
||||||
|
<input type="range" id="variation" name="variation" min="0" max="50">
|
||||||
|
<br/>
|
||||||
|
<input type="checkbox" id="autoct" name="autoct">
|
||||||
|
<label for="autoct">Ideal distance based on speed of follower</label>
|
||||||
|
<br/>
|
||||||
|
<input type="checkbox" id="hardfail" name="hardfail">
|
||||||
|
<label for="hardfail">Enable hardfail mode</label>
|
||||||
|
<br/>
|
||||||
|
<input type="button" value="START" id="start"/>
|
||||||
|
<input type="button" value="STOP" id="stop"/>
|
||||||
|
<input type="button" value="RESET" id="reset"/>
|
||||||
|
|
||||||
|
|
||||||
|
</div>
|
||||||
|
|
||||||
|
</body>
|
||||||
|
<script>
|
||||||
|
|
||||||
|
var vp = document.getElementById("vp");
|
||||||
|
var vpv = document.getElementById("vpv");
|
||||||
|
|
||||||
|
vp.value = 960;
|
||||||
|
vp.addEventListener("input", (event) => {
|
||||||
|
vpv.innerHTML = vp.value;
|
||||||
|
canv.width = vp.value;
|
||||||
|
});
|
||||||
|
|
||||||
|
var ppm = document.getElementById("ppm");
|
||||||
|
var ppmv = document.getElementById("ppmv");
|
||||||
|
|
||||||
|
ppm.value = 10;
|
||||||
|
ppm.addEventListener("input", (event) => {
|
||||||
|
ppmv.innerHTML = ppm.value;
|
||||||
|
|
||||||
|
renderOffset = renderOffset/oneMeter * ppm.value;
|
||||||
|
clearZoneCenter = clearZoneCenter/oneMeter * ppm.value;
|
||||||
|
clearZoneVariation = clearZoneVariation/oneMeter * ppm.value;
|
||||||
|
followX = followX/oneMeter * ppm.value;
|
||||||
|
|
||||||
|
oneMeter = ppm.value;
|
||||||
|
|
||||||
|
});
|
||||||
|
|
||||||
|
var off = document.getElementById("off");
|
||||||
|
var offv = document.getElementById("offv");
|
||||||
|
|
||||||
|
off.value = 75;
|
||||||
|
off.addEventListener("input", (event) => {
|
||||||
|
offv.innerHTML = off.value;
|
||||||
|
renderOffset = off.value * oneMeter;
|
||||||
|
});
|
||||||
|
|
||||||
|
var leadAcc = document.getElementById("leadAcc");
|
||||||
|
var leadAccv = document.getElementById("leadAccv");
|
||||||
|
|
||||||
|
leadAcc.value = 0;
|
||||||
|
leadAcc.addEventListener("input", (event) => {
|
||||||
|
leadAccv.innerHTML = leadAcc.value;
|
||||||
|
leadAccSlid = leadAcc.value;
|
||||||
|
postSettings(false);
|
||||||
|
});
|
||||||
|
|
||||||
|
var meanDist = document.getElementById("meanDist");
|
||||||
|
var meanDistv = document.getElementById("meanDistv");
|
||||||
|
|
||||||
|
meanDist.value = -10;
|
||||||
|
meanDist.addEventListener("input", (event) => {
|
||||||
|
meanDistv.innerHTML = meanDist.value;
|
||||||
|
if(!autocenter) clearZoneCenter = meanDist.value * oneMeter;
|
||||||
|
postSettings(false);
|
||||||
|
});
|
||||||
|
|
||||||
|
var variation = document.getElementById("variation");
|
||||||
|
var variationv = document.getElementById("variationv");
|
||||||
|
|
||||||
|
variation.value = 10;
|
||||||
|
variation.addEventListener("input", (event) => {
|
||||||
|
variationv.innerHTML = variation.value;
|
||||||
|
clearZoneVariation = variation.value * oneMeter;
|
||||||
|
postSettings(false);
|
||||||
|
});
|
||||||
|
|
||||||
|
var autoct = document.getElementById("autoct");
|
||||||
|
|
||||||
|
autoct.addEventListener("input", (event) => {
|
||||||
|
autocenter = autoct.checked;
|
||||||
|
postSettings(false);
|
||||||
|
});
|
||||||
|
|
||||||
|
var hardf = document.getElementById("hardfail");
|
||||||
|
|
||||||
|
hardf.addEventListener("input", (event) => {
|
||||||
|
hardfail = hardf.checked;
|
||||||
|
postSettings(false);
|
||||||
|
});
|
||||||
|
|
||||||
|
var start = document.getElementById("start");
|
||||||
|
|
||||||
|
start.addEventListener("click", (event) => {
|
||||||
|
runS = true;
|
||||||
|
postSettings(false);
|
||||||
|
});
|
||||||
|
|
||||||
|
var stop = document.getElementById("stop");
|
||||||
|
|
||||||
|
stop.addEventListener("click", (event) => {
|
||||||
|
runS = false;
|
||||||
|
postSettings(false);
|
||||||
|
});
|
||||||
|
|
||||||
|
|
||||||
|
var reset = document.getElementById("reset");
|
||||||
|
|
||||||
|
reset.addEventListener("click", (event) => {
|
||||||
|
postSettings(true);
|
||||||
|
});
|
||||||
|
|
||||||
|
|
||||||
|
var sock = new WebSocket("ws://127.0.0.1:6123");
|
||||||
|
sock.addEventListener('open', (event) => {
|
||||||
|
congood = true;
|
||||||
|
});
|
||||||
|
|
||||||
|
sock.addEventListener('message', (event) => {
|
||||||
|
console.log(event.data);
|
||||||
|
var js = JSON.parse(event.data);
|
||||||
|
spdLD = js.leadingVelocityMpS;
|
||||||
|
spdFL = js.followingVelocityMpS;
|
||||||
|
leadX = js.leadingPosMeter * oneMeter;
|
||||||
|
followX = js.followingPosMeter * oneMeter;
|
||||||
|
if(autocenter){
|
||||||
|
clearZoneCenter = js.clearZoneCenter * oneMeter;
|
||||||
|
meanDist.value = js.clearZoneCenter;
|
||||||
|
meanDistv.value = js.clearZoneCenter;
|
||||||
|
}
|
||||||
|
inBounds = js.inBounds;
|
||||||
|
});
|
||||||
|
|
||||||
|
sock.addEventListener('close', (event) => {
|
||||||
|
congood = false;
|
||||||
|
});
|
||||||
|
|
||||||
|
|
||||||
|
var autocenter = autoct.checked;
|
||||||
|
var hardfail = hardf.checked;
|
||||||
|
var runS = false;
|
||||||
|
var congood = false;
|
||||||
|
var inBounds = true;
|
||||||
|
var leadAccSlid = 0;
|
||||||
|
|
||||||
|
var canv = document.getElementById("main")
|
||||||
|
var ctx = canv.getContext("2d")
|
||||||
|
|
||||||
|
var oneMeter = 10;
|
||||||
|
|
||||||
|
var leadX = 0;
|
||||||
|
var spdLD = 2.21;
|
||||||
|
var renderOffset = 75 * oneMeter;
|
||||||
|
|
||||||
|
var followX = -10 * oneMeter;
|
||||||
|
var spdFL = 0;
|
||||||
|
|
||||||
|
var clearZoneCenter = -10 * oneMeter;
|
||||||
|
var clearZoneVariation = 10 * oneMeter;
|
||||||
|
|
||||||
|
function sleep(milliseconds) {
|
||||||
|
return new Promise(resolve => setTimeout(resolve, milliseconds));
|
||||||
|
}
|
||||||
|
|
||||||
|
function postSettings(reset){
|
||||||
|
var json = {autocenter: autocenter, hardfail: hardfail, runS: runS, reset: reset, clearZoneCenter: meanDist.value, clearZoneVariation: variation.value, leadAccSlid: leadAccSlid};
|
||||||
|
sock.send(JSON.stringify(json));
|
||||||
|
}
|
||||||
|
|
||||||
|
function drawTrack(){
|
||||||
|
ctx.fillStyle = "#073642"
|
||||||
|
ctx.fillRect(0,0,canv.width,canv.height);
|
||||||
|
|
||||||
|
ctx.fillStyle = inBounds ? "#859900A0":"#cb4b16A0"
|
||||||
|
zoneCut = 0;
|
||||||
|
if(renderOffset + clearZoneCenter + clearZoneVariation > renderOffset) zoneCut = clearZoneCenter + clearZoneVariation;
|
||||||
|
ctx.fillRect(renderOffset + clearZoneCenter - clearZoneVariation, (canv.height-(96 - 48 + 32 + 48))/2,clearZoneVariation*2 - zoneCut,64);
|
||||||
|
|
||||||
|
ctx.fillStyle = "#000"
|
||||||
|
ctx.fillRect(renderOffset,(canv.height-(96 - 48 + 32 + 16))/2,32,32);
|
||||||
|
|
||||||
|
ctx.fillStyle = "#000"
|
||||||
|
ctx.fillRect(renderOffset - (leadX-followX)-32,(canv.height-(96 - 48 + 32 + 16))/2,32,32);
|
||||||
|
|
||||||
|
ctx.fillStyle = "#b58900"
|
||||||
|
ctx.fillRect(renderOffset - (leadX-followX),(canv.height-(96 - 48 + 4 + 16))/2,renderOffset - (renderOffset - (leadX-followX)),4);
|
||||||
|
ctx.font = "20px sans-serif";
|
||||||
|
|
||||||
|
var dstLen = (renderOffset - (renderOffset - (leadX-followX)))/oneMeter;
|
||||||
|
var dstStr = (Math.round(dstLen * 100) / 100).toFixed(2) + "m";
|
||||||
|
|
||||||
|
ctx.fillText(dstStr,Math.max(0,(leadX-followX)/2 + renderOffset - (leadX-followX) - ctx.measureText(dstStr).width/2), (canv.height-(96 - 48 + 16 + 16))/2);
|
||||||
|
|
||||||
|
for(var i = 0; i < Math.ceil(canv.width/100.0) + 1; i++){
|
||||||
|
ctx.fillStyle = "#dc322f";
|
||||||
|
ctx.fillRect((i-1)*100 + (100 - (leadX % 100)),16,50,48);
|
||||||
|
ctx.fillRect((i-1)*100 + (100 - (leadX % 100)),canv.height-(48 + 96),50,48);
|
||||||
|
ctx.fillStyle = "#fdf6e3";
|
||||||
|
ctx.fillRect((i-1)*100 + 50 + (100 - (leadX % 100)),16,50,48);
|
||||||
|
ctx.fillRect((i-1)*100 + 50 + (100 - (leadX % 100)),canv.height-(48 + 96),50,48);
|
||||||
|
}
|
||||||
|
var inval = 25 * oneMeter;
|
||||||
|
for(var i = 0; i < Math.ceil(canv.width/inval) + 1; i++){
|
||||||
|
ctx.font = "32px sans-serif";
|
||||||
|
ctx.fillStyle = "#657b83";
|
||||||
|
ctx.fillRect(i*inval - inval + (inval - (leadX % inval)) - 1,canv.height-(48+96),2,85);
|
||||||
|
ctx.fillText(Math.round((((leadX - renderOffset + i*inval - inval + (inval - (leadX % inval)))/oneMeter))) + "m", i*inval - inval + (inval - (leadX % inval)) + 4,canv.height-64);
|
||||||
|
}
|
||||||
|
ctx.font = "20px sans-serif";
|
||||||
|
|
||||||
|
ctx.fillText("Velocity Follow: " + (Math.round(spdFL * 100) / 100).toFixed(2) + "m/s",8,canv.height-8);
|
||||||
|
ctx.fillText("Velocity Leading: " + (Math.round(spdLD * 100) / 100).toFixed(2) + "m/s",8,canv.height-32);
|
||||||
|
ctx.fillText("RUN: " + runS,300,canv.height-8);
|
||||||
|
ctx.fillText("CONGOOD: " + congood,300,canv.height-32);
|
||||||
|
}
|
||||||
|
|
||||||
|
async function tickGame(){
|
||||||
|
while(true) {
|
||||||
|
if(hardfail && !inBounds) runS = false;
|
||||||
|
drawTrack();
|
||||||
|
await sleep(17);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
tickGame();
|
||||||
|
|
||||||
|
</script>
|
||||||
|
</html>
|
||||||
273
src/simulator/simulator/sim.py
Normal file
273
src/simulator/simulator/sim.py
Normal file
@ -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()
|
||||||
23
src/simulator/test/test_copyright.py
Normal file
23
src/simulator/test/test_copyright.py
Normal file
@ -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'
|
||||||
25
src/simulator/test/test_flake8.py
Normal file
25
src/simulator/test/test_flake8.py
Normal file
@ -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)
|
||||||
23
src/simulator/test/test_pep257.py
Normal file
23
src/simulator/test/test_pep257.py
Normal file
@ -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'
|
||||||
Loading…
Reference in New Issue
Block a user