From fa3dd54d0c031cc5f74a50f34f8689b8a596da44 Mon Sep 17 00:00:00 2001
From: Giuseppe Silano <g.silano89@gmail.com>
Date: Wed, 26 Aug 2020 09:49:45 +0200
Subject: [PATCH] Changes propagation #29

---
 CHANGELOG.rst                                 |   5 +-
 .../multiple_bebop_without_controller.launch  | 129 +++++++++
 launch/multiple_task2_world.launch            | 261 ++++++++++++++++++
 package.xml                                   |   1 +
 resource/waypoints0.txt                       |   4 +
 resource/waypoints1.txt                       |   4 +
 resource/waypoints2.txt                       |   4 +
 resource/waypoints3.txt                       |   4 +
 urdf/multirotor_base.urdf.xacro               |  10 +-
 worlds/basic.world                            |  26 +-
 10 files changed, 432 insertions(+), 16 deletions(-)
 create mode 100755 launch/multiple_bebop_without_controller.launch
 create mode 100755 launch/multiple_task2_world.launch
 create mode 100755 resource/waypoints0.txt
 create mode 100755 resource/waypoints1.txt
 create mode 100755 resource/waypoints2.txt
 create mode 100755 resource/waypoints3.txt
 mode change 100755 => 100644 urdf/multirotor_base.urdf.xacro
 mode change 100755 => 100644 worlds/basic.world

diff --git a/CHANGELOG.rst b/CHANGELOG.rst
index a1fab52..4840e65 100755
--- a/CHANGELOG.rst
+++ b/CHANGELOG.rst
@@ -2,10 +2,11 @@
 Changelog for package BebopS
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-2.0.2 (2020-08-22)
+2.0.2 (2020-08-26)
 ------------------
+* Add multi-robot example #29
 * Add a default RVIZ configuration file #28
-* Contributors: tomlogan501
+* Contributors: tomlogan501, Collin-Thornton
 
 2.0.1 (2019-12-26)
 ------------------
