diff --git a/apps/api/src/index.ts b/apps/api/src/index.ts index fa29ee8..5fb37b9 100644 --- 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 index 5e5b56e..c5b9fe4 100644 --- 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(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 new file mode 100644 index 0000000..1121200 --- /dev/null +++ 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, 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 new file mode 100644 index 0000000..3e1121f --- /dev/null +++ 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 new file mode 100644 index 0000000..17ac9c5 --- /dev/null +++ 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 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 new file mode 100644 index 0000000..91d3b80 --- /dev/null +++ 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 The current color of the color sensor. + */ + CompletableFuture 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 index 6250121..df52570 100644 --- 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 index 6cbad6e..e5b2882 100644 --- 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 index b6e327b..83e63e0 100644 --- 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 index 10d586a..38316d6 100644 --- 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,6 +1,8 @@ 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 { @@ -8,11 +10,16 @@ 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 index 36f33dc..65f6b66 100644 --- 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 index 8074c73..720e82c 100644 --- a/apps/ros2/src/raros/package.xml +++ b/apps/ros2/src/raros/package.xml @@ -12,6 +12,7 @@ rclpy std_msgs python-rpi.gpio-pip + python3-adafruit-blinka-pip ament_copyright ament_flake8 diff --git a/apps/ros2/src/raros/raros/color_sensor.py b/apps/ros2/src/raros/raros/color_sensor.py new file mode 100644 index 0000000..d7563d1 --- /dev/null +++ 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 index 2262b7e..3051f1f 100644 --- 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 index 394b5ed..ac4ef66 100644 --- 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 index dc506e0..abda443 100644 --- 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();