diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..aad3747 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +.hg diff --git a/Gazebo_exercise/gazebo7_7.14.0_exercise/CMakeLists.txt b/Gazebo_exercise/gazebo7_7.14.0_exercise/CMakeLists.txt index d1ca00c..079dc15 100644 --- a/Gazebo_exercise/gazebo7_7.14.0_exercise/CMakeLists.txt +++ b/Gazebo_exercise/gazebo7_7.14.0_exercise/CMakeLists.txt @@ -17,7 +17,7 @@ set (GAZEBO_MINOR_VERSION 14) set (GAZEBO_PATCH_VERSION 0) set (GAZEBO_VERSION ${GAZEBO_MAJOR_VERSION}.${GAZEBO_MINOR_VERSION}) -set (GAZEBO_VERSION_FULL ${GAZEBO_MAJOR_VERSION}.${GAZEBO_MINOR_VERSION}.${GAZEBO_PATCH_VERSION}) +set (GAZEBO_VERSION_FULL ${GAZEBO_MAJOR_VERSION}.${GAZEBO_MINOR_VERSION}.${GAZEBO_PATCH_VERSION}-exercise) message (STATUS "${PROJECT_NAME} version ${GAZEBO_VERSION_FULL}") diff --git a/Gazebo_exercise/gazebo7_7.14.0_exercise/Exercise_Changelog.md b/Gazebo_exercise/gazebo7_7.14.0_exercise/Exercise_Changelog.md new file mode 100644 index 0000000..426e2de --- /dev/null +++ b/Gazebo_exercise/gazebo7_7.14.0_exercise/Exercise_Changelog.md @@ -0,0 +1,25 @@ +## Gazebo 7.14.0 + +## Gazebo-exercise-0.2 (2019-05-23) + +1. 添加优化统一SDF标签exercise-opt，原标签parallel失效 + +1. 添加dxProcessIslands模块OpenMP并行优化 + * [SDF控制标签] updatephysics_dxprocessislands + +1. 添加dxSpace构建类型选择 + * [SDF控制标签] collide_space + +1. 添加默认dxSpace类型（dxHashSpace）碰撞处理模块OpenMp并行优化 + * [SDF控制标签] dxhashspace_collide + +1. 去掉事件处理模块(worldUpdateBegin)中的线程池等并行优化措施，只保留OpenMP + * [相关文件]/gazebo/common/Event.hh + * [SDF控制标签] event_signal + +## Gazebo-exercise-0.1 (2019-04-01) + +1. 添加事件处理模块（worldUpdateBegin）中OpenMp、Tbb、线程池、C++11Thread等并行优化措施 + * [相关文件]/gazebo/common/Event.hh + +1. 添加并行处理优化控制SDF标签parallel \ No newline at end of file diff --git a/Gazebo_exercise/gazebo7_7.14.0_exercise/Exercise_README.md b/Gazebo_exercise/gazebo7_7.14.0_exercise/Exercise_README.md new file mode 100644 index 0000000..71e3ea0 --- /dev/null +++ b/Gazebo_exercise/gazebo7_7.14.0_exercise/Exercise_README.md @@ -0,0 +1,61 @@ +# Gazebo-exercise安装&使用说明文档 +2019-05-23 by airc_exercise@163.com +``` +______________________________________________________________ +| This is Gazebo-exercis-0.2 | +| by AIRC-01 | +| | +|---------------------- WorldUpdateBegin --------------------| +| Multithreading event_signal: ON | +| Multithreading event_signal: OFF | +| Using OpenMP with n threads | +|---------------------- UpdateCollision ---------------------| +| Using dSweepAndPruneSpace | +| Using dHashSpace | +| Multithreading dHashSpace: ON | +| Using OpenMP with n threads | +| Multithreading dHashSpace: OFF | +|---------------------- UpdatePhysics -----------------------| +| Multithreading dxprocessislands: ON | +| Using OpenMP with n threads | +| Multithreading dxprocessislands: OFF | +| This code is running with NO OPTIMIZATION ! | +|____________________________________________________________| +``` + +## 安装第三方依赖库 +由于Gazebo-exercise是基于官方版本Gazebo7_7.14.0进行开发，因此，安装过程与Gazebo7_7.14.0类似，需要首先安装一些第三方依赖库。关于依赖库的具体安装过程不在这里赘述，需要注意的是这些库的版本需要跟Gazebo的版本相匹配。 +- 安装ign-math_2.8.0 +- 安装protobuf-2.6.1 +- 安装sdformat_4.4.0 + +## 安装Gazebo-exercise +``` + mkdir build + cd build + cmake ../ + make -jX（X为编译时启用的线程数，其根据CPU核心数确定，不要超过CPU核心数） + sudo make install +``` +## 使用Gazebo-exercise中的优化 +当前版本针对Gazebo仿真流程中的预处理（WorldUpdateBegin）、碰撞更新（UpdateCollision）以及物理更新（UpdatePhysics）等模块分别进行了优化，主要采用了基于OpenMP的多线程优化，也有小部分其他优化措施。为了使用这些优化，需要在仿真的主世界文件（一般以.world结尾）中，添加如下所示的优化标签： +``` + +... + + + 1 + + + + ... + +``` + +标签释义如下： +- 标签：优化总开关。 +- 标签：用于设置模型预处理模块并行优化参数，parallel_type为1~5表示OpenMp优化（一般设置为1即可，其他是采用不同的任务并发方式），threads为开启的线程数；parallel_type为其他数字时表示不开启优化。 +- 标签：用于设置碰撞更新模块中碰撞空间的类型，值为1时表示SAP空间类型，其他数字表示默认的Hash空间类型。 +- 标签：用于设置碰撞更新模块中dxHashSpace::collide模块并行参数，当标签设置为1时，该参数没有意义；其他情况下，threads大于1，表示采用OpenMP并行优化，threads为开启的线程数，否则不开启优化。 +- 标签：用于设置物理更新模块中dxProcessIslands模块并行参数；parallel_type为1~5表示OpenMp优化（一般设置为1即可，其他是采用不同的任务并发方式），threads为开启的线程数；parallel_type为其他数字时表示不开启优化。 + diff --git a/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/include/ode/collision.h b/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/include/ode/collision.h index e89726c..288e8d3 100644 --- a/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/include/ode/collision.h +++ b/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/include/ode/collision.h @@ -824,7 +824,6 @@ ODE_API int dCollide (dGeomID o1, dGeomID o2, int flags, dContactGeom *contact, */ ODE_API void dSpaceCollide (dSpaceID space, void *data, dNearCallback *callback); - /** * @brief Determines which geoms from one space may potentially intersect with * geoms from another space, and calls the callback function for each candidate diff --git a/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/include/ode/collision_space.h b/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/include/ode/collision_space.h index bf7ef9b..1c89e9a 100644 --- a/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/include/ode/collision_space.h +++ b/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/include/ode/collision_space.h @@ -51,6 +51,7 @@ typedef void dNearCallback (void *data, dGeomID o1, dGeomID o2); ODE_API dSpaceID dSimpleSpaceCreate (dSpaceID space); ODE_API dSpaceID dHashSpaceCreate (dSpaceID space); +ODE_API dSpaceID dHashSpaceCreate2 (dSpaceID space, int number); //Added by zenglei@20190523 ODE_API dSpaceID dQuadTreeSpaceCreate (dSpaceID space, const dVector3 Center, const dVector3 Extents, int Depth); @@ -65,8 +66,6 @@ ODE_API dSpaceID dQuadTreeSpaceCreate (dSpaceID space, const dVector3 Center, co ODE_API dSpaceID dSweepAndPruneSpaceCreate( dSpaceID space, int axisorder ); - - ODE_API void dSpaceDestroy (dSpaceID); ODE_API void dHashSpaceSetLevels (dSpaceID space, int minlevel, int maxlevel); diff --git a/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/include/ode/objects.h b/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/include/ode/objects.h index 610c44c..5855078 100644 --- a/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/include/ode/objects.h +++ b/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/include/ode/objects.h @@ -83,7 +83,11 @@ ODE_API dBodyID dWorldGetBody(dWorldID world, int id); */ ODE_API void dWorldDestroy (dWorldID world); - +//////////////////// AIRC begin //////////////////// + // Added by zenglei@2019-05-23, Modified by YYY@yyy-yy-yy + // [Description] set parallel info for UpdatePhysics +ODE_API void dWorldSetIslandsInfo(dWorldID w, int type, int threads); +//////////////////// AIRC end //////////////////// /** * @brief Set the world's global gravity vector. * diff --git a/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/src/collision_space.cpp b/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/src/collision_space.cpp index a075208..d3b9f04 100644 --- a/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/src/collision_space.cpp +++ b/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/src/collision_space.cpp @@ -34,6 +34,7 @@ spaces #include "collision_kernel.h" #include "collision_space_internal.h" #include "util.h" +#include //Added by zenglei@20190523 #ifdef _MSC_VER #pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" @@ -376,13 +377,16 @@ struct dxHashSpace : public dxSpace { int global_minlevel; // smallest hash table level to put AABBs in int global_maxlevel; // objects that need a level larger than this will be // put in a "big objects" list instead of a hash table + int threadsforOpenMp; //the number of thread to OpenMp by zenglei@20190523 dxHashSpace (dSpaceID _space); + dxHashSpace (dSpaceID _space, int number); //set the number of thread to OpenMp by zenglei@20190523 void setLevels (int minlevel, int maxlevel); void getLevels (int *minlevel, int *maxlevel); void cleanGeoms(); void collide (void *data, dNearCallback *callback); void collide2 (void *data, dxGeom *geom, dNearCallback *callback); + // void collide_parallel(void *data, int threads, dNearCallback *callback); //Added by zenglei@20190523 }; @@ -391,9 +395,19 @@ dxHashSpace::dxHashSpace (dSpaceID _space) : dxSpace (_space) type = dHashSpaceClass; global_minlevel = -3; global_maxlevel = 10; + threadsforOpenMp = -1; } - - +//////////////////// AIRC begin //////////////////// + // Added by zenglei@2019-05-23, Modified by YYY@yyy-yy-yy + // [Description] set the number of thread to OpenMp +dxHashSpace::dxHashSpace (dSpaceID _space, int number) : dxSpace (_space) +{ + type = dHashSpaceClass; + global_minlevel = -3; + global_maxlevel = 10; + threadsforOpenMp = number; +} +//////////////////// AIRC end //////////////////// void dxHashSpace::setLevels (int minlevel, int maxlevel) { dAASSERT (minlevel <= maxlevel); @@ -401,7 +415,6 @@ void dxHashSpace::setLevels (int minlevel, int maxlevel) global_maxlevel = maxlevel; } - void dxHashSpace::getLevels (int *minlevel, int *maxlevel) { if (minlevel) *minlevel = global_minlevel; @@ -423,7 +436,9 @@ void dxHashSpace::cleanGeoms() lock_count--; } - +//////////////////// AIRC begin //////////////////// + // Added by zenglei@2019-05-23, Modified by YYY@yyy-yy-yy + // [Description] Parallel for collide void dxHashSpace::collide (void *_data, dNearCallback *callback) { dAASSERT(this && callback); @@ -529,47 +544,102 @@ void dxHashSpace::collide (void *_data, dNearCallback *callback) // same cells for collisions, and then check for other AABBs in all // intersecting higher level cells. - int db[6]; // discrete bounds at current level - for (aabb2=first_aabb; aabb2; aabb2=aabb2->next) + if(threadsforOpenMp > 0) { - // we are searching for collisions with aabb - for (i=0; i<6; i++) db[i] = aabb2->dbounds[i]; - for (int level = aabb2->level; level <= maxlevel; level++) { - for (int xi = db[0]; xi <= db[1]; xi++) { - for (int yi = db[2]; yi <= db[3]; yi++) { - for (int zi = db[4]; zi <= db[5]; zi++) { - // get the hash index - unsigned long hi = getVirtualAddress (level,xi,yi,zi) % sz; - // search all nodes at this index - Node *node; - for (node = table[hi]; node; node=node->next) { - // node points to an AABB that may intersect aabb - if (node->aabb == aabb2) continue; - if (node->aabb->level == level && - node->x == xi && node->y == yi && node->z == zi) { - // see if aabb and node->aabb have already been tested - // against each other - unsigned char mask; - if (aabb2->index <= node->aabb->index) { - i = (aabb2->index * tested_rowsize)+(node->aabb->index >> 3); - mask = 1 << (node->aabb->index & 7); + std::vector vecAABB2; + for (aabb2=first_aabb; aabb2; aabb2=aabb2->next) + vecAABB2.push_back(aabb2); + omp_set_num_threads(threadsforOpenMp); + #pragma omp parallel for + for(int index = 0; index < (int)vecAABB2.size(); index++) + { + int db[6]; // discrete bounds at current level + int k; + // we are searching for collisions with aabb + for (k=0; k<6; k++) db[k] = vecAABB2[index]->dbounds[k]; + for (int level = vecAABB2[index]->level; level <= maxlevel; level++) { + for (int xi = db[0]; xi <= db[1]; xi++) { + for (int yi = db[2]; yi <= db[3]; yi++) { + for (int zi = db[4]; zi <= db[5]; zi++) { + // get the hash index + unsigned long hi = getVirtualAddress (level,xi,yi,zi) % sz; + // search all nodes at this index + Node *node; + for (node = table[hi]; node; node=node->next) { + // node points to an AABB that may intersect aabb + if (node->aabb == vecAABB2[index]) continue; + if (node->aabb->level == level && + node->x == xi && node->y == yi && node->z == zi) { + // see if aabb and node->aabb have already been tested + // against each other + unsigned char mask; + if (vecAABB2[index]->index <= node->aabb->index) { + k = (vecAABB2[index]->index * tested_rowsize)+(node->aabb->index >> 3); + mask = 1 << (node->aabb->index & 7); + } + else { + k = (node->aabb->index * tested_rowsize)+(vecAABB2[index]->index >> 3); + mask = 1 << (vecAABB2[index]->index & 7); + } + dIASSERT (k >= 0 && k < (tested_rowsize*n)); + if ((tested[k] & mask)==0) { + collideAABBs (vecAABB2[index]->geom,node->aabb->geom,_data,callback); + } + tested[k] |= mask; + } + } } - else { - i = (node->aabb->index * tested_rowsize)+(aabb2->index >> 3); - mask = 1 << (aabb2->index & 7); + } + } + // get the discrete bounds for the next level up + for (k=0; k<6; k++) db[k] >>= 1; + } + } + } + else + { + int db[6]; // discrete bounds at current level + for (aabb2=first_aabb; aabb2; aabb2=aabb2->next) + { + // we are searching for collisions with aabb + for (i=0; i<6; i++) db[i] = aabb2->dbounds[i]; + for (int level = aabb2->level; level <= maxlevel; level++) { + for (int xi = db[0]; xi <= db[1]; xi++) { + for (int yi = db[2]; yi <= db[3]; yi++) { + for (int zi = db[4]; zi <= db[5]; zi++) { + // get the hash index + unsigned long hi = getVirtualAddress (level,xi,yi,zi) % sz; + // search all nodes at this index + Node *node; + for (node = table[hi]; node; node=node->next) { + // node points to an AABB that may intersect aabb + if (node->aabb == aabb2) continue; + if (node->aabb->level == level && + node->x == xi && node->y == yi && node->z == zi) { + // see if aabb and node->aabb have already been tested + // against each other + unsigned char mask; + if (aabb2->index <= node->aabb->index) { + i = (aabb2->index * tested_rowsize)+(node->aabb->index >> 3); + mask = 1 << (node->aabb->index & 7); + } + else { + i = (node->aabb->index * tested_rowsize)+(aabb2->index >> 3); + mask = 1 << (aabb2->index & 7); + } + dIASSERT (i >= 0 && i < (tested_rowsize*n)); + if ((tested[i] & mask)==0) { + collideAABBs (aabb2->geom,node->aabb->geom,_data,callback); + } + tested[i] |= mask; } - dIASSERT (i >= 0 && i < (tested_rowsize*n)); - if ((tested[i] & mask)==0) { - collideAABBs (aabb2->geom,node->aabb->geom,_data,callback); - } - tested[i] |= mask; } } } } + // get the discrete bounds for the next level up + for (i=0; i<6; i++) db[i] >>= 1; } - // get the discrete bounds for the next level up - for (i=0; i<6; i++) db[i] >>= 1; } } @@ -591,7 +661,7 @@ void dxHashSpace::collide (void *_data, dNearCallback *callback) lock_count--; } - +//////////////////// AIRC end //////////////////// void dxHashSpace::collide2 (void *_data, dxGeom *geom, dNearCallback *callback) @@ -627,6 +697,10 @@ dxSpace *dHashSpaceCreate (dxSpace *space) return new dxHashSpace (space); } +dxSpace *dHashSpaceCreate2 (dxSpace *space, int number) +{ + return new dxHashSpace (space, number); +} void dHashSpaceSetLevels (dxSpace *space, int minlevel, int maxlevel) { @@ -754,7 +828,6 @@ int dSpaceGetClass (dxSpace *space) return space->type; } - void dSpaceCollide (dxSpace *space, void *data, dNearCallback *callback) { dAASSERT (space && callback); diff --git a/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/src/objects.h b/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/src/objects.h index 22c1d80..6aac25f 100644 --- a/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/src/objects.h +++ b/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/src/objects.h @@ -204,6 +204,30 @@ struct dxWorld : public dBase { dReal max_angular_speed; // limit the angular velocity to this magnitude boost::threadpool::pool *threadpool; boost::threadpool::pool *row_threadpool; + //////////////////// AIRC begin //////////////////// + // Added by zenglei@2019-05-23, Modified by YYY@yyy-yy-yy + // [Description] struct parallel info for UpdatePhysics + struct ParallelParameter //Added by zenglei@20190515 + { + /// \brief the parallel type + int type; + + /// \brief the number of threads + int numbers_of_thread; + + void Init() + { + type = 0; + numbers_of_thread = 0; + }; + + void SetParameters(int type_, int threads_) + { + type = type_; + numbers_of_thread = threads_; + }; + }islands_parallel; + //////////////////// AIRC end //////////////////// }; diff --git a/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/src/ode.cpp b/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/src/ode.cpp index 9bff73c..f13305c 100644 --- a/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/src/ode.cpp +++ b/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/src/ode.cpp @@ -126,6 +126,15 @@ static int listHasLoops (dObject *first) return 0; } +//////////////////// AIRC begin //////////////////// + // Added by zenglei@2019-05-23, Modified by YYY@yyy-yy-yy + // [Description] set parallel info for UpdatePhysics +ODE_API void dWorldSetIslandsInfo(dWorldID w, int type, int threads) +{ + dAASSERT(w); + w->islands_parallel.SetParameters(type, threads); +} +//////////////////// AIRC end //////////////////// // check the validity of the world data structures diff --git a/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/src/util.cpp b/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/src/util.cpp index 38236af..fe1f31b 100644 --- a/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/src/util.cpp +++ b/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/src/util.cpp @@ -28,6 +28,7 @@ #include #include #include +#include #undef REPORT_THREAD_TIMING #undef TIMING @@ -623,6 +624,51 @@ void dxProcessIslands (dxWorld *world, dReal stepsize, dstepper_fn_t stepper) cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6; printf(">>>>>>>>>>>> start island spawn threads at time %f\n",cur_time); #endif + //////////////////// AIRC begin //////////////////// + // Added by zenglei@2019-05-23, Modified by YYY@yyy-yy-yy + // [Description] rewrite the logic for UpdatePhysics + if(world->islands_parallel.type > 0) + { + std::vector vecIslandInfo; + for (int const *sizescurr = islandsizes; sizescurr != sizesend; sizescurr += sizeelements) + { + struct dIslandInfo dIs; + dIs.bcount = sizescurr[0]; + dIs.jcount = sizescurr[1]; + dxStepWorkingMemory *island_wmem = world->island_wmems[island_index++]; + dIASSERT(island_wmem != NULL); + dIs.island_context = island_wmem->GetWorldProcessingContext(); + dIs.bodystart = bodystart; + dIs.jointstart = jointstart; + vecIslandInfo.push_back(dIs); + bodystart += dIs.bcount; + jointstart += dIs.jcount; + + } + switch(world->islands_parallel.type) + { + case 1: + { + omp_set_num_threads(world->islands_parallel.numbers_of_thread); + #pragma omp parallel for + for(int i = 0; i < (int)vecIslandInfo.size(); i++) + { + dxProcessOneIsland(vecIslandInfo[i].island_context, world, stepsize, stepper,vecIslandInfo[i].bodystart, vecIslandInfo[i].bcount, vecIslandInfo[i].jointstart, vecIslandInfo[i].jcount); + } + break; + } + // case 2: + // { + // //TBB + // break; + // } + // //TODO...待完善C++11thread等 + default: + break; + } + + }else + { for (int const *sizescurr = islandsizes; sizescurr != sizesend; sizescurr += sizeelements) { int bcount = sizescurr[0]; @@ -655,8 +701,8 @@ void dxProcessIslands (dxWorld *world, dReal stepsize, dstepper_fn_t stepper) #endif IFTIMING(dTimerEnd()); IFTIMING(dTimerReport (stdout,1)); - - + } + //////////////////// AIRC end //////////////////// #ifdef REPORT_THREAD_TIMING gettimeofday(&tv,NULL); double end_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6; diff --git a/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/src/util.h b/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/src/util.h index af7efeb..71304ab 100644 --- a/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/src/util.h +++ b/Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/src/util.h @@ -67,7 +67,18 @@ void dInternalHandleAutoDisabling (dxWorld *world, dReal stepsize); void dxStepBody (dxBody *b, dReal h); - +//////////////////// AIRC begin //////////////////// + // Added by zenglei@2019-05-23, Modified by YYY@yyy-yy-yy + // [Description] struct for rewrite UpdatePhysics +struct dIslandInfo +{ + int bcount; + int jcount; + dxBody *const * bodystart; + dxJoint *const * jointstart; + dxWorldProcessContext *island_context; +}; +//////////////////// AIRC end //////////////////// struct dxWorldProcessMemoryManager: public dBase { diff --git a/Gazebo_exercise/gazebo7_7.14.0_exercise/gazebo/Server.cc b/Gazebo_exercise/gazebo7_7.14.0_exercise/gazebo/Server.cc index 1dadfff..4db32ed 100644 --- a/Gazebo_exercise/gazebo7_7.14.0_exercise/gazebo/Server.cc +++ b/Gazebo_exercise/gazebo7_7.14.0_exercise/gazebo/Server.cc @@ -476,15 +476,15 @@ bool Server::LoadImpl(sdf::ElementPtr _elem, //////////////////// AIRC begin //////////////////// // Added by zenglei@2018-12-26 // Read the parallel parameter configuration in world file - if(_elem->GetElement("world")->HasElement("parallel")) - { - sdf::ElementPtr eventElem = _elem->GetElement("world")->GetElement("parallel"); - event::Events::worldUpdateBegin.SetThreadInfo(eventElem->Get("method"),eventElem->Get("numbers_of_thread"), eventElem->Get("size_of_block"), eventElem->Get("type")); + // if(_elem->GetElement("world")->HasElement("parallel")) + // { + // sdf::ElementPtr eventElem = _elem->GetElement("world")->GetElement("parallel"); + // event::Events::worldUpdateBegin.SetThreadInfo(eventElem->Get("method"),eventElem->Get("numbers_of_thread"), eventElem->Get("size_of_block"), eventElem->Get("type")); - if(6 == eventElem->Get("method")) - event::Events::worldUpdateBegin.InitThreadPool(eventElem->Get("numbers_of_thread")); - // std::cout<<"=======numbers_of_thread: "<Get("numbers_of_thread")<<"\t size_of_block: "<Get("size_of_block")<<"============="<Get("method")) + // // event::Events::worldUpdateBegin.InitThreadPool(eventElem->Get("numbers_of_thread")); + // // std::cout<<"=======numbers_of_thread: "<Get("numbers_of_thread")<<"\t size_of_block: "<Get("size_of_block")<<"============="<dataPtr->node = transport::NodePtr(new transport::Node()); diff --git a/Gazebo_exercise/gazebo7_7.14.0_exercise/gazebo/common/Event.hh b/Gazebo_exercise/gazebo7_7.14.0_exercise/gazebo/common/Event.hh index f35607e..ebdb71c 100644 --- a/Gazebo_exercise/gazebo7_7.14.0_exercise/gazebo/common/Event.hh +++ b/Gazebo_exercise/gazebo7_7.14.0_exercise/gazebo/common/Event.hh @@ -37,14 +37,6 @@ //////////////////// AIRC begin //////////////////// // Added by zenglei@2019-01-09 -#include -#include -#include -#include -#include -#include -#include -#include #include //////////////////// AIRC end //////////////////// @@ -255,8 +247,7 @@ public: //////////////////// AIRC begin //////////////////// // Added by zenglei@2018-12-25 public: - void SetThreadInfo(int method,int thread_num, int block_size, int type); - void InitThreadPool(int thread_num); + void SetThreadInfo(int method,int thread_num); //////////////////// AIRC end //////////////////// /// \brief Access the signal. @@ -518,84 +509,6 @@ public: (*vecCallback[i])(_p); } } - else if(6 == method_) //Threadpool - { - for( int i = 0; i < vecLength; i += 8 ) - { - int start = i; - int end = vecLength; - if(callbackPool && callbackPool->size() > 0) - { - callbackPool->schedule([&vecCallback, start, end,_p]{ - for(int index = start; index < start + 8 && index < end; index++) - (*vecCallback[index])(_p); - }); - } - } - if(callbackPool && callbackPool->size() > 0) - callbackPool->wait(); - } - else if(7 == method_) //C++11 Thread - { - vecThread.clear(); - int length = vecLength / thread_num_; - for(int i = 0; i < thread_num_; i++) - { - int start = i * length; - int end = start + length; - vecThread.push_back(std::thread([&vecCallback, start, end, vecLength, _p](){ - for(int index = start; index < end && index < vecLength; index++) - (*vecCallback[index])(_p); - })); - } - for(auto &thr: vecThread) - thr.join(); - } - else if(8 == method_) //Intel::tbb - { - switch(type_) - { - case 0: - { - parallel_for(tbb::blocked_range(0, vecCallback.size(), block_size_), - [&vecCallback, &_p](const tbb::blocked_range &_r){ - for (size_t i = _r.begin(); i != _r.end(); i++) - { - (*vecCallback[i])(_p); - } - }, tbb::auto_partitioner() - ); - break; - } - case 1: - { - tbb::affinity_partitioner ap; - parallel_for(tbb::blocked_range(0, vecCallback.size(), block_size_), - [&vecCallback, &_p](const tbb::blocked_range &_r){ - for (size_t i = _r.begin(); i != _r.end(); i++) - { - (*vecCallback[i])(_p); - } - }, ap - ); - break; - } - case 2: - { - parallel_for(tbb::blocked_range(0, vecCallback.size(), block_size_), - [&vecCallback, &_p](const tbb::blocked_range &_r){ - for (size_t i = _r.begin(); i != _r.end(); i++) - { - (*vecCallback[i])(_p); - } - }, tbb::simple_partitioner() - ); - break; - } - default: - break; - } - } } //////////////////// AIRC end //////////////////// } @@ -828,12 +741,7 @@ private: private: EventTPrivate *myDataPtr; int thread_num_; - int method_; - int block_size_; - int type_; - - std::vector vecThread; - boost::threadpool::thread_pool< > *callbackPool; + int method_; //////////////////// AIRC end //////////////////// }; @@ -845,12 +753,9 @@ EventT::EventT() { this->myDataPtr = static_cast *>(this->dataPtr); //////////////////// AIRC begin //////////////////// - // Added by zenglei@2018-12-25 - callbackPool = NULL; + // Added by zenglei@2018-12-25 method_ = 0; - thread_num_ = 0; - block_size_ = 8; - type_ = 0; + thread_num_ = 0; //////////////////// AIRC end //////////////////// } @@ -859,13 +764,6 @@ template EventT::~EventT() { this->myDataPtr->connections.clear(); - - if(callbackPool) - { - callbackPool->wait(); - delete callbackPool; - callbackPool = NULL; - } } /// \brief Adds a connection. @@ -908,24 +806,10 @@ unsigned int EventT::ConnectionCount() const //////////////////// AIRC begin //////////////////// //Added by zenglei@2018-12-25 template -void EventT::SetThreadInfo(int method,int thread_num, int block_size, int type) +void EventT::SetThreadInfo(int method,int thread_num) { method_ = method; thread_num_ = thread_num; - block_size_ = block_size; - type_ = type; -} -//////////////////// AIRC end //////////////////// - -//////////////////// AIRC begin //////////////////// -//Added by zenglei@2019-01-09 -template -void EventT::InitThreadPool(int thread_num) -{ - if(callbackPool == NULL) - { - callbackPool = new boost::threadpool::thread_pool< >(thread_num); - } } //////////////////// AIRC end //////////////////// diff --git a/Gazebo_exercise/gazebo7_7.14.0_exercise/gazebo/physics/World.cc b/Gazebo_exercise/gazebo7_7.14.0_exercise/gazebo/physics/World.cc index 1e47fff..7c6f500 100644 --- a/Gazebo_exercise/gazebo7_7.14.0_exercise/gazebo/physics/World.cc +++ b/Gazebo_exercise/gazebo7_7.14.0_exercise/gazebo/physics/World.cc @@ -247,7 +247,114 @@ void World::Load(sdf::ElementPtr _sdf) "~/light/modify"); this->dataPtr->lightFactoryPub = this->dataPtr->node->Advertise( "~/factory/light"); + + +//////////////////// AIRC begin //////////////////// + // Added by zenglei@2019-05-23 + // Read the parallel parameter configuration in world file + std::cout << "______________________________________________________________" << std::endl; + std::cout << "| This is Gazebo-exercis-0.2 |" << std::endl; + std::cout << "| by AIRC-01 |" << std::endl; + std::cout << "| |" << std::endl; + + if(this->dataPtr->sdf->HasElement("exercise_opt")) + { + sdf::ElementPtr optElem = this->dataPtr->sdf->GetElement("exercise_opt"); + // The configuration of worldUpdateBegin optimization + std::cout << "|---------------------- WorldUpdateBegin --------------------|" << std::endl; + if(optElem->GetElement("event_signal")) + { + sdf::ElementPtr eventElem = this->dataPtr->sdf->GetElement("exercise_opt")->GetElement("event_signal"); + int para_type = eventElem->Get("parallel_type"); + switch (para_type) + { + case 1: + std::cout << "| Multithreading event_signal: ON |" << std::endl; + std::cout << "| Using OpenMP with " << eventElem->Get("threads") << " threads |" << std::endl; + break; + default: + std::cout << "| Multithreading event_signal: OFF |" << std::endl; + break; + } + event::Events::worldUpdateBegin.SetThreadInfo(eventElem->Get("parallel_type"),eventElem->Get("threads")); + }else + { + std::cout << "| Multithreading event_signal: OFF |" << std::endl; + event::Events::worldUpdateBegin.SetThreadInfo(0,0); + } + + std::cout << "|---------------------- UpdateCollision ---------------------|" << std::endl; + // The configuration of UpdateCollision optimization + if(optElem->HasElement("collide_space")) + { + collide_space = optElem->Get("collide_space"); + } + else + { + collide_space = 0; + } + + switch (collide_space) + { + case 1: // Using dSweepAndPruneSpace + std::cout << "| Using dSweepAndPruneSpace |" << std::endl; + break; + default: // Using dHashSpace + std::cout << "| Using dHashSpace |" << std::endl; + if(optElem->HasElement("dxhashspace_collide")) + { + number_of_thread = optElem->GetElement("dxhashspace_collide")->Get("threads"); + if (number_of_thread > 1) + { + std::cout << "| Multithreading dHashSpace: ON |" << std::endl; + std::cout << "| Using OpenMP with " << number_of_thread << " threads |" << std::endl; + } + else + { + number_of_thread = 0; + std::cout << "| Multithreading dHashSpace: OFF |" << std::endl; + } + } + break; + } + + std::cout << "|---------------------- UpdatePhysics -----------------------|" << std::endl; + // The configuration of UpdatePhysics optimization + if(optElem->HasElement("updatephysics_dxprocessislands")) + { + sdf::ElementPtr isLandsElem = optElem->GetElement("updatephysics_dxprocessislands"); + parallel_type = isLandsElem->Get("parallel_type"); + threads = isLandsElem->Get("threads"); + + switch (parallel_type) + { + case 1: + std::cout << "| Multithreading dxprocessislands: ON |" << std::endl; + std::cout << "| Using OpenMP with " << threads << " threads |" << std::endl; + break; + default: + std::cout << "| Multithreading dxprocessislands: OFF |" << std::endl; + break; + } + }else + { + std::cout << "| Multithreading dxprocessislands: OFF |" << std::endl; + parallel_type = 0; + threads = 0; + } + + }else + { + parallel_type = 0; + threads = 0; + collide_space = 0; + number_of_thread = 0; + event::Events::worldUpdateBegin.SetThreadInfo(0,0); + std::cout << "| This code is running with NO OPTIMIZATION ! |" << std::endl; + } + std::cout<<"|____________________________________________________________|" << std::endl; + ////////////////// AIRC end //////////////////// // This should come before loading of entities sdf::ElementPtr physicsElem = this->dataPtr->sdf->GetElement("physics"); diff --git a/Gazebo_exercise/gazebo7_7.14.0_exercise/gazebo/physics/World.hh b/Gazebo_exercise/gazebo7_7.14.0_exercise/gazebo/physics/World.hh index 11ecf42..e1ca7cb 100644 --- a/Gazebo_exercise/gazebo7_7.14.0_exercise/gazebo/physics/World.hh +++ b/Gazebo_exercise/gazebo7_7.14.0_exercise/gazebo/physics/World.hh @@ -587,6 +587,21 @@ namespace gazebo /// Friend SimbodyPhysics so that it has access to dataPtr->dirtyPoses private: friend class SimbodyPhysics; + //////////////////// AIRC begin //////////////////// + // Added by XXX@xxx-xx-xx, Modified by YYY@yyy-yy-yy + // [Description] zzzzzzzzzzzzzzzzzzzz + /// the collide space type + public: int collide_space; + + /// the thread number to parallel the module collide + public: int number_of_thread; + + /// the method to optimizatize the UpdatePhysics + public: int parallel_type; + + /// the thread number to parallel the UpdatePhysics + public: int threads; + //////////////////// AIRC end //////////////////// }; /// \} } diff --git a/Gazebo_exercise/gazebo7_7.14.0_exercise/gazebo/physics/ode/ODEPhysics.cc b/Gazebo_exercise/gazebo7_7.14.0_exercise/gazebo/physics/ode/ODEPhysics.cc index 6a36171..8624946 100644 --- a/Gazebo_exercise/gazebo7_7.14.0_exercise/gazebo/physics/ode/ODEPhysics.cc +++ b/Gazebo_exercise/gazebo7_7.14.0_exercise/gazebo/physics/ode/ODEPhysics.cc @@ -147,10 +147,35 @@ ODEPhysics::ODEPhysics(WorldPtr _world) dAllocateODEDataForThread(dAllocateMaskAll); this->dataPtr->worldId = dWorldCreate(); + //////////////////// AIRC begin //////////////////// + // Added by zenglei@2019-05-23, Modified by YYY@yyy-yy-yy + // [Description] Select the type of collide space and set the parallel info for module dxProcessIslands + switch(_world->collide_space) + { + case 0: + { + if(_world->number_of_thread > 1) + { + this->dataPtr->spaceId = dHashSpaceCreate2(0, _world->number_of_thread); + dHashSpaceSetLevels(this->dataPtr->spaceId, -2, 8); + }else + { + this->dataPtr->spaceId = dHashSpaceCreate(0); + dHashSpaceSetLevels(this->dataPtr->spaceId, -2, 8); + } + break; + } + case 1: + { + this->dataPtr->spaceId = dSweepAndPruneSpaceCreate(0, 3); + break; + } + default: + break; + } - this->dataPtr->spaceId = dHashSpaceCreate(0); - dHashSpaceSetLevels(this->dataPtr->spaceId, -2, 8); - + dWorldSetIslandsInfo(this->dataPtr->worldId,_world->parallel_type, _world->threads); + //////////////////// AIRC end //////////////////// this->dataPtr->contactGroup = dJointGroupCreate(0); this->dataPtr->colliders.resize(100);