From 24feea6346db3ea09391cfc0fb4d683748459f73 Mon Sep 17 00:00:00 2001
From: Tarek Taha <tarek.taha@gmail.com>
Date: Wed, 16 Sep 2015 16:56:52 +0400
Subject: [PATCH] - changes to allow for multi-uav support

---
 launch/ardrone.launch                       |  2 +
 launch/iris.launch                          |  2 +
 launch/multi_uav.launch                     | 55 +++++++++++++++++++++
 launch/multicopter.launch                   |  5 +-
 launch/multicopter_w.launch                 |  2 +
 launch/multicopter_x.launch                 |  2 +
 src/platforms/ros/nodes/mavlink/mavlink.cpp |  9 ++--
 src/platforms/ros/nodes/mavlink/mavlink.h   |  2 +-
 8 files changed, 74 insertions(+), 5 deletions(-)
 create mode 100644 launch/multi_uav.launch

diff --git a/launch/ardrone.launch b/launch/ardrone.launch
index f43bbf4709..db980731fe 100644
--- a/launch/ardrone.launch
+++ b/launch/ardrone.launch
@@ -1,8 +1,10 @@
 <launch>
 <arg name="ns"/>
+<arg name="mavlink_fcu_url" default="udp://localhost:14565@localhost:14560"/>
 
 <include file="$(find px4)/launch/multicopter_x.launch">
 	<arg name="ns" value="$(arg ns)"/>
+	<arg name="mavlink_fcu_url" value="$(arg mavlink_fcu_url)" />
 </include>
 
 <group ns="$(arg ns)">
diff --git a/launch/iris.launch b/launch/iris.launch
index 5231e3215b..798b761d76 100644
--- a/launch/iris.launch
+++ b/launch/iris.launch
@@ -1,8 +1,10 @@
 <launch>
 <arg name="ns"/>
+<arg name="mavlink_fcu_url" default="udp://localhost:14565@localhost:14560"/>
 
 <include file="$(find px4)/launch/multicopter_w.launch">
 	<arg name="ns" value="$(arg ns)"/>
+	<arg name="mavlink_fcu_url" value="$(arg mavlink_fcu_url)" />
 </include>
 
 <group ns="$(arg ns)">
diff --git a/launch/multi_uav.launch b/launch/multi_uav.launch
new file mode 100644
index 0000000000..6fb1589e80
--- /dev/null
+++ b/launch/multi_uav.launch
@@ -0,0 +1,55 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="headless" default="false"/>
+  <arg name="gui" default="true"/>
+  <arg name="enable_logging" default="false"/>
+  <arg name="enable_ground_truth" default="true"/>
+  <arg name="debug" default="false"/>
+  <arg name="ns" default="iris"/>
+  <arg name="log_file" default="$(arg ns)"/>
+
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="$(find rotors_gazebo)/worlds/basic.world"/>
+    <arg name="debug" value="$(arg debug)" />
+    <arg name="headless" value="$(arg headless)"/>
+    <arg name="gui" value="$(arg gui)"/>
+  </include>
+  
+   <group ns="iris">
+      <include file="$(find rotors_gazebo)/launch/spawn_iris.launch">
+        <arg name="model" value="$(find rotors_description)/urdf/iris_gesture_sensors.gazebo" />
+        <arg name="enable_logging" value="$(arg enable_logging)" />
+        <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
+        <arg name="log_file" value="$(arg log_file)"/>
+        <arg name="x" value="1"/>
+        <arg name="y" value="0"/>
+      </include>
+      <arg name="fcu_url" default="udp://localhost:14560@localhost:14565" />
+      <include file="$(find mavros)/launch/px4.launch">
+        <arg name="fcu_url" value="$(arg fcu_url)" />
+      </include>
+   </group>
+   <include file="$(find px4)/launch/iris.launch">
+     <arg name="ns" value="iris"/>
+   </include>   
+      
+   <group ns="ardrone">
+      <include file="$(find rotors_gazebo)/launch/spawn_ardrone.launch">
+        <arg name="model" value="$(find rotors_description)/urdf/ardrone_base.xacro" />
+        <arg name="enable_logging" value="$(arg enable_logging)" />
+        <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
+        <arg name="log_file" value="$(arg log_file)"/>
+        <arg name="x" value="-1"/>
+        <arg name="y" value="0"/>
+      </include>
+      <arg name="fcu_url" default="udp://localhost:14570@localhost:14575" />
+      <include file="$(find mavros)/launch/px4.launch">
+        <arg name="fcu_url" value="$(arg fcu_url)" />
+      </include>
+   </group>
+   <include file="$(find px4)/launch/ardrone.launch">
+	<arg name="ns" value="ardrone"/>
+	<arg name="mavlink_fcu_url" value="udp://localhost:14575@localhost:14570"/>
+   </include>
+
+</launch>   
diff --git a/launch/multicopter.launch b/launch/multicopter.launch
index 6882f24137..bf68da95dd 100644
--- a/launch/multicopter.launch
+++ b/launch/multicopter.launch
@@ -1,5 +1,6 @@
 <launch>
 <arg name="ns"/>
