francWhite · francWhite · Nov 18, 2023 · Nov 18, 2023 · Nov 18, 2023
diff --git a/apps/api/src/index.ts b/apps/api/src/index.ts
@@ -6,6 +6,7 @@ import { magnetRouter } from './routes/magnet.routes';
 import { rosService } from './ros.service';
 import { buzzerRouter } from './routes/buzzer.routes';
 import { actionRouter } from './routes/action.routes';
+import { colorRouter } from './routes/color.routes';
 
 dotenv.config();
 
@@ -16,6 +17,7 @@ app.use('/api/controller', controllerRouter);
 app.use('/api/magnet', magnetRouter);
 app.use('/api/buzzer', buzzerRouter);
 app.use('/api/actions', actionRouter);
+app.use('/api/color', colorRouter);
 
 rosService.connect();
 

diff --git a/apps/api/src/ros.service.ts b/apps/api/src/ros.service.ts
@@ -78,6 +78,19 @@ class RosService {
     goal.on('status', statusCallback);
     goal.send();
   }
+
+  readSingleMessageFromTopic<T>(topicName: string, messageType: string, callback: (message: T) => void) {
+    const topic = new Topic({
+      ros: this.ros,
+      name: topicName,
+      messageType: messageType,
+    });
+
+    topic.subscribe((message) => {
+      callback(message as T);
+      topic.unsubscribe();
+    });
+  }
 }
 
 export const rosService = new RosService();
diff --git a/apps/api/src/routes/color.routes.ts b/apps/api/src/routes/color.routes.ts
@@ -0,0 +1,26 @@
+import express, { Request, Response } from 'express';
+import { rosService } from '../ros.service';
+
+export const colorRouter = express.Router();
+colorRouter.use(express.json());
+
+colorRouter.get('/', async (req: Request<undefined, undefined, undefined>, res: Response) => {
+  console.log(req.originalUrl, req.body);
+
+  rosService.readSingleMessageFromTopic('/raros/color_sensor/color', 'std_msgs/msg/ColorRGBA', (message: ColorRGBA) => {
+    res
+      .json({
+        red: message.r,
+        green: message.g,
+        blue: message.b,
+      })
+      .send();
+  });
+});
+
+type ColorRGBA = {
+  r: number;
+  g: number;
+  b: number;
+  a: number;
+};
diff --git a/apps/client/src/main/java/ch/hslu/raros/client/connector/Color.java b/apps/client/src/main/java/ch/hslu/raros/client/connector/Color.java
@@ -0,0 +1,4 @@
+package ch.hslu.raros.client.connector;
+
+public record Color(int red, int green, int blue) {
+}
diff --git a/apps/client/src/main/java/ch/hslu/raros/client/connector/ColorApiService.java b/apps/client/src/main/java/ch/hslu/raros/client/connector/ColorApiService.java
@@ -0,0 +1,30 @@
+package ch.hslu.raros.client.connector;
+
+import ch.hslu.raros.client.util.JsonSerializer;
+
+import java.net.URI;
+import java.net.http.HttpClient;
+import java.net.http.HttpRequest;
+import java.net.http.HttpResponse;
+import java.util.concurrent.CompletableFuture;
+
+class ColorApiService implements ColorService {
+  private final URI apiUri;
+
+  public ColorApiService(URI apiBaseUri) {
+    this.apiUri = apiBaseUri.resolve("/api/color/");
+  }
+
+  @Override
+  public CompletableFuture<Color> GetColor() {
+    var request = HttpRequest.newBuilder(apiUri)
+      .GET()
+      .build();
+
+    try (var httpClient = HttpClient.newHttpClient()) {
+      return httpClient.sendAsync(request, HttpResponse.BodyHandlers.ofString())
+        .thenApply(HttpResponse::body)
+        .thenApply(s -> JsonSerializer.deserialize(s, Color.class));
+    }
+  }
+}
diff --git a/apps/client/src/main/java/ch/hslu/raros/client/connector/ColorService.java b/apps/client/src/main/java/ch/hslu/raros/client/connector/ColorService.java
@@ -0,0 +1,12 @@
+package ch.hslu.raros.client.connector;
+
+import java.util.concurrent.CompletableFuture;
+
+public interface ColorService {
+  /**
+   * Gets the current color of the color sensor.
+   * @return CompletableFuture<Color> The current color of the color sensor.
+   */
+  CompletableFuture<Color> GetColor();
+}
+
diff --git a/apps/client/src/main/java/ch/hslu/raros/client/connector/ServiceFactory.java b/apps/client/src/main/java/ch/hslu/raros/client/connector/ServiceFactory.java
@@ -15,4 +15,8 @@ public static MagnetService createMagnetService(URI apiBaseUri) {
   public static BuzzerService createBuzzerService(URI apiBaseUri) {
     return new BuzzerApiService(apiBaseUri);
   }
+
+  public static ColorService createColorService(URI apiBaseUri) {
+    return new ColorApiService(apiBaseUri);
+  }
 }
