p81523470
/
p9fj35fl6
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

gazebo-exercise-0.2 version 

1. Add a global exercise-opt sdf label
2. Add OpenMP version of dxProcessIslands()
3. Add choice of SAP collide_space
4. Add OpenMP version of dxHashSpace::collide()
5. Delete threadpool version of event_signal
This commit is contained in:
AIRC exercise 2019-05-24 10:14:20 +08:00
parent 5fd6736eb6
commit 3d9a93497b
17 changed files with 462 additions and 179 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored Normal file
View File

@ -0,0 +1 @@
.hg

2
Gazebo_exercise/gazebo7_7.14.0_exercise/CMakeLists.txt
View File

@ -17,7 +17,7 @@ set (GAZEBO_MINOR_VERSION 14)
set (GAZEBO_PATCH_VERSION 0) set (GAZEBO_PATCH_VERSION 0)
set (GAZEBO_VERSION ${GAZEBO_MAJOR_VERSION}.${GAZEBO_MINOR_VERSION}) 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}") message (STATUS "${PROJECT_NAME} version ${GAZEBO_VERSION_FULL}")

25
Gazebo_exercise/gazebo7_7.14.0_exercise/Exercise_Changelog.md Normal file
View File

@ -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

61
Gazebo_exercise/gazebo7_7.14.0_exercise/Exercise_README.md Normal file
View File

@ -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结尾）中，添加如下所示的优化标签：
```
<world>
...
<exercise_opt>
<event_signal parallel_type=1 threads=2 />
<collide_space>1</collide_space>
<dxhashspace_collide threads=2 />
<updatephysics_dxprocessislands parallel_type=1 threads=2 />
</exercise_opt>
...
</world>
```
标签释义如下：
- <exercise_opt>标签：优化总开关。
- <event_signal>标签：用于设置模型预处理模块并行优化参数，parallel_type为1~5表示OpenMp优化（一般设置为1即可，其他是采用不同的任务并发方式），threads为开启的线程数；parallel_type为其他数字时表示不开启优化。
- <collide_space>标签：用于设置碰撞更新模块中碰撞空间的类型，值为1时表示SAP空间类型，其他数字表示默认的Hash空间类型。
- <dxhashspace_collide>标签：用于设置碰撞更新模块中dxHashSpace::collide模块并行参数，当<collide_space>标签设置为1时，该参数没有意义；其他情况下，threads大于1，表示采用OpenMP并行优化，threads为开启的线程数，否则不开启优化。
- <updatephysics_dxprocessislands>标签：用于设置物理更新模块中dxProcessIslands模块并行参数；parallel_type为1~5表示OpenMp优化（一般设置为1即可，其他是采用不同的任务并发方式），threads为开启的线程数；parallel_type为其他数字时表示不开启优化。

1
Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/include/ode/collision.h
View File

@ -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); ODE_API void dSpaceCollide (dSpaceID space, void *data, dNearCallback *callback);
/** /**
* @brief Determines which geoms from one space may potentially intersect with * @brief Determines which geoms from one space may potentially intersect with
* geoms from another space, and calls the callback function for each candidate * geoms from another space, and calls the callback function for each candidate

3
Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/include/ode/collision_space.h
View File

@ -51,6 +51,7 @@ typedef void dNearCallback (void *data, dGeomID o1, dGeomID o2);
ODE_API dSpaceID dSimpleSpaceCreate (dSpaceID space); ODE_API dSpaceID dSimpleSpaceCreate (dSpaceID space);
ODE_API dSpaceID dHashSpaceCreate (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); 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 dSpaceID dSweepAndPruneSpaceCreate( dSpaceID space, int axisorder );
ODE_API void dSpaceDestroy (dSpaceID); ODE_API void dSpaceDestroy (dSpaceID);
ODE_API void dHashSpaceSetLevels (dSpaceID space, int minlevel, int maxlevel); ODE_API void dHashSpaceSetLevels (dSpaceID space, int minlevel, int maxlevel);

6
Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/include/ode/objects.h
View File

@ -83,7 +83,11 @@ ODE_API dBodyID dWorldGetBody(dWorldID world, int id);
*/ */
ODE_API void dWorldDestroy (dWorldID world); 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. * @brief Set the world's global gravity vector.
* *

151
Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/src/collision_space.cpp
View File