diff --git a/launch/multiple_bebop_without_controller.launch b/launch/multiple_bebop_without_controller.launch
new file mode 100755
index 0000000..a9a2abd
--- /dev/null
+++ b/launch/multiple_bebop_without_controller.launch
@@ -0,0 +1,129 @@
+<?xml version="1.0"?>
+
+<launch>
+    <arg name="name" default="bebop"/>
+    <arg name="number" default="1"/>
+    <arg name="x" default="0.0" />
+    <arg name="y" default="0.0" />
+    <arg name="z" default="0.0" />
+    <arg name="use_sim_time" default="true"/>
+    <arg name="world_name" default="basic"/>
+    <arg name="wind_force" default="0.0"/>
+    <arg name="wind_start" default="5.0"/>
+    <arg name="wind_duration" default="40.0"/>
+    <arg name="wind_direction_x" default="0.0"/>
+    <arg name="wind_direction_y" default="0.0"/>
+    <arg name="wind_direction_z" default="0.0"/>
+    <arg name="enable_odometry_sensor_with_noise" default="false"/>
+    <!-- The "disable_odometry_sensor_with_noise" variable values is equal to true if "enable_odometry_sensor_with_noise"
+    is false, and viceversa -->
+    <arg name="disable_odometry_sensor_with_noise" value="true" unless="$(arg enable_odometry_sensor_with_noise)"/>
+    <arg name="disable_odometry_sensor_with_noise" value="false" if="$(arg enable_odometry_sensor_with_noise)"/>
+    <arg name="enable_ground_truth_sensor" default="false"/>
+    <arg name="enable_wind_plugin" default="false"/>
+    <arg name="enable_laser1D" default="false"/>
+    <arg name="enable_imu" default="true"/>
+
+    <env name="GAZEBO_MODEL_PATH" value="${GAZEBO_MODEL_PATH}:$(find bebop_simulator)/models"/>
+    <env name="GAZEBO_RESOURCE_PATH" value="${GAZEBO_RESOURCE_PATH}:$(find bebop_simulator)/models"/>
+    <include file="$(find gazebo_ros)/launch/empty_world.launch">
+       <arg name="world_name" value="$(find bebop_simulator)/worlds/$(arg world_name).world" />
+    </include>
+
+    <!-- Launch Bebop_0 -->
+    <group ns="bebop_0">
+    <include file="$(find bebop_simulator)/launch/spawn_bebop.launch">
+       <arg name="name" value="$(arg name)0" />
+       <arg name="x" value="0.63661977"/>
+       <arg name="y" value="0"/>
+       <arg name="z" value="0"/>
+       <arg name="wind_force" value="$(arg wind_force)"/>
+       <arg name="wind_start" value="$(arg wind_start)"/>
+       <arg name="wind_duration" value="$(arg wind_duration)"/>
+       <arg name="wind_direction_x" value="$(arg wind_direction_x)"/>
+       <arg name="wind_direction_y" value="$(arg wind_direction_y)"/>
+       <arg name="wind_direction_z" value="$(arg wind_direction_z)"/>
+       <!-- The disable_odometry_sensor_with_noise will only become true if enable_odometry_sensor_with_noise will is false.
+       In this way, only one odometry sensor will be simulated: with or without noise. -->
+       <arg name="enable_odometry_sensor_with_noise" value="$(arg enable_odometry_sensor_with_noise)"/>
+       <arg name="disable_odometry_sensor_with_noise" value="$(arg disable_odometry_sensor_with_noise)" />
+       <arg name="enable_ground_truth_sensor" value="$(arg enable_ground_truth_sensor)"/>
+       <arg name="enable_wind_plugin" value="$(arg enable_wind_plugin)"/>
+       <arg name="enable_laser1D" value="$(arg enable_laser1D)"/>
+       <arg name="enable_imu" value="$(arg enable_imu)"/>
+    </include>
+    </group>
+
+   <!-- Launch Bebop_1 -->
+   <group ns="bebop_1">
+    <include file="$(find bebop_simulator)/launch/spawn_bebop.launch">
+       <arg name="name" value="$(arg name)1" />
+       <arg name="x" value="0"/>
+       <arg name="y" value="0.63661977"/>
+       <arg name="z" value="$(arg z)"/>
+       <arg name="wind_force" value="$(arg wind_force)"/>
+       <arg name="wind_start" value="$(arg wind_start)"/>
+       <arg name="wind_duration" value="$(arg wind_duration)"/>
+       <arg name="wind_direction_x" value="$(arg wind_direction_x)"/>
+       <arg name="wind_direction_y" value="$(arg wind_direction_y)"/>
+       <arg name="wind_direction_z" value="$(arg wind_direction_z)"/>
+       <!-- The disable_odometry_sensor_with_noise will only become true if enable_odometry_sensor_with_noise will is false.
+       In this way, only one odometry sensor will be simulated: with or without noise. -->
+       <arg name="enable_odometry_sensor_with_noise" value="$(arg enable_odometry_sensor_with_noise)"/>
+       <arg name="disable_odometry_sensor_with_noise" value="$(arg disable_odometry_sensor_with_noise)" />
+       <arg name="enable_ground_truth_sensor" value="$(arg enable_ground_truth_sensor)"/>
+       <arg name="enable_wind_plugin" value="$(arg enable_wind_plugin)"/>
+       <arg name="enable_laser1D" value="$(arg enable_laser1D)"/>
+       <arg name="enable_imu" value="$(arg enable_imu)"/>
+    </include>
+   </group>
+
+   <!-- Launch Bebop_2 -->
+   <group ns="bebop_2">
+    <include file="$(find bebop_simulator)/launch/spawn_bebop.launch">
+       <arg name="name" value="$(arg name)2" />
+       <arg name="x" value="-0.63661977"/>
+       <arg name="y" value="0"/>
+       <arg name="z" value="$(arg z)"/>
+       <arg name="wind_force" value="$(arg wind_force)"/>
+       <arg name="wind_start" value="$(arg wind_start)"/>
+       <arg name="wind_duration" value="$(arg wind_duration)"/>
+       <arg name="wind_direction_x" value="$(arg wind_direction_x)"/>
+       <arg name="wind_direction_y" value="$(arg wind_direction_y)"/>
+       <arg name="wind_direction_z" value="$(arg wind_direction_z)"/>
+       <!-- The disable_odometry_sensor_with_noise will only become true if enable_odometry_sensor_with_noise will is false.
+       In this way, only one odometry sensor will be simulated: with or without noise. -->
+       <arg name="enable_odometry_sensor_with_noise" value="$(arg enable_odometry_sensor_with_noise)"/>
+       <arg name="disable_odometry_sensor_with_noise" value="$(arg disable_odometry_sensor_with_noise)" />
+       <arg name="enable_ground_truth_sensor" value="$(arg enable_ground_truth_sensor)"/>
+       <arg name="enable_wind_plugin" value="$(arg enable_wind_plugin)"/>
+       <arg name="enable_laser1D" value="$(arg enable_laser1D)"/>
+       <arg name="enable_imu" value="$(arg enable_imu)"/>
+    </include>
+    </group>
+
+   <!-- Launch Bebop_3 -->
+   <group ns="bebop_3">
+    <include file="$(find bebop_simulator)/launch/spawn_bebop.launch">
+       <arg name="name" value="$(arg name)3" />
+       <arg name="x" value="0"/>
+       <arg name="y" value="-0.63661977"/>
+       <arg name="z" value="$(arg z)"/>
+       <arg name="wind_force" value="$(arg wind_force)"/>
+       <arg name="wind_start" value="$(arg wind_start)"/>
+       <arg name="wind_duration" value="$(arg wind_duration)"/>
+       <arg name="wind_direction_x" value="$(arg wind_direction_x)"/>
+       <arg name="wind_direction_y" value="$(arg wind_direction_y)"/>
+       <arg name="wind_direction_z" value="$(arg wind_direction_z)"/>
+       <!-- The disable_odometry_sensor_with_noise will only become true if enable_odometry_sensor_with_noise will is false.
+       In this way, only one odometry sensor will be simulated: with or without noise. -->
+       <arg name="enable_odometry_sensor_with_noise" value="$(arg enable_odometry_sensor_with_noise)"/>
+       <arg name="disable_odometry_sensor_with_noise" value="$(arg disable_odometry_sensor_with_noise)" />
+       <arg name="enable_ground_truth_sensor" value="$(arg enable_ground_truth_sensor)"/>
+       <arg name="enable_wind_plugin" value="$(arg enable_wind_plugin)"/>
+       <arg name="enable_laser1D" value="$(arg enable_laser1D)"/>
+       <arg name="enable_imu" value="$(arg enable_imu)"/>
+    </include>
+    </group>
+
+</launch>
diff --git a/launch/multiple_task2_world.launch b/launch/multiple_task2_world.launch
new file mode 100755
index 0000000..b330260
--- /dev/null
+++ b/launch/multiple_task2_world.launch
@@ -0,0 +1,261 @@
+<?xml version="1.0"?>
+
+<launch>
+    <arg name="name" default="bebop"/>
+    <arg name="plotName" default="bebopPlots"/>
+    <arg name="x" default="0.0" />
+    <arg name="y" default="0.0" />
+    <arg name="z" default="0.0" />
+    <arg name="use_sim_time" default="true"/>
+    <arg name="world_name" default="basic"/>
+    <arg name="wind_force" default="0.25"/>
+    <arg name="wind_start" default="5.0"/>
+    <arg name="wind_duration" default="40.0"/>
+    <arg name="wind_direction_x" default="1.0"/>
+    <arg name="wind_direction_y" default="0.0"/>
+    <arg name="wind_direction_z" default="0.0"/>
+    <arg name="csvFilesStoring" default="false"/>
+    <arg name="csvFilesStoringTime" default="60.0"/> <!-- seconds -->
+    <arg name="user_account" default="cthornton"/> <!-- account name of your pc -->
+    <arg name="waypoint_filter" default="true"/>
+    <arg name="EKFActive" default="false"/>
+    <arg name="enable_odometry_sensor_with_noise" default="false"/>
+    <arg name="disable_odometry_sensor_with_noise" default="true"/>
+    <arg name="enable_ground_truth_sensor" default="false"/>
+    <arg name="enable_wind_plugin" default="false"/>
+    <arg name="enable_laser1D" default="false"/>
+    <arg name="enable_plots" default="false"/>
+    <arg name="enable_imu" default="true"/>
+
+   <!-- Set up the Gazebo world -->
+  <env name="GAZEBO_MODEL_PATH" value="${GAZEBO_MODEL_PATH}:$(find bebop_simulator)/models"/>
+  <env name="GAZEBO_RESOURCE_PATH" value="${GAZEBO_RESOURCE_PATH}:$(find bebop_simulator)/models"/>
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="$(find bebop_simulator)/worlds/$(arg world_name).world" />
+  </include>
+
+   <!-- Launch Bebop_0 -->
+    <group ns="$(arg name)_0">
+    <include file="$(find bebop_simulator)/launch/spawn_bebop.launch">
+       <arg name="name" value="$(arg name)0" />
+       <arg name="x" value="0"/>
+       <arg name="y" value="0"/>
+       <arg name="z" value="0"/>
+       <arg name="wind_force" value="$(arg wind_force)"/>
+       <arg name="wind_start" value="$(arg wind_start)"/>
+       <arg name="wind_duration" value="$(arg wind_duration)"/>
+       <arg name="wind_direction_x" value="$(arg wind_direction_x)"/>
+       <arg name="wind_direction_y" value="$(arg wind_direction_y)"/>
+       <arg name="wind_direction_z" value="$(arg wind_direction_z)"/>
+       <!-- The disable_odometry_sensor_with_noise will only become true if enable_odometry_sensor_with_noise will is false.
+       In this way, only one odometry sensor will be simulated: with or without noise. -->
+       <arg name="enable_odometry_sensor_with_noise" value="$(arg enable_odometry_sensor_with_noise)"/>
+       <param name="disable_odometry_sensor_with_noise" value="false" unless="$(arg enable_odometry_sensor_with_noise)" />
+       <arg name="disable_odometry_sensor_with_noise" value="$(arg disable_odometry_sensor_with_noise)"/>
+       <arg name="enable_ground_truth_sensor" value="$(arg enable_ground_truth_sensor)"/>
+       <arg name="enable_wind_plugin" value="$(arg enable_wind_plugin)"/>
+       <arg name="enable_laser1D" value="$(arg enable_laser1D)"/>
+       <arg name="enable_imu" value="$(arg enable_imu)"/>
+    </include>
+
+    <!-- Launch the controller for Bebop_0 -->
+    <node name="position_controller_node" pkg="bebop_simulator" type="position_controller_node" output="screen">
+      <rosparam command="load" file="$(find bebop_simulator)/resource/controller_bebop.yaml" />
+      <rosparam command="load" file="$(find bebop_simulator)/resource/bebop.yaml" />
+      <rosparam command="load" file="$(find bebop_simulator)/resource/EKF_matrix.yaml" />
+      <rosparam command="load" file="$(find bebop_simulator)/resource/waypoint_filter.yaml" />
+      <param name="use_sim_time" value="$(arg use_sim_time)" />
+      <param name="csvFilesStoring" value="$(arg csvFilesStoring)"/>
+      <param name="csvFilesStoringTime" value="$(arg csvFilesStoringTime)"/>
+      <param name="user_account" value="$(arg user_account)"/>
+      <param name="waypoint_filter" value="$(arg waypoint_filter)"/>
+      <param name="EKFActive" value="$(arg EKFActive)"/>
+      <remap from="odometry" to="/$(arg name)0/odometry" />
+      <remap from="odometry_gt" to="$(arg name)0/odometry_gt" />
+      <remap from="filteredOutput" to="$(arg name)0/filteredOutput" />
+      <remap from="referenceAngles" to="$(arg name)0/referenceAngles" />
+      <remap from="stateErrors" to="$(arg name)0/stateErrors" />
+      <remap from="smoothedTrajectory" to="$(arg name)0/smoothedTrajectory" />
+      <remap from="command/trajectory" to="$(arg name)0/command/trajectory" />
+      <remap from="uTerrComponents" to="$(arg name)0/uTerrComponents" />
+      <remap from="zVelocityComponents" to="$(arg name)0/zVelocityComponents" />
+      <remap from="positionAndVelocityErrors" to="$(arg name)0/positionAndVelocityErrors" />
+      <remap from="angularAndAngularVelocityErrors" to="$(arg name)0/angularAndAngularVelocityErrors" />
+      <remap from="command/motor_speed" to="$(arg name)0/command/motors" />
+    </node>
+
+    <!-- Launch the trajectory generator for Bebop_0 -->
+   <group ns="$(arg name)0">
+       <node name="waypoint_example" pkg="bebop_simulator" type="waypoint_example" output="screen" args="$(find bebop_simulator)/resource/waypoints0.txt"/>
+    </group>
+    </group>
+
+   <!-- Launch Bebop_1 -->
+    <group ns="$(arg name)_1">
+    <include file="$(find bebop_simulator)/launch/spawn_bebop.launch">
+       <arg name="name" value="$(arg name)1" />
+       <arg name="x" value="1"/>
+       <arg name="y" value="0"/>
+       <arg name="z" value="0"/>
+       <arg name="wind_force" value="$(arg wind_force)"/>
+       <arg name="wind_start" value="$(arg wind_start)"/>
+       <arg name="wind_duration" value="$(arg wind_duration)"/>
+       <arg name="wind_direction_x" value="$(arg wind_direction_x)"/>
+       <arg name="wind_direction_y" value="$(arg wind_direction_y)"/>
+       <arg name="wind_direction_z" value="$(arg wind_direction_z)"/>
+       <!-- The disable_odometry_sensor_with_noise will only become true if enable_odometry_sensor_with_noise will is false.
+       In this way, only one odometry sensor will be simulated: with or without noise. -->
+       <arg name="enable_odometry_sensor_with_noise" value="$(arg enable_odometry_sensor_with_noise)"/>
+       <param name="disable_odometry_sensor_with_noise" value="false" unless="$(arg enable_odometry_sensor_with_noise)" />
+       <arg name="disable_odometry_sensor_with_noise" value="$(arg disable_odometry_sensor_with_noise)"/>
+       <arg name="enable_ground_truth_sensor" value="$(arg enable_ground_truth_sensor)"/>
+       <arg name="enable_wind_plugin" value="$(arg enable_wind_plugin)"/>
+       <arg name="enable_laser1D" value="$(arg enable_laser1D)"/>
+       <arg name="enable_imu" value="$(arg enable_imu)"/>
+    </include>
+
+    <!-- Launch the controller for Bebop_1 -->
+    <node name="position_controller_node" pkg="bebop_simulator" type="position_controller_node" output="screen">
+      <rosparam command="load" file="$(find bebop_simulator)/resource/controller_bebop.yaml" />
+      <rosparam command="load" file="$(find bebop_simulator)/resource/bebop.yaml" />
+      <rosparam command="load" file="$(find bebop_simulator)/resource/EKF_matrix.yaml" />
+      <rosparam command="load" file="$(find bebop_simulator)/resource/waypoint_filter.yaml" />
+      <param name="use_sim_time" value="$(arg use_sim_time)" />
+      <param name="csvFilesStoring" value="$(arg csvFilesStoring)"/>
+      <param name="csvFilesStoringTime" value="$(arg csvFilesStoringTime)"/>
+      <param name="user_account" value="$(arg user_account)"/>
+      <param name="waypoint_filter" value="$(arg waypoint_filter)"/>
+      <param name="EKFActive" value="$(arg EKFActive)"/>
+      <remap from="odometry" to="/$(arg name)1/odometry" />
+      <remap from="odometry_gt" to="$(arg name)1/odometry_gt" />
+      <remap from="filteredOutput" to="$(arg name)1/filteredOutput" />
+      <remap from="referenceAngles" to="$(arg name)1/referenceAngles" />
+      <remap from="stateErrors" to="$(arg name)1/stateErrors" />
+      <remap from="smoothedTrajectory" to="$(arg name)1/smoothedTrajectory" />
+      <remap from="command/trajectory" to="$(arg name)1/command/trajectory" />
+      <remap from="uTerrComponents" to="$(arg name)1/uTerrComponents" />
+      <remap from="zVelocityComponents" to="$(arg name)1/zVelocityComponents" />
+      <remap from="positionAndVelocityErrors" to="$(arg name)1/positionAndVelocityErrors" />
+      <remap from="angularAndAngularVelocityErrors" to="$(arg name)1/angularAndAngularVelocityErrors" />
+      <remap from="command/motor_speed" to="$(arg name)1/command/motors" />
+    </node>
+
+    <!-- Launch the trajectory generator for Bebop_1-->
+   <group ns="$(arg name)1">
+       <node name="waypoint_example" pkg="bebop_simulator" type="waypoint_example" output="screen" args="$(find bebop_simulator)/resource/waypoints1.txt"/>
+    </group>
+    </group>
+
+   <!-- Launch Bebop_2 -->
+    <group ns="$(arg name)_2">
+    <include file="$(find bebop_simulator)/launch/spawn_bebop.launch">
+       <arg name="name" value="$(arg name)2" />
+       <arg name="x" value="2"/>
+       <arg name="y" value="0"/>
+       <arg name="z" value="0"/>
+       <arg name="wind_force" value="$(arg wind_force)"/>
+       <arg name="wind_start" value="$(arg wind_start)"/>
+       <arg name="wind_duration" value="$(arg wind_duration)"/>
+       <arg name="wind_direction_x" value="$(arg wind_direction_x)"/>
+       <arg name="wind_direction_y" value="$(arg wind_direction_y)"/>
+       <arg name="wind_direction_z" value="$(arg wind_direction_z)"/>
+       <!-- The disable_odometry_sensor_with_noise will only become true if enable_odometry_sensor_with_noise will is false.
+       In this way, only one odometry sensor will be simulated: with or without noise. -->
+       <arg name="enable_odometry_sensor_with_noise" value="$(arg enable_odometry_sensor_with_noise)"/>
+       <param name="disable_odometry_sensor_with_noise" value="false" unless="$(arg enable_odometry_sensor_with_noise)" />
+       <arg name="disable_odometry_sensor_with_noise" value="$(arg disable_odometry_sensor_with_noise)"/>
+       <arg name="enable_ground_truth_sensor" value="$(arg enable_ground_truth_sensor)"/>
+       <arg name="enable_wind_plugin" value="$(arg enable_wind_plugin)"/>
+       <arg name="enable_laser1D" value="$(arg enable_laser1D)"/>
+       <arg name="enable_imu" value="$(arg enable_imu)"/>
+    </include>
+
+    <!-- Launch the controller for Bebop_2 -->
+    <node name="position_controller_node" pkg="bebop_simulator" type="position_controller_node" output="screen">
+      <rosparam command="load" file="$(find bebop_simulator)/resource/controller_bebop.yaml" />
+      <rosparam command="load" file="$(find bebop_simulator)/resource/bebop.yaml" />
+      <rosparam command="load" file="$(find bebop_simulator)/resource/EKF_matrix.yaml" />
+      <rosparam command="load" file="$(find bebop_simulator)/resource/waypoint_filter.yaml" />
+      <param name="use_sim_time" value="$(arg use_sim_time)" />
+      <param name="csvFilesStoring" value="$(arg csvFilesStoring)"/>
+      <param name="csvFilesStoringTime" value="$(arg csvFilesStoringTime)"/>
+      <param name="user_account" value="$(arg user_account)"/>
+      <param name="waypoint_filter" value="$(arg waypoint_filter)"/>
+      <param name="EKFActive" value="$(arg EKFActive)"/>
+      <remap from="odometry" to="/$(arg name)2/odometry" />
+      <remap from="odometry_gt" to="$(arg name)2/odometry_gt" />
+      <remap from="filteredOutput" to="$(arg name)2/filteredOutput" />
+      <remap from="referenceAngles" to="$(arg name)2/referenceAngles" />
+      <remap from="stateErrors" to="$(arg name)2/stateErrors" />
+      <remap from="smoothedTrajectory" to="$(arg name)2/smoothedTrajectory" />
+      <remap from="command/trajectory" to="$(arg name)2/command/trajectory" />
+      <remap from="uTerrComponents" to="$(arg name)2/uTerrComponents" />
+      <remap from="zVelocityComponents" to="$(arg name)2/zVelocityComponents" />
+      <remap from="positionAndVelocityErrors" to="$(arg name)2/positionAndVelocityErrors" />
+      <remap from="angularAndAngularVelocityErrors" to="$(arg name)2/angularAndAngularVelocityErrors" />
+      <remap from="command/motor_speed" to="$(arg name)2/command/motors" />
+    </node>
+
+    <!-- Launch the trajectory generator for Bebop_2 -->
+   <group ns="$(arg name)2">
+       <node name="waypoint_example" pkg="bebop_simulator" type="waypoint_example" output="screen" args="$(find bebop_simulator)/resource/waypoints2.txt"/>
+    </group>
+    </group>
+
+   <!-- Launch Bebop_3 -->
+    <group ns="$(arg name)_3">
+    <include file="$(find bebop_simulator)/launch/spawn_bebop.launch">
+       <arg name="name" value="$(arg name)3" />
+       <arg name="x" value="3"/>
+       <arg name="y" value="0"/>
+       <arg name="z" value="0"/>
+       <arg name="wind_force" value="$(arg wind_force)"/>
+       <arg name="wind_start" value="$(arg wind_start)"/>
+       <arg name="wind_duration" value="$(arg wind_duration)"/>
+       <arg name="wind_direction_x" value="$(arg wind_direction_x)"/>
+       <arg name="wind_direction_y" value="$(arg wind_direction_y)"/>
+       <arg name="wind_direction_z" value="$(arg wind_direction_z)"/>
+       <!-- The disable_odometry_sensor_with_noise will only become true if enable_odometry_sensor_with_noise will is false.
+       In this way, only one odometry sensor will be simulated: with or without noise. -->
+       <arg name="enable_odometry_sensor_with_noise" value="$(arg enable_odometry_sensor_with_noise)"/>
+       <param name="disable_odometry_sensor_with_noise" value="false" unless="$(arg enable_odometry_sensor_with_noise)" />
+       <arg name="disable_odometry_sensor_with_noise" value="$(arg disable_odometry_sensor_with_noise)"/>
+       <arg name="enable_ground_truth_sensor" value="$(arg enable_ground_truth_sensor)"/>
+       <arg name="enable_wind_plugin" value="$(arg enable_wind_plugin)"/>
+       <arg name="enable_laser1D" value="$(arg enable_laser1D)"/>
+       <arg name="enable_imu" value="$(arg enable_imu)"/>
+    </include>
+
+    <!-- Launch the controller for Bebop_3 -->
+    <node name="position_controller_node" pkg="bebop_simulator" type="position_controller_node" output="screen">
+      <rosparam command="load" file="$(find bebop_simulator)/resource/controller_bebop.yaml" />
+      <rosparam command="load" file="$(find bebop_simulator)/resource/bebop.yaml" />
+      <rosparam command="load" file="$(find bebop_simulator)/resource/EKF_matrix.yaml" />
+      <rosparam command="load" file="$(find bebop_simulator)/resource/waypoint_filter.yaml" />
+      <param name="use_sim_time" value="$(arg use_sim_time)" />
+      <param name="csvFilesStoring" value="$(arg csvFilesStoring)"/>
+      <param name="csvFilesStoringTime" value="$(arg csvFilesStoringTime)"/>
+      <param name="user_account" value="$(arg user_account)"/>
+      <param name="waypoint_filter" value="$(arg waypoint_filter)"/>
+      <param name="EKFActive" value="$(arg EKFActive)"/>
+      <remap from="odometry" to="/$(arg name)3/odometry" />
+      <remap from="odometry_gt" to="$(arg name)3/odometry_gt" />
+      <remap from="filteredOutput" to="$(arg name)3/filteredOutput" />
+      <remap from="referenceAngles" to="$(arg name)3/referenceAngles" />
+      <remap from="stateErrors" to="$(arg name)3/stateErrors" />
+      <remap from="smoothedTrajectory" to="$(arg name)3/smoothedTrajectory" />
+      <remap from="command/trajectory" to="$(arg name)3/command/trajectory" />
+      <remap from="uTerrComponents" to="$(arg name)3/uTerrComponents" />
+      <remap from="zVelocityComponents" to="$(arg name)3/zVelocityComponents" />
+      <remap from="positionAndVelocityErrors" to="$(arg name)3/positionAndVelocityErrors" />
+      <remap from="angularAndAngularVelocityErrors" to="$(arg name)3/angularAndAngularVelocityErrors" />
+      <remap from="command/motor_speed" to="$(arg name)3/command/motors" />
+    </node>
+
+    <!-- Launch the trajectory generator for Bebop_3 -->
+   <group ns="$(arg name)3">
+       <node name="waypoint_example" pkg="bebop_simulator" type="waypoint_example" output="screen" args="$(find bebop_simulator)/resource/waypoints3.txt"/>
+    </group>
+    </group>
+
+</launch>
diff --git a/package.xml b/package.xml
index 5ae22a4..97cfde6 100755
--- a/package.xml
+++ b/package.xml
@@ -12,6 +12,7 @@
   <author>Luigi Iannelli</author>
   <author>Antun Ivanovic</author>
   <author>tomlogan501</author>
