195 lines
5.6 KiB
Python
Executable File
195 lines
5.6 KiB
Python
Executable File
# Copyright 2011 Brown University Robotics.
|
|
# Copyright 2017 Open Source Robotics Foundation, Inc.
|
|
# All rights reserved.
|
|
#
|
|
# Software License Agreement (BSD License 2.0)
|
|
#
|
|
# Redistribution and use in source and binary forms, with or without
|
|
# modification, are permitted provided that the following conditions
|
|
# are met:
|
|
#
|
|
# * Redistributions of source code must retain the above copyright
|
|
# notice, this list of conditions and the following disclaimer.
|
|
# * Redistributions in binary form must reproduce the above
|
|
# copyright notice, this list of conditions and the following
|
|
# disclaimer in the documentation and/or other materials provided
|
|
# with the distribution.
|
|
# * Neither the name of the Willow Garage nor the names of its
|
|
# contributors may be used to endorse or promote products derived
|
|
# from this software without specific prior written permission.
|
|
#
|
|
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
# POSSIBILITY OF SUCH DAMAGE.
|
|
|
|
import sys
|
|
import threading
|
|
|
|
import geometry_msgs.msg
|
|
import rclpy
|
|
|
|
|
|
page= """
|
|
<!DOCTYPE HTML>
|
|
<html>
|
|
<script src="http://bobboteck.github.io/joy/joy.js">
|
|
</script>
|
|
<script type="text/javascript">
|
|
// Create JoyStick object into the DIV 'joyDiv'
|
|
function httpGetAsync(theUrl, callback)
|
|
{
|
|
var xmlHttp = new XMLHttpRequest();
|
|
xmlHttp.onreadystatechange = function() {
|
|
if (xmlHttp.readyState == 4 && xmlHttp.status == 200)
|
|
callback(xmlHttp.responseText);
|
|
}
|
|
xmlHttp.open("GET", theUrl, true); // true for asynchronous
|
|
xmlHttp.send(null);
|
|
}
|
|
window.addEventListener("load", () => {
|
|
joy = new JoyStick('joyDiv');
|
|
setInterval(function(){
|
|
joy.GetX();
|
|
httpGetAsync("/update?x="+joy.GetX()+"&y="+joy.GetY(), () => {});
|
|
}, 200);});
|
|
</script>
|
|
<body>
|
|
<div id="joyDiv" style="width:500px;height:500px;margin-bottom:20px;"></div>
|
|
</body>
|
|
</html>
|
|
"""
|
|
|
|
moveBindings = {
|
|
}
|
|
|
|
speedBindings = {
|
|
}
|
|
|
|
from flask import Flask, request, redirect
|
|
|
|
|
|
app = Flask(__name__)
|
|
|
|
rclpy.init()
|
|
|
|
node = rclpy.create_node('twist_web')
|
|
|
|
@app.route("/")
|
|
def hello_world():
|
|
return page
|
|
|
|
@app.route('/update', methods=['GET'])
|
|
def handle_get():
|
|
global twist_msg, twist, pub
|
|
if request.method == 'GET':
|
|
pos = (int(request.args['y']),int(request.args['x']))
|
|
print(pos)
|
|
if stamped:
|
|
twist_msg.header.stamp = node.get_clock().now().to_msg()
|
|
twist.linear.x = pos[0] / 500
|
|
twist.linear.y = 0.0
|
|
twist.linear.z = 0.0
|
|
twist.angular.x = 0.0
|
|
twist.angular.y = 0.0
|
|
twist.angular.z = -pos[1] /100
|
|
pub.publish(twist_msg)
|
|
return "ok"
|
|
|
|
def main():
|
|
global node, pub, spinner, TwistMsg, stamped, frame_id, twist, twist_msg
|
|
# parameters
|
|
stamped = node.declare_parameter('stamped', True).value
|
|
frame_id = node.declare_parameter('frame_id', '').value
|
|
if not stamped and frame_id:
|
|
raise Exception("'frame_id' can only be set when 'stamped' is True")
|
|
|
|
if stamped:
|
|
TwistMsg = geometry_msgs.msg.TwistStamped
|
|
else:
|
|
TwistMsg = geometry_msgs.msg.Twist
|
|
|
|
twist_msg = TwistMsg()
|
|
if stamped:
|
|
twist = twist_msg.twist
|
|
twist_msg.header.stamp = node.get_clock().now().to_msg()
|
|
twist_msg.header.frame_id = frame_id
|
|
else:
|
|
twist = twist_msg
|
|
pub = node.create_publisher(TwistMsg, 'staubi_base_controller/cmd_vel', 10)
|
|
|
|
|
|
speed = 0.5
|
|
turn = 1.0
|
|
x = 0.0
|
|
y = 0.0
|
|
z = 0.0
|
|
th = 0.0
|
|
status = 0.0
|
|
|
|
|
|
if stamped:
|
|
twist = twist_msg.twist
|
|
twist_msg.header.stamp = node.get_clock().now().to_msg()
|
|
twist_msg.header.frame_id = frame_id
|
|
else:
|
|
twist = twist_msg
|
|
|
|
try:
|
|
return
|
|
print(msg)
|
|
print(vels(speed, turn))
|
|
while True:
|
|
key = getKey(settings)
|
|
if key in moveBindings.keys():
|
|
x = moveBindings[key][0]
|
|
y = moveBindings[key][1]
|
|
z = moveBindings[key][2]
|
|
th = moveBindings[key][3]
|
|
elif key in speedBindings.keys():
|
|
speed = speed * speedBindings[key][0]
|
|
turn = turn * speedBindings[key][1]
|
|
|
|
print(vels(speed, turn))
|
|
if (status == 14):
|
|
print(msg)
|
|
status = (status + 1) % 15
|
|
else:
|
|
x = 0.0
|
|
y = 0.0
|
|
z = 0.0
|
|
th = 0.0
|
|
if (key == '\x03'):
|
|
break
|
|
|
|
if stamped:
|
|
twist_msg.header.stamp = node.get_clock().now().to_msg()
|
|
|
|
twist.linear.x = x * speed
|
|
twist.linear.y = y * speed
|
|
twist.linear.z = z * speed
|
|
twist.angular.x = 0.0
|
|
twist.angular.y = 0.0
|
|
twist.angular.z = th * turn
|
|
pub.publish(twist_msg)
|
|
|
|
except Exception as e:
|
|
print(e)
|
|
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|
|
spinner = threading.Thread(target=rclpy.spin, args=(node,))
|
|
spinner.start()
|
|
app.run(host='0.0.0.0')
|
|
|