From c0e54f156d4739a7a2aabcca7991413b48461f58 Mon Sep 17 00:00:00 2001
From: zhangshuai <zhangshuai9999@outlook.com>
Date: Fri, 10 May 2019 09:59:00 +0800
Subject: [PATCH] modify_5

---
 Gazebo_Distributed_MPI/mpi_run/hostfile       |  4 +
 Gazebo_Distributed_MPI/mpi_run/mpirun_test.sh |  9 ++-
 Gazebo_Distributed_MPI/mpi_run/try.sh         |  3 +
 .../launch/hector_2_single_gazebo.launch      |  3 +-
 .../launch/hector_2_single_gazebo_test.launch | 30 ++++++++
 .../launch/hector_4_single_gazebo.launch      |  3 +-
 .../launch/hector_add_1_test.launch           | 10 +--
 .../launch/spawn_quadrotor.launch             |  2 +-
 .../launch/spawn_quadrotor_1.launch           | 74 +++++++++++++++++++
 .../worlds/kunming_airport.world              | 13 +++-
 ...unming_airport_distribution_mpi_test.world | 47 ++++++++++++
 11 files changed, 185 insertions(+), 13 deletions(-)
 create mode 100644 Gazebo_Distributed_MPI/mpi_run/hostfile
 create mode 100755 Gazebo_Distributed_MPI/mpi_run/try.sh
 create mode 100644 Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_2_single_gazebo_test.launch
 create mode 100644 Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_1.launch
 create mode 100644 Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/worlds/kunming_airport_distribution_mpi_test.world

diff --git a/Gazebo_Distributed_MPI/mpi_run/hostfile b/Gazebo_Distributed_MPI/mpi_run/hostfile
new file mode 100644
index 0000000..abddf32
--- /dev/null
+++ b/Gazebo_Distributed_MPI/mpi_run/hostfile
@@ -0,0 +1,4 @@
+localhost slots=2
+#anwen slots=2
+zhangshuai-ThinkPad-X250 slots=2
+
diff --git a/Gazebo_Distributed_MPI/mpi_run/mpirun_test.sh b/Gazebo_Distributed_MPI/mpi_run/mpirun_test.sh
index 914227a..51251cf 100755
--- a/Gazebo_Distributed_MPI/mpi_run/mpirun_test.sh
+++ b/Gazebo_Distributed_MPI/mpi_run/mpirun_test.sh
@@ -2,4 +2,11 @@ mpirun -machinefile ./machinefile -np 1 -x GAZEBO_MASTER_URI=http://localhost:11
 
 mpirun -machinefile ./machinefile -np 1 -x GAZEBO_MASTER_URI=http://localhost:11346 gazebo --verbose /home/zhangshuai/Git/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/worlds/kunming_airport_distribution_2hector_mpi_test.world : -np 1 -x GAZEBO_MASTER_URI=http://localhost:11347 gazebo --verbose /home/zhangshuai/Git/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/worlds/kunming_airport_distribution_2hector_mpi_test.world
 