+  <author>Collin-Thornton</author>
 
   <license>Apache</license>
 
diff --git a/resource/waypoints0.txt b/resource/waypoints0.txt
new file mode 100755
index 0000000..ad88533
--- /dev/null
+++ b/resource/waypoints0.txt
@@ -0,0 +1,4 @@
+5 0.0 0.0 1.0 0.0
+5 0.0 -1.0 1.0 0.0
+5 0.0 0.0 1.0 0.0
+5 0.0 0.0 0.0 0.0
\ No newline at end of file
diff --git a/resource/waypoints1.txt b/resource/waypoints1.txt
new file mode 100755
index 0000000..5c5fa83
--- /dev/null
+++ b/resource/waypoints1.txt
@@ -0,0 +1,4 @@
+5 1.0 0.0 1.0 0.0
+5 1.0 1.0 1.0 0.0
+5 1.0 0.0 1.0 0.0
+5 1.0 0.0 0.0 0.0
\ No newline at end of file
diff --git a/resource/waypoints2.txt b/resource/waypoints2.txt
new file mode 100755
index 0000000..97dac94
--- /dev/null
+++ b/resource/waypoints2.txt
@@ -0,0 +1,4 @@
+5 2.0 0.0 1.0 0.0
+5 2.0 -1.0 1.0 0.0
+5 2.0 0.0 1.0 0.0
+5 2.0 0.0 0.0 0.0
\ No newline at end of file
diff --git a/resource/waypoints3.txt b/resource/waypoints3.txt
new file mode 100755
index 0000000..e67e347
--- /dev/null
+++ b/resource/waypoints3.txt
@@ -0,0 +1,4 @@
+5 3.0 0.0 1.0 0.0
+5 3.0 1.0 1.0 0.0
+5 3.0 0.0 1.0 0.0
+5 3.0 0.0 0.0 0.0
\ No newline at end of file
diff --git a/urdf/multirotor_base.urdf.xacro b/urdf/multirotor_base.urdf.xacro
old mode 100755
new mode 100644
index 586f036..1cdfafe
--- a/urdf/multirotor_base.urdf.xacro
+++ b/urdf/multirotor_base.urdf.xacro
@@ -71,8 +71,8 @@
       <!-- <limit effort="2000" velocity="${max_rot_velocity}" /> -->
       <parent link="${parent}" />
       <child link="rotor_${motor_number}" />
