Merge pull request #164 from purdue-arc/car-pico-control

Car pico control

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/
 

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 <ArduinoHttpClient.h>
+#include <WiFi.h>
 #include <ros.h>
-#include <std_msgs/Bool.h>
 
-#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<std_msgs::Bool> 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
+    }
+  }
 }

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()