diff --git a/apps/client/src/main/java/ch/hslu/raros/client/controller/RobotController.java b/apps/client/src/main/java/ch/hslu/raros/client/controller/RobotController.java
@@ -1,5 +1,7 @@
 package ch.hslu.raros.client.controller;
 
+import ch.hslu.raros.client.connector.Color;
+
 public interface RobotController {
 
   /**
@@ -32,5 +34,11 @@ public interface RobotController {
    * @param duration  The duration of the tone in ms.
    */
   void PlayToneAsync(int frequency, int duration);
+
+  /**
+   * Reads the current color of the color sensor.
+   * @return Color The last color read from the color sensor.
+   */
+  Color GetColor();
 }
 
diff --git a/apps/client/src/main/java/ch/hslu/raros/client/controller/RobotControllerFactory.java b/apps/client/src/main/java/ch/hslu/raros/client/controller/RobotControllerFactory.java
@@ -9,7 +9,8 @@ public static RobotController create(URI apiBaseUri) {
     return new RobotControllerImpl(
       new ActionAwaiterImpl(ServiceFactory.createActionService(apiBaseUri)),
       ServiceFactory.createMagnetService(apiBaseUri),
-      ServiceFactory.createBuzzerService(apiBaseUri)
+      ServiceFactory.createBuzzerService(apiBaseUri),
+      ServiceFactory.createColorService(apiBaseUri)
     );
   }
 }
diff --git a/apps/client/src/main/java/ch/hslu/raros/client/controller/RobotControllerImpl.java b/apps/client/src/main/java/ch/hslu/raros/client/controller/RobotControllerImpl.java
@@ -1,18 +1,25 @@
 package ch.hslu.raros.client.controller;
 
 import ch.hslu.raros.client.connector.BuzzerService;