-mpirun -machinefile ./machinefile -np 2 -x GAZEBO_MASTER_URI=http://localhost:11346 gazebo --verbose /home/zhangshuai/Git/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/worlds/kunming_airport_distribution_2hector_mpi_test.world : -np 2 -x GAZEBO_MASTER_URI=http://localhost:11347 gazebo --verbose /home/zhangshuai/Git/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/worlds/kunming_airport_distribution_2hector_mpi_test.world : -np 2 -x GAZEBO_MASTER_URI=http://localhost:11348 gazebo --verbose /home/zhangshuai/Git/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/worlds/kunming_airport_distribution_2hector_mpi_test.world
\ No newline at end of file
+mpirun -machinefile ./machinefile -np 2 -x GAZEBO_MASTER_URI=http://localhost:11346 gazebo --verbose /home/zhangshuai/Git/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/worlds/kunming_airport_distribution_2hector_mpi_test.world : -np 2 -x GAZEBO_MASTER_URI=http://localhost:11347 gazebo --verbose /home/zhangshuai/Git/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/worlds/kunming_airport_distribution_2hector_mpi_test.world : -np 2 -x GAZEBO_MASTER_URI=http://localhost:11348 gazebo --verbose /home/zhangshuai/Git/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/worlds/kunming_airport_distribution_2hector_mpi_test.world
+
+mpiexec -hostfile hostfile -np 1 -x GAZEBO_MASTER_URI=http://localhost:11346 gazebo --verbose /home/zhangshuai/Git/Gazebo_Distributed_MPI/mpi_run/kunming_airport_distribution_30hector_mpi_test.world : -np 1 -x GAZEBO_MASTER_URI=http://localhost:11347 gzserver --verbose /home/zhangshuai/Git/Gazebo_Distributed_MPI/mpi_run/kunming_airport_distribution_30hector_mpi_test.world
+
+mpiexec -hostfile hostfile -np 1 -x GAZEBO_MASTER_URI=http://localhost:11346 gzserver -e ode --verbose /home/zhangshuai/Git/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/worlds/empty_origin.world -s /home/zhangshuai/catkin_ws/devel/lib/libgazebo_ros_paths_plugin.so -s /home/zhangshuai/catkin_ws/devel/lib/libgazebo_ros_api_plugin.so : -np 1 -x GAZEBO_MASTER_URI=http://localhost:11347 gzserver -e ode --verbose /home/zhangshuai/Git/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/worlds/empty_origin.world -s /home/zhangshuai/catkin_ws/devel/lib/libgazebo_ros_paths_plugin.so -s /home/zhangshuai/catkin_ws/devel/lib/libgazebo_ros_api_plugin.so
+
+
+mpiexec -np 1 -x GAZEBO_MASTER_URI=http://localhost:11346 gazebo -e ode --verbose /home/zhangshuai/Git/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/worlds/kunming_airport.world -s /home/zhangshuai/catkin_ws/devel/lib/libgazebo_ros_paths_plugin.so -s /home/zhangshuai/catkin_ws/devel/lib/libgazebo_ros_api_plugin.so -gname xxx : -np 1 -x GAZEBO_MASTER_URI=http://localhost:11347 gazebo -e ode --verbose /home/zhangshuai/Git/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/worlds/kunming_airport.world -s /home/zhangshuai/catkin_ws/devel/lib/libgazebo_ros_paths_plugin.so -s /home/zhangshuai/catkin_ws/devel/lib/libgazebo_ros_api_plugin.so -gname yyy
\ No newline at end of file
diff --git a/Gazebo_Distributed_MPI/mpi_run/try.sh b/Gazebo_Distributed_MPI/mpi_run/try.sh
new file mode 100755
index 0000000..d0d42d8
--- /dev/null
+++ b/Gazebo_Distributed_MPI/mpi_run/try.sh
@@ -0,0 +1,3 @@
+#!/bin/sh
+
+mpiexec -hostfile hostfile -np 1 -x GAZEBO_MASTER_URI=http://localhost:11346 gazebo --verbose /home/zhangshuai/Git/Gazebo_Distributed_MPI/mpi_run/kunming_airport_distribution_30hector_mpi_test.world : -np 1 -x GAZEBO_MASTER_URI=http://localhost:11347 gzserver --verbose /home/zhangshuai/Git/Gazebo_Distributed_MPI/mpi_run/kunming_airport_distribution_30hector_mpi_test.world
diff --git a/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_2_single_gazebo.launch b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_2_single_gazebo.launch
index d174d72..b0aacf8 100644
--- a/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_2_single_gazebo.launch
+++ b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_2_single_gazebo.launch
@@ -8,8 +8,7 @@
     <arg name="gui" value="true"/>
     <arg name="verbose" value="false"/>
     <arg name="debug" value="false"/>
-    <arg name="world_name" value="$(find hector_quadrotor_gazebo)/worlds/kunming_airport
-    .world"/>
+    <arg name="world_name" value="$(find hector_quadrotor_gazebo)/worlds/kunming_airport.world"/>
   </include>
 
   <group ns="bebop_0">
diff --git a/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_2_single_gazebo_test.launch b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_2_single_gazebo_test.launch
new file mode 100644
index 0000000..8f91a69
--- /dev/null
+++ b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_2_single_gazebo_test.launch
@@ -0,0 +1,30 @@
+<!-- This is a launch file that runs the bare minimum requirements to get -->
+<!-- gazebo running for a fixed-wing aircraft -->
+
+<launch>
+
+  <group ns="bebop_0">
+    <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+      <arg name="name" value="bebop_0" />
+      <arg name="model" value="$(find hector_quadrotor_description)/urdf/quadrotor_with_downward_cam.gazebo.xacro"/>
+      <arg name="tf_prefix" value="bebop_0" />
+      <arg name="x" value="0.0" />
+      <arg name="y" value="0.0" />
+      <arg name="z" value="0.186" />
+    </include>
+  </group>
+
+  <group ns="bebop_1">
+    <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+      <arg name="name" value="bebop_1" />
+      <arg name="model" value="$(find hector_quadrotor_description)/urdf/quadrotor_with_downward_cam.gazebo.xacro"/>
+      <arg name="tf_prefix" value="bebop_1" />
+      <arg name="x" value="0.0" />
+      <arg name="y" value="5.0" />
+      <arg name="z" value="0.186" />
+    </include>
+  </group>
+
+  <node name="simple_takeoff" pkg="hector" type="hector_simple_takeoff"/>
+
+</launch>
diff --git a/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_4_single_gazebo.launch b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_4_single_gazebo.launch
index 9f8ad8c..7c8530c 100644
--- a/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_4_single_gazebo.launch
+++ b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_4_single_gazebo.launch
@@ -8,8 +8,7 @@
     <arg name="gui" value="true"/>
     <arg name="verbose" value="false"/>
     <arg name="debug" value="false"/>