@ -34,6 +34,7 @@ spaces
#include "collision_kernel.h" #include "collision_kernel.h"
#include "collision_space_internal.h" #include "collision_space_internal.h"
#include "util.h" #include "util.h"
#include <omp.h> //Added by zenglei@20190523
#ifdef _MSC_VER #ifdef _MSC_VER
#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" #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_minlevel; // smallest hash table level to put AABBs in
int global_maxlevel; // objects that need a level larger than this will be int global_maxlevel; // objects that need a level larger than this will be
// put in a "big objects" list instead of a hash table // 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);
dxHashSpace (dSpaceID _space, int number); //set the number of thread to OpenMp by zenglei@20190523
void setLevels (int minlevel, int maxlevel); void setLevels (int minlevel, int maxlevel);
void getLevels (int *minlevel, int *maxlevel); void getLevels (int *minlevel, int *maxlevel);
void cleanGeoms(); void cleanGeoms();
void collide (void *data, dNearCallback *callback); void collide (void *data, dNearCallback *callback);
void collide2 (void *data, dxGeom *geom, 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; type = dHashSpaceClass;
global_minlevel = -3; global_minlevel = -3;
global_maxlevel = 10; 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) void dxHashSpace::setLevels (int minlevel, int maxlevel)
{ {
dAASSERT (minlevel <= maxlevel); dAASSERT (minlevel <= maxlevel);
@ -401,7 +415,6 @@ void dxHashSpace::setLevels (int minlevel, int maxlevel)
global_maxlevel = maxlevel; global_maxlevel = maxlevel;
} }
void dxHashSpace::getLevels (int *minlevel, int *maxlevel) void dxHashSpace::getLevels (int *minlevel, int *maxlevel)
{ {
if (minlevel) *minlevel = global_minlevel; if (minlevel) *minlevel = global_minlevel;
@ -423,7 +436,9 @@ void dxHashSpace::cleanGeoms()
lock_count--; 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) void dxHashSpace::collide (void *_data, dNearCallback *callback)
{ {
dAASSERT(this && 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 // same cells for collisions, and then check for other AABBs in all
// intersecting higher level cells. // intersecting higher level cells.
int db[6]; // discrete bounds at current level if(threadsforOpenMp > 0)
for (aabb2=first_aabb; aabb2; aabb2=aabb2->next)
{ {
// we are searching for collisions with aabb std::vector<dxAABB *> vecAABB2;
for (i=0; i<6; i++) db[i] = aabb2->dbounds[i]; for (aabb2=first_aabb; aabb2; aabb2=aabb2->next)
for (int level = aabb2->level; level <= maxlevel; level++) { vecAABB2.push_back(aabb2);
for (int xi = db[0]; xi <= db[1]; xi++) { omp_set_num_threads(threadsforOpenMp);
for (int yi = db[2]; yi <= db[3]; yi++) { #pragma omp parallel for
for (int zi = db[4]; zi <= db[5]; zi++) { for(int index = 0; index < (int)vecAABB2.size(); index++)
// get the hash index {
unsigned long hi = getVirtualAddress (level,xi,yi,zi) % sz; int db[6]; // discrete bounds at current level
// search all nodes at this index int k;
Node *node; // we are searching for collisions with aabb
for (node = table[hi]; node; node=node->next) { for (k=0; k<6; k++) db[k] = vecAABB2[index]->dbounds[k];
// node points to an AABB that may intersect aabb for (int level = vecAABB2[index]->level; level <= maxlevel; level++) {
if (node->aabb == aabb2) continue; for (int xi = db[0]; xi <= db[1]; xi++) {
if (node->aabb->level == level && for (int yi = db[2]; yi <= db[3]; yi++) {
node->x == xi && node->y == yi && node->z == zi) { for (int zi = db[4]; zi <= db[5]; zi++) {
// see if aabb and node->aabb have already been tested // get the hash index
// against each other unsigned long hi = getVirtualAddress (level,xi,yi,zi) % sz;
unsigned char mask; // search all nodes at this index
if (aabb2->index <= node->aabb->index) { Node *node;
i = (aabb2->index * tested_rowsize)+(node->aabb->index >> 3); for (node = table[hi]; node; node=node->next) {
mask = 1 << (node->aabb->index & 7); // 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--; lock_count--;
} }
//////////////////// AIRC end ////////////////////
void dxHashSpace::collide2 (void *_data, dxGeom *geom, void dxHashSpace::collide2 (void *_data, dxGeom *geom,
dNearCallback *callback) dNearCallback *callback)
@ -627,6 +697,10 @@ dxSpace *dHashSpaceCreate (dxSpace *space)
return new dxHashSpace (space); return new dxHashSpace (space);
} }
dxSpace *dHashSpaceCreate2 (dxSpace *space, int number)
{
return new dxHashSpace (space, number);
}
void dHashSpaceSetLevels (dxSpace *space, int minlevel, int maxlevel) void dHashSpaceSetLevels (dxSpace *space, int minlevel, int maxlevel)
{ {
@ -754,7 +828,6 @@ int dSpaceGetClass (dxSpace *space)
return space->type; return space->type;
} }
void dSpaceCollide (dxSpace *space, void *data, dNearCallback *callback) void dSpaceCollide (dxSpace *space, void *data, dNearCallback *callback)
{ {
dAASSERT (space && callback); dAASSERT (space && callback);

24
Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/src/objects.h
View File

@ -204,6 +204,30 @@ struct dxWorld : public dBase {
dReal max_angular_speed; // limit the angular velocity to this magnitude dReal max_angular_speed; // limit the angular velocity to this magnitude
boost::threadpool::pool *threadpool; boost::threadpool::pool *threadpool;
boost::threadpool::pool *row_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 ////////////////////
}; };

9
Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/src/ode.cpp
View File

@ -126,6 +126,15 @@ static int listHasLoops (dObject *first)
return 0; 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 // check the validity of the world data structures

50
Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/src/util.cpp
View File

@ -28,6 +28,7 @@
#include <boost/thread/recursive_mutex.hpp> #include <boost/thread/recursive_mutex.hpp>
#include <boost/bind.hpp> #include <boost/bind.hpp>
#include <ode/timer.h> #include <ode/timer.h>
#include <omp.h>
#undef REPORT_THREAD_TIMING #undef REPORT_THREAD_TIMING
#undef 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; cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
printf(">>>>>>>>>>>> start island spawn threads at time %f\n",cur_time); printf(">>>>>>>>>>>> start island spawn threads at time %f\n",cur_time);
#endif #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<struct dIslandInfo> 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) { for (int const *sizescurr = islandsizes; sizescurr != sizesend; sizescurr += sizeelements) {
int bcount = sizescurr[0]; int bcount = sizescurr[0];
@ -655,8 +701,8 @@ void dxProcessIslands (dxWorld *world, dReal stepsize, dstepper_fn_t stepper)
#endif #endif
IFTIMING(dTimerEnd()); IFTIMING(dTimerEnd());
IFTIMING(dTimerReport (stdout,1)); IFTIMING(dTimerReport (stdout,1));
}
//////////////////// AIRC end ////////////////////
#ifdef REPORT_THREAD_TIMING #ifdef REPORT_THREAD_TIMING
gettimeofday(&tv,NULL); gettimeofday(&tv,NULL);
double end_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6; double end_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;

13
Gazebo_exercise/gazebo7_7.14.0_exercise/deps/opende/src/util.h
View File

@ -67,7 +67,18 @@
void dInternalHandleAutoDisabling (dxWorld *world, dReal stepsize); void dInternalHandleAutoDisabling (dxWorld *world, dReal stepsize);
void dxStepBody (dxBody *b, dReal h); 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: struct dxWorldProcessMemoryManager:
public dBase public dBase
{ {

16
Gazebo_exercise/gazebo7_7.14.0_exercise/gazebo/Server.cc
View File

@ -476,15 +476,15 @@ bool Server::LoadImpl(sdf::ElementPtr _elem,
//////////////////// AIRC begin //////////////////// //////////////////// AIRC begin ////////////////////
// Added by zenglei@2018-12-26 // Added by zenglei@2018-12-26
// Read the parallel parameter configuration in world file // Read the parallel parameter configuration in world file
if(_elem->GetElement("world")->HasElement("parallel")) // if(_elem->GetElement("world")->HasElement("parallel"))
{ // {
sdf::ElementPtr eventElem = _elem->GetElement("world")->GetElement("parallel"); // sdf::ElementPtr eventElem = _elem->GetElement("world")->GetElement("parallel");
event::Events::worldUpdateBegin.SetThreadInfo(eventElem->Get<int>("method"),eventElem->Get<int>("numbers_of_thread"), eventElem->Get<int>("size_of_block"), eventElem->Get<int>("type")); // event::Events::worldUpdateBegin.SetThreadInfo(eventElem->Get<int>("method"),eventElem->Get<int>("numbers_of_thread"), eventElem->Get<int>("size_of_block"), eventElem->Get<int>("type"));
if(6 == eventElem->Get<int>("method")) // //if(6 == eventElem->Get<int>("method"))
event::Events::worldUpdateBegin.InitThreadPool(eventElem->Get<int>("numbers_of_thread")); // // event::Events::worldUpdateBegin.InitThreadPool(eventElem->Get<int>("numbers_of_thread"));
// std::cout<<"=======numbers_of_thread: "<<eventElem->Get<int>("numbers_of_thread")<<"\t size_of_block: "<<eventElem->Get<int>("size_of_block")<<"============="<<std::endl; // // std::cout<<"=======numbers_of_thread: "<<eventElem->Get<int>("numbers_of_thread")<<"\t size_of_block: "<<eventElem->Get<int>("size_of_block")<<"============="<<std::endl;
} // }
//////////////////// AIRC end //////////////////// //////////////////// AIRC end ////////////////////
this->dataPtr->node = transport::NodePtr(new transport::Node()); this->dataPtr->node = transport::NodePtr(new transport::Node());

126
Gazebo_exercise/gazebo7_7.14.0_exercise/gazebo/common/Event.hh
View File

@ -37,14 +37,6 @@
//////////////////// AIRC begin //////////////////// //////////////////// AIRC begin ////////////////////
// Added by zenglei@2019-01-09 // Added by zenglei@2019-01-09
#include <boost/thread.hpp>
#include <boost/threadpool.hpp>
#include <tbb/parallel_for.h>
#include <tbb/blocked_range.h>
#include <sdf/sdf.hh>
#include <thread>
#include <tbb/partitioner.h>
#include <future>
#include <omp.h> #include <omp.h>
//////////////////// AIRC end //////////////////// //////////////////// AIRC end ////////////////////
@ -255,8 +247,7 @@ public:
//////////////////// AIRC begin //////////////////// //////////////////// AIRC begin ////////////////////
// Added by zenglei@2018-12-25 // Added by zenglei@2018-12-25
public: public:
void SetThreadInfo(int method,int thread_num, int block_size, int type); void SetThreadInfo(int method,int thread_num);
void InitThreadPool(int thread_num);
//////////////////// AIRC end //////////////////// //////////////////// AIRC end ////////////////////
/// \brief Access the signal. /// \brief Access the signal.
@ -518,84 +509,6 @@ public:
(*vecCallback[i])(_p); (*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<size_t>(0, vecCallback.size(), block_size_),
[&vecCallback, &_p](const tbb::blocked_range<size_t> &_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<size_t>(0, vecCallback.size(), block_size_),
[&vecCallback, &_p](const tbb::blocked_range<size_t> &_r){
for (size_t i = _r.begin(); i != _r.end(); i++)
{
(*vecCallback[i])(_p);
}
}, ap
);
break;
}
case 2:
{
parallel_for(tbb::blocked_range<size_t>(0, vecCallback.size(), block_size_),
[&vecCallback, &_p](const tbb::blocked_range<size_t> &_r){
for (size_t i = _r.begin(); i != _r.end(); i++)
{
(*vecCallback[i])(_p);
}
}, tbb::simple_partitioner()
);
break;
}
default:
break;
}
}
} }
//////////////////// AIRC end //////////////////// //////////////////// AIRC end ////////////////////
} }
@ -828,12 +741,7 @@ private:
private: private:
EventTPrivate<T> *myDataPtr; EventTPrivate<T> *myDataPtr;
int thread_num_; int thread_num_;
int method_; int method_;
int block_size_;
int type_;
std::vector<std::thread> vecThread;
boost::threadpool::thread_pool< > *callbackPool;
//////////////////// AIRC end //////////////////// //////////////////// AIRC end ////////////////////
}; };
@ -845,12 +753,9 @@ EventT<T>::EventT()
{ {
this->myDataPtr = static_cast<EventTPrivate<T> *>(this->dataPtr); this->myDataPtr = static_cast<EventTPrivate<T> *>(this->dataPtr);
//////////////////// AIRC begin //////////////////// //////////////////// AIRC begin ////////////////////
// Added by zenglei@2018-12-25 // Added by zenglei@2018-12-25
callbackPool = NULL;
method_ = 0; method_ = 0;
thread_num_ = 0; thread_num_ = 0;
block_size_ = 8;
type_ = 0;
//////////////////// AIRC end //////////////////// //////////////////// AIRC end ////////////////////
} }
@ -859,13 +764,6 @@ template <typename T>
EventT<T>::~EventT() EventT<T>::~EventT()
{ {
this->myDataPtr->connections.clear(); this->myDataPtr->connections.clear();
if(callbackPool)
{
callbackPool->wait();
delete callbackPool;
callbackPool = NULL;
}
} }
/// \brief Adds a connection. /// \brief Adds a connection.
@ -908,24 +806,10 @@ unsigned int EventT<T>::ConnectionCount() const
//////////////////// AIRC begin //////////////////// //////////////////// AIRC begin ////////////////////
//Added by zenglei@2018-12-25 //Added by zenglei@2018-12-25
template <typename T> template <typename T>
void EventT<T>::SetThreadInfo(int method,int thread_num, int block_size, int type) void EventT<T>::SetThreadInfo(int method,int thread_num)
{ {
method_ = method; method_ = method;
thread_num_ = thread_num; thread_num_ = thread_num;
block_size_ = block_size;
type_ = type;
}
//////////////////// AIRC end ////////////////////
//////////////////// AIRC begin ////////////////////
//Added by zenglei@2019-01-09
template <typename T>
void EventT<T>::InitThreadPool(int thread_num)
{
if(callbackPool == NULL)
{
callbackPool = new boost::threadpool::thread_pool< >(thread_num);
}
} }
//////////////////// AIRC end //////////////////// //////////////////// AIRC end ////////////////////

107
Gazebo_exercise/gazebo7_7.14.0_exercise/gazebo/physics/World.cc
View File

@ -247,7 +247,114 @@ void World::Load(sdf::ElementPtr _sdf)
"~/light/modify"); "~/light/modify");
this->dataPtr->lightFactoryPub = this->dataPtr->node->Advertise<msgs::Light>( this->dataPtr->lightFactoryPub = this->dataPtr->node->Advertise<msgs::Light>(
"~/factory/light"); "~/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<int>("parallel_type");
switch (para_type)
{
case 1:
std::cout << "| Multithreading event_signal: ON |" << std::endl;
std::cout << "| Using OpenMP with " << eventElem->Get<int>("threads") << " threads |" << std::endl;
break;
default:
std::cout << "| Multithreading event_signal: OFF |" << std::endl;
break;
}
event::Events::worldUpdateBegin.SetThreadInfo(eventElem->Get<int>("parallel_type"),eventElem->Get<int>("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<int>("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<int>("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<int>("parallel_type");
threads = isLandsElem->Get<int>("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 // This should come before loading of entities
sdf::ElementPtr physicsElem = this->dataPtr->sdf->GetElement("physics"); sdf::ElementPtr physicsElem = this->dataPtr->sdf->GetElement("physics");

15
Gazebo_exercise/gazebo7_7.14.0_exercise/gazebo/physics/World.hh
View File

@ -587,6 +587,21 @@ namespace gazebo
/// Friend SimbodyPhysics so that it has access to dataPtr->dirtyPoses /// Friend SimbodyPhysics so that it has access to dataPtr->dirtyPoses
private: friend class SimbodyPhysics; 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 ////////////////////
}; };
/// \} /// \}
} }

31
Gazebo_exercise/gazebo7_7.14.0_exercise/gazebo/physics/ode/ODEPhysics.cc
View File

@ -147,10 +147,35 @@ ODEPhysics::ODEPhysics(WorldPtr _world)
dAllocateODEDataForThread(dAllocateMaskAll); dAllocateODEDataForThread(dAllocateMaskAll);
this->dataPtr->worldId = dWorldCreate(); 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); dWorldSetIslandsInfo(this->dataPtr->worldId,_world->parallel_type, _world->threads);
dHashSpaceSetLevels(this->dataPtr->spaceId, -2, 8); //////////////////// AIRC end ////////////////////
this->dataPtr->contactGroup = dJointGroupCreate(0); this->dataPtr->contactGroup = dJointGroupCreate(0);
this->dataPtr->colliders.resize(100); this->dataPtr->colliders.resize(100);