diff --git a/src/main/java/RobotControl/CommInterfaceAgent.java b/src/main/java/RobotControl/CommInterfaceAgent.java index 7c8b207..0fc7a03 100755 --- a/src/main/java/RobotControl/CommInterfaceAgent.java +++ b/src/main/java/RobotControl/CommInterfaceAgent.java @@ -1,6 +1,7 @@ package RobotControl; import RabbitMQUtils.RabbitMQConsumer; +import RobotMsgs.CompositeMessage; import RobotMsgs.IMU; import RobotMsgs.Sonar; import RobotMsgs.UWB; @@ -13,6 +14,7 @@ import jade.lang.acl.ACLMessage; import java.util.Arrays; +import java.util.HashMap; import static jade.lang.acl.ACLMessage.INFORM; @@ -22,7 +24,7 @@ public class CommInterfaceAgent extends Agent { private final RabbitMQConsumer sonarConsumer; private final RabbitMQConsumer imuConsumer; - private static final String TARGET_AGENT = "AgentProducer"; + private static final String TARGET_AGENT_PREFIX = "RobotAgent"; Gson gson = new Gson(); @@ -41,9 +43,10 @@ private void startConsumerThread() throws Exception { imuConsumer.startConsume(); } - private void sendACLMessage(String msgBody) { + private void sendACLMessage(String msgBody, String targetAgent, String msgType) { ACLMessage messageTemplate = new ACLMessage(INFORM); - messageTemplate.addReceiver(new AID(TARGET_AGENT, AID.ISLOCALNAME)); + messageTemplate.addReceiver(new AID(TARGET_AGENT_PREFIX+targetAgent, AID.ISLOCALNAME)); + messageTemplate.setLanguage(msgType); messageTemplate.setContent(msgBody); send(messageTemplate); @@ -60,38 +63,41 @@ public void setup() { addBehaviour(new CyclicBehaviour() { @Override public void action() { - SequentialBehaviour Sequential = new SequentialBehaviour(); - addBehaviour(Sequential); - Sequential.addSubBehaviour(new OneShotBehaviour() { - @Override - public void action() { - if (uwbConsumer.getIsNewMessage()) { - try { - // Example for sending the recieved UWB message to RobotController Agent - sendACLMessage(uwbConsumer.getMsgJSON().toString()); - } catch (Exception e) { - e.printStackTrace(); - } + if (uwbConsumer.getIsNewMessage()) { + try { + // Example for sending the recieved UWB message to RobotController Agent + UWB[] uwbArray = gson.fromJson(uwbConsumer.getMsgJSON().toString(), UWB[].class); + for (UWB uwb : uwbArray) { + sendACLMessage(gson.toJson(uwb), String.valueOf(uwb.robot_id), "uwb"); } - if (sonarConsumer.getIsNewMessage()) { - try { - // Example for parsing the Sonar message into a JSON object. Check RobotMsgs package. - Sonar[] sonarArray = gson.fromJson(sonarConsumer.getMsgJSON().toString(), Sonar[].class); -// System.out.println(sonarArray[0].header.frame_id); - } catch (Exception e) { - e.printStackTrace(); - } - } - if (imuConsumer.getIsNewMessage()) { - try { - IMU[] imuArray = gson.fromJson(imuConsumer.getMsgJSON().toString(), IMU[].class); -// System.out.println(imuArray[0].header.frame_id); - } catch (Exception e) { - e.printStackTrace(); - } + } catch (Exception e) { + e.printStackTrace(); + } + } + if (sonarConsumer.getIsNewMessage()) { + try { + // Example for parsing the Sonar message into a JSON object. Check RobotMsgs package. + Sonar[] sonarArray = gson.fromJson(sonarConsumer.getMsgJSON().toString(), Sonar[].class); + for (Sonar sonar : sonarArray) { + String[] robotFrameId = sonar.header.frame_id.split("_"); + String robotId = robotFrameId[robotFrameId.length-1]; + sendACLMessage(gson.toJson(sonar), robotId, "sonar"); } + } catch (Exception e) { + e.printStackTrace(); } - }); + } + + + +// if (imuConsumer.getIsNewMessage()) { +// try { +// IMU[] imuArray = gson.fromJson(imuConsumer.getMsgJSON().toString(), IMU[].class); +// +// } catch (Exception e) { +// e.printStackTrace(); +// } +// } } }); diff --git a/src/main/java/RobotControl/Container.java b/src/main/java/RobotControl/Container.java index 722ff04..c55e43a 100755 --- a/src/main/java/RobotControl/Container.java +++ b/src/main/java/RobotControl/Container.java @@ -23,11 +23,20 @@ public static void main(String[] args) { Container.start(); - AgentController agentProducer=agentContainer.createNewAgent("AgentProducer", - "RobotControl.RobotControllerAgent",new Object[]{}); + AgentController robotAgent0=agentContainer.createNewAgent("robotAgent0", + "RobotControl.RobotAgent",new Object[]{'0'}); + AgentController robotAgent1=agentContainer.createNewAgent("robotAgent1", + "RobotControl.RobotAgent",new Object[]{'1'}); + AgentController robotAgent2=agentContainer.createNewAgent("robotAgent2", + "RobotControl.RobotAgent",new Object[]{'2'}); + AgentController robotAgent3=agentContainer.createNewAgent("robotAgent3", + "RobotControl.RobotAgent",new Object[]{'3'}); AgentController agentConsumer=agentContainer.createNewAgent("AgentConsumer", "RobotControl.CommInterfaceAgent",new Object[]{}); - agentProducer.start(); + robotAgent0.start(); + robotAgent1.start(); +// robotAgent2.start(); +// robotAgent3.start(); agentConsumer.start(); } catch (Exception e) { // TODO Auto-generated catch block diff --git a/src/main/java/RobotControl/RobotAgent.java b/src/main/java/RobotControl/RobotAgent.java new file mode 100644 index 0000000..a15b3fb --- /dev/null +++ b/src/main/java/RobotControl/RobotAgent.java @@ -0,0 +1,211 @@ +package RobotControl; + +import RabbitMQUtils.RabbitMQPublisher; +import RobotMsgs.RobotControl; +import RobotMsgs.Sonar; +import RobotMsgs.UWB; +import com.google.gson.Gson; +import jade.core.Agent; +import jade.core.behaviours.CyclicBehaviour; +import jade.lang.acl.ACLMessage; +import jade.lang.acl.MessageTemplate; + +import java.io.IOException; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +public class RobotAgent extends Agent { + private final RabbitMQPublisher rabbitMQPublisher; + + private int robotId = 0; + + List pointsArray = new ArrayList(); + + double linearSpeed = 0.1; + double angularSpeed = 0.25; + + Gson gson = new Gson(); + + UWB uwb; + + boolean hardStop = false; + + public RobotAgent() throws Exception { + rabbitMQPublisher = new RabbitMQPublisher("robot_ctrl"); + } + + private UWB initRobot() { + UWB a = new UWB(); + a.robot_id = this.robotId; + a.orientation.z = 90.0; + return a; + } + + private void setPoints() { + if (robotId == 0) { + pointsArray.add(new double[]{1.38, 0.0, -180.0}); + pointsArray.add(new double[]{1.32, 2.7, -180.0}); + pointsArray.add(new double[]{1.32, 6.10, -180.0}); + pointsArray.add(new double[]{-0.24, 6.13, -180.0}); + pointsArray.add(new double[]{1.32, 6.10, -180.0}); + pointsArray.add(new double[]{1.32, 2.7, -180.0}); + } else if (robotId == 1) { + pointsArray.add(new double[]{-3.8, -0.45, -180.0}); + pointsArray.add(new double[]{-3.8, 2.7, -180.0}); + pointsArray.add(new double[]{-3.8, 6.10, -180.0}); + pointsArray.add(new double[]{-2.75, 6.10, -180.0}); + pointsArray.add(new double[]{-3.8, 6.10, -180.0}); + pointsArray.add(new double[]{-3.8, 2.7, -180.0}); + } else if (robotId == 2) { + pointsArray.add(new double[]{-3.8, 0.45, -180.0}); + pointsArray.add(new double[]{-3.8, -2.7, -180.0}); + pointsArray.add(new double[]{-3.8, -6.10, -180.0}); + pointsArray.add(new double[]{-2.75, -6.10, -180.0}); + pointsArray.add(new double[]{-3.8, -6.10, -180.0}); + pointsArray.add(new double[]{-3.8, -2.7, -180.0}); + } else { + pointsArray.add(new double[]{1.38, 0.0, -180.0}); + pointsArray.add(new double[]{1.32, -2.7, -180.0}); + pointsArray.add(new double[]{1.32, -6.10, -180.0}); + pointsArray.add(new double[]{-0.24, -6.13, -180.0}); + pointsArray.add(new double[]{1.32, -6.10, -180.0}); + pointsArray.add(new double[]{1.32, -2.7, -180.0}); + } + + + } + + private double[] getCurrentGoal() { + return pointsArray.get(0); + } + + private double getGoalDistance(double curr_pos_x, double curr_pos_y) { + double x_diff = getCurrentGoal()[0] - curr_pos_x; + double y_diff = getCurrentGoal()[1] - curr_pos_y; + + return Math.sqrt(Math.pow(x_diff, 2) + Math.pow(y_diff, 2)); + } + + private void checkCurrentGoal(double goalDistance) { + if (goalDistance < 0.5) { + System.out.printf("Robot_%d CURRENT GOAL REACHED\n", robotId); + pointsArray.remove(0); + if (pointsArray.size() == 3) { + System.out.printf("Robot_%d PARCEL PLACED. RETURNING TO PICKUP STATION\n", robotId); + } + } + if (pointsArray.isEmpty()) { + System.out.printf("Robot_%d PARCEL PICKED UP. GOING TO DROP STATION\n", robotId); + setPoints(); + } + } + + private RobotControl composeRobotControlMsg(double linear_x, double angular_z) { + RobotControl msg = new RobotControl(); + + msg.position.x = linear_x; + msg.orientation.z = angular_z; + + return msg; + } + + private void robotControlFlow(ACLMessage Message) { + + UWB robotPosMsg = gson.fromJson(Message.getContent(), UWB.class); + RobotControl[] robotCtrlMsgs = new RobotControl[1]; + + if (robotPosMsg == null) { + return; + } + + // This logic is a basic control algorithm without any PID controller. + // Basically checks every step if the robot close enough to goal + // And update the angular and linear speed for getting close + + double posX = robotPosMsg.position.x; + double posY = robotPosMsg.position.y; + double yaw = Math.toRadians(robotPosMsg.orientation.z); + checkCurrentGoal(getGoalDistance(posX, posY)); + + double goalX = getCurrentGoal()[0]; + double goalY = getCurrentGoal()[1]; + + + double pathAngle = Math.atan2(goalY - posY, goalX - posX) - yaw; + double diffGoalAngle = Math.atan2(Math.sin(pathAngle), Math.cos(pathAngle)); + + double linear; + double angular = angularSpeed * diffGoalAngle; + + double distance = getGoalDistance(posX, posY); + // This part is for adjusting approach speed of the robot. Right now it is really slow for testing. + if (distance < 0.1 || Math.abs(Math.toDegrees(diffGoalAngle)) > 25) { + linear = 0.1; + } else if (linearSpeed * distance < 0.1) + linear = 1.75; + else + linear = 2.0; + + if (Math.abs(Math.toDegrees(diffGoalAngle)) < 2) + angular = 0; + else if (angular > 0) + angular = Math.max(angular, 0.5); + else + angular = Math.min(angular, -0.5); + + uwb = robotPosMsg; + if (hardStop) { + System.out.printf("Robot_%d DETECTED OBSTACLE. SLOWING DOWN\n", robotId); + linear = -0.75; + } + RobotControl ctrlMsg = composeRobotControlMsg(linear, angular); + ctrlMsg.robot_id = robotId; + + robotCtrlMsgs[0] = ctrlMsg; + + try { + String msgString = gson.toJson(robotCtrlMsgs); + rabbitMQPublisher.sendMessage(msgString); + + } catch (IOException e) { + throw new RuntimeException(e); + } + + } + + private void sonarFlow(ACLMessage Message) { + Sonar robotSonarMsg = gson.fromJson(Message.getContent(), Sonar.class); + + if (robotSonarMsg != null) { + hardStop = robotSonarMsg.ranges[0] < 1.0; + } + } + + @Override + public void setup() { + Object[] args = getArguments(); + robotId = Integer.parseInt(args[0].toString()); + uwb = this.initRobot(); + setPoints(); + System.out.printf("Robot_%d INITIALIZED\n", robotId); + + addBehaviour(new CyclicBehaviour() { + @Override + public void action() { + ACLMessage uwbMessage = receive(MessageTemplate.MatchLanguage("uwb")); + ACLMessage sonarMessage = receive(MessageTemplate.MatchLanguage("sonar")); + if (uwbMessage != null) { + robotControlFlow(uwbMessage); + } + if (sonarMessage != null) { + sonarFlow(sonarMessage); + } + if (sonarMessage == null && uwbMessage == null) { + block(); + } + } + }); + + } +} diff --git a/src/main/java/RobotControl/RobotControllerAgent.java b/src/main/java/RobotControl/RobotControllerAgent.java index c08647d..c838890 100755 --- a/src/main/java/RobotControl/RobotControllerAgent.java +++ b/src/main/java/RobotControl/RobotControllerAgent.java @@ -135,9 +135,9 @@ public void action() { linear = 0; } else if (linearSpeed * distance < 0.1) - linear = 0.1; + linear = 0.25; else - linear = 0.2; + linear = 0.5; if (Math.abs(Math.toDegrees(diffGoalAngle)) < 2) angular = 0; diff --git a/src/main/java/RobotMsgs/CompositeMessage.java b/src/main/java/RobotMsgs/CompositeMessage.java new file mode 100644 index 0000000..804c153 --- /dev/null +++ b/src/main/java/RobotMsgs/CompositeMessage.java @@ -0,0 +1,6 @@ +package RobotMsgs; + +public class CompositeMessage { + public UWB uwb; + public Sonar sonar; +}