From 682a93f8abeac502db39684f262143d7488810c9 Mon Sep 17 00:00:00 2001 From: dhmudalige Date: Wed, 28 Feb 2024 20:46:15 +0530 Subject: [PATCH 1/3] Update CompassSensor.java --- .../swarm/robot/sensors/CompassSensor.java | 70 +++++++++++++++++-- 1 file changed, 66 insertions(+), 4 deletions(-) diff --git a/src/main/java/swarm/robot/sensors/CompassSensor.java b/src/main/java/swarm/robot/sensors/CompassSensor.java index 5565415..77c4798 100644 --- a/src/main/java/swarm/robot/sensors/CompassSensor.java +++ b/src/main/java/swarm/robot/sensors/CompassSensor.java @@ -1,22 +1,84 @@ package swarm.robot.sensors; +import org.json.simple.parser.ParseException; +import swarm.mqtt.MqttMsg; import swarm.mqtt.RobotMqttClient; import swarm.robot.Robot; +import swarm.robot.exception.SensorException; + +import java.util.HashMap; /** * Compass Sensors Emulator class * - * @author TBD + * @author Dinuka Mudalige */ public class CompassSensor extends AbstractSensor { + private enum mqttTopic { + COMPASS_OUT + } + + private final static int MQTT_TIMEOUT = 2000; + + private boolean compassLock = false; + private HashMap topicsSub = new HashMap<>(); +// private CompassReadingType compass; + private double heading; public CompassSensor(Robot robot, RobotMqttClient mqttClient) { super(robot, mqttClient); + subscribe(CompassSensor.mqttTopic.COMPASS_OUT, "sensor/compass/" + robotId + "/?"); + } + + /** + * Subscribe to a MQTT topic + * + * @param key Subscription topic key + * @param topic Subscription topic value + */ + private void subscribe(CompassSensor.mqttTopic key, String topic) { + topicsSub.put(key, topic); + robotMqttClient.subscribe(topic); } - public double readComapss() throws Exception { - // To be implemented to return the heading direction of the robot + /** + * Handle compassSensor related MQTT subscriptions + * + * @param robot Robot object + * @param m Subscription topic received object + */ + @Override + public void handleSubscription(Robot robot, MqttMsg m) throws RuntimeException{ + heading = Integer.parseInt(m.message); + } + + /** + * Get the emulated compass sensor reading from the simulator + * + * @return heading as double + * @throws SensorException + */ + public double readCompass() throws Exception { + compassLock = true; // Acquire the compass sensor lock + + long startTime = System.currentTimeMillis(); + boolean timeout = false; + + while (compassLock && !timeout) { + try { + robot.handleSubscribeQueue(); + } catch (ParseException e) { + e.printStackTrace(); + } + + robot.delay(100); + timeout = (System.currentTimeMillis() - startTime > MQTT_TIMEOUT); + } + + if (timeout) { + throw new SensorException("Compass sensor timeout"); + } - return 0; + return heading; } } From 340096870d8a111ebec8c1c15c17b71f31585bcf Mon Sep 17 00:00:00 2001 From: dhmudalige Date: Tue, 5 Mar 2024 22:09:11 +0530 Subject: [PATCH 2/3] Update project updated the virtual compass reading technique --- src/main/java/Robots/MyTestRobot.java | 2 +- src/main/java/swarm/robot/Robot.java | 2 +- .../swarm/robot/sensors/CompassSensor.java | 19 +++++++++++++++++-- 3 files changed, 19 insertions(+), 4 deletions(-) diff --git a/src/main/java/Robots/MyTestRobot.java b/src/main/java/Robots/MyTestRobot.java index d4b249a..6a4b66c 100644 --- a/src/main/java/Robots/MyTestRobot.java +++ b/src/main/java/Robots/MyTestRobot.java @@ -17,7 +17,7 @@ public void loop() throws Exception { super.loop(); if (state == robotState.RUN) { - System.out.println("Test"); + System.out.println("\t\t Compass reading: " + compassSensor.readCompass()); delay(1000); } } diff --git a/src/main/java/swarm/robot/Robot.java b/src/main/java/swarm/robot/Robot.java index e6823bf..5314e69 100644 --- a/src/main/java/swarm/robot/Robot.java +++ b/src/main/java/swarm/robot/Robot.java @@ -159,7 +159,7 @@ public final void handleSubscribeQueue() throws ParseException { // System.out.println("proximity sensor message received"); proximitySensor.handleSubscription(this, m); } else if (m.topicGroups[1].equals("compass")) { - // System.out.println("compass sensor message received"); + System.out.println("compass sensor message received"); compassSensor.handleSubscription(this, m); } diff --git a/src/main/java/swarm/robot/sensors/CompassSensor.java b/src/main/java/swarm/robot/sensors/CompassSensor.java index 77c4798..fbc84b5 100644 --- a/src/main/java/swarm/robot/sensors/CompassSensor.java +++ b/src/main/java/swarm/robot/sensors/CompassSensor.java @@ -4,6 +4,7 @@ import swarm.mqtt.MqttMsg; import swarm.mqtt.RobotMqttClient; import swarm.robot.Robot; +import swarm.robot.VirtualRobot; import swarm.robot.exception.SensorException; import java.util.HashMap; @@ -49,7 +50,8 @@ private void subscribe(CompassSensor.mqttTopic key, String topic) { */ @Override public void handleSubscription(Robot robot, MqttMsg m) throws RuntimeException{ - heading = Integer.parseInt(m.message); + heading = Integer.parseInt(m.message); // physical robot's heading directly from the MQTT subscription + System.out.println(" Received topic: " + m.topic + " = "+ heading); } /** @@ -59,6 +61,19 @@ public void handleSubscription(Robot robot, MqttMsg m) throws RuntimeException{ * @throws SensorException */ public double readCompass() throws Exception { + try { + if (robot instanceof VirtualRobot) { + heading = robot.coordinates.getHeading(); + } else { + robot.handleSubscribeQueue(); + } + } catch (ParseException e) { + e.printStackTrace(); + } + robot.delay(100); + return heading; + } + /* public double readCompass() throws Exception { compassLock = true; // Acquire the compass sensor lock long startTime = System.currentTimeMillis(); @@ -80,5 +95,5 @@ public double readCompass() throws Exception { } return heading; - } + } */ } From cfdf83a2be54c8842bfabf84b0ff3ab82817fbb6 Mon Sep 17 00:00:00 2001 From: dhmudalige Date: Tue, 5 Mar 2024 22:10:21 +0530 Subject: [PATCH 3/3] Test implementation tested with ObstacleAvoidRobot --- src/main/java/Robots/ObstacleAvoidRobot.java | 63 ++++++++++++++++++++ src/main/java/swarm/App.java | 3 +- 2 files changed, 65 insertions(+), 1 deletion(-) create mode 100644 src/main/java/Robots/ObstacleAvoidRobot.java diff --git a/src/main/java/Robots/ObstacleAvoidRobot.java b/src/main/java/Robots/ObstacleAvoidRobot.java new file mode 100644 index 0000000..05c0d72 --- /dev/null +++ b/src/main/java/Robots/ObstacleAvoidRobot.java @@ -0,0 +1,63 @@ +// This robot will move freely, avoiding obstacles +// Written by Nuwan Jaliyagoda + +package Robots; + +import swarm.robot.VirtualRobot; + +public class ObstacleAvoidRobot extends VirtualRobot { + + // The minimum distance that robot tries to keep with the obstacles + private int distanceThreshold = 15; + + // The default movement speed + private int defaultMoveSpeed = 100; + + public ObstacleAvoidRobot(int id, double x, double y, double heading) { + super(id, x, y, heading); + } + + public void setup() { + super.setup(); + } + + @Override + public void loop() throws Exception { + super.loop(); + + if (state == robotState.RUN) { + double dist = distSensor.getDistance(); + + if (dist < distanceThreshold) { + // Generate a random number in [-1000,1000] range + // if even, rotate CW, otherwise rotate CCW an angle depends on the random + // number + int random = -1000 + ((int) ((Math.random() * 2000))); + int sign = (random % 2 == 0) ? 1 : -1; + + System.out.println("\t Wall detected, go back and rotate " + ((sign > 0) ? "CW" : "CCW")); + + // Go back a little + motion.move(-100, -100, 900); + + // rotate + int loopCount = 0; + while (distSensor.getDistance() < distanceThreshold && loopCount < 5) { + // Maximum 5 tries to rotate and find a obstale free path + motion.rotate((int) (defaultMoveSpeed * 0.75 * sign), 1000); + loopCount++; + } + + // rotate a little more + motion.rotate((int) (defaultMoveSpeed * 0.75 * sign), 500); + + + System.out.println("\t\t Compass reading: " + compassSensor.readCompass()); + + } else { + motion.move(defaultMoveSpeed, defaultMoveSpeed, 1000); + } + } + + } +} diff --git a/src/main/java/swarm/App.java b/src/main/java/swarm/App.java index f2f58f9..79227a9 100644 --- a/src/main/java/swarm/App.java +++ b/src/main/java/swarm/App.java @@ -34,7 +34,8 @@ public static void main(String[] args) { reader.close(); // Start a single robot - Robot robot = new MyTestRobot(10, 0, 0, 90); +// Robot robot = new MyTestRobot(4, 18, 6, 180); + Robot robot = new ObstacleAvoidRobot(4, 18, 6, 180); new Thread(robot).start(); // // Start a swarm of robots