-    </joint>
 
+    </joint>
     <!-- TODO(ff): not currently set because it's not yet supported -->
     <!-- <gazebo reference="rotor_${motor_number}_joint"> <axis> <xyz>0 0 1</xyz> 
       <limit> <velocity> ${max_rot_velocity} </velocity> </limit> </axis> </gazebo> -->
@@ -81,7 +81,6 @@
         <mass value="${mass_rotor}" /> <!-- [kg] -->
         <xacro:insert_block name="inertia" />
       </inertial>
-
       <visual>
         <geometry>
           <!-- <cylinder length="0.005" radius="${radius_rotor}"/> --> <!-- [m] -->
@@ -90,14 +89,11 @@
           <!-- <box size="${2*radius_rotor} 0.01 0.005"/> -->
         </geometry>
       </visual>
-
       <collision>
         <geometry>
-          <!-- <cylinder length="0.005" radius="${radius_rotor}"/> --> <!-- [m] -->
           <cylinder length="0.005" radius="${radius_rotor}" /> <!-- [m] -->
         </geometry>
       </collision>
-
     </link>
 
     <gazebo>
@@ -110,11 +106,11 @@
         <maxRotVelocity>${max_rot_velocity}</maxRotVelocity>
         <motorConstant>${motor_constant}</motorConstant>
         <momentConstant>${moment_constant}</momentConstant>
