Add robotic leader-follower demo to ROS tests

d3scomp · Nov 4, 2015 · e8b0bec · e8b0bec
1 parent 405764c
commit e8b0bec
Show file tree

Hide file tree

Showing 5 changed files with 302 additions and 0 deletions.
diff --git a/jdeeco-ros/test/cz/cuni/mff/d3s/jdeeco/ros/leaderfollowerdemo/FollowerRobot.java b/jdeeco-ros/test/cz/cuni/mff/d3s/jdeeco/ros/leaderfollowerdemo/FollowerRobot.java
@@ -0,0 +1,91 @@
+package cz.cuni.mff.d3s.jdeeco.ros.leaderfollowerdemo;
+
+import java.util.Random;
+
+import cz.cuni.mff.d3s.deeco.annotations.Component;
+import cz.cuni.mff.d3s.deeco.annotations.In;
+import cz.cuni.mff.d3s.deeco.annotations.InOut;
+import cz.cuni.mff.d3s.deeco.annotations.Local;
+import cz.cuni.mff.d3s.deeco.annotations.PeriodicScheduling;
+import cz.cuni.mff.d3s.deeco.annotations.Process;
+import cz.cuni.mff.d3s.deeco.task.ParamHolder;
+import cz.cuni.mff.d3s.jdeeco.position.Position;
+import cz.cuni.mff.d3s.jdeeco.ros.Positioning;
+import cz.cuni.mff.d3s.jdeeco.ros.datatypes.Orientation;
+import cz.cuni.mff.d3s.jdeeco.ros.datatypes.PoseWithCovariance;
+
+@Component
+public class FollowerRobot {
+	/**
+	 * Id of the vehicle component.
+	 */
+	public String id;
+
+	/**
+	 * Robots current position
+	 */
+	public Position position;
+
+	/**
+	 * Robots destination
+	 */
+	public Position destination;
+
+	@Local
+	public Positioning positioning;
+
+	@Local
+	public Position goal;
+
+	@Local
+	public static Random rand = new Random(42);
+
+
+	public FollowerRobot(String id, Positioning positioning) {
+		this.id = id;
+		this.positioning = positioning;
+		this.position = new Position(0, 0, 0);
+	}
+
+	@Process
+	@PeriodicScheduling(period = 100)
+	public static void sense(@InOut("position") ParamHolder<Position> position, @In("positioning") Positioning positioning) {
+		PoseWithCovariance pos = positioning.getPosition();
+		if(pos != null) {
+			position.value = new Position(pos.position.x, pos.position.y, pos.position.z);
+		}
+	}
+
+	@Process
+	@PeriodicScheduling(period = 1000)
+	public static void reportStatus(
+			@In("id") String id,
+			@In("position") Position position) {
+
+		System.out.format("Follower: Id: %s, pos: %s%n", id, position.toString());
+	}
+
+	@Process
+	@PeriodicScheduling(period = 500)
+	public static void planRouteAndDrive(
+			@In("id") String id,
+			@In("position") Position position,
+			@In("positioning") Positioning positioning,
+			@In("destination") Position destination,
+			@InOut("goal") ParamHolder<Position> goal) throws Exception {
+		// If we have the leader position
+		if(destination != null) {
+			System.out.format("Follower: Leader at: %s %n", destination);
+
+			goal.value = destination;
+			positioning.setSimpleGoal(new cz.cuni.mff.d3s.jdeeco.ros.datatypes.Position(
+					goal.value.x + rand.nextDouble() - 0.5,
+					goal.value.y + rand.nextDouble() - 0.5,
+					goal.value.z),
+					new Orientation(0, 0, 0, 1));
+			System.out.println("Follower: Goal set: " + goal.value.toString());
+		} else {
+			System.out.println("Follower: No leader position !!!");
+		}
+	}
+}
diff --git a/jdeeco-ros/test/cz/cuni/mff/d3s/jdeeco/ros/leaderfollowerdemo/LeaderFollowerEnsemble.java b/jdeeco-ros/test/cz/cuni/mff/d3s/jdeeco/ros/leaderfollowerdemo/LeaderFollowerEnsemble.java
@@ -0,0 +1,28 @@
+package cz.cuni.mff.d3s.jdeeco.ros.leaderfollowerdemo;
+
+import cz.cuni.mff.d3s.deeco.annotations.Ensemble;
+import cz.cuni.mff.d3s.deeco.annotations.In;
+import cz.cuni.mff.d3s.deeco.annotations.KnowledgeExchange;
+import cz.cuni.mff.d3s.deeco.annotations.Membership;
+import cz.cuni.mff.d3s.deeco.annotations.Out;
+import cz.cuni.mff.d3s.deeco.annotations.PeriodicScheduling;
+import cz.cuni.mff.d3s.deeco.logging.Log;
+import cz.cuni.mff.d3s.deeco.task.ParamHolder;
+import cz.cuni.mff.d3s.jdeeco.position.Position;
+
+@Ensemble
+@PeriodicScheduling(period = 100)
+public class LeaderFollowerEnsemble {
+	@Membership
+	public static boolean membership(@In("coord.id") String coordId, @In("coord.position") Position coordPosition,
+			@In("member.id") String memberId, @In("member.position") Position memberPosition) {
+		Log.i(LeaderFollowerEnsemble.class.getName() + " MEMBERSHIP");
+		return coordId.equals("robot_0") && memberId.equals("robot_1") && coordPosition.euclidDistanceTo(memberPosition) < 200;
+	}
+
+	@KnowledgeExchange
+	public static void exchange(@In("coord.position") Position coordPosition, @Out("member.destination") ParamHolder<Position> memberPosition) {
+		Log.i(LeaderFollowerEnsemble.class.getName() + " EXCHANGE");
+		memberPosition.value = coordPosition;
+	}
+}
diff --git a/jdeeco-ros/test/cz/cuni/mff/d3s/jdeeco/ros/leaderfollowerdemo/LeaderRobot.java b/jdeeco-ros/test/cz/cuni/mff/d3s/jdeeco/ros/leaderfollowerdemo/LeaderRobot.java
@@ -0,0 +1,115 @@
+package cz.cuni.mff.d3s.jdeeco.ros.leaderfollowerdemo;
+
+import java.util.LinkedList;
+import java.util.List;
+
+import cz.cuni.mff.d3s.deeco.annotations.Component;
+import cz.cuni.mff.d3s.deeco.annotations.In;
+import cz.cuni.mff.d3s.deeco.annotations.InOut;
+import cz.cuni.mff.d3s.deeco.annotations.Local;
+import cz.cuni.mff.d3s.deeco.annotations.PeriodicScheduling;
+import cz.cuni.mff.d3s.deeco.annotations.Process;
+import cz.cuni.mff.d3s.deeco.task.ParamHolder;
+import cz.cuni.mff.d3s.jdeeco.position.Position;
+import cz.cuni.mff.d3s.jdeeco.ros.Positioning;
+import cz.cuni.mff.d3s.jdeeco.ros.datatypes.Orientation;
+import cz.cuni.mff.d3s.jdeeco.ros.datatypes.PoseWithCovariance;
+
+@Component
+public class LeaderRobot {
+	/**
+	 * Id of the vehicle component.
+	 */
+	public String id;
+
+	public Position position;
+
+	@Local
+	public List<Position> route;
+
+	@Local
+	public Positioning positioning;
+
+	@Local
+	public Position goal;
+
+	public LeaderRobot(String id, Positioning positioning) {
+		this.id = id;
+		this.positioning = positioning;
+		this.position = new Position(0, 0, 0);
+
+		route = new LinkedList<>();				
+		route.add(new Position(13, 2, 0));
+		route.add(new Position(3, 14, 0));
+		route.add(new Position(17, 4, 0));
+		route.add(new Position(13, 2, 0));
+		route.add(new Position(28, 14, 0));
+	}
+
+	@Process
+	@PeriodicScheduling(period = 100)
+	public static void sense(@InOut("position") ParamHolder<Position> position, @In("positioning") Positioning positioning) {
+		PoseWithCovariance pos = positioning.getPosition();
+		if(pos != null) {
+			position.value = new Position(pos.position.x, pos.position.y, pos.position.z);
+		}
+	}
+
+	@Process
+	@PeriodicScheduling(period = 1000)
+	public static void reportStatus(
+			@In("id") String id,
+			@In("position") Position position) {
+
+		System.out.format("Leader: Id: %s, pos: %s%n", id, position.toString());		
+	}
+
+	@Process
+	@PeriodicScheduling(period = 2000)
+	public static void planRouteAndDrive(
+			@In("id") String id,
+			@In("position") Position position,
+			@In("positioning") Positioning positioning,
+			@InOut("route") ParamHolder<List<Position>> route,
+			@InOut("goal") ParamHolder<Position> goal) throws Exception {
+
+		// If goal not set go to the first one
+		if(goal.value == null) {
+			goal.value = route.value.get(0);
+
+			positioning.setSimpleGoal(new cz.cuni.mff.d3s.jdeeco.ros.datatypes.Position(
+					goal.value.x,
+					goal.value.y,
+					goal.value.z),
+					new Orientation(0, 0, 0, 1));
+		}
+
+		if(positioning.getMoveBaseResult() != null) {
+			switch(positioning.getMoveBaseResult().status) {
+			case Succeeded:
+				Position reached = route.value.get(0);
+				System.out.format("Leader: At: %s reached %s%n", position, reached);
+				route.value.remove(reached);
+				route.value.add(reached);
+				goal.value = route.value.get(0);
+
+				positioning.setSimpleGoal(new cz.cuni.mff.d3s.jdeeco.ros.datatypes.Position(
+						goal.value.x,
+						goal.value.y,
+						goal.value.z),
+						new Orientation(0, 0, 0, 1));
+			break;
+			case Rejected:
+				positioning.setSimpleGoal(new cz.cuni.mff.d3s.jdeeco.ros.datatypes.Position(
+						goal.value.x,
+						goal.value.y,
+						goal.value.z),
+						new Orientation(0, 0, 0, 1));
+			break;
+			default:
+			}
+
+			System.out.println("Leader: result: " + positioning.getMoveBaseResult().toString());
+		}
+	}
+}
diff --git a/jdeeco-ros/test/cz/cuni/mff/d3s/jdeeco/ros/leaderfollowerdemo/RobotLeaderFollowerDemo.java b/jdeeco-ros/test/cz/cuni/mff/d3s/jdeeco/ros/leaderfollowerdemo/RobotLeaderFollowerDemo.java
@@ -0,0 +1,59 @@
+package cz.cuni.mff.d3s.jdeeco.ros.leaderfollowerdemo;
+
+import java.io.IOException;
+
+import cz.cuni.mff.d3s.deeco.annotations.processor.AnnotationProcessorException;
+import cz.cuni.mff.d3s.deeco.runners.DEECoSimulation;
+import cz.cuni.mff.d3s.deeco.runtime.DEECoException;
+import cz.cuni.mff.d3s.deeco.runtime.DEECoNode;
+import cz.cuni.mff.d3s.jdeeco.network.Network;
+import cz.cuni.mff.d3s.jdeeco.network.device.SimpleBroadcastDevice;
+import cz.cuni.mff.d3s.jdeeco.network.l2.strategy.KnowledgeInsertingStrategy;
+import cz.cuni.mff.d3s.jdeeco.publishing.DefaultKnowledgePublisher;
+import cz.cuni.mff.d3s.jdeeco.ros.Positioning;
+import cz.cuni.mff.d3s.jdeeco.ros.RosServices;
+import cz.cuni.mff.d3s.jdeeco.ros.sim.ROSSimulation;
+
+/**
+ * Example of vehicles traveling across the map
+ * 
+ * @author Vladimir Matena <matena@d3s.mff.cuni.cz>
+ *
+ */
+public class RobotLeaderFollowerDemo {
+	public static void main(String[] args) throws AnnotationProcessorException, InterruptedException, DEECoException,
+			InstantiationException, IllegalAccessException, IOException {
+		new RobotLeaderFollowerDemo().testTravel();
+	}
+
+	public void testTravel() throws AnnotationProcessorException, InterruptedException, DEECoException,
+			InstantiationException, IllegalAccessException, IOException {
+
+		ROSSimulation rosSim = new ROSSimulation("http://192.168.56.101:11311", "192.168.56.1");
+
+		// Create main application container
+		DEECoSimulation realm = new DEECoSimulation(rosSim.getTimer());
+
+
+		// Configure loop-back networking for all nodes
+		realm.addPlugin(Network.class);
+		realm.addPlugin(DefaultKnowledgePublisher.class);
+		realm.addPlugin(KnowledgeInsertingStrategy.class);
+		realm.addPlugin(new SimpleBroadcastDevice(0, 0, 100000, SimpleBroadcastDevice.DEFAULT_MTU));
+
+		Positioning robot0Pos = new Positioning();
+		RosServices robot0Services = rosSim.createROSServices("/robot_0");
+		DEECoNode robot0 = realm.createNode(0, robot0Pos, robot0Services);
+		robot0.deployComponent(new LeaderRobot("robot_0", robot0Pos));
+		robot0.deployEnsemble(LeaderFollowerEnsemble.class);
+
+		Positioning robot1Pos = new Positioning();
+		RosServices robot1Services = rosSim.createROSServices("/robot_1");
+		DEECoNode robot1 = realm.createNode(1, robot1Pos, robot1Services);
+		robot1.deployComponent(new FollowerRobot("robot_1", robot1Pos));
+		robot1.deployEnsemble(LeaderFollowerEnsemble.class);
+
+		// Simulate for specified time
+		realm.start(7 * 60000);
+	}
+}
diff --git a/jdeeco-ros/test/cz/cuni/mff/d3s/jdeeco/ros/leaderfollowerdemo/package-info.java b/jdeeco-ros/test/cz/cuni/mff/d3s/jdeeco/ros/leaderfollowerdemo/package-info.java
@@ -0,0 +1,9 @@
+/**
+ * This package contains leader-follower ROS simulation demo 
+ * 
+ * In the demo one robot cycles some way-points while the second one tries to follow.
+ * 
+ * @author Vladimir Matena <matena@d3s.mff.cuni.cz>
+ *
+ */
+package cz.cuni.mff.d3s.jdeeco.ros.leaderfollowerdemo;