Skip to content

Update for PAAMS Demo #7

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
DogukanAltay wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Update for PAAMS Demo #7

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
70 changes: 38 additions & 32 deletions src/main/java/RobotControl/CommInterfaceAgent.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
package RobotControl;

import RabbitMQUtils.RabbitMQConsumer;
import RobotMsgs.CompositeMessage;
import RobotMsgs.IMU;
import RobotMsgs.Sonar;
import RobotMsgs.UWB;
Expand All @@ -13,6 +14,7 @@
import jade.lang.acl.ACLMessage;

import java.util.Arrays;
import java.util.HashMap;

import static jade.lang.acl.ACLMessage.INFORM;

Expand All @@ -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();

Expand All @@ -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);
Expand All @@ -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();
// }
// }
}
});

Expand Down
15 changes: 12 additions & 3 deletions src/main/java/RobotControl/Container.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
211 changes: 211 additions & 0 deletions src/main/java/RobotControl/RobotAgent.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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<double[]> pointsArray = new ArrayList<double[]>();

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

}
}
4 changes: 2 additions & 2 deletions src/main/java/RobotControl/RobotControllerAgent.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
6 changes: 6 additions & 0 deletions src/main/java/RobotMsgs/CompositeMessage.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
package RobotMsgs;

public class CompositeMessage {
public UWB uwb;
public Sonar sonar;
}