+import ch.hslu.raros.client.connector.Color;
+import ch.hslu.raros.client.connector.ColorService;
 import ch.hslu.raros.client.connector.MagnetService;
 
 class RobotControllerImpl implements RobotController {
 
   private final ActionAwaiter actionAwaiter;
   private final MagnetService magnetService;
   private final BuzzerService buzzerService;
+  private final ColorService colorService;
 
-  public RobotControllerImpl(ActionAwaiter actionAwaiter, MagnetService magnetService, BuzzerService buzzerService) {
+  public RobotControllerImpl(ActionAwaiter actionAwaiter,
+                             MagnetService magnetService,
+                             BuzzerService buzzerService,
+                             ColorService colorService) {
     this.actionAwaiter = actionAwaiter;
     this.magnetService = magnetService;
     this.buzzerService = buzzerService;
+    this.colorService = colorService;
   }
 
   @Override
@@ -40,4 +47,9 @@ public void PlayTone(int frequency, int duration) {
   public void PlayToneAsync(int frequency, int duration) {
     this.buzzerService.PlayTone(frequency, duration);
   }
+
+  @Override
+  public Color GetColor() {
+    return this.colorService.GetColor().join();
+  }
 }
diff --git a/apps/ros2/src/raros/launch/raros_launch.py b/apps/ros2/src/raros/launch/raros_launch.py
@@ -4,6 +4,12 @@
 
 def generate_launch_description():
     return LaunchDescription([
+        Node(
+            package='raros',
+            namespace='raros',
+            executable='action_api',
+            name='action_api'
+        ),
         Node(
             package='raros',
             namespace='raros',
@@ -19,7 +25,7 @@ def generate_launch_description():
         Node(
             package='raros',
             namespace='raros',
-            executable='action_api',
-            name='action_api'
+            executable='color_sensor',
+            name='color_sensor'
         ),
     ])
diff --git a/apps/ros2/src/raros/package.xml b/apps/ros2/src/raros/package.xml
@@ -12,6 +12,7 @@
   <depend>rclpy</depend>
   <depend>std_msgs</depend>
   <depend>python-rpi.gpio-pip</depend>
+  <depend>python3-adafruit-blinka-pip</depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

diff --git a/apps/ros2/src/raros/raros/color_sensor.py b/apps/ros2/src/raros/raros/color_sensor.py
@@ -0,0 +1,52 @@
+import rclpy
+import board
+from typing import Optional
+from busio import I2C
+from adafruit_tcs34725 import TCS34725
+
+from rclpy.node import Node
+from std_msgs.msg import ColorRGBA
+
+
+class ColorSensor(Node):
+    def __init__(self):
+        super().__init__('color_sensor')
+        self.get_logger().info('color_sensor node started')
+        self.publisher = self.create_publisher(ColorRGBA, 'color_sensor/color', 10)
+        self.timer = self.create_timer(0.5, self.timer_callback)
+        self.i2c: Optional[I2C] = None
+        self.sensor: Optional[TCS34725] = None
+
+    def setup_i2c(self):
+        self.i2c = board.I2C()
+        self.sensor = TCS34725(self.i2c)
+        self.sensor.gain = 4
+        self.sensor.integration_time = 480
+
+    def timer_callback(self):
+        color_rgb = self.sensor.color_rgb_bytes
+        msg = ColorRGBA()
+        msg.r = float(color_rgb[0])
+        msg.g = float(color_rgb[1])
+        msg.b = float(color_rgb[2])
+        msg.a = 1.0
+        self.publisher.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    node = ColorSensor()
+
+    try:
+        node.setup_i2c()
+        rclpy.spin(node)
+        node.destroy_node()
+        rclpy.shutdown()
+    except KeyboardInterrupt:
+        pass
+    except Exception as e:
+        node.get_logger().error(f'Error: {e}')
+        raise e
+    finally:
+        board.I2C().deinit()
diff --git a/apps/ros2/src/raros/setup.py b/apps/ros2/src/raros/setup.py
@@ -26,6 +26,7 @@
             'action_api = raros.action_api:main',
             'magnet = raros.magnet:main',
             'buzzer = raros.buzzer:main',
+            'color_sensor = raros.color_sensor:main',
         ],
     },
 )

diff --git a/doc/arch/nodes.puml b/doc/arch/nodes.puml
@@ -7,6 +7,7 @@ frame "Raspberry Pi" as pi{
     node color_sensor
     node ultrasonic_sensors
     node buzzer
+    node status
 
     queue "motor_cmd"
     queue "motor_status"

diff --git a/examples/raros-client-example/src/main/java/ch/hslu/raros/example/Main.java b/examples/raros-client-example/src/main/java/ch/hslu/raros/example/Main.java
@@ -4,6 +4,7 @@
 import ch.hslu.raros.client.controller.RobotControllerFactory;
 
 import java.net.URI;
+import java.text.MessageFormat;
 import java.util.Scanner;
 
 public class Main {
@@ -14,6 +15,7 @@ public static void main(String[] args) {
     System.out.println("Press");
     System.out.println("  m to control magnet");
     System.out.println("  b to control buzzer");
+    System.out.println("  c to get color");
 
     var input = scanner.next();
     switch (input) {
@@ -23,6 +25,9 @@ public static void main(String[] args) {
       case "b":
         buzzerExample(robotController, scanner);
         break;
+      case "c":
+        colorExample(robotController, scanner);
+        break;
     }
   }
 
@@ -92,6 +97,28 @@ private static void buzzerExample(RobotController robotController, Scanner scann
     }
   }
 
+  private static void colorExample(RobotController robotController, Scanner scanner) {
+    System.out.println("Press");
+    System.out.println("  c to get color");
+    System.out.println("  q to quit");
+
+    while (true) {
+      System.out.print("Enter input: ");
+      var input = scanner.next();
+      switch (input) {
+        case "c":
+          var color = robotController.GetColor();
+          System.out.println(MessageFormat.format("Color (R,G,B): {0}, {1}, {2}", color.red(), color.green(), color.blue()));
+          break;
+        case "q":
+          return;
+        default:
+          System.out.println("Invalid input.");
+          break;
+      }
+    }
+  }
+
   private static int getFrequency(Scanner scanner) {
     System.out.print("Enter frequency: ");
     return scanner.nextInt();