diff --git a/docker/Dockerfile b/docker/Dockerfile index 3f2099814..578a8055f 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -28,7 +28,7 @@ RUN apt update && \ ros-$ROS_DISTRO-rosbridge-server \ ros-$ROS_DISTRO-rosdoc-lite && \ # pip - pip3 install pybullet gym[box2d] stable-baselines3 pfilter optuna && \ + pip3 install pybullet gym[box2d] stable-baselines3 pfilter optuna websockets && \ # clean up rm -rf /var/lib/apt/lists/* /root/.cache/pip/ diff --git a/rktl_control/firmware/hardware_interface/hardware_interface.ino b/rktl_control/firmware/hardware_interface/hardware_interface.ino index 716458f40..6e48e223c 100644 --- a/rktl_control/firmware/hardware_interface/hardware_interface.ino +++ b/rktl_control/firmware/hardware_interface/hardware_interface.ino @@ -1,97 +1,70 @@ /********************************************************************* -Teensyduino sketch used to control six cars at once over six radio links. +Arduino-Pico sketch used to control a car over a WebSocket. License: BSD 3-Clause License Copyright (c) 2023, Autonomous Robotics Club of Purdue (Purdue ARC) All rights reserved. *********************************************************************/ +#include +#include #include -#include -#include "CarLink.hpp" +#define PACKET_SIZE 8 -// ros node handle -ros::NodeHandle nh; +char ssid[] = "SSID GOES HERE"; +char pass[] = "PASSWORD GOES HERE"; -// radio links to each car -CarLink car0{&nh, 0, 23}; -CarLink car1{&nh, 1, 22}; -CarLink car2{&nh, 2, 21}; -CarLink car3{&nh, 3, 20}; -CarLink car4{&nh, 4, 10}; -CarLink car5{&nh, 5, 9}; +char serverAddress[] = "IP ADDRESS OF WEBSOCKET GOES HERE"; // server address +int port = WEBSOCKET PORT GOES HERE; // this is meant to cause a compilation error if it's not properly set -// helper functions -void enable_all() { - car0.enable(); - car1.enable(); - car2.enable(); - car3.enable(); - car4.enable(); - car5.enable(); - digitalWrite(LED_BUILTIN, HIGH); -} - -void disable_all() { - car0.disable(); - car1.disable(); - car2.disable(); - car3.disable(); - car4.disable(); - car5.disable(); - digitalWrite(LED_BUILTIN, LOW); -} +WiFiClient wifi; +WebSocketClient client = WebSocketClient(wifi, serverAddress, port); +int status = WL_IDLE_STATUS; -bool update_all() { - return car0.update_params() - && car1.update_params() - && car2.update_params() - && car3.update_params() - && car4.update_params() - && car5.update_params(); -} +typedef struct { + float throttle; + float steering; +} packet_t; -// flag for if params have been properly set -bool configured = false; -// enabled / disable callback and subscriber -void enable_callback(const std_msgs::Bool& enable) { - if (enable.data && configured) { - enable_all(); - } else { - disable_all(); - } -} +typedef union { + packet_t packet_obj; + byte array[PACKET_SIZE]; +} packet_union_t; -// subscriber object -ros::Subscriber enable_sub{"enable", enable_callback}; +packet_union_t my_packet = {0.0, 0.0}; void setup() { - // LED pin - pinMode(LED_BUILTIN, OUTPUT); - digitalWrite(LED_BUILTIN, LOW); - - // init node - nh.initNode(); - nh.subscribe(enable_sub); + Serial.begin(9600); + while (status != WL_CONNECTED) { + Serial.print("Connecting to "); + Serial.println(ssid); + status = WiFi.begin(ssid, pass); + } - // wait until connected - while (!nh.connected()) { - nh.spinOnce(); - } - - // get params - configured = update_all(); + IPAddress ip = WiFi.localIP(); + Serial.print("IP Address: "); + Serial.println(ip); } void loop() { - if (!nh.connected()) { - configured = false; - disable_all(); - } else if (!configured) { - configured = update_all(); - } + client.begin(); - nh.spinOnce(); + while (client.connected()) { + Serial.println("Connected to socket"); + + int messageSize = client.parseMessage(); + + if (messageSize >= PACKET_SIZE) { + Serial.println("Receiving packet"); + client.read(my_packet.array, PACKET_SIZE); + Serial.print("Throttle: "); + Serial.println(my_packet.packet_obj.throttle); + Serial.print("Steering: "); + Serial.println(my_packet.packet_obj.steering); + } else if (messageSize != 0) { + Serial.println("Bad message size"); // don't know if it receives multiple bytes between repitions + } + } } diff --git a/rktl_control/nodes/websocket_node.py b/rktl_control/nodes/websocket_node.py new file mode 100644 index 000000000..662643bce --- /dev/null +++ b/rktl_control/nodes/websocket_node.py @@ -0,0 +1,56 @@ +# C + +import asyncio +import os +import websockets + +import rospy +from rktl_msgs.msg import ControlEffort + +car_num = 0; # How do we get this? + +running = True + +throttle = 25565 +steering = 25566 + +async def socket_handler(websocket): + while running: + packet = bytearray(throttle.to_bytes(4, byteorder='little')) + packet.extend(steering.to_bytes(4, byteorder='little')) + print(packet) + print(len(packet)) + await websocket.send(packet) + +async def main(): + try: + async with websockets.serve(socket_handler, None, 8765): + await asyncio.Future() + except asyncio.CancelledError: + running = False + print("Quitting") + rospy.signal_shutdown() + os._exit(0) + +def receive_callback(thr, str, data): + throttle = data.throttle + steering = data.steering + # print(f"Throttle: \{throttle} Steering: \{steering}") + + +if __name__ == '__main__': + print("Initing") + print("Starting") + rospy.init_node('car_websocket', anonymous=True) + rospy.Subscriber(f'/cars/car{car_num}/effort', ControlEffort, receive_callback) + print("Running") + loop = asyncio.get_event_loop() + task = asyncio.run(main()) + for signal in [SIGINT, SIGTERM]: + loop.add_signal_handler(signal, task.cancel) + try: + loop.run_until_complete(task) + print("Spinning") + finally: + rospy.signal_shutdown() + loop.close()