+<arg name="mavlink_fcu_url"/>
 
 <group ns="$(arg ns)">
 	<node pkg="joy" name="joy_node" type="joy_node"/>
@@ -10,7 +11,9 @@
 	<node pkg="px4" name="position_estimator" type="position_estimator"/>
 	<node pkg="px4" name="mc_att_control" type="mc_att_control"/>
 	<node pkg="px4" name="mc_pos_control" type="mc_pos_control"/>
-	<node pkg="px4" name="mavlink" type="mavlink"/>
+	<node pkg="px4" name="mavlink" type="mavlink">  
+	  <param name="mavlink_fcu_url" value="$(arg mavlink_fcu_url)" type="str"/>
+	</node>
 	<!-- <node pkg="rosbag" type="record" name="record" output="screen" args="-a -O px4_multicopter"/> -->
 </group>
 
diff --git a/launch/multicopter_w.launch b/launch/multicopter_w.launch
index 66c30d186f..617357452f 100644
--- a/launch/multicopter_w.launch
+++ b/launch/multicopter_w.launch
@@ -1,8 +1,10 @@
 <launch>
 <arg name="ns"/>
+<arg name="mavlink_fcu_url"/>
 
 <include file="$(find px4)/launch/multicopter.launch">
 	<arg name="ns" value="$(arg ns)"/>
+	<arg name="mavlink_fcu_url" value="$(arg mavlink_fcu_url)" />
 </include>
 
 <group ns="$(arg ns)">
diff --git a/launch/multicopter_x.launch b/launch/multicopter_x.launch
index c686eba390..b7db46dcb5 100644
--- a/launch/multicopter_x.launch
+++ b/launch/multicopter_x.launch
@@ -1,8 +1,10 @@
 <launch>
 <arg name="ns"/>
+<arg name="mavlink_fcu_url"/>
 
 <include file="$(find px4)/launch/multicopter.launch">
 	<arg name="ns" value="$(arg ns)"/>
+	<arg name="mavlink_fcu_url" value="$(arg mavlink_fcu_url)" />
 </include>
 
 <group ns="$(arg ns)">
diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp
index c92dd0843d..567acc200e 100644
--- a/src/platforms/ros/nodes/mavlink/mavlink.cpp
+++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp
@@ -46,7 +46,7 @@
 
 using namespace px4;
 
-Mavlink::Mavlink() :
+Mavlink::Mavlink(std::string mavlink_fcu_url) :
 	_n(),
 	_v_att_sub(_n.subscribe("vehicle_attitude", 1, &Mavlink::VehicleAttitudeCallback, this)),
 	_v_local_pos_sub(_n.subscribe("vehicle_local_position", 1, &Mavlink::VehicleLocalPositionCallback, this)),
@@ -55,7 +55,7 @@ Mavlink::Mavlink() :
 	_offboard_control_mode_pub(_n.advertise<offboard_control_mode>("offboard_control_mode", 1)),
 	_force_sp_pub(_n.advertise<vehicle_force_setpoint>("vehicle_force_setpoint", 1))
 {
-	_link = mavconn::MAVConnInterface::open_url("udp://localhost:14565@localhost:14560");
+	_link = mavconn::MAVConnInterface::open_url(mavlink_fcu_url);
 	_link->message_received.connect(boost::bind(&Mavlink::handle_msg, this, _1, _2, _3));
 	_att_sp = {};
 	_offboard_control_mode = {};
@@ -64,7 +64,10 @@ Mavlink::Mavlink() :
 int main(int argc, char **argv)
 {
 	ros::init(argc, argv, "mavlink");
-	Mavlink m;
+	ros::NodeHandle privateNh("~");
+	std::string mavlink_fcu_url;
+	privateNh.param<std::string>("mavlink_fcu_url",      mavlink_fcu_url,   std::string("udp://localhost:14565@localhost:14560"));
+	Mavlink m(mavlink_fcu_url);
 	ros::spin();
 	return 0;
 }
diff --git a/src/platforms/ros/nodes/mavlink/mavlink.h b/src/platforms/ros/nodes/mavlink/mavlink.h
index 8b7d08d242..af9e019993 100644
--- a/src/platforms/ros/nodes/mavlink/mavlink.h
+++ b/src/platforms/ros/nodes/mavlink/mavlink.h
@@ -55,7 +55,7 @@ namespace px4
 class Mavlink
 {
 public:
-	Mavlink();
+	Mavlink(std::string mavlink_fcu_url);
 
 	~Mavlink() {}