-        <commandTopic>${robot_namespace}/command/motors</commandTopic>
+        <commandSubTopic>${robot_namespace}/command/motors</commandSubTopic>
         <motorNumber>${motor_number}</motorNumber>
         <rotorDragCoefficient>${rotor_drag_coefficient}</rotorDragCoefficient>
         <rollingMomentCoefficient>${rolling_moment_coefficient}</rollingMomentCoefficient>
-        <motorVelocityTopic>${robot_namespace}/motor_vel/${motor_number}</motorVelocityTopic>
+        <motorSpeedPubTopic>${robot_namespace}/motor_vel/${motor_number}</motorSpeedPubTopic> 
         <rotorVelocitySlowdownSim>${rotor_velocity_slowdown_sim}</rotorVelocitySlowdownSim>
       </plugin>
     </gazebo>
diff --git a/worlds/basic.world b/worlds/basic.world
old mode 100755
new mode 100644
index eef0338..d4ae87b
--- a/worlds/basic.world
+++ b/worlds/basic.world
@@ -1,4 +1,5 @@
 <?xml version="1.0" ?>
+<?xml-model href="http://sdformat.org/schemas/root.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <sdf version="1.4">
   <world name="default">
     <include>
@@ -7,16 +8,21 @@
     <include>
       <uri>model://sun</uri>
     </include>