-    <arg name="world_name" value="$(find hector_quadrotor_gazebo)/worlds/kunming_airport
-    .world"/>
+    <arg name="world_name" value="$(find hector_quadrotor_gazebo)/worlds/kunming_airport.world"/>
   </include>
 
   <group ns="bebop_0">
diff --git a/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_add_1_test.launch b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_add_1_test.launch
index 4da92ff..2e137a3 100644
--- a/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_add_1_test.launch
+++ b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_add_1_test.launch
@@ -3,13 +3,13 @@
 
 <launch>
 
-  <group ns="bebop_2">
-    <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
-      <arg name="name" value="bebop_2" />
+  <group ns="bebop_1">
+    <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor_1.launch">
+      <arg name="name" value="bebop_1" />
       <arg name="model" value="$(find hector_quadrotor_description)/urdf/quadrotor_with_downward_cam.gazebo.xacro"/>
-      <arg name="tf_prefix" value="bebop_2" />
+      <arg name="tf_prefix" value="bebop_1" />
       <arg name="x" value="0" />
-      <arg name="y" value="10" />
+      <arg name="y" value="0" />
       <arg name="z" value="0.186" />
     </include>
   </group>
diff --git a/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor.launch b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor.launch
index 6b6eb47..a624bb8 100644
--- a/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor.launch
+++ b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor.launch
@@ -2,7 +2,7 @@
 
 <launch>
   <arg name="name" default="quadrotor"/>
-  <arg name="model" default="$(find hector_quadrotor_description)/urdf/box.gazebo.xacro"/>
+  <arg name="model" default="$(find hector_quadrotor_description)/urdf/quadrotor_with_downward_cam.gazebo.xacro"/>
   <arg name="tf_prefix" default="$(optenv ROS_NAMESPACE)"/>
   <arg name="controller_definition" default="$(find hector_quadrotor_controller)/launch/controller.launch"/>
 
diff --git a/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_1.launch b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_1.launch
new file mode 100644
index 0000000..3d2079b
--- /dev/null
+++ b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_1.launch
@@ -0,0 +1,74 @@
+<?xml version="1.0"?>
+
+<launch>
+  <arg name="name" default="quadrotor"/>
+  <arg name="model" default="$(find hector_quadrotor_description)/urdf/quadrotor_with_downward_cam.gazebo.xacro"/>
+  <arg name="tf_prefix" default="$(optenv ROS_NAMESPACE)"/>
+  <arg name="controller_definition" default="$(find hector_quadrotor_controller)/launch/controller.launch"/>
+
+  <arg name="x" default="0.0"/>
+  <arg name="y" default="0.0"/>
+  <arg name="z" default="0.3"/>
+
+  <arg name="use_ground_truth_for_tf" default="true" />
+  <arg name="use_ground_truth_for_control" default="true" />
+  <arg name="use_pose_estimation" if="$(arg use_ground_truth_for_control)" default="false"/>
+  <arg name="use_pose_estimation" unless="$(arg use_ground_truth_for_control)" default="true"/>
+
+  <arg name="world_frame" default="world"/> <!-- This should actually be "/world". See https://github.com/ros-simulation/gazebo_ros_pkgs/pull/324 -->
+  <arg name="base_link_frame" default="$(arg tf_prefix)/base_link"/>
+
+  <!-- send the robot XML to param server -->
+  <param name="robot_description" command="$(find xacro)/xacro '$(arg model)' name:=$(arg name) base_link_frame:=$(arg base_link_frame) world_frame:=$(arg world_frame)" />
+  <param name="tf_prefix" type="string" value="$(arg tf_prefix)" />
+  <param name="base_link_frame" type="string" value="$(arg base_link_frame)"/>
+  <param name="world_frame" type="string" value="$(arg world_frame)"/>
+
+  <!-- push robot_description to factory and spawn robot in gazebo -->
+  <node name="spawn_robot" pkg="gazebo_ros" type="spawn_model1"
+        args="-param robot_description
+           -urdf
+           -x $(arg x)
+           -y $(arg y)
+           -z $(arg z)
+           -model $(arg name)"
+        respawn="false" output="screen"/>
+
+  <!-- start robot state publisher -->
+  <!-- <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen" >
+    <param name="publish_frequency" type="double" value="50.0" />
+  </node> -->
+
+  <!-- publish state and tf -->
+  <!-- <node name="ground_truth_to_tf" pkg="message_to_tf" type="message_to_tf" output="screen">
+    <param name="odometry_topic" value="ground_truth/state" />
+    <param name="frame_id" value="/$(arg world_frame)" />
+    <param name="tf_prefix" value="$(arg tf_prefix)" if="$(arg use_ground_truth_for_tf)" />
+    <param name="tf_prefix" value="$(arg tf_prefix)/ground_truth" unless="$(arg use_ground_truth_for_tf)" />
+  </node>
+  <group if="$(arg use_pose_estimation)">
+    <node name="pose_estimation" pkg="hector_quadrotor_pose_estimation" type="hector_quadrotor_pose_estimation" output="screen">
+      <rosparam file="$(find hector_quadrotor_pose_estimation)/params/simulation.yaml" />
+      <param name="nav_frame" value="$(arg tf_prefix)/nav" />
+      <param name="publish_world_nav_transform" value="true" />
+      <param name="tf_prefix" value="$(arg tf_prefix)" unless="$(arg use_ground_truth_for_tf)" />
+      <param name="tf_prefix" value="$(arg tf_prefix)/pose_estimation" if="$(arg use_ground_truth_for_tf)" />
+    </node>
+  </group> -->
+
+  <!-- spawn controller -->
+  <group if="$(arg use_ground_truth_for_control)">
+    <param name="controller/state_topic" value="" />
+    <param name="controller/imu_topic" value="" />
+  </group>
+  <group unless="$(arg use_ground_truth_for_control)">
+    <param name="controller/state_topic" value="state" />
+    <param name="controller/imu_topic" value="imu" />
+  </group>
+  <include file="$(arg controller_definition)" />
+
+  <arg name="motors" default="robbe_2827-34_epp1045" />
+  <rosparam command="load" file="$(find hector_quadrotor_model)/param/quadrotor_aerodynamics.yaml" />
+  <rosparam command="load" file="$(find hector_quadrotor_model)/param/$(arg motors).yaml" />
+
+</launch>
diff --git a/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/worlds/kunming_airport.world b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/worlds/kunming_airport.world
index de9ca87..4be8f55 100644
--- a/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/worlds/kunming_airport.world
+++ b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/worlds/kunming_airport.world
@@ -30,8 +30,17 @@
     <!-- <parallel method=0 numbers_of_thread=20 size_of_block=0 type=0 /> -->
 
     <!-- <distributed type=0 ip="192.168.1.231" port=8888 flag=0 /> -->
