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