-
-    <!-- Turn off the shadows -->
+    
+    <!-- turn off shadows {-->
     <scene>
-      <shadows>0</shadows>
+      <shadows>false</shadows>
+      <sky>
+        <clouds/>
+      </sky>
     </scene>
+    <!--}-->
 
     <!-- Only one ROS interface plugin is required per world, as any other plugin can connect a Gazebo
          topic to a ROS topic (or vise versa). -->
     <plugin name="ros_interface_plugin" filename="librotors_gazebo_ros_interface_plugin.so"/>
-
+    
+    <!-- coordinate system {-->
     <spherical_coordinates>
       <surface_model>EARTH_WGS84</surface_model>
       <latitude_deg>47.3667</latitude_deg>
@@ -24,24 +30,30 @@
       <elevation>500.0</elevation>
       <heading_deg>0</heading_deg>
     </spherical_coordinates>
-    <physics type='ode'>
+    <!--}-->
+    
+    <!-- physics engine {-->
+    <physics name="default_physics" default="0" type='ode'>
       <ode>
         <solver>
           <type>quick</type>
-          <iters>1000</iters>
+          <iters>10</iters>
           <sor>1.3</sor>
         </solver>
         <constraints>
           <cfm>0</cfm>
           <erp>0.2</erp>
-          <contact_max_correcting_vel>100</contact_max_correcting_vel>
+          <contact_max_correcting_vel>1000</contact_max_correcting_vel>
           <contact_surface_layer>0.001</contact_surface_layer>
         </constraints>
       </ode>
       <max_step_size>0.001</max_step_size>
       <real_time_factor>1</real_time_factor>
       <real_time_update_rate>1000</real_time_update_rate>
+      <magnetic_field>6.0e-06 2.3e-05 -4.2e-05</magnetic_field>
       <gravity>0 0 -9.8</gravity>
     </physics>
+    <!--}-->
+    
   </world>
 </sdf>