+
+    <distribution flag=0>
+<!-- <gazebo_id num=0>
+<model_name>bebop_0</model_name>
+</gazebo_id>
+<gazebo_id num=1>
+<model_name>bebop_1</model_name>
+</gazebo_id> -->
+</distribution>
     
-    <distribution gazebo_local_ID=0 port=8888 flag=0>
+    <!--<distribution gazebo_local_ID=0 port=8888 flag=0>
       <gazebo_id num=0 ip="192.168.1.7">
         <model_name>bebop_0</model_name>
         <model_name>bebop_1</model_name>
@@ -53,7 +62,7 @@
         <model_name>bebop_13</model_name>
         <model_name>bebop_14</model_name>
       </gazebo_id>
-    </distribution>
+    </distribution>-->
 <!-- <actor name="animated_quadrotor_0">
       <include>
         <uri>model://quadrotor</uri>
diff --git a/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/worlds/kunming_airport_distribution_mpi_test.world b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/worlds/kunming_airport_distribution_mpi_test.world
new file mode 100644
index 0000000..d6d1c36
--- /dev/null
+++ b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/worlds/kunming_airport_distribution_mpi_test.world
@@ -0,0 +1,47 @@
+<?xml version="1.0" ?>
+<sdf version="1.4">
+  <world name="default">
+    <!-- A global light source -->
+    <include>
+      <uri>model://sun</uri>
+    </include>
+
+    <include>
+      <uri>model://kunming_airport</uri>
+      <pose>0 0 0  0 0 0</pose>
+    </include>
+
+    <scene>
+      <ambient>0.68 0.68 0.68 1.0</ambient>
+      <sky>
+        <sunrise/>
+        <clouds>
+          <speed>0</speed>
+        </clouds>
+      </sky>
+    </scene>
+
+    <!-- For distributed simulation -->
+    <distribution flag=0>
+<gazebo_id num=0>
+<model_name>bebop_0</model_name>
+</gazebo_id>
+<gazebo_id num=1>
+<model_name>bebop_1</model_name>
+</gazebo_id>
+</distribution>
+
+<physics type="ode" name="ode">
+      <real_time_update_rate>0</real_time_update_rate>
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>0</real_time_factor>
+      <!-- <ode>
+        <solver>
+          <thread_position_correction>0</thread_position_correction>
+          <island_threads>8</island_threads>
+        </solver>
+      </ode> -->
+    </physics>
+
+  </world>
+</sdf>