From 2f8e7d499784ab8fd047a7c58f4f5d90ed9810e6 Mon Sep 17 00:00:00 2001 From: zhangshuai Date: Mon, 23 Sep 2019 09:29:34 +0800 Subject: [PATCH] 1. MPI_Allgatherv, version 0.6.2 2. a little modification in MPI_Gazebo_launch.sh and mygzserver 3. reduce link::update in worldUpdateBegin 4. modify time computing method in world.cc --- Gazebo_Distributed_MPI/gazebo/common/Event.hh | 21 + Gazebo_Distributed_MPI/gazebo/physics/Link.cc | 28 +- .../gazebo/physics/World.cc | 73 +- .../mpi_run/MPI_Gazebo_end.sh | 8 +- .../mpi_run/MPI_Gazebo_launch.sh | 35 +- Gazebo_Distributed_MPI/mpi_run/mygzserver | 1 + .../launch/hector_100_single_gazebo.launch | 1116 +++++++++++++++++ ...ebo_mpi_spawn_test_seperate_takeoff.launch | 128 ++ 8 files changed, 1374 insertions(+), 36 deletions(-) create mode 100644 Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_100_single_gazebo.launch create mode 100644 Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_10_single_gazebo_mpi_spawn_test_seperate_takeoff.launch diff --git a/Gazebo_Distributed_MPI/gazebo/common/Event.hh b/Gazebo_Distributed_MPI/gazebo/common/Event.hh index 49d7586..c7f38f6 100644 --- a/Gazebo_Distributed_MPI/gazebo/common/Event.hh +++ b/Gazebo_Distributed_MPI/gazebo/common/Event.hh @@ -35,6 +35,9 @@ #include #include "gazebo/util/system.hh" +#include //Added by zhangshuai 2019.09.09 +#include //Added by zhangshuai 2019.09.09 + namespace gazebo { /// \ingroup gazebo_event @@ -381,8 +384,26 @@ namespace gazebo this->myDataPtr->signaled = true; for (auto iter: this->myDataPtr->connections) { + //std::cout << iter.first << std::endl; //zhangshuai test 2019.09.10 + + //struct timespec ts; + //clock_gettime(CLOCK_REALTIME, &ts); + //long cur_time; + //long update_time; + + //cur_time = (double)ts.tv_sec + (double)ts.tv_nsec / 1.e9;<< std::scientific + + //cur_time = ts.tv_nsec; + if (iter.second->on) (*iter.second->callback)(_p); + + //clock_gettime(CLOCK_REALTIME, &ts); + //update_time = ts.tv_nsec - cur_time; + + //std::cout << "===== time1 : " << cur_time << std::endl; //zhangshuai test 2019.09.10 + //std::cout << "===== time2 : " << ts.tv_nsec << std::endl; //zhangshuai test 2019.09.10 + //std::cout << "===== OnUpdate time : " << update_time << std::endl; //zhangshuai test 2019.09.10 } } diff --git a/Gazebo_Distributed_MPI/gazebo/physics/Link.cc b/Gazebo_Distributed_MPI/gazebo/physics/Link.cc index 6ea8a1a..c7473fd 100644 --- a/Gazebo_Distributed_MPI/gazebo/physics/Link.cc +++ b/Gazebo_Distributed_MPI/gazebo/physics/Link.cc @@ -246,8 +246,32 @@ void Link::Load(sdf::ElementPtr _sdf) } } - this->connections.push_back(event::Events::ConnectWorldUpdateBegin( - boost::bind(&Link::Update, this, _1))); + //this->connections.push_back(event::Events::ConnectWorldUpdateBegin( + // boost::bind(&Link::Update, this, _1))); + + // Modified by zhangshuai 2019.09.10 ----Begin + if (this->GetWorld()->GetFlag() == 1) + { + // Judge if the model to load is simulated in this gazebo + for (unsigned int i = 0; i < this->GetWorld()->GetDistribution()->GetGazeboIDPtr(this->GetWorld()->GetGazeboLocalID())->GetModelCount(); i++) + { + if (this->GetModel()->GetName() == this->GetWorld()->GetDistribution()->GetGazeboIDPtr(this->GetWorld()->GetGazeboLocalID())->GetModelName(i)) + { + // Original part + this->connections.push_back(event::Events::ConnectWorldUpdateBegin( + boost::bind(&Link::Update, this, _1))); + // Original part + } + } + } + else + { + // Original part + this->connections.push_back(event::Events::ConnectWorldUpdateBegin( + boost::bind(&Link::Update, this, _1))); + // Original part + } + // Modified by zhangshuai 2019.09.10 ----End std::string topicName = "~/" + this->GetScopedName() + "/wrench"; boost::replace_all(topicName, "::", "/"); diff --git a/Gazebo_Distributed_MPI/gazebo/physics/World.cc b/Gazebo_Distributed_MPI/gazebo/physics/World.cc index 5184b17..a90dae1 100644 --- a/Gazebo_Distributed_MPI/gazebo/physics/World.cc +++ b/Gazebo_Distributed_MPI/gazebo/physics/World.cc @@ -107,6 +107,21 @@ double before_mpiTime = 0; double mpiTime = 0; double after_mpiTime = 0; double wholeUpdataTime = 0; + +double before_worldUpdateBeginTime2 = 0; +double worldUpdateBeginTime2 = 0; +double modelUpdateFuncTime2 = 0; +double updateCollisionTime2 = 0; +double loggingTime2 = 0; +double before_updatePhysicsTime2 = 0; +double updatePhysicsTime2 = 0; +double before_mpiTime2 = 0; +double mpiTime2 = 0; +double after_mpiTime2 = 0; +double wholeUpdataTime2 = 0; +double simtime = 0; +double realtime = 0; +uint64_t start_iterations = 0; #endif // Added by zhangshuai 2019.04.03 for count time ----End @@ -271,7 +286,13 @@ void World::Load(sdf::ElementPtr _sdf) this->dataPtr->lightFactoryPub = this->dataPtr->node->Advertise( "~/factory/light"); - // Added by zhangshuai based on zenglei 2019.04.23 ----Begin + // Added by zhangshuai based on zenglei 2019.09.23 ----Begin + std::cout << " ____________________________________________________________" << std::endl; + std::cout << "| This is Gazebo-Exercise-MPI 0.6 |" << std::endl; + std::cout << "| by AIRC-01 |" << std::endl; + std::cout << "|____________________________________________________________|" << std::endl; + // Added by zhangshuai based on zenglei 2019.04.23 ----End + if (this->dataPtr->sdf->HasElement("distribution")) { sdf::ElementPtr distributionElem = this->dataPtr->sdf->GetElement("distribution"); @@ -1048,26 +1069,44 @@ void World::Update() #ifdef USE_COUNT_TIME // std::cout << "===== end iterations:\t" << this->dataPtr->iterations << std::endl; + if (this->dataPtr->iterations == 0) + { + before_worldUpdateBeginTime2 = before_worldUpdateBeginTime; + worldUpdateBeginTime2 = worldUpdateBeginTime; + modelUpdateFuncTime2 = modelUpdateFuncTime; + updateCollisionTime2 = updateCollisionTime; + loggingTime2 = loggingTime; + before_updatePhysicsTime2 = before_updatePhysicsTime; + updatePhysicsTime2 = updatePhysicsTime; + before_mpiTime2 = before_mpiTime; + mpiTime2 = mpiTime; + after_mpiTime2 = after_mpiTime; + wholeUpdataTime2 = wholeUpdataTime; + simtime = this->GetSimTime().Double(); + realtime = this->GetRealTime().Double(); + start_iterations = this->dataPtr->iterations; + } + if (this->dataPtr->iterations == 100000) { // if (this->gazeboLocalID == 0) // { - std::cout << "===== average before_worldUpdateBeginTime:\t" << before_worldUpdateBeginTime * 1000.0 / this->dataPtr->iterations << "\tms =====\t" << this->gazeboLocalID << std::endl; - std::cout << "===== average worldUpdateBeginTime:\t" << worldUpdateBeginTime * 1000.0 / this->dataPtr->iterations << "\tms =====\t" << this->gazeboLocalID << std::endl; - std::cout << "===== average modelUpdateFuncTime:\t" << modelUpdateFuncTime * 1000.0 / this->dataPtr->iterations << "\tms =====\t" << this->gazeboLocalID << std::endl; - std::cout << "===== average updateCollisionTime:\t" << updateCollisionTime * 1000.0 / this->dataPtr->iterations << "\tms =====\t" << this->gazeboLocalID << std::endl; - std::cout << "===== average loggingTime:\t" << loggingTime * 1000.0 / this->dataPtr->iterations << "\tms =====\t" << this->gazeboLocalID << std::endl; - std::cout << "===== average before_updatePhysicsTime:\t" << before_updatePhysicsTime * 1000.0 / this->dataPtr->iterations << "\tms =====\t" << this->gazeboLocalID << std::endl; - std::cout << "===== average updatePhysicsTime:\t" << updatePhysicsTime * 1000.0 / this->dataPtr->iterations << "\tms =====\t" << this->gazeboLocalID << std::endl; - std::cout << "===== average before_mpiTime:\t" << before_mpiTime * 1000.0 / this->dataPtr->iterations << "\tms =====\t" << this->gazeboLocalID << std::endl; - std::cout << "===== average mpiTime:\t" << mpiTime * 1000.0 / this->dataPtr->iterations << "\tms =====\t" << this->gazeboLocalID << std::endl; - std::cout << "===== average after_mpiTime:\t" << after_mpiTime * 1000.0 / this->dataPtr->iterations << "\tms =====\t" << this->gazeboLocalID << std::endl; - double wholeMpiTime = before_mpiTime + mpiTime + after_mpiTime; - std::cout << "===== average wholeMpiTime:\t" << wholeMpiTime * 1000.0 / this->dataPtr->iterations << "\tms =====\t" << this->gazeboLocalID << std::endl; - double wholeAddTime = before_worldUpdateBeginTime + worldUpdateBeginTime + modelUpdateFuncTime + updateCollisionTime + loggingTime + before_updatePhysicsTime + updatePhysicsTime + before_mpiTime + mpiTime + after_mpiTime; - std::cout << "===== average wholeAddTime:\t" << wholeAddTime * 1000.0 / this->dataPtr->iterations << "\tms =====\t" << this->gazeboLocalID << std::endl; - std::cout << "===== average wholeUpdateTime:\t" << wholeUpdataTime * 1000.0 / this->dataPtr->iterations << "\tms =====\t" << this->gazeboLocalID << std::endl; - std::cout << "===== average RTF:\t" << this->GetSimTime().Double() / this->GetRealTime().Double() << "\t =====\t" << this->gazeboLocalID << std::endl; + std::cout << "===== average before_worldUpdateBeginTime:\t" << (before_worldUpdateBeginTime - before_worldUpdateBeginTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations) << "\tms =====\t" << this->gazeboLocalID << std::endl; + std::cout << "===== average worldUpdateBeginTime:\t" << (worldUpdateBeginTime - worldUpdateBeginTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations) << "\tms =====\t" << this->gazeboLocalID << std::endl; + std::cout << "===== average modelUpdateFuncTime:\t" << (modelUpdateFuncTime - modelUpdateFuncTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations) << "\tms =====\t" << this->gazeboLocalID << std::endl; + std::cout << "===== average updateCollisionTime:\t" << (updateCollisionTime - updateCollisionTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations) << "\tms =====\t" << this->gazeboLocalID << std::endl; + std::cout << "===== average loggingTime:\t" << (loggingTime - loggingTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations) << "\tms =====\t" << this->gazeboLocalID << std::endl; + std::cout << "===== average before_updatePhysicsTime:\t" << (before_updatePhysicsTime - before_updatePhysicsTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations) << "\tms =====\t" << this->gazeboLocalID << std::endl; + std::cout << "===== average updatePhysicsTime:\t" << (updatePhysicsTime - updatePhysicsTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations) << "\tms =====\t" << this->gazeboLocalID << std::endl; + std::cout << "===== average before_mpiTime:\t" << (before_mpiTime - before_mpiTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations) << "\tms =====\t" << this->gazeboLocalID << std::endl; + std::cout << "===== average mpiTime:\t" << (mpiTime - mpiTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations) << "\tms =====\t" << this->gazeboLocalID << std::endl; + std::cout << "===== average after_mpiTime:\t" << (after_mpiTime - after_mpiTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations) << "\tms =====\t" << this->gazeboLocalID << std::endl; + double wholeMpiTime = before_mpiTime + mpiTime + after_mpiTime - (before_mpiTime2 + mpiTime2 + after_mpiTime2); + std::cout << "===== average wholeMpiTime:\t" << wholeMpiTime * 1000.0 / (this->dataPtr->iterations - start_iterations) << "\tms =====\t" << this->gazeboLocalID << std::endl; + double wholeAddTime = before_worldUpdateBeginTime + worldUpdateBeginTime + modelUpdateFuncTime + updateCollisionTime + loggingTime + before_updatePhysicsTime + updatePhysicsTime + before_mpiTime + mpiTime + after_mpiTime - (before_worldUpdateBeginTime2 + worldUpdateBeginTime2 + modelUpdateFuncTime2 + updateCollisionTime2 + loggingTime2 + before_updatePhysicsTime2 + updatePhysicsTime2 + before_mpiTime2 + mpiTime2 + after_mpiTime2); + std::cout << "===== average wholeAddTime:\t" << wholeAddTime * 1000.0 / (this->dataPtr->iterations - start_iterations) << "\tms =====\t" << this->gazeboLocalID << std::endl; + std::cout << "===== average wholeUpdateTime:\t" << (wholeUpdataTime - wholeUpdataTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations) << "\tms =====\t" << this->gazeboLocalID << std::endl; + std::cout << "===== average RTF:\t" << (this->GetSimTime().Double() - simtime) / (this->GetRealTime().Double() - realtime) << "\t =====\t" << this->gazeboLocalID << std::endl; // } } #endif diff --git a/Gazebo_Distributed_MPI/mpi_run/MPI_Gazebo_end.sh b/Gazebo_Distributed_MPI/mpi_run/MPI_Gazebo_end.sh index e329e26..0f2705d 100755 --- a/Gazebo_Distributed_MPI/mpi_run/MPI_Gazebo_end.sh +++ b/Gazebo_Distributed_MPI/mpi_run/MPI_Gazebo_end.sh @@ -3,10 +3,12 @@ # Author: Zhang Shuai # # Date: 2019-08-26 # # Description: End a MPI Gazebo simulation task # -# Usage: ./MPI_Gazebo_end.sh [hostfile] # +# Usage: ./MPI_Gazebo_end.sh [hostfile] [shell's path] # +# Example: ./MPI_Gazebo_launch.sh hostfile ~/Git/Gazebo_Distributed_MPI/mpi_run # ##################################################################################################################################### host_file=$1 +shell_path=$2 host_count=0 hosts=() @@ -29,10 +31,10 @@ do echo "host_name:${hosts[$i]}" done -#登入各节点kill相关进程以结束仿真(1.目前方式比较暴力,后续改进;2.cd ~/Git/Gazebo_Distributed_MPI/mpi_run 还要进行灵活性优化) +#登入各节点kill相关进程以结束仿真(1.目前方式比较暴力,后续改进。) for((i=0;i /dev/null 2>&1 "cd ~/Git/Gazebo_Distributed_MPI/mpi_run; + ssh -f -n ${hosts[$i]} > /dev/null 2>&1 "cd $shell_path; ./kill.sh; ./kill.sh; ./kill.sh; diff --git a/Gazebo_Distributed_MPI/mpi_run/MPI_Gazebo_launch.sh b/Gazebo_Distributed_MPI/mpi_run/MPI_Gazebo_launch.sh index 23ffd8a..c01a1ec 100755 --- a/Gazebo_Distributed_MPI/mpi_run/MPI_Gazebo_launch.sh +++ b/Gazebo_Distributed_MPI/mpi_run/MPI_Gazebo_launch.sh @@ -1,22 +1,26 @@ #!/bin/bash -##################################################################################################################################### -# Author: Zhang Shuai # -# Date: 2019-05-23 # -# Description: launch a MPI Gazebo simulation task # -# Usage: ./MPI_Gazebo_launch.sh [ros packdge's name] [launch file's name] [workspace's path] [world's path and name] [hostfile] # -##################################################################################################################################### +################################################################################################################################################### +# Author: Zhang Shuai # +# Date: 2019-05-23 # +# Description: launch a MPI Gazebo simulation task # +# Usage: ./MPI_Gazebo_launch.sh [ros packdge's name] [launch file's name] [workspace's path] [world's path and name] [hostfile] [shell's path] # +# Example: ./MPI_Gazebo_launch.sh rosplane_sim fixedwing_text_three_100_withoutfast_mpi.launch ~/zhangshuai/DA # +# ~/zhangshuai/DA/src/da/rosplane/rosplane_sim/worlds/flying_mpi_100.world hostfile ~/Git/Gazebo_Distributed_MPI/mpi_run # +################################################################################################################################################### packdge_name=$1 launch_file_name=$2 workspace_path=$3 world_file=$4 host_file=$5 +shell_path=$6 echo $packdge_name echo $launch_file_name echo $workspace_path echo $world_file echo $host_file +echo $shell_path host_count=0 hosts=() @@ -39,26 +43,29 @@ do echo "host_name:${hosts[$i]}" done -#登入各节点启动roscore (source还有改进空间,ROS运行的工作目录应该作为参数传进来) +#登入各节点启动roscore for((i=0;i /dev/null 2>&1 "cd ~/Git/Gazebo_Distributed_MPI/mpi_run; + ssh -f -n ${hosts[$i]} > /dev/null 2>&1 "cd $shell_path; ./kill.sh; ./kill.sh; ./kill.sh; diff --git a/Gazebo_Distributed_MPI/mpi_run/mygzserver b/Gazebo_Distributed_MPI/mpi_run/mygzserver index 9941188..369e6d8 100755 --- a/Gazebo_Distributed_MPI/mpi_run/mygzserver +++ b/Gazebo_Distributed_MPI/mpi_run/mygzserver @@ -5,5 +5,6 @@ source /opt/ros/kinetic/setup.bash source $2/devel/setup.bash export DISPLAY=:0.0 +export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib:/usr/local/lib/gazebo-7/plugins:/opt/ros/kinetic/lib:/opt/ros/kinetic/lib/x86_64-linux-gnu /usr/local/bin/gzserver -u -e ode --verbose $1 -s /opt/ros/kinetic/lib/libgazebo_ros_paths_plugin.so -s /opt/ros/kinetic/lib/libgazebo_ros_api_plugin.so diff --git a/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_100_single_gazebo.launch b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_100_single_gazebo.launch new file mode 100644 index 0000000..dfbe2f9 --- /dev/null +++ b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_100_single_gazebo.launch @@ -0,0 +1,1116 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_10_single_gazebo_mpi_spawn_test_seperate_takeoff.launch b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_10_single_gazebo_mpi_spawn_test_seperate_takeoff.launch new file mode 100644 index 0000000..3034158 --- /dev/null +++ b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_10_single_gazebo_mpi_spawn_test_seperate_takeoff.launch @@ -0,0 +1,128 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +