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

利用OpenMp、TBB、 C++Thread优化模型预处理模块

This commit is contained in:
XunMeng2017 2019-03-25 11:35:23 +08:00
parent fc18a77bd1
commit a194b02935
87 changed files with 6873 additions and 685 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
README
View File

@ -1,33 +1,21 @@
gazebo7.14源码、 gazebo依赖sdformat标签功能包、Hector四旋翼无人机仿真功能包
----------------------------------------
本包包括gazebo7.14源码包，此为原始版本，后续gazebo优化基于此进行开发；gazebo依赖sdformat标签功能包，提供gazebo需要的sdf标签支持；Hector四旋翼无人机仿真功能包，用于30架Hector四旋翼无人机案例gazebo仿真支持
使用说明
------------
一、编译与安装Gazebo
1、cd gazebo
2、mkdir build
3、cd build
4、cmake ../
5、sudo make -jX（X为编译时启用的线程数，其根据CPU核心数确定，不要超过CPU核心数）
6、sudo make install
注意：利用cmake生成makefile期间，可能会出现依赖库缺失的情况，需要利用sudo apt install或源码编译安装方式进行依赖库的安装
二、编译与安装sdformat
1、cd sdformat
2、mkdir build
3、cd build
4、cmake ../
5、sudo make -jX（X为编译时启用的线程数，其根据CPU核心数确定，不要超过CPU核心数）
6、sudo make install
注意：编译安装sdformat包后，需要将sdformat包下sdf文件夹下1.*（1.0、1.2、1.3、1.4、1.5、1.6）标签库拷贝到/usr/share/sdformat目录下。
三、编译运行Hector_Quadrotor_Case四旋翼无人机仿真案例
1、cd Hector_Quadrotor_Case
2、catkin_make
3、source devel/setup.bash
4、roslaunch hector_quadrotor_gazebo hector_quadrotor_one_node_30.launch
注意事项：
1、针对G+R架构下Gazebo7.14上运行30架Hector四旋翼无人机达不到1的情况，基于程序插桩技术，得到Gazebo仿真运行瓶颈——模型预处理模块，该模块由Gazebo事件机制实现。
2、针对该模块采用并行技术进行优化，并行手段包括OpenMp、TBB、线程池、C++11Thtead。在sdformat包中增加<parallel method=1 numbers_of_thread=20 size_of_block=0 type=0 />标签用于设置并行参数。其中parallel标签各属性含义如下：
numbers_of_thread: 使用OpenMp、C++11Thread、线程池优化时指定的线程数量
method:
0 串行
1至5 OPENMP优化(设置为1即可)
6 线程池优化(该功能暂时屏蔽)
7 使用C++11线程进行并行
8 使用TBB进行并行，此时size_of_block表示TBB分块参数，type表示分类器（0：auto_partitioner 1: affinity_partitioner 2: simple_partitioner）
3、针对Gazebo仿真图像感知功能对Gazebo仿真仿真性能产生较大影响的情况，将Gazebo仿真计算与Gazebo图像感知分离，增加<distributed type=0 ip="192.168.1.4" port=8888 flag=0 />标签，用于设置分布式架构下仿真同步参数。其中distributed标签各属性含义如下：
flag:
0 不启用图像感知分离功能
1 启用图像感知分离功能
type:
0 gazebo为仿真计算端
1 gazebo为图像感知端
ip: 图像感知分离情况下远端的IP
port: 图像感知分离情况下远端的端口
4、编译安装sdformat包后，需要将sdformat包下sdf文件夹下1.*（1.0、1.2、1.3、1.4、1.5、1.6）标签库拷贝到/usr/share/sdformat目录下。
5、利用cmake编译gazebo期间，可能会出现依赖库缺失的情况，需利用sudo apt install或源码方式进行缺失依赖库的安装。

59
gazebo/.vscode/settings.json vendored Normal file
View File

@ -0,0 +1,59 @@
{
"files.associations": {
"ostream": "cpp",
"cctype": "cpp",
"clocale": "cpp",
"cmath": "cpp",
"csignal": "cpp",
"cstdarg": "cpp",
"cstddef": "cpp",
"cstdio": "cpp",
"cstdlib": "cpp",
"cstring": "cpp",
"ctime": "cpp",
"cwchar": "cpp",
"cwctype": "cpp",
"array": "cpp",
"atomic": "cpp",
"strstream": "cpp",
"*.tcc": "cpp",
"bitset": "cpp",
"chrono": "cpp",
"codecvt": "cpp",
"complex": "cpp",
"condition_variable": "cpp",
"cstdint": "cpp",
"deque": "cpp",
"list": "cpp",
"unordered_map": "cpp",
"unordered_set": "cpp",
"vector": "cpp",
"exception": "cpp",
"hash_map": "cpp",
"hash_set": "cpp",
"fstream": "cpp",
"functional": "cpp",
"future": "cpp",
"initializer_list": "cpp",
"iomanip": "cpp",
"iosfwd": "cpp",
"iostream": "cpp",
"istream": "cpp",
"limits": "cpp",
"memory": "cpp",
"mutex": "cpp",
"new": "cpp",
"numeric": "cpp",
"ratio": "cpp",
"sstream": "cpp",
"stdexcept": "cpp",
"streambuf": "cpp",
"system_error": "cpp",
"thread": "cpp",
"tuple": "cpp",
"type_traits": "cpp",
"typeindex": "cpp",
"typeinfo": "cpp",
"utility": "cpp"
}
}

2
gazebo/CMakeLists.txt
View File

@ -236,7 +236,7 @@ if (NOT MSVC)
set(UNFILTERED_FLAGS "-std=c++11")
endif()
set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${VALID_CXX_FLAGS} ${UNFILTERED_FLAGS}")
set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${VALID_CXX_FLAGS} ${UNFILTERED_FLAGS} -fopenmp") #-O2 -fopenmp
#################################################
# OS Specific initialization

2
gazebo/deps/CMakeLists.txt
View File

@ -1,5 +1,5 @@
add_subdirectory(opende)
#add_subdirectory(parallel_quickstep) #Added by zenglei@2018-11-27
if (NOT CCD_FOUND)
add_subdirectory(libccd)
endif()

5
gazebo/deps/opende/include/ode/common.h
View File

@ -27,6 +27,11 @@
#include <math.h>
//Add by zenglei@2018-11-21
//#ifndef USE_TPROW
//#define USE_TPROW
//#endif
#ifdef __cplusplus
extern "C" {
#endif

2
gazebo/deps/opende/include/ode/objects.h
View File

@ -398,7 +398,7 @@ ODE_API int dWorldRobustStep (dWorldID, dReal stepsize);
* @ingroup world
*/
ODE_API int dWorldQuickStep (dWorldID w, dReal stepsize);
// ODE_API int dWorldQuickStep1 (dWorldID w, dReal stepsize, double &dxReallocateWorldProcessContextValue, double &dxProcessIslandsValue); //Added by zenglei@20190322
/**
* @brief Converts an impulse to a force.

57
gazebo/deps/opende/src/ode.cpp
View File

@ -41,12 +41,23 @@
#include "odetls.h"
#include "robuststep.h"
#include "gazebo/countTime.hh" //Added by zenglei@2018-12-06
// #include <fstream> //Added by zenglei@2018-12-06
// misc defines
#define ALLOCA dALLOCA16
//****************************************************************************
// utility
// #ifdef USE_COUNT_TIME
// extern double quickStepperTimeValue;
// extern double computeBeforeLcpValue;
// extern double lcpTimeValue;
// extern double updateBodyValue; //Added by zenglei@2018-12-06
// #endif
dObject::dObject(dxWorld *w)
{
@ -1962,6 +1973,7 @@ int dWorldSetStepMemoryManager(dWorldID w, const dWorldStepMemoryFunctionsInfo *
int dWorldStep (dWorldID w, dReal stepsize)
{
// std::cout<<"----------------dWorldStep-------------------\n"<<std::endl; //Added by zenglei@2018-12-06
dUASSERT (w,"bad world argument");
dUASSERT (stepsize > 0,"stepsize must be > 0");
@ -1977,17 +1989,60 @@ int dWorldStep (dWorldID w, dReal stepsize)
return result;
}
// int dWorldQuickStep1 (dWorldID w, dReal stepsize, double &dxReallocateWorldProcessContextValue, double &dxProcessIslandsValue) //Added by zenglei@20190322
// {
// // std::cout<<"----------------dWorldQuickStep-------------------\n"<<std::endl; //Added by zenglei@2018-12-06
// dUASSERT (w,"bad world argument");
// dUASSERT (stepsize > 0,"stepsize must be > 0");
// bool result = false;
// #ifdef USE_COUNT_TIME //Added by zenglei@2018-12-06
// struct timeval tv;
// double cur_time,tmp_time;
// gettimeofday(&tv,NULL);
// cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
// #endif
// if (dxReallocateWorldProcessContext (w, stepsize, &dxEstimateQuickStepMemoryRequirements))
// {
// #ifdef USE_COUNT_TIME //Added by zenglei@2018-12-06
// gettimeofday(&tv,NULL);
// tmp_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
// dxReallocateWorldProcessContextValue += tmp_time - cur_time;
// cur_time = tmp_time;
// #endif
// dxProcessIslands (w, stepsize, &dxQuickStepper);
// #ifdef USE_COUNT_TIME //Added by zenglei@2018-12-06
// gettimeofday(&tv,NULL);
// tmp_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
// dxProcessIslandsValue += tmp_time - cur_time;
// // std::cout<<"dxReallocateValue: "<<dxReallocateWorldProcessContextValue<<std::endl;
// // std::cout<<"dxProcessValue: "<<dxProcessIslandsValue<<std::endl;
// #endif
// result = true;
// }
// return result;
// }
int dWorldQuickStep (dWorldID w, dReal stepsize)
{
// std::cout<<"----------------dWorldQuickStep-------------------\n"<<std::endl; //Added by zenglei@2018-12-06
dUASSERT (w,"bad world argument");
dUASSERT (stepsize > 0,"stepsize must be > 0");
bool result = false;
if (dxReallocateWorldProcessContext (w, stepsize, &dxEstimateQuickStepMemoryRequirements))
{
dxProcessIslands (w, stepsize, &dxQuickStepper);
result = true;
}

78
gazebo/deps/opende/src/quickstep.cpp
View File

@ -43,9 +43,13 @@
#include "quickstep_pgs_lcp.h"
#include "quickstep_update_bodies.h"
#include "gazebo/countTime.hh" //Added by zenglei@2018-12-06
// #include <fstream> //Added by zenglei@2018-12-06
using namespace ode;
using namespace quickstep;
void computeRHSPrecon(dxWorldProcessContext *context, const int m, const int nb,
dRealPtr MOI, dxBody * const *body,
const dReal /*stepsize1*/, dRealMutablePtr /*c*/, dRealMutablePtr J,
@ -100,10 +104,31 @@ void computeRHSPrecon(dxWorldProcessContext *context, const int m, const int nb,
} END_STATE_SAVE(context, tmp2state);
}
#ifdef USE_COUNT_TIME
double quickStepperTimeValue = 0; //Added by zenglei@2018-12-06
double computeBeforeLcpValue = 0;
double lcpTimeValue = 0;
double updateBodyValue = 0;
long nStep = 0;
#endif
// #ifdef USE_COUNT_TIME
// double quickStepperduration = 0; //Added by zhangshuai@2018-12-13
// #endif
void dxQuickStepper (dxWorldProcessContext *context,
dxWorld *world, dxBody * const *body, int nb,
dxJoint * const *_joint, int _nj, dReal stepsize)
{
nStep++;
// std::cout<<"------------------dxQuickStepper-----------------"<<std::endl;
#ifdef USE_COUNT_TIME //Added by zenglei@2018-12-06
struct timeval tv;
double start_time,cur_time,tmp_time;
gettimeofday(&tv,NULL);
start_time = cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
IFTIMING(dTimerStart("preprocessing"));
const dReal stepsize1 = dRecip(stepsize);
@ -130,6 +155,8 @@ void dxQuickStepper (dxWorldProcessContext *context,
dMatrix3 tmp;
dxBody *b_ptr = *bodycurr;
// std::cout<<"------------------body-----------------"<<(bodycurr-body)/sizeof(body[0])<<std::endl; //Added by zhangshuai@2018-12-14
// compute inverse inertia tensor in global frame
dMultiply2_333 (tmp,b_ptr->invI,b_ptr->posr.R);
dMultiply0_333 (invMOIrow,b_ptr->posr.R,tmp);
@ -263,13 +290,14 @@ void dxQuickStepper (dxWorldProcessContext *context,
dSetZero(caccel_erp, nb*6);
dSetZero(lambda, m);
dSetZero(lambda_erp, m);
// std::cout<<"-------------m_value: "<<m<<"---------------"<<std::endl; //added by zenglei@2018.12.18
// Get Joint Information, setup Jacobians by calling getInfo2.
if (m > 0) {
dReal *cfm, *lo, *hi, *rhs, *rhs_erp, *rhs_precon;
dReal *c_v_max;
{
int mlocal = m;
const unsigned jelements = mlocal*12;
@ -499,6 +527,13 @@ void dxQuickStepper (dxWorldProcessContext *context,
dSetZero (lambda_erp,m);
}
#ifdef USE_COUNT_TIME //Added by zenglei@2018-12-06
gettimeofday(&tv,NULL);
tmp_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
computeBeforeLcpValue += tmp_time - cur_time;
cur_time = tmp_time;
#endif
BEGIN_STATE_SAVE(context, lcpstate) {
IFTIMING (dTimerNow ("solving LCP problem"));
// solve the LCP problem and get lambda and invM*constraint_force
@ -519,6 +554,13 @@ void dxQuickStepper (dxWorldProcessContext *context,
);
} END_STATE_SAVE(context, lcpstate);
#ifdef USE_COUNT_TIME //Added by zenglei@2018-12-06
gettimeofday(&tv,NULL);
tmp_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
lcpTimeValue += tmp_time - cur_time;
cur_time = tmp_time;
#endif
if (world->qs.warm_start > 0)
{
@ -549,6 +591,40 @@ void dxQuickStepper (dxWorldProcessContext *context,
m, mfb, body, nb, jointiinfos, nj, stepsize,
lambda, caccel, caccel_erp, Jcopy, invMOI);
#ifdef USE_COUNT_TIME //Added by zenglei@2018-12-06
gettimeofday(&tv,NULL);
tmp_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
updateBodyValue += tmp_time - cur_time;
quickStepperTimeValue += tmp_time - start_time;
// std::cout<<"===============nStep: "<<nStep<<"================"<<std::endl; //Added by zenglei@20190322
if(nStep == 3000000) //Added by zenglei@20190322
{
std::cout<<"QuickStep: "<< quickStepperTimeValue<<"\tcomputeBeforeLcp: "<<computeBeforeLcpValue<<std::endl;
std::cout<<"\tlcpTime: "<<lcpTimeValue<<"\tupdateBody: "<<updateBodyValue<<std::endl;
}
// std::cout<<"QuickStep: "<< quickStepperTimeValue<<"\tcomputeBeforeLcp: "<<computeBeforeLcpValue<<std::endl;
// std::cout<<"\tlcpTime: "<<lcpTimeValue<<"\tupdateBody: "<<updateBodyValue<<std::endl;
// std::ofstream outputquickStepper("/tmp/quickStepperTimeValue");
// outputquickStepper<<quickStepperTimeValue;
// outputquickStepper.close();
// std::ofstream outputcomputeBeforeLcp("/tmp/computeBeforeLcpValue");
// outputcomputeBeforeLcp<<computeBeforeLcpValue;
// outputcomputeBeforeLcp.close();
// std::ofstream outputlcpTime("/tmp/lcpTimeValue");
// outputlcpTime<<lcpTimeValue;
// outputlcpTime.close();
// std::ofstream outputupdateBody("/tmp/updateBodyValue");
// outputupdateBody<<updateBodyValue;
// outputupdateBody.close();
#endif
// #ifdef USE_COUNT_TIME //Added by zhangshuai@2018-12-13
// gettimeofday(&tv,NULL);
// tmp_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
// quickStepperduration = tmp_time - start_time;
// std::cout<<"QuickStepDuration: "<< quickStepperduration<<std::endl;
// #endif
}
size_t dxEstimateQuickStepMemoryRequirements (

6
gazebo/deps/opende/src/quickstep_cg_lcp.cpp
View File

@ -31,9 +31,9 @@
#include "joints/joint.h"
#include "util.h"
#ifndef _WIN32
#include <sys/time.h>
#endif
// #ifndef _WIN32 //Added by zenglei@2018-12-10
// #include <sys/time.h>
// #endif
#include "quickstep_util.h"
#include "quickstep_cg_lcp.h"

3
gazebo/deps/opende/src/quickstep_pgs_lcp.cpp
View File

@ -51,6 +51,7 @@ using namespace ode;
static void* ComputeRows(void *p)
{
// std::cout<<"---------------------ComputeRows---------------------"<<std::endl; //Added by zenglei@2018-12-18
dxPGSLCPParameters *params = (dxPGSLCPParameters *)p;
#ifdef REPORT_THREAD_TIMING
@ -981,7 +982,7 @@ void quickstep::PGS_LCP (dxWorldProcessContext *context,
#endif
)
{
// std::cout<<"----------------PGS_LCP----------------"<<std::endl; //added by zenglei@2018.12.18
// precompute iMJ = inv(M)*J'
dReal *iMJ = context->AllocateArray<dReal> (m*12);
compute_invM_JT (m,J,iMJ,jb,body,invMOI);

64
gazebo/deps/opende/src/quickstep_util.cpp
View File

@ -30,10 +30,11 @@
#include "objects.h"
#include "joints/joint.h"
#include "util.h"
//#include <omp.h> //Add by zenglei@2018-12-03
#ifndef _WIN32
#include <sys/time.h>
#endif
// #ifndef _WIN32 //Added by zenglei@2018-12-10
// #include <sys/time.h>
// #endif
#include "quickstep_util.h"
@ -43,33 +44,54 @@ void quickstep::Multiply1_12q1 (dReal *A, const dReal *B, const dReal *C, int q)
{
dIASSERT (q>0 && A && B && C);
dReal a = 0;
dReal b = 0;
dReal c = 0;
dReal d = 0;
dReal e = 0;
dReal f = 0;
// dReal a = 0; //Closed by zenglei@2018-12-03
// dReal b = 0;
// dReal c = 0;
// dReal d = 0;
// dReal e = 0;
// dReal f = 0;
A[0] = 0; //Added by zenglei@2018-12-10
A[1] = 0;
A[2] = 0;
A[3] = 0;
A[4] = 0;
A[5] = 0;
dReal s;
// Closed by zenglei@2018-12-03
// for(int i=0, k = 0; i<q; k += 12, i++)
// {
// s = C[i]; //C[i] and B[n+k] cannot overlap because its value has been read into a temporary.
// //For the rest of the loop, the only memory dependency (array) is from B[]
// a += B[ k] * s;
// b += B[1+k] * s;
// c += B[2+k] * s;
// d += B[3+k] * s;
// e += B[4+k] * s;
// f += B[5+k] * s;
// }
// A[0] = a;
// A[1] = b;
// A[2] = c;
// A[3] = d;
// A[4] = e;
// A[5] = f;
//Added by zenglei@2018-12-03
// #pragma omp parallel for
for(int i=0, k = 0; i<q; k += 12, i++)
{
s = C[i]; //C[i] and B[n+k] cannot overlap because its value has been read into a temporary.
//For the rest of the loop, the only memory dependency (array) is from B[]
a += B[ k] * s;
b += B[1+k] * s;
c += B[2+k] * s;
d += B[3+k] * s;
e += B[4+k] * s;
f += B[5+k] * s;
A[0] += B[ k] * s;
A[1] += B[1+k] * s;
A[2] += B[2+k] * s;
A[3] += B[3+k] * s;
A[4] += B[4+k] * s;
A[5] += B[5+k] * s;
}
A[0] = a;
A[1] = b;
A[2] = c;
A[3] = d;
A[4] = e;
A[5] = f;
}
//***************************************************************************

2
gazebo/deps/opende/src/quickstep_util.h
View File

@ -86,7 +86,7 @@ typedef dReal *dRealMutablePtr;
// during the solution. depending on the situation, this can help a lot
// or hardly at all, but it doesn't seem to hurt.
// #define RANDOMLY_REORDER_CONSTRAINTS 1
#define RANDOMLY_REORDER_CONSTRAINTS 1
#undef LOCK_WHILE_RANDOMLY_REORDER_CONSTRAINTS
//***************************************************************************

27
gazebo/deps/opende/src/util.cpp
View File

@ -29,6 +29,8 @@
#include <boost/bind.hpp>
#include <ode/timer.h>
// #include "gazebo/countTime.hh" //Added by zenglei@2018-12-06
#undef REPORT_THREAD_TIMING
#undef TIMING
#ifdef TIMING
@ -568,6 +570,13 @@ static size_t BuildIslandsAndEstimateStepperMemoryRequirements(dxWorldProcessCon
// bodies will not be included in the simulation. disabled bodies are
// re-enabled if they are found to be part of an active island.
#ifdef USE_COUNT_TIME //Added by zenglei@2018-12-06
double thread_pool_wait_time = 0;
// gettimeofday(&tv,NULL);
// start_time = cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
void dxProcessOneIsland(dxWorldProcessContext *island_context, dxWorld *world, dReal stepsize, dstepper_fn_t stepper,
dxBody *const* bodystart,
int bcount,
@ -633,7 +642,7 @@ void dxProcessIslands (dxWorld *world, dReal stepsize, dstepper_fn_t stepper)
dIASSERT(island_wmem != NULL);
dxWorldProcessContext *island_context = island_wmem->GetWorldProcessingContext();
#define USE_TPISLAND
#define USE_TPISLAND //Closed by zenglei@2018-11-26
#ifdef USE_TPISLAND
IFTIMING(dTimerNow("scheduling island"));
//printf("debug opende tp %d\n",world->threadpool->size());
@ -648,11 +657,27 @@ void dxProcessIslands (dxWorld *world, dReal stepsize, dstepper_fn_t stepper)
bodystart += bcount;
jointstart += jcount;
}
#ifdef USE_COUNT_TIME //Added by zenglei@2018-12-06
struct timeval tv;
double start_time,cur_time,tmp_time;
gettimeofday(&tv,NULL);
start_time = cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
#ifdef USE_TPISLAND
IFTIMING(dTimerNow("islands wait"));
if (world->threadpool && world->threadpool->size() > 0)
world->threadpool->wait();
#endif
#ifdef USE_COUNT_TIME //Added by zhangshuai@2018-12-13
gettimeofday(&tv,NULL);
tmp_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
thread_pool_wait_time += tmp_time - start_time;
std::cout<<"thread_pool_wait_time: "<< thread_pool_wait_time<<std::endl;
#endif
IFTIMING(dTimerEnd());
IFTIMING(dTimerReport (stdout,1));

1
gazebo/deps/parallel_quickstep/CMakeLists.txt
View File

@ -51,6 +51,7 @@ set(USE_CPU "1")
# uncomment to enable
################################################
# CMake 2.8.0 or greater required for built-in CUDA module
#Opened by zenglei@2018-12-04
#if( ${CMAKE_MINOR_VERSION} GREATER 6.2 )
# if( ${CMAKE_MINOR_VERSION} LESS 8 )
# set(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/CMakeModules/;${CMAKE_MODULE_PATH}")

6
gazebo/deps/parallel_quickstep/src/openmp_kernels.cpp
View File

@ -16,7 +16,7 @@ namespace parallel_ode
template <typename T>
void ompZeroVector( T *buffer, int bufferSize )
{
//#pragma omp parallel for
// #pragma omp parallel for //Opened by zenglei@2018-12-04
for(int index = 0; index < bufferSize; ++index) {
buffer[ index ] = parallel_zero<T>();
}
@ -38,7 +38,7 @@ void ompPGSReduce( typename vec4<T>::Type *fc0,
const Vec4T zeroVec = make_vec4( (T)0.0 );
//#pragma omp parallel for
// #pragma omp parallel for //Opened by zenglei@2018-12-04
for(int index = 0; index < bodySize; ++index) {
int nextIndex = index + reductionStride;
@ -77,7 +77,7 @@ void ompPGSSolve( int4 *bodyIDs,
{
typedef typename vec4<T>::Type Vec4T;
//#pragma omp parallel for
// #pragma omp parallel for //Opened by zenglei@2018-12-04
for(int localIndex = 0; localIndex < numConstraints; ++localIndex) {
const int index = localIndex + offset;

6
gazebo/deps/parallel_quickstep/src/openmp_utils.h
View File

@ -9,13 +9,13 @@ static inline void myAtomicVecAdd(typename vec4<T>::Type& a, typename vec4<T>::T
//#pragma omp critical
a += b;
/*
#pragma omp atomic
#pragma omp atomic //Closed by zenglei@2019-02-11
a.x += b.x;
#pragma omp atomic
#pragma omp atomic //Closed by zenglei@2019-02-11
a.y += b.y;
#pragma omp atomic
#pragma omp atomic //Closed by zenglei@2019-02-11
a.z += b.z;
*/
}

1
gazebo/examples/plugins/camera/camera_move.cc
View File

@ -38,6 +38,7 @@ namespace gazebo
// Called by the world update start event
public: void OnUpdate()
{
std::cout<<"====================CameraMove::OnUpdate================="<<std::endl; //Added by zenglei@20190306
math::Vector3 v(0.03, 0, 0);
math::Pose pose = this->model->GetWorldPose();
v = pose.rot * v;

1
gazebo/examples/plugins/custom_messages/custom_messages.cc
View File

@ -39,6 +39,7 @@ namespace gazebo
// Called by the world update start event
public: void OnUpdate()
{
std::cout<<"=====================CustomMessages::OnUpdate===================="<<std::endl; //Added by zenglei@20190306
my_msgs::Custom msg;
msg.set_my_data("hello");
this->pub->Publish(msg);

1
gazebo/examples/plugins/model_push/model_push.cc
View File

@ -38,6 +38,7 @@ namespace gazebo
// Called by the world update start event
public: void OnUpdate(const common::UpdateInfo & /*_info*/)
{
std::cout<<"======================ModelPush::OnUpdate==================="<<std::endl; //Added by zenglei@20190306
// Apply a small linear velocity to the model.
this->model->SetLinearVel(math::Vector3(.3, 0, 0));
}

1
gazebo/examples/plugins/pr2_pose_test.cc
View File

@ -58,6 +58,7 @@ namespace gazebo
// Called by the world update start event
public: void OnUpdate()
{
std::cout<<"=======================PR2PoseTest::OnUpdate===================="<<std::endl; //Added by zenglei@20190306
this->simTime = this->world->GetSimTime();
math::Pose orig_pose = this->model->GetWorldPose();

1
gazebo/examples/plugins/projector/projector.cc
View File

@ -50,6 +50,7 @@ namespace gazebo
//////////////////////////////////////////////////
public: void OnUpdate()
{
std::cout<<"=====================ProjectorPlugin::OnUpdate==================="<<std::endl; //Added by zenglei@20190306
if (common::Time::GetWallTime() - this->prevTime > common::Time(2, 0))
{
this->state = !this->state;

1
gazebo/examples/plugins/ray_test.cc
View File

@ -58,6 +58,7 @@ namespace gazebo
// Called by the world update start event
public: void OnUpdate()
{
std::cout<<"=====================RayTest::OnUpdate================"<<std::endl; //Added by zenglei@20190306
// do something on update
// gzdbg << "plugin update\n";
}

5
gazebo/gazebo/CMakeLists.txt
View File

@ -1,5 +1,10 @@
############################
#set(FASTRTPS_INCLUDE_DIR /usr/local/include) #Added for fastrtps@20190220
#set(FASTRTPS_LIBDIR /usr/local/lib)
include_directories(SYSTEM
${OPENGL_INCLUDE_DIR}
# ${FASTRTPS_INCLUDE_DIR} #Added for fastrtps@20190220
${OGRE_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS}
${PROTOBUF_INCLUDE_DIR}

64
gazebo/gazebo/Server.cc
View File

@ -59,6 +59,9 @@
#include "gazebo/Master.hh"
#include "gazebo/Server.hh"
#include "gazebo/countTime.hh" //Added by zenglei@2018-12-04
namespace po = boost::program_options;
using namespace gazebo;
@ -131,6 +134,13 @@ void Server::PrintUsage()
/////////////////////////////////////////////////
bool Server::ParseArgs(int _argc, char **_argv)
{
#ifdef USE_COUNT_TIME //Added by zenglei@2018-12-04
struct timeval tv; //Added by zenglei@2018-12-04
double start_time,tmp_time;
gettimeofday(&tv,NULL);
start_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
// Save a copy of argc and argv for consumption by system plugins
this->dataPtr->systemPluginsArgc = _argc;
this->dataPtr->systemPluginsArgv = new char*[_argc];
@ -187,7 +197,7 @@ bool Server::ParseArgs(int _argc, char **_argv)
po::positional_options_description positionalDesc;
positionalDesc.add("world_file", 1).add("pass_through", -1);
try
try //Closed by zenglei@2019-02-11
{
po::store(po::command_line_parser(_argc, _argv).options(desc).positional(
positionalDesc).allow_unregistered().run(), this->dataPtr->vm);
@ -363,6 +373,12 @@ bool Server::ParseArgs(int _argc, char **_argv)
this->ProcessParams();
#ifdef USE_COUNT_TIME //Added by zenglei@2018-12-04
gettimeofday(&tv,NULL); //Added by zenglei@2018-12-04
tmp_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
std::cout<<"Server::ParseArgs: "<<tmp_time - start_time<<std::endl;
#endif
return true;
}
@ -472,7 +488,21 @@ bool Server::LoadImpl(sdf::ElementPtr _elem,
gzthrow("Failed to load the World\n" << e);
}
}
//Added by zenglei@2018-12-26----Begin
// if(_elem->GetElement("world")->HasElement("event"))
if(_elem->GetElement("world")->HasElement("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"));
// if(6 == eventElem->Get<int>("method"))
// 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;
}
else
{
event::Events::worldUpdateBegin.SetThreadInfo(0,0,0,0);
}
//Added by zenglei@2018-12-26----End
this->dataPtr->node = transport::NodePtr(new transport::Node());
this->dataPtr->node->Init("/gazebo");
this->dataPtr->serverSub =
@ -530,6 +560,14 @@ void Server::Fini()
/////////////////////////////////////////////////
void Server::Run()
{
#ifdef USE_COUNT_TIME //Added by zenglei@2018-12-04
struct timeval tv; //Added by zenglei@2018-12-04
double start_time,cur_time,tmp_time;
gettimeofday(&tv,NULL);
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
start_time = cur_time;
#endif
#ifndef _WIN32
// Now that we're about to run, install a signal handler to allow for
// graceful shutdown on Ctrl-C.
@ -567,10 +605,24 @@ void Server::Run()
<< "to unsigned integer\n";
}
}
#ifdef USE_COUNT_TIME //Added by zenglei@2018-12-04
gettimeofday(&tv,NULL); //Added by zenglei@2018-12-04
tmp_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
std::cout<<"This Cost Time: "<<tmp_time-cur_time<<std::endl;
cur_time = tmp_time;
#endif
// Run each world. Each world starts a new thread
physics::run_worlds(iterations);
#ifdef USE_COUNT_TIME //Added by zenglei@2018-12-04
gettimeofday(&tv,NULL); //Added by zenglei@2018-12-04
tmp_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
std::cout<<"physics::run_worlds: "<<tmp_time - cur_time<<"\n";
cur_time = tmp_time;
#endif
this->dataPtr->initialized = true;
// Update the sensors.
@ -583,6 +635,14 @@ void Server::Run()
// Shutdown gazebo
gazebo::shutdown();
#ifdef USE_COUNT_TIME //Added by zenglei@2018-12-04
gettimeofday(&tv,NULL); //Added by zenglei@2018-12-04
tmp_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
std::cout<<"Here Cost Time: "<<tmp_time - cur_time<<std::endl;
std::cout<<"Server::Run: "<<tmp_time - start_time<<"\n";
#endif
}
/////////////////////////////////////////////////

1658
gazebo/gazebo/common/Event.hh
View File

File diff suppressed because it is too large Load Diff

771
gazebo/gazebo/common/Event_1.hh Normal file
View File

@ -0,0 +1,771 @@
/*
* Copyright (C) 2012 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#ifndef _GAZEBO_EVENT_HH_
#define _GAZEBO_EVENT_HH_
#include <atomic>
#include <iostream>
#include <vector>
#include <map>
#include <memory>
#include <mutex>
#include <list>
#include <boost/function.hpp>
#include <boost/bind.hpp>
#include <gazebo/gazebo_config.h>
#include <gazebo/common/Time.hh>
#include <gazebo/common/CommonTypes.hh>
#include <gazebo/math/Helpers.hh>
#include "gazebo/util/system.hh"
// #include "gazebo/countTime.hh" //Added by zhangshuai@2018-12-17
// #include <iostream> //Added by zhangshuai@2018-12-17
namespace gazebo
{
/// \ingroup gazebo_event
/// \brief Event namespace
namespace event
{
/// \addtogroup gazebo_event Events
/// \brief Signals and connections to send and receive event-based
/// triggers.
/// \{
/// \cond
// Private data members for Event class.
// This must be in the header due to templatization.
class GZ_COMMON_VISIBLE EventPrivate
{
// \brief Constructor
public:
EventPrivate();
/// \brief True if the event has been signaled.
public:
bool signaled;
};
/// \endcond
/// \class Event Event.hh common/common.hh
/// \brief Base class for all events
class GZ_COMMON_VISIBLE Event
{
/// \brief Constructor
public:
Event();
/// \brief Destructor
public:
virtual ~Event();
/// \brief Disconnect
/// \param[in] _c A pointer to a connection
public:
virtual void Disconnect(ConnectionPtr _c) = 0;
/// \brief Disconnect
/// \param[in] _id Integer ID of a connection
public:
virtual void Disconnect(int _id) = 0;
/// \brief Get whether this event has been signaled.
/// \return True if the event has been signaled.
public:
bool GetSignaled() const;
/// \brief Allow subclasses to initialize their own data pointer.
/// \param[in] _d Reference to data pointer.
protected:
Event(EventPrivate &_d);
/// \brief Data pointer.
protected:
EventPrivate *dataPtr;
};
/// \cond
// Private data members for Connection class.
class GZ_COMMON_VISIBLE ConnectionPrivate
{
/// \brief Constructor.
public:
ConnectionPrivate();
/// \brief Constructor.
/// \param[in] _e Event pointer to connect with
/// \param[in] _i Unique id
public:
ConnectionPrivate(Event *_e, int _i);
/// \brief the event for this connection
public:
Event *event;
/// \brief the id set in the constructor
public:
int id;
/// \brief set during the constructor
public:
common::Time creationTime;
};
/// \endcond
/// \brief A class that encapsulates a connection.
class GZ_COMMON_VISIBLE Connection
{
/// \brief Constructor.
public:
Connection();
/// \brief Constructor.
/// \param[in] _e Event pointer to connect with.
/// \param[in] _i Unique id.
public:
Connection(Event *_e, int _i);
/// \brief Destructor.
public:
~Connection();
/// \brief Get the id of this connection.
/// \return The id of this connection.
public:
int GetId() const;
/// \brief Private data pointer.
private:
ConnectionPrivate *dataPtr;
/// \brief Friend class.
public:
template <typename T>
friend class EventT;
};
/// \internal
template <typename T>
class EventConnection
{
/// \brief Constructor
public:
EventConnection(const bool _on,
boost::function<T> *_cb)
: callback(_cb)
{
// Windows Visual Studio 2012 does not have atomic_bool constructor,
// so we have to set "on" using operator=
this->on = _on;
}
/// \brief On/off value for the event callback
public:
std::atomic_bool on;
/// \brief Callback function
public:
std::shared_ptr<boost::function<T>> callback;
};
/// \cond
// Private data members for EventT<T> class.
template <typename T>
class EventTPrivate : public EventPrivate
{
/// \def EvtConnectionMap
/// \brief Event Connection map typedef.
typedef std::map<int, std::shared_ptr<EventConnection<T>>>
EvtConnectionMap;
/// \brief Array of connection callbacks.
public:
EvtConnectionMap connections;
/// \brief A thread lock.
public:
std::mutex mutex;
/// \brief List of connections to remove
public:
std::list<typename EvtConnectionMap::const_iterator>
connectionsToRemove;
};
/// \endcond
/// \class EventT Event.hh common/common.hh
/// \brief A class for event processing.
template <typename T>
class EventT : public Event
{
/// \brief Constructor.
public:
EventT();
/// \brief Destructor.
public:
virtual ~EventT();
/// \brief Connect a callback to this event.
/// \param[in] _subscriber Pointer to a callback function.
/// \return A Connection object, which will automatically call
/// Disconnect when it goes out of scope.
public:
ConnectionPtr Connect(const boost::function<T> &_subscriber);
/// \brief Disconnect a callback to this event.
/// \param[in] _c The connection to disconnect.
public:
virtual void Disconnect(ConnectionPtr _c);
/// \brief Disconnect a callback to this event.
/// \param[in] _id The id of the connection to disconnect.
public:
virtual void Disconnect(int _id);
/// \brief Get the number of connections.
/// \return Number of connection to this Event.
public:
unsigned int ConnectionCount() const;
/// \brief Access the signal.
public:
void operator()()
{
this->Signal();
}
/// \brief Signal the event with one parameter.
/// \param[in] _p the parameter.
public:
template <typename P>
void operator()(const P &_p)
{
this->Signal(_p);
}
/// \brief Signal the event with two parameters.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
public:
template <typename P1, typename P2>
void operator()(const P1 &_p1, const P2 &_p2)
{
this->Signal(_p1, _p2);
}
/// \brief Signal the event with three parameters.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
public:
template <typename P1, typename P2, typename P3>
void operator()(const P1 &_p1, const P2 &_p2, const P3 &_p3)
{
this->Signal(_p1, _p2, _p3);
}
/// \brief Signal the event with four parameters.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
/// \param[in] _p4 the first parameter.
public:
template <typename P1, typename P2, typename P3, typename P4>
void operator()(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4)
{
this->Signal(_p1, _p2, _p3, _p4);
}
/// \brief Signal the event with five parameters.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
/// \param[in] _p4 the first parameter.
/// \param[in] _p5 the fift parameter.
public:
template <typename P1, typename P2, typename P3, typename P4,
typename P5>
void operator()(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5)
{
this->Signal(_p1, _p2, _p3, _p4, _p5);
}
/// \brief Signal the event with six parameters.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
/// \param[in] _p4 the first parameter.
/// \param[in] _p5 the fift parameter.
/// \param[in] _p6 the sixt parameter.
public:
template <typename P1, typename P2, typename P3, typename P4,
typename P5, typename P6>
void operator()(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5, const P6 &_p6)
{
this->Signal(_p1, _p2, _p3, _p4, _p5, _p6);
}
/// \brief Signal the event with seven parameters.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
/// \param[in] _p4 the first parameter.
/// \param[in] _p5 the fifth parameter.
/// \param[in] _p6 the sixth parameter.
/// \param[in] _p7 the seventh parameter.
public:
template <typename P1, typename P2, typename P3, typename P4,
typename P5, typename P6, typename P7>
void operator()(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5, const P6 &_p6,
const P7 &_p7)
{
this->Signal(_p1, _p2, _p3, _p4, _p5, _p6, _p7);
}
/// \brief Signal the event with eight parameters.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
/// \param[in] _p4 the first parameter.
/// \param[in] _p5 the fifth parameter.
/// \param[in] _p6 the sixth parameter.
/// \param[in] _p7 the seventh parameter.
/// \param[in] _p8 the eighth parameter.
public:
template <typename P1, typename P2, typename P3, typename P4,
typename P5, typename P6, typename P7, typename P8>
void operator()(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5, const P6 &_p6,
const P7 &_p7, const P8 &_p8)
{
this->Signal(_p1, _p2, _p3, _p4, _p5, _p6, _p7, _p8);
}
/// \brief Signal the event with nine parameters.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
/// \param[in] _p4 the first parameter.
/// \param[in] _p5 the fifth parameter.
/// \param[in] _p6 the sixth parameter.
/// \param[in] _p7 the seventh parameter.
/// \param[in] _p8 the eighth parameter.
/// \param[in] _p9 the ninth parameter.
public:
template <typename P1, typename P2, typename P3, typename P4,
typename P5, typename P6, typename P7, typename P8,
typename P9>
void operator()(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5, const P6 &_p6,
const P7 &_p7, const P8 &_p8, const P9 &_p9)
{
this->Signal(_p1, _p2, _p3, _p4, _p5, _p6, _p7, _p8, _p9);
}
/// \brief Signal the event with ten parameters.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
/// \param[in] _p4 the first parameter.
/// \param[in] _p5 the fifth parameter.
/// \param[in] _p6 the sixth parameter.
/// \param[in] _p7 the seventh parameter.
/// \param[in] _p8 the eighth parameter.
/// \param[in] _p9 the ninth parameter.
/// \param[in] _p10 the tenth parameter.
public:
template <typename P1, typename P2, typename P3, typename P4,
typename P5, typename P6, typename P7, typename P8,
typename P9, typename P10>
void operator()(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5, const P6 &_p6,
const P7 &_p7, const P8 &_p8, const P9 &_p9,
const P10 &_p10)
{
this->Signal(_p1, _p2, _p3, _p4, _p5, _p6, _p7, _p8, _p9, _p10);
}
/// \brief Signal the event for all subscribers.
public:
void Signal()
{
// std::cout<<"------------------Signal0-----------------"<<std::endl; //Added by zhangshuai@2018-12-17
this->Cleanup();
this->myDataPtr->signaled = true;
for (auto iter : this->myDataPtr->connections)
{
if (iter.second->on)
(*iter.second->callback)();
}
}
/// \brief Signal the event with one parameter.
/// \param[in] _p parameter.
public:
template <typename P>
void Signal(const P &_p)
{
// std::cout<<"------------------Signal1-----------------"<<std::endl; //Added by zhangshuai@2018-12-17
this->Cleanup();
this->myDataPtr->signaled = true;
for (auto iter : this->myDataPtr->connections)
{
if (iter.second->on)
(*iter.second->callback)(_p);
}
}
/// \brief Signal the event with two parameter.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
public:
template <typename P1, typename P2>
void Signal(const P1 &_p1, const P2 &_p2)
{
// std::cout<<"------------------Signal2-----------------"<<std::endl; //Added by zhangshuai@2018-12-17
this->Cleanup();
this->myDataPtr->signaled = true;
for (auto iter : this->myDataPtr->connections)
{
if (iter.second->on)
(*iter.second->callback)(_p1, _p2);
}
}
/// \brief Signal the event with three parameter.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
public:
template <typename P1, typename P2, typename P3>
void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3)
{
// std::cout<<"------------------Signal3-----------------"<<std::endl; //Added by zhangshuai@2018-12-17
this->Cleanup();
this->myDataPtr->signaled = true;
for (auto iter : this->myDataPtr->connections)
{
if (iter.second->on)
(*iter.second->callback)(_p1, _p2, _p3);
}
}
/// \brief Signal the event with four parameter.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
/// \param[in] _p4 the first parameter.
public:
template <typename P1, typename P2, typename P3, typename P4>
void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4)
{
// std::cout<<"------------------Signal4-----------------"<<std::endl; //Added by zhangshuai@2018-12-17
this->Cleanup();
this->myDataPtr->signaled = true;
for (auto iter : this->myDataPtr->connections)
{
if (iter.second->on)
(*iter.second->callback)(_p1, _p2, _p3, _p4);
}
}
/// \brief Signal the event with five parameter.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
/// \param[in] _p4 the first parameter.
/// \param[in] _p5 the fifth parameter.
public:
template <typename P1, typename P2, typename P3, typename P4,
typename P5>
void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5)
{
// std::cout<<"------------------Signal5-----------------"<<std::endl; //Added by zhangshuai@2018-12-17
this->Cleanup();
this->myDataPtr->signaled = true;
for (auto iter : this->myDataPtr->connections)
{
if (iter.second->on)
(*iter.second->callback)(_p1, _p2, _p3, _p4, _p5);
}
}
/// \brief Signal the event with six parameter.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
/// \param[in] _p4 the first parameter.
/// \param[in] _p5 the fifth parameter.
/// \param[in] _p6 the sixth parameter.
public:
template <typename P1, typename P2, typename P3, typename P4,
typename P5, typename P6>
void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5, const P6 &_p6)
{
// std::cout<<"------------------Signal6-----------------"<<std::endl; //Added by zhangshuai@2018-12-17
this->Cleanup();
this->myDataPtr->signaled = true;
for (auto iter : this->myDataPtr->connections)
{
if (iter.second->on)
(*iter.second->callback)(_p1, _p2, _p3, _p4, _p5, _p6);
}
}
/// \brief Signal the event with seven parameter.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
/// \param[in] _p4 the first parameter.
/// \param[in] _p5 the fifth parameter.
/// \param[in] _p6 the sixth parameter.
/// \param[in] _p7 the seventh parameter.
public:
template <typename P1, typename P2, typename P3, typename P4,
typename P5, typename P6, typename P7>
void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5, const P6 &_p6, const P7 &_p7)
{
// std::cout<<"------------------Signal7-----------------"<<std::endl; //Added by zhangshuai@2018-12-17
this->Cleanup();
this->myDataPtr->signaled = true;
for (auto iter : this->myDataPtr->connections.begin())
{
if (iter.second->on)
(*iter.second->callback)(_p1, _p2, _p3, _p4, _p5, _p6, _p7);
}
}
/// \brief Signal the event with eight parameter.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
/// \param[in] _p4 the first parameter.
/// \param[in] _p5 the fifth parameter.
/// \param[in] _p6 the sixth parameter.
/// \param[in] _p7 the seventh parameter.
/// \param[in] _p8 the eighth parameter.
public:
template <typename P1, typename P2, typename P3, typename P4,
typename P5, typename P6, typename P7, typename P8>
void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5, const P6 &_p6, const P7 &_p7,
const P8 &_p8)
{
// std::cout<<"------------------Signal8-----------------"<<std::endl; //Added by zhangshuai@2018-12-17
this->Cleanup();
this->myDataPtr->signaled = true;
for (auto iter : this->myDataPtr->connections)
{
if (iter.second->on)
{
(*iter.second->callback)(_p1, _p2, _p3, _p4, _p5, _p6, _p7, _p8);
}
}
}
/// \brief Signal the event with nine parameter.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
/// \param[in] _p4 the first parameter.
/// \param[in] _p5 the fifth parameter.
/// \param[in] _p6 the sixth parameter.
/// \param[in] _p7 the seventh parameter.
/// \param[in] _p8 the eighth parameter.
/// \param[in] _p9 the ninth parameter.
public:
template <typename P1, typename P2, typename P3, typename P4,
typename P5, typename P6, typename P7, typename P8,
typename P9>
void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5, const P6 &_p6, const P7 &_p7,
const P8 &_p8, const P9 &_p9)
{
// std::cout<<"------------------Signal9-----------------"<<std::endl; //Added by zhangshuai@2018-12-17
this->Cleanup();
this->myDataPtr->signaled = true;
for (auto iter : this->myDataPtr->connections)
{
if (iter.second->on)
{
(*iter.second->callback)(
_p1, _p2, _p3, _p4, _p5, _p6, _p7, _p8, _p9);
}
}
}
/// \brief Signal the event with ten parameter.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
/// \param[in] _p4 the first parameter.
/// \param[in] _p5 the fifth parameter.
/// \param[in] _p6 the sixth parameter.
/// \param[in] _p7 the seventh parameter.
/// \param[in] _p8 the eighth parameter.
/// \param[in] _p9 the ninth parameter.
/// \param[in] _p10 the tenth parameter.
public:
template <typename P1, typename P2, typename P3, typename P4,
typename P5, typename P6, typename P7, typename P8,
typename P9, typename P10>
void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5, const P6 &_p6, const P7 &_p7,
const P8 &_p8, const P9 &_p9, const P10 &_p10)
{
// std::cout<<"------------------Signal10-----------------"<<std::endl; //Added by zhangshuai@2018-12-17
this->Cleanup();
this->myDataPtr->signaled = true;
for (auto iter : this->myDataPtr->connections)
{
if (iter.second->on)
{
(*iter.second->callback)(
_p1, _p2, _p3, _p4, _p5, _p6, _p7, _p8, _p9, _p10);
}
}
}
/// \internal
/// \brief Removes queued connections.
/// We assume that this function is called from a Signal function.
private:
void Cleanup();
/// \brief Private data pointer.
private:
EventTPrivate<T> *myDataPtr;
};
/// \brief Constructor.
template <typename T>
EventT<T>::EventT()
: Event(*(new EventTPrivate<T>()))
{
this->myDataPtr = static_cast<EventTPrivate<T> *>(this->dataPtr);
}
/// \brief Destructor. Deletes all the associated connections.
template <typename T>
EventT<T>::~EventT()
{
this->myDataPtr->connections.clear();
}
/// \brief Adds a connection.
/// \param[in] _subscriber the subscriber to connect.
template <typename T>
ConnectionPtr EventT<T>::Connect(const boost::function<T> &_subscriber)
{
// #ifdef USE_COUNT_TIME //Added by zhangshuai@2018-12-17
// struct timeval tv;
// double cur_time, tmp_time;
// gettimeofday(&tv, NULL);
// cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
// #endif
// std::cout<<"------------------Connect-----------------"<<std::endl; //Added by zhangshuai@2018-12-17
int index = 0;
if (!this->myDataPtr->connections.empty())
{
auto const &iter = this->myDataPtr->connections.rbegin();
index = iter->first + 1;
}
this->myDataPtr->connections[index].reset(new EventConnection<T>(true,
new boost::function<T>(_subscriber)));
return ConnectionPtr(new Connection(this, index));
// #ifdef USE_COUNT_TIME //Added by zhangshuai@2018-12-17
// gettimeofday(&tv, NULL);
// tmp_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
// std::cout << "xxx: " << tmp_time - cur_time << std::endl;
// #endif
}
/// \brief Removes a connection.
/// \param[in] _c the connection.
template <typename T>
void EventT<T>::Disconnect(ConnectionPtr _c)
{
if (!_c)
return;
this->Disconnect(_c->GetId());
_c->dataPtr->event = NULL;
_c->dataPtr->id = -1;
}
/// \brief Get the number of connections.
/// \return Number of connections.
template <typename T>
unsigned int EventT<T>::ConnectionCount() const
{
return this->myDataPtr->connections.size();
}
/// \brief Removes a connection.
/// \param[in] _id the connection index.
template <typename T>
void EventT<T>::Disconnect(int _id)
{
// Find the connection
auto const &it = this->myDataPtr->connections.find(_id);
if (it != this->myDataPtr->connections.end())
{
it->second->on = false;
this->myDataPtr->connectionsToRemove.push_back(it);
}
}
/////////////////////////////////////////////
template <typename T>
void EventT<T>::Cleanup()
{
std::lock_guard<std::mutex> lock(this->myDataPtr->mutex);
// Remove all queue connections.
for (auto &conn : this->myDataPtr->connectionsToRemove)
this->myDataPtr->connections.erase(conn);
this->myDataPtr->connectionsToRemove.clear();
}
/// \}
} // namespace event
} // namespace gazebo
#endif

771
gazebo/gazebo/common/Event_2.hh Normal file
View File

@ -0,0 +1,771 @@
/*
* Copyright (C) 2012 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#ifndef _GAZEBO_EVENT_HH_
#define _GAZEBO_EVENT_HH_
#include <atomic>
#include <iostream>
#include <vector>
#include <map>
#include <memory>
#include <mutex>
#include <list>
#include <boost/function.hpp>
#include <boost/bind.hpp>
#include <gazebo/gazebo_config.h>
#include <gazebo/common/Time.hh>
#include <gazebo/common/CommonTypes.hh>
#include <gazebo/math/Helpers.hh>
#include "gazebo/util/system.hh"
// #include "gazebo/countTime.hh" //Added by zhangshuai@2018-12-17
// #include <iostream> //Added by zhangshuai@2018-12-17
namespace gazebo
{
/// \ingroup gazebo_event
/// \brief Event namespace
namespace event
{
/// \addtogroup gazebo_event Events
/// \brief Signals and connections to send and receive event-based
/// triggers.
/// \{
/// \cond
// Private data members for Event class.
// This must be in the header due to templatization.
class GZ_COMMON_VISIBLE EventPrivate
{
// \brief Constructor
public:
EventPrivate();
/// \brief True if the event has been signaled.
public:
bool signaled;
};
/// \endcond
/// \class Event Event.hh common/common.hh
/// \brief Base class for all events
class GZ_COMMON_VISIBLE Event
{
/// \brief Constructor
public:
Event();
/// \brief Destructor
public:
virtual ~Event();
/// \brief Disconnect
/// \param[in] _c A pointer to a connection
public:
virtual void Disconnect(ConnectionPtr _c) = 0;
/// \brief Disconnect
/// \param[in] _id Integer ID of a connection
public:
virtual void Disconnect(int _id) = 0;
/// \brief Get whether this event has been signaled.
/// \return True if the event has been signaled.
public:
bool GetSignaled() const;
/// \brief Allow subclasses to initialize their own data pointer.
/// \param[in] _d Reference to data pointer.
protected:
Event(EventPrivate &_d);
/// \brief Data pointer.
protected:
EventPrivate *dataPtr;
};
/// \cond
// Private data members for Connection class.
class GZ_COMMON_VISIBLE ConnectionPrivate
{
/// \brief Constructor.
public:
ConnectionPrivate();
/// \brief Constructor.
/// \param[in] _e Event pointer to connect with
/// \param[in] _i Unique id
public:
ConnectionPrivate(Event *_e, int _i);
/// \brief the event for this connection
public:
Event *event;
/// \brief the id set in the constructor
public:
int id;
/// \brief set during the constructor
public:
common::Time creationTime;
};
/// \endcond
/// \brief A class that encapsulates a connection.
class GZ_COMMON_VISIBLE Connection
{
/// \brief Constructor.
public:
Connection();
/// \brief Constructor.
/// \param[in] _e Event pointer to connect with.
/// \param[in] _i Unique id.
public:
Connection(Event *_e, int _i);
/// \brief Destructor.
public:
~Connection();
/// \brief Get the id of this connection.
/// \return The id of this connection.
public:
int GetId() const;
/// \brief Private data pointer.
private:
ConnectionPrivate *dataPtr;
/// \brief Friend class.
public:
template <typename T>
friend class EventT;
};
/// \internal
template <typename T>
class EventConnection
{
/// \brief Constructor
public:
EventConnection(const bool _on,
boost::function<T> *_cb)
: callback(_cb)
{
// Windows Visual Studio 2012 does not have atomic_bool constructor,
// so we have to set "on" using operator=
this->on = _on;
}
/// \brief On/off value for the event callback
public:
std::atomic_bool on;
/// \brief Callback function
public:
std::shared_ptr<boost::function<T>> callback;
};
/// \cond
// Private data members for EventT<T> class.
template <typename T>
class EventTPrivate : public EventPrivate
{
/// \def EvtConnectionMap
/// \brief Event Connection map typedef.
typedef std::map<int, std::shared_ptr<EventConnection<T>>>
EvtConnectionMap;
/// \brief Array of connection callbacks.
public:
EvtConnectionMap connections;
/// \brief A thread lock.
public:
std::mutex mutex;
/// \brief List of connections to remove
public:
std::list<typename EvtConnectionMap::const_iterator>
connectionsToRemove;
};
/// \endcond
/// \class EventT Event.hh common/common.hh
/// \brief A class for event processing.
template <typename T>
class EventT : public Event
{
/// \brief Constructor.
public:
EventT();
/// \brief Destructor.
public:
virtual ~EventT();
/// \brief Connect a callback to this event.
/// \param[in] _subscriber Pointer to a callback function.
/// \return A Connection object, which will automatically call
/// Disconnect when it goes out of scope.
public:
ConnectionPtr Connect(const boost::function<T> &_subscriber);
/// \brief Disconnect a callback to this event.
/// \param[in] _c The connection to disconnect.
public:
virtual void Disconnect(ConnectionPtr _c);
/// \brief Disconnect a callback to this event.
/// \param[in] _id The id of the connection to disconnect.
public:
virtual void Disconnect(int _id);
/// \brief Get the number of connections.
/// \return Number of connection to this Event.
public:
unsigned int ConnectionCount() const;
/// \brief Access the signal.
public:
void operator()()
{
this->Signal();
}
/// \brief Signal the event with one parameter.
/// \param[in] _p the parameter.
public:
template <typename P>
void operator()(const P &_p)
{
this->Signal(_p);
}
/// \brief Signal the event with two parameters.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
public:
template <typename P1, typename P2>
void operator()(const P1 &_p1, const P2 &_p2)
{
this->Signal(_p1, _p2);
}
/// \brief Signal the event with three parameters.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
public:
template <typename P1, typename P2, typename P3>
void operator()(const P1 &_p1, const P2 &_p2, const P3 &_p3)
{
this->Signal(_p1, _p2, _p3);
}
/// \brief Signal the event with four parameters.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
/// \param[in] _p4 the first parameter.
public:
template <typename P1, typename P2, typename P3, typename P4>
void operator()(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4)
{
this->Signal(_p1, _p2, _p3, _p4);
}
/// \brief Signal the event with five parameters.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
/// \param[in] _p4 the first parameter.
/// \param[in] _p5 the fift parameter.
public:
template <typename P1, typename P2, typename P3, typename P4,
typename P5>
void operator()(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5)
{
this->Signal(_p1, _p2, _p3, _p4, _p5);
}
/// \brief Signal the event with six parameters.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
/// \param[in] _p4 the first parameter.
/// \param[in] _p5 the fift parameter.
/// \param[in] _p6 the sixt parameter.
public:
template <typename P1, typename P2, typename P3, typename P4,
typename P5, typename P6>
void operator()(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5, const P6 &_p6)
{
this->Signal(_p1, _p2, _p3, _p4, _p5, _p6);
}
/// \brief Signal the event with seven parameters.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
/// \param[in] _p4 the first parameter.
/// \param[in] _p5 the fifth parameter.
/// \param[in] _p6 the sixth parameter.
/// \param[in] _p7 the seventh parameter.
public:
template <typename P1, typename P2, typename P3, typename P4,
typename P5, typename P6, typename P7>
void operator()(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5, const P6 &_p6,
const P7 &_p7)
{
this->Signal(_p1, _p2, _p3, _p4, _p5, _p6, _p7);
}
/// \brief Signal the event with eight parameters.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
/// \param[in] _p4 the first parameter.
/// \param[in] _p5 the fifth parameter.
/// \param[in] _p6 the sixth parameter.
/// \param[in] _p7 the seventh parameter.
/// \param[in] _p8 the eighth parameter.
public:
template <typename P1, typename P2, typename P3, typename P4,
typename P5, typename P6, typename P7, typename P8>
void operator()(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5, const P6 &_p6,
const P7 &_p7, const P8 &_p8)
{
this->Signal(_p1, _p2, _p3, _p4, _p5, _p6, _p7, _p8);
}
/// \brief Signal the event with nine parameters.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
/// \param[in] _p4 the first parameter.
/// \param[in] _p5 the fifth parameter.
/// \param[in] _p6 the sixth parameter.
/// \param[in] _p7 the seventh parameter.
/// \param[in] _p8 the eighth parameter.
/// \param[in] _p9 the ninth parameter.
public:
template <typename P1, typename P2, typename P3, typename P4,
typename P5, typename P6, typename P7, typename P8,
typename P9>
void operator()(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5, const P6 &_p6,
const P7 &_p7, const P8 &_p8, const P9 &_p9)
{
this->Signal(_p1, _p2, _p3, _p4, _p5, _p6, _p7, _p8, _p9);
}
/// \brief Signal the event with ten parameters.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
/// \param[in] _p4 the first parameter.
/// \param[in] _p5 the fifth parameter.
/// \param[in] _p6 the sixth parameter.
/// \param[in] _p7 the seventh parameter.
/// \param[in] _p8 the eighth parameter.
/// \param[in] _p9 the ninth parameter.
/// \param[in] _p10 the tenth parameter.
public:
template <typename P1, typename P2, typename P3, typename P4,
typename P5, typename P6, typename P7, typename P8,
typename P9, typename P10>
void operator()(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5, const P6 &_p6,
const P7 &_p7, const P8 &_p8, const P9 &_p9,
const P10 &_p10)
{
this->Signal(_p1, _p2, _p3, _p4, _p5, _p6, _p7, _p8, _p9, _p10);
}
/// \brief Signal the event for all subscribers.
public:
void Signal()
{
// std::cout<<"------------------Signal0-----------------"<<std::endl; //Added by zhangshuai@2018-12-17
this->Cleanup();
this->myDataPtr->signaled = true;
for (auto iter : this->myDataPtr->connections)
{
if (iter.second->on)
(*iter.second->callback)();
}
}
/// \brief Signal the event with one parameter.
/// \param[in] _p parameter.
public:
template <typename P>
void Signal(const P &_p)
{
// std::cout<<"------------------Signal1-----------------"<<std::endl; //Added by zhangshuai@2018-12-17
this->Cleanup();
this->myDataPtr->signaled = true;
for (auto iter : this->myDataPtr->connections)
{
if (iter.second->on)
(*iter.second->callback)(_p);
}
}
/// \brief Signal the event with two parameter.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
public:
template <typename P1, typename P2>
void Signal(const P1 &_p1, const P2 &_p2)
{
// std::cout<<"------------------Signal2-----------------"<<std::endl; //Added by zhangshuai@2018-12-17
this->Cleanup();
this->myDataPtr->signaled = true;
for (auto iter : this->myDataPtr->connections)
{
if (iter.second->on)
(*iter.second->callback)(_p1, _p2);
}
}
/// \brief Signal the event with three parameter.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
public:
template <typename P1, typename P2, typename P3>
void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3)
{
// std::cout<<"------------------Signal3-----------------"<<std::endl; //Added by zhangshuai@2018-12-17
this->Cleanup();
this->myDataPtr->signaled = true;
for (auto iter : this->myDataPtr->connections)
{
if (iter.second->on)
(*iter.second->callback)(_p1, _p2, _p3);
}
}
/// \brief Signal the event with four parameter.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
/// \param[in] _p4 the first parameter.
public:
template <typename P1, typename P2, typename P3, typename P4>
void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4)
{
// std::cout<<"------------------Signal4-----------------"<<std::endl; //Added by zhangshuai@2018-12-17
this->Cleanup();
this->myDataPtr->signaled = true;
for (auto iter : this->myDataPtr->connections)
{
if (iter.second->on)
(*iter.second->callback)(_p1, _p2, _p3, _p4);
}
}
/// \brief Signal the event with five parameter.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
/// \param[in] _p4 the first parameter.
/// \param[in] _p5 the fifth parameter.
public:
template <typename P1, typename P2, typename P3, typename P4,
typename P5>
void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5)
{
// std::cout<<"------------------Signal5-----------------"<<std::endl; //Added by zhangshuai@2018-12-17
this->Cleanup();
this->myDataPtr->signaled = true;
for (auto iter : this->myDataPtr->connections)
{
if (iter.second->on)
(*iter.second->callback)(_p1, _p2, _p3, _p4, _p5);
}
}
/// \brief Signal the event with six parameter.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
/// \param[in] _p4 the first parameter.
/// \param[in] _p5 the fifth parameter.
/// \param[in] _p6 the sixth parameter.
public:
template <typename P1, typename P2, typename P3, typename P4,
typename P5, typename P6>
void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5, const P6 &_p6)
{
// std::cout<<"------------------Signal6-----------------"<<std::endl; //Added by zhangshuai@2018-12-17
this->Cleanup();
this->myDataPtr->signaled = true;
for (auto iter : this->myDataPtr->connections)
{
if (iter.second->on)
(*iter.second->callback)(_p1, _p2, _p3, _p4, _p5, _p6);
}
}
/// \brief Signal the event with seven parameter.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
/// \param[in] _p4 the first parameter.
/// \param[in] _p5 the fifth parameter.
/// \param[in] _p6 the sixth parameter.
/// \param[in] _p7 the seventh parameter.
public:
template <typename P1, typename P2, typename P3, typename P4,
typename P5, typename P6, typename P7>
void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5, const P6 &_p6, const P7 &_p7)
{
// std::cout<<"------------------Signal7-----------------"<<std::endl; //Added by zhangshuai@2018-12-17
this->Cleanup();
this->myDataPtr->signaled = true;
for (auto iter : this->myDataPtr->connections.begin())
{
if (iter.second->on)
(*iter.second->callback)(_p1, _p2, _p3, _p4, _p5, _p6, _p7);
}
}
/// \brief Signal the event with eight parameter.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
/// \param[in] _p4 the first parameter.
/// \param[in] _p5 the fifth parameter.
/// \param[in] _p6 the sixth parameter.
/// \param[in] _p7 the seventh parameter.
/// \param[in] _p8 the eighth parameter.
public:
template <typename P1, typename P2, typename P3, typename P4,
typename P5, typename P6, typename P7, typename P8>
void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5, const P6 &_p6, const P7 &_p7,
const P8 &_p8)
{
// std::cout<<"------------------Signal8-----------------"<<std::endl; //Added by zhangshuai@2018-12-17
this->Cleanup();
this->myDataPtr->signaled = true;
for (auto iter : this->myDataPtr->connections)
{
if (iter.second->on)
{
(*iter.second->callback)(_p1, _p2, _p3, _p4, _p5, _p6, _p7, _p8);
}
}
}
/// \brief Signal the event with nine parameter.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
/// \param[in] _p4 the first parameter.
/// \param[in] _p5 the fifth parameter.
/// \param[in] _p6 the sixth parameter.
/// \param[in] _p7 the seventh parameter.
/// \param[in] _p8 the eighth parameter.
/// \param[in] _p9 the ninth parameter.
public:
template <typename P1, typename P2, typename P3, typename P4,
typename P5, typename P6, typename P7, typename P8,
typename P9>
void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5, const P6 &_p6, const P7 &_p7,
const P8 &_p8, const P9 &_p9)
{
// std::cout<<"------------------Signal9-----------------"<<std::endl; //Added by zhangshuai@2018-12-17
this->Cleanup();
this->myDataPtr->signaled = true;
for (auto iter : this->myDataPtr->connections)
{
if (iter.second->on)
{
(*iter.second->callback)(
_p1, _p2, _p3, _p4, _p5, _p6, _p7, _p8, _p9);
}
}
}
/// \brief Signal the event with ten parameter.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
/// \param[in] _p3 the second parameter.
/// \param[in] _p4 the first parameter.
/// \param[in] _p5 the fifth parameter.
/// \param[in] _p6 the sixth parameter.
/// \param[in] _p7 the seventh parameter.
/// \param[in] _p8 the eighth parameter.
/// \param[in] _p9 the ninth parameter.
/// \param[in] _p10 the tenth parameter.
public:
template <typename P1, typename P2, typename P3, typename P4,
typename P5, typename P6, typename P7, typename P8,
typename P9, typename P10>
void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5, const P6 &_p6, const P7 &_p7,
const P8 &_p8, const P9 &_p9, const P10 &_p10)
{
// std::cout<<"------------------Signal10-----------------"<<std::endl; //Added by zhangshuai@2018-12-17
this->Cleanup();
this->myDataPtr->signaled = true;
for (auto iter : this->myDataPtr->connections)
{
if (iter.second->on)
{
(*iter.second->callback)(
_p1, _p2, _p3, _p4, _p5, _p6, _p7, _p8, _p9, _p10);
}
}
}
/// \internal
/// \brief Removes queued connections.
/// We assume that this function is called from a Signal function.
private:
void Cleanup();
/// \brief Private data pointer.
private:
EventTPrivate<T> *myDataPtr;
};
/// \brief Constructor.
template <typename T>
EventT<T>::EventT()
: Event(*(new EventTPrivate<T>()))
{
this->myDataPtr = static_cast<EventTPrivate<T> *>(this->dataPtr);
}
/// \brief Destructor. Deletes all the associated connections.
template <typename T>
EventT<T>::~EventT()
{
this->myDataPtr->connections.clear();
}
/// \brief Adds a connection.
/// \param[in] _subscriber the subscriber to connect.
template <typename T>
ConnectionPtr EventT<T>::Connect(const boost::function<T> &_subscriber)
{
// #ifdef USE_COUNT_TIME //Added by zhangshuai@2018-12-17
// struct timeval tv;
// double cur_time, tmp_time;
// gettimeofday(&tv, NULL);
// cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
// #endif
// std::cout<<"------------------Connect-----------------"<<std::endl; //Added by zhangshuai@2018-12-17
int index = 0;
if (!this->myDataPtr->connections.empty())
{
auto const &iter = this->myDataPtr->connections.rbegin();
index = iter->first + 1;
}
this->myDataPtr->connections[index].reset(new EventConnection<T>(true,
new boost::function<T>(_subscriber)));
return ConnectionPtr(new Connection(this, index));
// #ifdef USE_COUNT_TIME //Added by zhangshuai@2018-12-17
// gettimeofday(&tv, NULL);
// tmp_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
// std::cout << "xxx: " << tmp_time - cur_time << std::endl;
// #endif
}
/// \brief Removes a connection.
/// \param[in] _c the connection.
template <typename T>
void EventT<T>::Disconnect(ConnectionPtr _c)
{
if (!_c)
return;
this->Disconnect(_c->GetId());
_c->dataPtr->event = NULL;
_c->dataPtr->id = -1;
}
/// \brief Get the number of connections.
/// \return Number of connections.
template <typename T>
unsigned int EventT<T>::ConnectionCount() const
{
return this->myDataPtr->connections.size();
}
/// \brief Removes a connection.
/// \param[in] _id the connection index.
template <typename T>
void EventT<T>::Disconnect(int _id)
{
// Find the connection
auto const &it = this->myDataPtr->connections.find(_id);
if (it != this->myDataPtr->connections.end())
{
it->second->on = false;
this->myDataPtr->connectionsToRemove.push_back(it);
}
}
/////////////////////////////////////////////
template <typename T>
void EventT<T>::Cleanup()
{
std::lock_guard<std::mutex> lock(this->myDataPtr->mutex);
// Remove all queue connections.
for (auto &conn : this->myDataPtr->connectionsToRemove)
this->myDataPtr->connections.erase(conn);
this->myDataPtr->connectionsToRemove.clear();
}
/// \}
} // namespace event
} // namespace gazebo
#endif

1549
gazebo/gazebo/common/Event_backup.hh Normal file
View File

File diff suppressed because it is too large Load Diff

8
gazebo/gazebo/common/Events.cc
View File

@ -16,8 +16,13 @@
*/
#include "gazebo/common/Events.hh"
// #include "gazebo/countTime.hh" //Added by zhangshuai@2018-12-17
// #include <fstream> //Added by zhangshuai@2018-12-17
// #include <iostream> //Added by zhangshuai@2018-12-17
using namespace gazebo;
using namespace event;
// using namespace std; //Added by zhangshuai@2018-12-17
EventT<void (bool)> Events::pause;
EventT<void ()> Events::step;
@ -29,8 +34,9 @@ EventT<void (std::string)> Events::entityCreated;
EventT<void (std::string, std::string)> Events::setSelectedEntity;
EventT<void (std::string)> Events::addEntity;
EventT<void (std::string)> Events::deleteEntity;
EventT<void (const common::UpdateInfo &)> Events::worldUpdateBegin;
EventT<void (const common::UpdateInfo &)> Events::beforePhysicsUpdate;
EventT<void ()> Events::worldUpdateEnd;

4
gazebo/gazebo/common/Events.hh
View File

@ -136,7 +136,9 @@ namespace gazebo
/// \return a connection
public: template<typename T>
static ConnectionPtr ConnectWorldUpdateBegin(T _subscriber)
{ return worldUpdateBegin.Connect(_subscriber); }
{
// std::cout<<"------------------ConnectWorldUpdateBegin-----------------"<<std::endl; //Added by zhangshuai@2018-12-17
return worldUpdateBegin.Connect(_subscriber); }
/// \brief Disconnect a boost::slot the the world update start signal
/// \param[in] _subscriber the subscriber to this event

13
gazebo/gazebo/countTime.hh Normal file
View File

@ -0,0 +1,13 @@
#ifndef _COUNT_TIME_HH_
#define _COUNT_TIME_HH_
#ifndef _WIN32 //Added by zenglei@2018-12-04
#include <sys/time.h>
#endif
#ifndef USE_COUNT_TIME
#define USE_COUNT_TIME
#endif
#endif

14
gazebo/gazebo/physics/CMakeLists.txt
View File

@ -3,6 +3,7 @@ include (${gazebo_cmake_dir}/GazeboUtils.cmake)
link_directories(
${CCD_LIBRARY_DIRS}
${Boost_LIBRARY_DIRS}
# ${FASTRTPS_LIBDIR} #Added for fastrtps@20190220
)
# Build in ODE by default
@ -84,6 +85,10 @@ set (sources ${sources}
UserCmdManager.cc
World.cc
WorldState.cc
# HelloWorld.cc #Added by zenglei for fastrtps@20190220
# HelloWorldPublisher.cc #Added by zenglei for fastrtps@20190220
# HelloWorldPubSubTypes.cc #Added by zenglei for fastrtps@20190220
# HelloWorldSubscriber.cc #Added by zenglei for fastrtps@20190220
)
set (headers
@ -136,7 +141,12 @@ set (headers
UniversalJoint.hh
UserCmdManager.hh
World.hh
WorldState.hh)
WorldState.hh
# HelloWorld.h #Added by zenglei for fastrtps@20190220
# HelloWorldPublisher.h #Added by zenglei for fastrtps@20190220
# HelloWorldPubSubTypes.h #Added by zenglei for fastrtps@20190220
# HelloWorldSubscriber.h #Added by zenglei for fastrtps@20190220
)
set (physics_headers "" CACHE INTERNAL "physics headers" FORCE)
foreach (hdr ${headers})
@ -153,6 +163,8 @@ target_link_libraries(gazebo_physics
gazebo_util
gazebo_ode
gazebo_opcode
# fastcdr #Added by zenglei for fastrtps@20190220
# fastrtps #Added by zenglei for fastrtps@20190220
${Boost_LIBRARIES}
)

1
gazebo/gazebo/physics/Entity.cc
View File

@ -650,6 +650,7 @@ void Entity::UpdateParameters(sdf::ElementPtr _sdf)
//////////////////////////////////////////////////
void Entity::UpdateAnimation(const common::UpdateInfo &_info)
{
std::cout<<"=====================Entity::UpdateAnimation================"<<std::endl; //Added by zenglei@20190306
common::PoseKeyFrame kf(0);
this->animation->AddTime((_info.simTime - this->prevAnimationTime).Double());

129
gazebo/gazebo/physics/HelloWorld.cc Normal file
View File

@ -0,0 +1,129 @@
// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/*!
* @file HelloWorld.cpp
* This source file contains the definition of the described types in the IDL file.
*
* This file was generated by the tool gen.
*/
#ifdef _WIN32
// Remove linker warning LNK4221 on Visual Studio
namespace { char dummy; }
#endif
#include "HelloWorld.h"
#include <fastcdr/Cdr.h>
using namespace eprosima::fastcdr::exception;
#include <utility>
HelloWorld::HelloWorld()
{
m_index = 0;
}
HelloWorld::~HelloWorld()
{
}
HelloWorld::HelloWorld(const HelloWorld &x)
{
m_index = x.m_index;
m_message = x.m_message;
}
HelloWorld::HelloWorld(HelloWorld &&x)
{
m_index = x.m_index;
m_message = std::move(x.m_message);
}
HelloWorld& HelloWorld::operator=(const HelloWorld &x)
{
m_index = x.m_index;
m_message = x.m_message;
return *this;
}
HelloWorld& HelloWorld::operator=(HelloWorld &&x)
{
m_index = x.m_index;
m_message = std::move(x.m_message);
return *this;
}
size_t HelloWorld::getMaxCdrSerializedSize(size_t current_alignment)
{
size_t initial_alignment = current_alignment;
current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4);
current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4) + 255 + 1;
return current_alignment - initial_alignment;
}
size_t HelloWorld::getCdrSerializedSize(const HelloWorld& data, size_t current_alignment)
{
size_t initial_alignment = current_alignment;
current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4);
current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4) + data.message().size() + 1;
return current_alignment - initial_alignment;
}
void HelloWorld::serialize(eprosima::fastcdr::Cdr &scdr) const
{
scdr << m_index;
scdr << m_message;
}
void HelloWorld::deserialize(eprosima::fastcdr::Cdr &dcdr)
{
dcdr >> m_index;
dcdr >> m_message;
}
size_t HelloWorld::getKeyMaxCdrSerializedSize(size_t current_alignment)
{
size_t current_align = current_alignment;
return current_align;
}
bool HelloWorld::isKeyDefined()
{
return false;
}
void HelloWorld::serializeKey(eprosima::fastcdr::Cdr &/*scdr*/) const
{
}

225
gazebo/gazebo/physics/HelloWorld.h Normal file
View File

@ -0,0 +1,225 @@
// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/*!
* @file HelloWorld.h
* This header file contains the declaration of the described types in the IDL file.
*
* This file was generated by the tool gen.
*/
#ifndef _HelloWorld_H_
#define _HelloWorld_H_
#include <stdint.h>
#include <array>
#include <string>
#include <vector>
#include <ios>
#include <iostream>
#include <fstream>
#if defined(_WIN32)
#if defined(EPROSIMA_USER_DLL_EXPORT)
#define eProsima_user_DllExport __declspec( dllexport )
#else
#define eProsima_user_DllExport
#endif
#else
#include <unistd.h>
#define eProsima_user_DllExport
#endif
#if defined(_WIN32)
#if defined(EPROSIMA_USER_DLL_EXPORT)
#if defined(HelloWorld_SOURCE)
#define HelloWorld_DllAPI __declspec( dllexport )
#else
#define HelloWorld_DllAPI __declspec( dllimport )
#endif // HelloWorld_SOURCE
#else
#define HelloWorld_DllAPI
#endif
#else
#define HelloWorld_DllAPI
#endif // _WIN32
namespace eprosima
{
namespace fastcdr
{
class Cdr;
}
}
/*!
* @brief This class represents the structure HelloWorld defined by the user in the IDL file.
* @ingroup HELLOWORLD
*/
class HelloWorld
{
public:
/*!
* @brief Default constructor.
*/
eProsima_user_DllExport HelloWorld();
/*!
* @brief Default destructor.
*/
eProsima_user_DllExport ~HelloWorld();
/*!
* @brief Copy constructor.
* @param x Reference to the object HelloWorld that will be copied.
*/
eProsima_user_DllExport HelloWorld(const HelloWorld &x);
/*!
* @brief Move constructor.
* @param x Reference to the object HelloWorld that will be copied.
*/
eProsima_user_DllExport HelloWorld(HelloWorld &&x);
/*!
* @brief Copy assignment.
* @param x Reference to the object HelloWorld that will be copied.
*/
eProsima_user_DllExport HelloWorld& operator=(const HelloWorld &x);
/*!
* @brief Move assignment.
* @param x Reference to the object HelloWorld that will be copied.
*/
eProsima_user_DllExport HelloWorld& operator=(HelloWorld &&x);
/*!
* @brief This function sets a value in member index
* @param _index New value for member index
*/
inline eProsima_user_DllExport void index(uint32_t _index)
{
m_index = _index;
}
/*!
* @brief This function returns the value of member index
* @return Value of member index
*/
inline eProsima_user_DllExport uint32_t index() const
{
return m_index;
}
/*!
* @brief This function returns a reference to member index
* @return Reference to member index
*/
inline eProsima_user_DllExport uint32_t& index()
{
return m_index;
}
/*!
* @brief This function copies the value in member message
* @param _message New value to be copied in member message
*/
inline eProsima_user_DllExport void message(const std::string &_message)
{
m_message = _message;
}
/*!
* @brief This function moves the value in member message
* @param _message New value to be moved in member message
*/
inline eProsima_user_DllExport void message(std::string &&_message)
{
m_message = std::move(_message);
}
/*!
* @brief This function returns a constant reference to member message
* @return Constant reference to member message
*/
inline eProsima_user_DllExport const std::string& message() const
{
return m_message;
}
/*!
* @brief This function returns a reference to member message
* @return Reference to member message
*/
inline eProsima_user_DllExport std::string& message()
{
return m_message;
}
/*!
* @brief This function returns the maximum serialized size of an object
* depending on the buffer alignment.
* @param current_alignment Buffer alignment.
* @return Maximum serialized size.
*/
eProsima_user_DllExport static size_t getMaxCdrSerializedSize(size_t current_alignment = 0);
/*!
* @brief This function returns the serialized size of a data depending on the buffer alignment.
* @param data Data which is calculated its serialized size.
* @param current_alignment Buffer alignment.
* @return Serialized size.
*/
eProsima_user_DllExport static size_t getCdrSerializedSize(const HelloWorld& data, size_t current_alignment = 0);
/*!
* @brief This function serializes an object using CDR serialization.
* @param cdr CDR serialization object.
*/
eProsima_user_DllExport void serialize(eprosima::fastcdr::Cdr &cdr) const;
/*!
* @brief This function deserializes an object using CDR serialization.
* @param cdr CDR serialization object.
*/
eProsima_user_DllExport void deserialize(eprosima::fastcdr::Cdr &cdr);
/*!
* @brief This function returns the maximum serialized size of the Key of an object
* depending on the buffer alignment.
* @param current_alignment Buffer alignment.
* @return Maximum serialized size.
*/
eProsima_user_DllExport static size_t getKeyMaxCdrSerializedSize(size_t current_alignment = 0);
/*!
* @brief This function tells you if the Key has been defined for this type
*/
eProsima_user_DllExport static bool isKeyDefined();
/*!
* @brief This function serializes the key members of an object using CDR serialization.
* @param cdr CDR serialization object.
*/
eProsima_user_DllExport void serializeKey(eprosima::fastcdr::Cdr &cdr) const;
private:
uint32_t m_index;
std::string m_message;
};
#endif // _HelloWorld_H_

121
gazebo/gazebo/physics/HelloWorldPubSubTypes.cc Normal file
View File

@ -0,0 +1,121 @@
// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/*!
* @file HelloWorldPubSubTypes.cpp
* This header file contains the implementation of the serialization functions.
*
* This file was generated by the tool fastcdrgen.
*/
#include <fastcdr/FastBuffer.h>
#include <fastcdr/Cdr.h>
#include "HelloWorldPubSubTypes.h"
using namespace eprosima::fastrtps;
using namespace eprosima::fastrtps::rtps;
HelloWorldPubSubType::HelloWorldPubSubType() {
setName("HelloWorld");
m_typeSize = (uint32_t)HelloWorld::getMaxCdrSerializedSize() + 4 /*encapsulation*/;
m_isGetKeyDefined = HelloWorld::isKeyDefined();
m_keyBuffer = (unsigned char*)malloc(HelloWorld::getKeyMaxCdrSerializedSize()>16 ? HelloWorld::getKeyMaxCdrSerializedSize() : 16);
}
HelloWorldPubSubType::~HelloWorldPubSubType() {
if(m_keyBuffer!=nullptr)
free(m_keyBuffer);
}
bool HelloWorldPubSubType::serialize(void *data, SerializedPayload_t *payload) {
HelloWorld *p_type = (HelloWorld*) data;
eprosima::fastcdr::FastBuffer fastbuffer((char*) payload->data, payload->max_size); // Object that manages the raw buffer.
eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN,
eprosima::fastcdr::Cdr::DDS_CDR);
payload->encapsulation = ser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE;
// Serialize encapsulation
ser.serialize_encapsulation();
try
{
p_type->serialize(ser); // Serialize the object:
}
catch(eprosima::fastcdr::exception::NotEnoughMemoryException& /*exception*/)
{
return false;
}
payload->length = (uint32_t)ser.getSerializedDataLength(); //Get the serialized length
return true;
}
bool HelloWorldPubSubType::deserialize(SerializedPayload_t* payload, void* data) {
HelloWorld* p_type = (HelloWorld*) data; //Convert DATA to pointer of your type
eprosima::fastcdr::FastBuffer fastbuffer((char*)payload->data, payload->length); // Object that manages the raw buffer.
eprosima::fastcdr::Cdr deser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN,
eprosima::fastcdr::Cdr::DDS_CDR); // Object that deserializes the data.
// Deserialize encapsulation.
deser.read_encapsulation();
payload->encapsulation = deser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE;
try
{
p_type->deserialize(deser); //Deserialize the object:
}
catch(eprosima::fastcdr::exception::NotEnoughMemoryException& /*exception*/)
{
return false;
}
return true;
}
std::function<uint32_t()> HelloWorldPubSubType::getSerializedSizeProvider(void* data) {
return [data]() -> uint32_t {
return (uint32_t)type::getCdrSerializedSize(*static_cast<HelloWorld*>(data)) + 4 /*encapsulation*/;
};
}
void* HelloWorldPubSubType::createData() {
return (void*)new HelloWorld();
}
void HelloWorldPubSubType::deleteData(void* data) {
delete((HelloWorld*)data);
}
bool HelloWorldPubSubType::getKey(void *data, InstanceHandle_t* handle, bool force_md5) {
if(!m_isGetKeyDefined)
return false;
HelloWorld* p_type = (HelloWorld*) data;
eprosima::fastcdr::FastBuffer fastbuffer((char*)m_keyBuffer,HelloWorld::getKeyMaxCdrSerializedSize()); // Object that manages the raw buffer.
eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::BIG_ENDIANNESS); // Object that serializes the data.
p_type->serializeKey(ser);
if(force_md5 || HelloWorld::getKeyMaxCdrSerializedSize()>16) {
m_md5.init();
m_md5.update(m_keyBuffer,(unsigned int)ser.getSerializedDataLength());
m_md5.finalize();
for(uint8_t i = 0;i<16;++i) {
handle->value[i] = m_md5.digest[i];
}
}
else {
for(uint8_t i = 0;i<16;++i) {
handle->value[i] = m_keyBuffer[i];
}
}
return true;
}

52
gazebo/gazebo/physics/HelloWorldPubSubTypes.h Normal file
View File

@ -0,0 +1,52 @@
// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/*!
* @file HelloWorldPubSubTypes.h
* This header file contains the declaration of the serialization functions.
*
* This file was generated by the tool fastcdrgen.
*/
#ifndef _HELLOWORLD_PUBSUBTYPES_H_
#define _HELLOWORLD_PUBSUBTYPES_H_
#include <fastrtps/TopicDataType.h>
#include "HelloWorld.h"
/*!
* @brief This class represents the TopicDataType of the type HelloWorld defined by the user in the IDL file.
* @ingroup HELLOWORLD
*/
class HelloWorldPubSubType : public eprosima::fastrtps::TopicDataType {
public:
typedef HelloWorld type;
HelloWorldPubSubType();
virtual ~HelloWorldPubSubType();
bool serialize(void *data, eprosima::fastrtps::rtps::SerializedPayload_t *payload);
bool deserialize(eprosima::fastrtps::rtps::SerializedPayload_t *payload, void *data);
std::function<uint32_t()> getSerializedSizeProvider(void* data);
bool getKey(void *data, eprosima::fastrtps::rtps::InstanceHandle_t *ihandle, bool force_md5=false);
void* createData();
void deleteData(void * data);
MD5 m_md5;
unsigned char* m_keyBuffer;
};
#endif // _HelloWorld_PUBSUBTYPE_H_

170
gazebo/gazebo/physics/HelloWorldPublisher.cc Normal file
View File

@ -0,0 +1,170 @@
// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/**
* @file HelloWorldPublisher.cpp
*
*/
#include "HelloWorldPublisher.h"
#include <fastrtps/participant/Participant.h>
#include <fastrtps/attributes/ParticipantAttributes.h>
#include <fastrtps/attributes/PublisherAttributes.h>
#include <fastrtps/publisher/Publisher.h>
#include <fastrtps/transport/TCPv4TransportDescriptor.h>
#include <fastrtps/transport/UDPv4TransportDescriptor.h>
#include <fastrtps/Domain.h>
#include <fastrtps/utils/eClock.h>
#include <fastrtps/utils/IPLocator.h>
using namespace eprosima::fastrtps;
using namespace eprosima::fastrtps::rtps;
HelloWorldPublisher::HelloWorldPublisher():mp_participant(nullptr),
mp_publisher(nullptr)
{
}
bool HelloWorldPublisher::init(const std::string &wan_ip, unsigned short port)
{
stop = false;
m_Hello.index(0);
m_Hello.message("HelloWorld");
ParticipantAttributes PParam;
PParam.rtps.builtin.domainId = 0;
PParam.rtps.builtin.leaseDuration = c_TimeInfinite;
PParam.rtps.builtin.leaseDuration_announcementperiod = Duration_t(5, 0);
PParam.rtps.setName("Participant_pub");
PParam.rtps.useBuiltinTransports = false;
std::shared_ptr<TCPv4TransportDescriptor> descriptor = std::make_shared<TCPv4TransportDescriptor>();
descriptor->wait_for_tcp_negotiation = false;
descriptor->sendBufferSize = 0;
descriptor->receiveBufferSize = 0;
//descriptor->set_WAN_address("127.0.0.1");
if (!wan_ip.empty())
{
descriptor->set_WAN_address(wan_ip);
std::cout << wan_ip << ":" << port << std::endl;
}
descriptor->add_listener_port(port);
PParam.rtps.userTransports.push_back(descriptor);
mp_participant = Domain::createParticipant(PParam);
if (mp_participant == nullptr)
return false;
//REGISTER THE TYPE
Domain::registerType(mp_participant, &m_type);
//CREATE THE PUBLISHER
PublisherAttributes Wparam;
Wparam.topic.topicKind = NO_KEY;
Wparam.topic.topicDataType = "HelloWorld";
Wparam.topic.topicName = "HelloWorldTopicTCP";
Wparam.topic.historyQos.kind = KEEP_LAST_HISTORY_QOS;
Wparam.topic.historyQos.depth = 30;
Wparam.topic.resourceLimitsQos.max_samples = 50;
Wparam.topic.resourceLimitsQos.allocated_samples = 20;
Wparam.times.heartbeatPeriod.seconds = 2;
Wparam.times.heartbeatPeriod.fraction = 200 * 1000 * 1000;
Wparam.qos.m_reliability.kind = RELIABLE_RELIABILITY_QOS;
mp_publisher = Domain::createPublisher(mp_participant, Wparam, (PublisherListener*)&m_listener);
if (mp_publisher == nullptr)
return false;
return true;
}
HelloWorldPublisher::~HelloWorldPublisher()
{
Domain::removeParticipant(mp_participant);
}
void HelloWorldPublisher::PubListener::onPublicationMatched(Publisher*, MatchingInfo& info)
{
if (info.status == MATCHED_MATCHING)
{
n_matched++;
firstConnected = true;
//logError(HW, "Matched");
std::cout << "[RTCP] Publisher matched" << std::endl;
}
else
{
n_matched--;
std::cout << "[RTCP] Publisher unmatched" << std::endl;
}
}
void HelloWorldPublisher::runThread(uint32_t samples, long sleep_ms)
{
if (samples == 0)
{
while (!stop)
{
if (publish(false))
{
//logError(HW, "SENT " << m_Hello.index());
std::cout << "[RTCP] Message: " << m_Hello.message() << " with index: " << m_Hello.index() << " SENT" << std::endl;
}
eClock::my_sleep(sleep_ms);
}
}
else
{
for (uint32_t i = 0; i < samples; ++i)
{
if (!publish())
--i;
else
{
std::cout << "[RTCP] Message: " << m_Hello.message() << " with index: " << m_Hello.index() << " SENT" << std::endl;
}
eClock::my_sleep(sleep_ms);
}
}
}
void HelloWorldPublisher::run(uint32_t samples, long sleep_ms)
{
std::thread thread(&HelloWorldPublisher::runThread, this, samples, sleep_ms);
if (samples == 0)
{
std::cout << "Publisher running. Please press enter to stop the Publisher at any time." << std::endl;
std::cin.ignore();
stop = true;
}
else
{
std::cout << "Publisher running " << samples << " samples." << std::endl;
}
thread.join();
}
bool HelloWorldPublisher::publish(bool waitForListener)
{
if (m_listener.firstConnected || !waitForListener || m_listener.n_matched > 0)
{
m_Hello.index(m_Hello.index() + 1);
mp_publisher->write((void*)&m_Hello);
return true;
}
return false;
}

62
gazebo/gazebo/physics/HelloWorldPublisher.h Normal file
View File

@ -0,0 +1,62 @@
// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/**
* @file HelloWorldPublisher.h
*
*/
#ifndef HELLOWORLDPUBLISHER_H_
#define HELLOWORLDPUBLISHER_H_
#include "HelloWorldPubSubTypes.h"
#include <fastrtps/fastrtps_fwd.h>
#include <fastrtps/attributes/PublisherAttributes.h>
#include <fastrtps/publisher/PublisherListener.h>
#include "HelloWorld.h"
class HelloWorldPublisher {
public:
HelloWorldPublisher();
virtual ~HelloWorldPublisher();
//!Initialize
bool init(const std::string &wan_ip, unsigned short port);
//!Publish a sample
bool publish(bool waitForListener = true);
//!Run for number samples
void run(uint32_t number, long sleep_ms);
private:
HelloWorld m_Hello;
eprosima::fastrtps::Participant* mp_participant;
eprosima::fastrtps::Publisher* mp_publisher;
bool stop;
class PubListener :public eprosima::fastrtps::PublisherListener
{
public:
PubListener() :n_matched(0), firstConnected(false) {};
~PubListener() {};
void onPublicationMatched(eprosima::fastrtps::Publisher* pub, eprosima::fastrtps::rtps::MatchingInfo& info);
int n_matched;
bool firstConnected;
}m_listener;
HelloWorldPubSubType m_type;
void runThread(uint32_t number, long sleep_ms);
};
#endif /* HELLOWORLDPUBLISHER_H_ */

13
gazebo/gazebo/physics/HelloWorldPublisher.xml Normal file
View File

@ -0,0 +1,13 @@
<staticdiscovery>
<participant>
<name>HelloWorldPublisher</name>
<writer>
<userId>1</userId>
<entityID>2</entityID>
<topicName>HelloWorldTopic1</topicName>
<topicDataType>HelloWorld</topicDataType>
<topicKind>NO_KEY</topicKind>
<reliabilityQos>RELIABLE_RELIABILITY_QOS</reliabilityQos>
</writer>
</participant>
</staticdiscovery>

142
gazebo/gazebo/physics/HelloWorldSubscriber.cc Normal file
View File

@ -0,0 +1,142 @@
// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/**
* @file HelloWorldSubscriber.cpp
*
*/
#include "HelloWorldSubscriber.h"
#include <fastrtps/participant/Participant.h>
#include <fastrtps/attributes/ParticipantAttributes.h>
#include <fastrtps/attributes/SubscriberAttributes.h>
#include <fastrtps/transport/UDPv4TransportDescriptor.h>
#include <fastrtps/transport/TCPv4TransportDescriptor.h>
#include <fastrtps/subscriber/Subscriber.h>
#include <fastrtps/Domain.h>
#include <fastrtps/utils/eClock.h>
#include <fastrtps/utils/IPLocator.h>
using namespace eprosima::fastrtps;
using namespace eprosima::fastrtps::rtps;
HelloWorldSubscriber::HelloWorldSubscriber()
: mp_participant(nullptr)
, mp_subscriber(nullptr)
{
}
bool HelloWorldSubscriber::init(const std::string &wan_ip, unsigned short port)
{
ParticipantAttributes PParam;
int32_t kind = LOCATOR_KIND_TCPv4;
Locator_t initial_peer_locator;
initial_peer_locator.kind = kind;
if (!wan_ip.empty())
{
IPLocator::setIPv4(initial_peer_locator, wan_ip);
std::cout << wan_ip << ":" << port << std::endl;
}
else
{
IPLocator::setIPv4(initial_peer_locator, "127.0.0.1");
}
initial_peer_locator.port = port;
PParam.rtps.builtin.initialPeersList.push_back(initial_peer_locator); // Publisher's meta channel
PParam.rtps.builtin.domainId = 0;
PParam.rtps.builtin.leaseDuration = c_TimeInfinite;
PParam.rtps.builtin.leaseDuration_announcementperiod = Duration_t(5, 0);
PParam.rtps.setName("Participant_sub");
PParam.rtps.useBuiltinTransports = false;
std::shared_ptr<TCPv4TransportDescriptor> descriptor = std::make_shared<TCPv4TransportDescriptor>();
descriptor->wait_for_tcp_negotiation = false;
PParam.rtps.userTransports.push_back(descriptor);
mp_participant = Domain::createParticipant(PParam);
if (mp_participant == nullptr)
return false;
//REGISTER THE TYPE
Domain::registerType(mp_participant, &m_type);
//CREATE THE SUBSCRIBER
SubscriberAttributes Rparam;
Rparam.topic.topicKind = NO_KEY;
Rparam.topic.topicDataType = "HelloWorld";
Rparam.topic.topicName = "HelloWorldTopicTCP";
Rparam.topic.historyQos.kind = KEEP_LAST_HISTORY_QOS;
Rparam.topic.historyQos.depth = 30;
Rparam.topic.resourceLimitsQos.max_samples = 50;
Rparam.topic.resourceLimitsQos.allocated_samples = 20;
Rparam.qos.m_reliability.kind = RELIABLE_RELIABILITY_QOS;
Rparam.qos.m_durability.kind = TRANSIENT_LOCAL_DURABILITY_QOS;
mp_subscriber = Domain::createSubscriber(mp_participant, Rparam, (SubscriberListener*)&m_listener);
if (mp_subscriber == nullptr)
return false;
return true;
}
HelloWorldSubscriber::~HelloWorldSubscriber()
{
Domain::removeParticipant(mp_participant);
}
void HelloWorldSubscriber::SubListener::onSubscriptionMatched(Subscriber*, MatchingInfo& info)
{
if (info.status == MATCHED_MATCHING)
{
n_matched++;
//logError(HW, "Matched");
std::cout << "[RTCP] Subscriber matched" << std::endl;
}
else
{
n_matched--;
std::cout << "[RTCP] Subscriber unmatched" << std::endl;
}
}
void HelloWorldSubscriber::SubListener::onNewDataMessage(Subscriber* sub)
{
if (sub->takeNextData((void*)&m_Hello, &m_info))
{
if (m_info.sampleKind == ALIVE)
{
this->n_samples++;
// Print your structure data here.
//logError(HW, "RECEIVED " << m_Hello.index());
std::cout << "[RTCP] Message " << m_Hello.message() << " " << m_Hello.index() << " RECEIVED" << std::endl;
}
}
}
void HelloWorldSubscriber::run()
{
std::cout << "[RTCP] Subscriber running. Please press enter to stop the Subscriber" << std::endl;
std::cin.ignore();
}
void HelloWorldSubscriber::run(uint32_t number)
{
std::cout << "[RTCP] Subscriber running until " << number << "samples have been received" << std::endl;
while (number < this->m_listener.n_samples)
eClock::my_sleep(500);
}

65
gazebo/gazebo/physics/HelloWorldSubscriber.h Normal file
View File

@ -0,0 +1,65 @@
// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/**
* @file HelloWorldSubscriber.h
*
*/
#ifndef HELLOWORLDSUBSCRIBER_H_
#define HELLOWORLDSUBSCRIBER_H_
#include "HelloWorldPubSubTypes.h"
#include <fastrtps/fastrtps_fwd.h>
#include <fastrtps/attributes/SubscriberAttributes.h>
#include <fastrtps/subscriber/SubscriberListener.h>
#include <fastrtps/subscriber/SampleInfo.h>
#include "HelloWorld.h"
class HelloWorldSubscriber {
public:
HelloWorldSubscriber();
virtual ~HelloWorldSubscriber();
//!Initialize the subscriber
bool init(const std::string &wan_ip, unsigned short port);
//!RUN the subscriber
void run();
//!Run the subscriber until number samples have been recevied.
void run(uint32_t number);
private:
eprosima::fastrtps::Participant* mp_participant;
eprosima::fastrtps::Subscriber* mp_subscriber;
public:
class SubListener :public eprosima::fastrtps::SubscriberListener
{
public:
SubListener() :n_matched(0), n_samples(0) {};
~SubListener() {};
void onSubscriptionMatched(eprosima::fastrtps::Subscriber* sub, eprosima::fastrtps::rtps::MatchingInfo& info);
void onNewDataMessage(eprosima::fastrtps::Subscriber* sub);
HelloWorld m_Hello;
eprosima::fastrtps::SampleInfo_t m_info;
int n_matched;
uint32_t n_samples;
}m_listener;
private:
HelloWorldPubSubType m_type;
};
#endif /* HELLOWORLDSUBSCRIBER_H_ */

13
gazebo/gazebo/physics/HelloWorldSubscriber.xml Normal file
View File

@ -0,0 +1,13 @@
<staticdiscovery>
<participant>
<name>HelloWorldSubscriber</name>
<reader>
<userId>3</userId>
<entityID>4</entityID>
<topicName>HelloWorldTopic1</topicName>
<topicDataType>HelloWorld</topicDataType>
<topicKind>NO_KEY</topicKind>
<reliabilityQos>RELIABLE_RELIABILITY_QOS</reliabilityQos>
</reader>
</participant>
</staticdiscovery>

1
gazebo/gazebo/physics/Link.cc
View File

@ -493,6 +493,7 @@ void Link::SetLaserRetro(float _retro)
//////////////////////////////////////////////////
void Link::Update(const common::UpdateInfo & /*_info*/)
{
// std::cout<<"=====================Link::Update================"<<std::endl; //Added by zenglei@20190306
#ifdef HAVE_OPENAL
if (this->audioSink)
{

756
gazebo/gazebo/physics/World.cc
View File

@ -84,6 +84,15 @@
#include "gazebo/physics/ContactManager.hh"
#include "gazebo/physics/Population.hh"
// #define DEST_IP_ADDRESS "192.168.1.4" //Added by zenglei@2019-01-22
// #define DEST_PORT 6666 //Added by zenglei@2019-01-22
#include "gazebo/countTime.hh" //Added by zenglei@2018-12-04
#include <thread> //Added by zenglei@2018-12-10
// #include <atomic> //Added by zenglei@2018-12-10
// #define THREAD_NUM 10 //Added by zenglei@2018-12-10
using namespace gazebo;
using namespace physics;
@ -157,6 +166,21 @@ World::World(const std::string &_name)
this->dataPtr->connections.push_back(
event::Events::ConnectPause(
boost::bind(&World::SetPaused, this, _1)));
// sock_fd = socket(AF_INET, SOCK_DGRAM, 0);
// if(sock_fd < 0)
// {
// perror("=================socket error================\n");
// return;
// }
// memset(&addr_serv, 0, sizeof(addr_serv));
// addr_serv.sin_family = AF_INET;
// addr_serv.sin_addr.s_addr = inet_addr(DEST_IP_ADDRESS);
// addr_serv.sin_port = htons(DEST_PORT);
// len = sizeof(addr_serv);
// if(mypub.init("192.168.1.4", 8888)) //Added by zenglei@20190219
// {
// mypub.run(10, 1);
// }
}
//////////////////////////////////////////////////
@ -166,6 +190,7 @@ World::~World()
delete this->dataPtr;
this->dataPtr = nullptr;
// Domain::stopAll();
}
//////////////////////////////////////////////////
@ -247,6 +272,22 @@ void World::Load(sdf::ElementPtr _sdf)
"~/light/modify");
this->dataPtr->lightFactoryPub = this->dataPtr->node->Advertise<msgs::Light>(
"~/factory/light");
//Added by zenglei@2019-01-23----Begin
if(this->dataPtr->sdf->HasElement("distributed"))
{
sdf::ElementPtr eventElem = this->dataPtr->sdf->GetElement("distributed");
type_ = eventElem->Get<int>("type");
ip = eventElem->Get<std::string>("ip");
port = eventElem->Get<int>("port");
flag = eventElem->Get<int>("flag");
// std::cout<<"Server IP: "<<ip<<"\t Server Port: "<<port<<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;
}
else
{
flag = 0;
}
//Added by zenglei@2019-01-23----End
// This should come before loading of entities
sdf::ElementPtr physicsElem = this->dataPtr->sdf->GetElement("physics");
@ -298,11 +339,11 @@ void World::Load(sdf::ElementPtr _sdf)
// TODO: Performance test to see if TBB model updating is necessary
// Choose threaded or unthreaded model updating depending on the number of
// models in the scene
// if (this->GetModelCount() < 20)
this->dataPtr->modelUpdateFunc = &World::ModelUpdateSingleLoop;
// if (this->GetModelCount() < 20) //Opened by zenglei@2018-11-26
this->dataPtr->modelUpdateFunc = &World::ModelUpdateSingleLoop;
// else
// this->dataPtr->modelUpdateFunc = &World::ModelUpdateTBB;
// this->dataPtr->modelUpdateFunc = &World::ModelUpdateTBB;
event::Events::worldCreated(this->GetName());
this->dataPtr->userCmdManager = UserCmdManagerPtr(
@ -398,10 +439,41 @@ void World::Init()
//////////////////////////////////////////////////
void World::Run(unsigned int _iterations)
{
#ifdef USE_COUNT_TIME //Added by zenglei@2018-12-04
struct timeval tv; //Added by zenglei@2018-12-04
double start_time,tmp_time;
gettimeofday(&tv,NULL);
start_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
this->dataPtr->stop = false;
this->dataPtr->stopIterations = _iterations;
std::cout<<"Server IP: "<<ip<<"\t Server Port: "<<port<<"\t Flag: "<<flag<<std::endl;
if(1 == flag)
{
if(0 == type_)
{
gazeboRecv.SetPort(port);
gazeboRecv.StartServer();
}
else if(1 == type_)
{
gazeboSend.SetIp(ip);
gazeboSend.SetPort(port);
gazeboSend.ConnectServer();
}
}
this->dataPtr->thread = new boost::thread(boost::bind(&World::RunLoop, this));
#ifdef USE_COUNT_TIME //Added by zenglei@2018-12-04
gettimeofday(&tv,NULL); //Added by zenglei@2018-12-04
tmp_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
std::cout<<"World::Run: "<<tmp_time - start_time<<std::endl;
#endif
}
//////////////////////////////////////////////////
@ -438,9 +510,42 @@ void World::Stop()
}
}
#ifdef USE_COUNT_TIME
extern double physicsStepFuncTime; //Added by zenglei@2018-12-06
extern double contactFeedbackTime; //Added by zenglei@20190322
// extern double dxReallocateWorldProcessContextValue;
// extern double dxProcessIslandsValue;
// // extern double tmpQuick;
// double quickStepperTimeValue; //Added by zenglei@2018-12-06
// extern double computeBeforeLcpValue;
// extern double lcpTimeValue;
// extern double updateBodyValue;
// extern double dxReallocateWorldProcessContextValue;
// extern double dxProcessIslandsValue;
#endif
#ifdef USE_COUNT_TIME //Added by zenglei@2018-12-07
double beforeUpdateTime = 0;
double UpdateTime = 0;
double ProcessTime = 0;
double publishModelPoses_time = 0;
double loggingTime = 0;
double beforeupdatePhysicsTime = 0;
// extern double dxReallocateWorldProcessContextValue; //Added by zenglei@2018-12-11
// extern double dxProcessIslandsValue;
#endif
//////////////////////////////////////////////////
void World::RunLoop()
{
#ifdef USE_COUNT_TIME //Added by zenglei@2018-12-04
struct timeval tv; //Added by zenglei@2018-12-04
double start_time,cur_time,tmp_time;
gettimeofday(&tv,NULL);
start_time = cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
this->dataPtr->physicsEngine->InitForThread();
this->dataPtr->startTime = common::Time::GetWallTime();
@ -456,19 +561,27 @@ void World::RunLoop()
this->dataPtr->prevStates[1] = WorldState(shared_from_this());
this->dataPtr->stateToggle = 0;
//Closed by zenglei@2018-11-26
this->dataPtr->logThread =
new boost::thread(boost::bind(&World::LogWorker, this));
#ifdef USE_COUNT_TIME //Added by zenglei@2018-12-04
gettimeofday(&tv,NULL); //Added by zenglei@2018-12-04
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
double updateTime=0,beforemodelUpdateTime=0,modelUpdateTime=0,updateCollisionTime=0,updatePhysicsTime=0;//modified by zhangshuai@2018-12-14
#endif
// gazeboRecv.connectServer();
if (!util::LogPlay::Instance()->IsOpen())
{
for (this->dataPtr->iterations = 0; !this->dataPtr->stop &&
(!this->dataPtr->stopIterations ||
(this->dataPtr->iterations < this->dataPtr->stopIterations));)
{
this->Step();
this->Step(updateTime,beforemodelUpdateTime,modelUpdateTime,updateCollisionTime,updatePhysicsTime);//modified by zhangshuai@2018-12-14
}
}
// Closed by zenglei@2018-11-26
else
{
this->dataPtr->enablePhysicsEngine = false;
@ -479,20 +592,62 @@ void World::RunLoop()
this->LogStep();
}
}
this->dataPtr->stop = true;
if (this->dataPtr->logThread)
#ifdef USE_COUNT_TIME
gettimeofday(&tv,NULL); //Added by zenglei@2018-12-04
//First output the Information of the simulation step
tmp_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
std::cout<<"World::Step: "<<tmp_time - cur_time<<std::endl;
std::cout<<"beforeUpdateTime: "<<beforeUpdateTime<<std::endl;
std::cout<<"UpdateTime: "<<UpdateTime<<std::endl;
std::cout<<"publishModelPoses_time: "<<publishModelPoses_time<<std::endl;
std::cout<<"the module of Update cost time: "<<updateTime<<std::endl;
std::cout<< beforemodelUpdateTime<<std::endl; //Added by zhangshuai@2018-12-14
std::cout<< modelUpdateTime<<std::endl;
std::cout<< updateCollisionTime<<std::endl;
std::cout<< updatePhysicsTime<<std::endl;
std::cout<< ProcessTime<<std::endl;
// std::cout<<"Parallel Module Cost: "<<timeCallBack<<std::endl; //Added by zenglei@20190305
std::cout<<"physicsStepFuncTime cost time: "<<physicsStepFuncTime<<std::endl;
std::cout<<"contactFeedbackTime cost time: "<<contactFeedbackTime<<std::endl; //Added by zenglei@20190322
// std::cout<<"quickStepperTimeValue cost time: "<<quickStepperTimeValue<<std::endl;
// std::cout<<"computeBeforeLcpValue cost time: "<<computeBeforeLcpValue<<std::endl;
// std::cout<<"lcpTimeValue cost time: "<<lcpTimeValue<<std::endl;
// std::cout<<"updateBodyValue cost time: "<<updateBodyValue<<std::endl;
// std::cout<<"dxReallocateWorldProcessContextValue cost time: "<<dxReallocateWorldProcessContextValue<<std::endl;
// std::cout<<"dxProcessIslandsValue cost time: "<<dxProcessIslandsValue<<std::endl;
#endif
if(1 == flag)
{
this->dataPtr->logCondition.notify_all();
if(0 == type_)
{
boost::mutex::scoped_lock lock(this->dataPtr->logMutex);
this->dataPtr->logCondition.notify_all();
gazeboRecv.Close();
}
this->dataPtr->logThread->join();
delete this->dataPtr->logThread;
this->dataPtr->logThread = nullptr;
else if(1 == type_)
{
gazeboSend.Close();
}
}
this->dataPtr->stop = true;
//Closed by zenglei@2018-11-26
// if (this->dataPtr->logThread)
// {
// this->dataPtr->logCondition.notify_all();
// {
// boost::mutex::scoped_lock lock(this->dataPtr->logMutex);
// this->dataPtr->logCondition.notify_all();
// }
// this->dataPtr->logThread->join();
// delete this->dataPtr->logThread;
// this->dataPtr->logThread = nullptr;
// }
#ifdef USE_COUNT_TIME
gettimeofday(&tv,NULL); //Added by zenglei@2018-12-04
tmp_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
std::cout<<tmp_time - start_time<<std::endl;
#endif
}
//////////////////////////////////////////////////
@ -559,6 +714,13 @@ bool World::SensorsInitialized() const
//////////////////////////////////////////////////
void World::Step()
{
struct timeval tv; //Added by zenglei@2018-12-04
double start_time,cur_time,tmp_time;
gettimeofday(&tv,NULL);
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
start_time = cur_time;
DIAG_TIMER_START("World::Step");
/// need this because ODE does not call dxReallocateWorldProcessContext()
@ -633,13 +795,218 @@ void World::Step()
this->dataPtr->pauseTime += stepTime;
}
}
this->ProcessMessages();
DIAG_TIMER_STOP("World::Step");
if (g_clearModels)
this->ClearModels();
gettimeofday(&tv,NULL); //Added by zenglei@2018-12-05
tmp_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
std::cout<<"World::Step cost time: "<<tmp_time - start_time<<std::endl;
}
//rewrite the Step function by zenglei@2018-12-05 modified by zhangshuai@2018-12-14
void World::Step(double &updateTime,double &beforemodelUpdateTime,double &modelUpdateTime,double &updateCollisionTime,double &updatePhysicsTime)
{
#ifdef USE_COUNT_TIME //Added by zenglei@2018-12-07
struct timeval tv;
double start_time,cur_time, tmp_time;
gettimeofday(&tv,NULL);
start_time = cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
DIAG_TIMER_START("World::Step");
/// need this because ODE does not call dxReallocateWorldProcessContext()
/// until dWorld.*Step
/// Plugins that manipulate joints (and probably other properties) require
/// one iteration of the physics engine. Do not remove this.
if (!this->dataPtr->pluginsLoaded && this->SensorsInitialized())
{
this->LoadPlugins();
this->dataPtr->pluginsLoaded = true;
}
DIAG_TIMER_LAP("World::Step", "loadPlugins");
// Send statistics about the world simulation
this->PublishWorldStats();
DIAG_TIMER_LAP("World::Step", "publishWorldStats");
double updatePeriod = this->dataPtr->physicsEngine->GetUpdatePeriod();
// sleep here to get the correct update rate
common::Time tmpTime = common::Time::GetWallTime();
common::Time sleepTime = this->dataPtr->prevStepWallTime +
common::Time(updatePeriod) - tmpTime - this->dataPtr->sleepOffset;
// std::cout<<"sleepTime1: "<<sleepTime<<std::endl; //Added by zenglei@2018-12-07
// std::cout<<"updatePeriod: "<<updatePeriod<<std::endl;
common::Time actualSleep = 0;
if (sleepTime > 0)
{
common::Time::Sleep(sleepTime);
actualSleep = common::Time::GetWallTime() - tmpTime;
// std::cout<<"sleepTime2: "<<sleepTime<<std::endl; //Added by zenglei@2018-12-07
}
else
sleepTime = 0;
// exponentially avg out
this->dataPtr->sleepOffset = (actualSleep - sleepTime) * 0.01 +
this->dataPtr->sleepOffset * 0.99;
DIAG_TIMER_LAP("World::Step", "sleepOffset");
// throttling update rate, with sleepOffset as tolerance
// the tolerance is needed as the sleep time is not exact
if (common::Time::GetWallTime() - this->dataPtr->prevStepWallTime +
this->dataPtr->sleepOffset >= common::Time(updatePeriod))
{
boost::recursive_mutex::scoped_lock lock(*this->dataPtr->worldUpdateMutex);
DIAG_TIMER_LAP("World::Step", "worldUpdateMutex");
this->dataPtr->prevStepWallTime = common::Time::GetWallTime();
double stepTime = this->dataPtr->physicsEngine->GetMaxStepSize();
if (!this->IsPaused() || this->dataPtr->stepInc > 0
|| this->dataPtr->needsReset)
{
// std::cout<<"=================start to sendto===============\n"<<std::endl;
// int send_num = sendto(sock_fd, (void *)&(this->dataPtr->iterations), sizeof(uint64_t), 0, (struct sockaddr *)&addr_serv, len);
// send_num = sendto(sock_fd, (void *)&(this->dataPtr->iterations), sizeof(uint64_t), 0, (struct sockaddr *)&addr_serv, len);
// if(send_num < 0)
// {
// perror("==================sendto error===============\n");
// return;
// }
// // std::cout<<"=================start to recvfrom===============\n"<<std::endl;
// int recv_num = recvfrom(sock_fd, recv_buf, sizeof(recv_buf), MSG_DONTWAIT, (struct sockaddr *)&addr_serv, (socklen_t *)&len);
// if(recv_num <= 0)
// {
// perror("==================recvfrom error==============\n");
// return;
// }
// uint64_t recvIter;
// memcpy(&recvIter, recv_buf, sizeof(uint64_t));
// if(this->dataPtr->iterations != recvIter)
// {
// std::cout<<"================Wait to aync==============\n"<<std::endl;
// return;
// }
// int nStepIter;
// while(1)
// {
// if(gazeboSend.sendSignal(this->dataPtr->iterations) < 0)
// continue;
// if(gazeboSend.receiveSignal(nStepIter) < 0)
// continue;
// if(nStepIter == this->dataPtr->iterations)
// break;
// }
// std::cout<<"============Succeed!!===========\n"<<std::endl;
// query timestep to allow dynamic time step size updates
if(1 == flag) //Added by zenglei@2019-01-23--begin
{
if(0 == type_)
{
uint64_t tmp = 0;
while(1)
{
if(gazeboRecv.sendData(this->dataPtr->iterations))
{
gazeboRecv.recvData(tmp);
if(tmp == this->dataPtr->iterations)
break;
}
}
}
else if(1 == type_)
{
uint64_t tmp = 0;
while(1)
{
tmp = 0;
gazeboSend.recvData(tmp);
// std::cout<<"tmp = "<< tmp<<"iter = "<<this->dataPtr->iterations<<std::endl;
if(tmp == this->dataPtr->iterations)
{
gazeboSend.sendData(this->dataPtr->iterations);
}
else
continue;
// std::cout<<"recv tmp: "<<tmp<<std::endl;
// std::cout<<"iterations: "<<this->dataPtr->iterations<<std::endl;
break;
}
}
// std::cout<<"tmp: "<<tmp<<std::endl;
// std::cout<<"iterations: "<<this->dataPtr->iterations<<std::endl;
}//Added by zenglei@2019-01-23--end
this->dataPtr->simTime += stepTime;
this->dataPtr->iterations++;
#ifdef USE_COUNT_TIME //Added by zenglei@2018-12-04
gettimeofday(&tv,NULL);
tmp_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
beforeUpdateTime += tmp_time - cur_time;
cur_time = tmp_time;
#endif
// this->Update(modelUpdateTime,updateCollisionTime,updatePhysicsTime);
this->Update(beforemodelUpdateTime,modelUpdateTime,updateCollisionTime,updatePhysicsTime); //Added by zhangshuai@2018-12-14
#ifdef USE_COUNT_TIME //Added by zenglei@2018-12-04
gettimeofday(&tv,NULL);
updateTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
#endif
DIAG_TIMER_LAP("World::Step", "update");
if (this->IsPaused() && this->dataPtr->stepInc > 0)
this->dataPtr->stepInc--;
}
else
{
// Flush the log record buffer, if there is data in it.
if (util::LogRecord::Instance()->BufferSize() > 0)
util::LogRecord::Instance()->Notify();
this->dataPtr->pauseTime += stepTime;
}
}
#ifdef USE_COUNT_TIME
gettimeofday(&tv,NULL); //Added by zenglei@2018-12-10
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
this->ProcessMessages();
#ifdef USE_COUNT_TIME
gettimeofday(&tv,NULL); //Added by zenglei@2018-12-10
ProcessTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
#endif
DIAG_TIMER_STOP("World::Step");
if (g_clearModels)
this->ClearModels();
#ifdef USE_COUNT_TIME
gettimeofday(&tv,NULL); //Added by zenglei@2018-12-10
UpdateTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - start_time;
#endif
}
//////////////////////////////////////////////////
@ -670,6 +1037,12 @@ void World::Step(unsigned int _steps)
//////////////////////////////////////////////////
void World::Update()
{
struct timeval tv; //Added by zenglei@2018-12-04
double start_time,cur_time,tmp_time;
gettimeofday(&tv,NULL);
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
start_time = cur_time;
DIAG_TIMER_START("World::Update");
if (this->dataPtr->needsReset)
@ -690,15 +1063,29 @@ void World::Update()
event::Events::worldUpdateBegin(this->dataPtr->updateInfo);
DIAG_TIMER_LAP("World::Update", "Events::worldUpdateBegin");
gettimeofday(&tv,NULL); //Added by zenglei＠2018-12-05
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
// Update all the models
(*this.*dataPtr->modelUpdateFunc)();
gettimeofday(&tv,NULL); //Added by zenglei@2018-12-05
tmp_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
std::cout<<"ModelUpdate Cost Time: "<<tmp_time - cur_time<<std::endl;
DIAG_TIMER_LAP("World::Update", "Model::Update");
gettimeofday(&tv,NULL); //Added by zenglei＠2018-12-05
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
// This must be called before PhysicsEngine::UpdatePhysics.
this->dataPtr->physicsEngine->UpdateCollision();
gettimeofday(&tv,NULL); //Added by zenglei@2018-12-05
tmp_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
std::cout<<"UpdateCollision Cost Time: "<<tmp_time - cur_time<<std::endl;
DIAG_TIMER_LAP("World::Update", "PhysicsEngine::UpdateCollision");
// Wait for logging to finish, if it's running.
@ -726,9 +1113,17 @@ void World::Update()
// Update the physics engine
if (this->dataPtr->enablePhysicsEngine && this->dataPtr->physicsEngine)
{
gettimeofday(&tv,NULL); //Added by zenglei＠2018-12-05
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
// This must be called directly after PhysicsEngine::UpdateCollision.
this->dataPtr->physicsEngine->UpdatePhysics();
gettimeofday(&tv,NULL); //Added by zenglei@2018-12-05
tmp_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
std::cout<<"UpdatePhysics Cost Time: "<<tmp_time - cur_time<<std::endl;
DIAG_TIMER_LAP("World::Update", "PhysicsEngine::UpdatePhysics");
// do this after physics update as
@ -763,6 +1158,184 @@ void World::Update()
event::Events::worldUpdateEnd();
DIAG_TIMER_STOP("World::Update");
gettimeofday(&tv,NULL); //Added by zenglei@2018-12-05
tmp_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
std::cout<<"World::Update cost time: "<<tmp_time - start_time<<std::endl;
}
//////////////////////////////////////////////////
// void World::Update(double &modelUpdateTime,double &updateCollisionTime,double &updatePhysicsTime)
void World::Update(double &beforemodelUpdateTime,double &modelUpdateTime,double &updateCollisionTime,double &updatePhysicsTime) //Added by zhangshuai@2018-12-14
{
// if(1 == flag)
// gazeboSend.sendSignal();
#ifdef USE_COUNT_TIME //Added by zenglei@2018-12-04
struct timeval tv;
double cur_time;
gettimeofday(&tv,NULL);
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
DIAG_TIMER_START("World::Update");
if (this->dataPtr->needsReset)
{
if (this->dataPtr->resetAll)
this->Reset();
else if (this->dataPtr->resetTimeOnly)
this->ResetTime();
else if (this->dataPtr->resetModelOnly)
this->ResetEntities(Base::MODEL);
this->dataPtr->needsReset = false;
return;
}
DIAG_TIMER_LAP("World::Update", "needsReset");
this->dataPtr->updateInfo.simTime = this->GetSimTime();
this->dataPtr->updateInfo.realTime = this->GetRealTime();
#ifdef USE_COUNT_TIME //Added by zhangshuai＠2018-12-14
gettimeofday(&tv,NULL);
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
// std::cout<<"*************event::Events::worldUpdateBegin----Begin************"<<std::endl; //Added by zenglei@20190306
event::Events::worldUpdateBegin(this->dataPtr->updateInfo);
// std::cout<<"*************event::Events::worldUpdateBegin----End************"<<std::endl; //Added by zenglei@20190306
#ifdef USE_COUNT_TIME //Added by zhangshuai@2018-12-14
gettimeofday(&tv,NULL);
beforemodelUpdateTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
#endif
DIAG_TIMER_LAP("World::Update", "Events::worldUpdateBegin");
#ifdef USE_COUNT_TIME //Added by zenglei＠2018-12-05
gettimeofday(&tv,NULL);
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
// Update all the models
(*this.*dataPtr->modelUpdateFunc)();
#ifdef USE_COUNT_TIME //Added by zenglei@2018-12-05
gettimeofday(&tv,NULL);
modelUpdateTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
#endif
DIAG_TIMER_LAP("World::Update", "Model::Update");
#ifdef USE_COUNT_TIME //Added by zenglei＠2018-12-05
gettimeofday(&tv,NULL);
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
// This must be called before PhysicsEngine::UpdatePhysics.
this->dataPtr->physicsEngine->UpdateCollision();
#ifdef USE_COUNT_TIME //Added by zenglei@2018-12-05
gettimeofday(&tv,NULL);
updateCollisionTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
#endif
DIAG_TIMER_LAP("World::Update", "PhysicsEngine::UpdateCollision");
#ifdef USE_COUNT_TIME //Added by zhangshuai＠2018-12-14
gettimeofday(&tv,NULL);
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
// Wait for logging to finish, if it's running.
if (util::LogRecord::Instance()->Running())
{
boost::mutex::scoped_lock lock(this->dataPtr->logMutex);
// It's possible the logWorker thread never processed the previous
// state. This checks to make sure that we don't continute until the log
// worker catchs up.
if (this->dataPtr->iterations - this->dataPtr->logPrevIteration > 1)
{
this->dataPtr->logCondition.notify_one();
this->dataPtr->logContinueCondition.wait(lock);
}
}
#ifdef USE_COUNT_TIME //Added by zhangshuai＠2018-12-14
gettimeofday(&tv,NULL);
loggingTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
#endif
#ifdef USE_COUNT_TIME //Added by zhangshuai＠2018-12-14
gettimeofday(&tv,NULL);
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
// Give clients a possibility to react to collisions before the physics
// gets updated.
this->dataPtr->updateInfo.realTime = this->GetRealTime();
// std::cout<<"*************event::Events::beforePhysicsUpdate----Begin************"<<std::endl; //Added by zenglei@20190306
event::Events::beforePhysicsUpdate(this->dataPtr->updateInfo);
// std::cout<<"*************event::Events::beforePhysicsUpdate----End************"<<std::endl; //Added by zenglei@20190306
#ifdef USE_COUNT_TIME //Added by zhangshuai＠2018-12-14
gettimeofday(&tv,NULL);
beforeupdatePhysicsTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
#endif
DIAG_TIMER_LAP("World::Update", "Events::beforePhysicsUpdate");
// Update the physics engine
if (this->dataPtr->enablePhysicsEngine && this->dataPtr->physicsEngine)
{
#ifdef USE_COUNT_TIME //Added by zenglei＠2018-12-05
gettimeofday(&tv,NULL);
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
// This must be called directly after PhysicsEngine::UpdateCollision.
this->dataPtr->physicsEngine->UpdatePhysics();
#ifdef USE_COUNT_TIME //Added by zenglei@2018-12-05
gettimeofday(&tv,NULL);
updatePhysicsTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
#endif
DIAG_TIMER_LAP("World::Update", "PhysicsEngine::UpdatePhysics");
// do this after physics update as
// ode --> MoveCallback sets the dirtyPoses
// and we need to propagate it into Entity::worldPose
{
// block any other pose updates (e.g. Joint::SetPosition)
boost::recursive_mutex::scoped_lock lock(
*this->dataPtr->physicsEngine->GetPhysicsUpdateMutex());
for (auto &dirtyEntity : this->dataPtr->dirtyPoses)
{
dirtyEntity->SetWorldPose(dirtyEntity->GetDirtyPose(), false);
}
this->dataPtr->dirtyPoses.clear();
}
DIAG_TIMER_LAP("World::Update", "SetWorldPose(dirtyPoses)");
}
// Only update state information if logging data.
if (util::LogRecord::Instance()->Running())
this->dataPtr->logCondition.notify_one();
DIAG_TIMER_LAP("World::Update", "LogRecordNotify");
// Output the contact information
this->dataPtr->physicsEngine->GetContactManager()->PublishContacts();
DIAG_TIMER_LAP("World::Update", "ContactManager::PublishContacts");
event::Events::worldUpdateEnd();
DIAG_TIMER_STOP("World::Update");
}
//////////////////////////////////////////////////
@ -1494,11 +2067,14 @@ void World::BuildSceneMsg(msgs::Scene &_scene, BasePtr _entity)
//////////////////////////////////////////////////
// void World::ModelUpdateTBB()
// void World::ModelUpdateTBB() //Opened by zenglei@2018-11-26
// {
// tbb::parallel_for (tbb::blocked_range<size_t>(0,
// this->dataPtr->models.size(), 10),
// this->dataPtr->models.size(), 25),
// ModelUpdate_TBB(&this->dataPtr->models));
// // tbb::parallel_for (tbb::blocked_range<size_t>(0, //Modified by zenglei@2018-12-10
// // this->dataPtr->models.size(), 12),
// // ModelUpdate_TBB(&this->dataPtr->models));
// }
//////////////////////////////////////////////////
@ -2463,10 +3039,56 @@ bool World::OnLog(std::ostringstream &_stream)
return true;
}
//////////////////////////////////////////////////
//Added by zenglei@2018-12-10-------Start
// void World::processMessageThreadFunc(std::vector<ModelPtr> &vecModel, int startIndex, int stepSize, msgs::PosesStamped &msg)
// {
// boost::mutex::scoped_lock lock(mu_process);
// unsigned int size = vecModel.size();
// std::list<ModelPtr> modelList;
// for(unsigned int i = startIndex; i < size; i+=stepSize)
// {
// modelList.push_back(vecModel[i]);
// while (!modelList.empty())
// {
// ModelPtr m = modelList.front();
// modelList.pop_front();
// msgs::Pose *poseMsg = msg.add_pose();
// // Publish the model's relative pose
// poseMsg->set_name(m->GetScopedName());
// poseMsg->set_id(m->GetId());
// msgs::Set(poseMsg, m->GetRelativePose().Ign());
// // Publish each of the model's child links relative poses
// Link_V links = m->GetLinks();
// // #ifdef USE_COUNT_TIME //Added by zenglei@2018-12-10
// // std::cout<<"links.size: "<<links.size()<<std::endl;
// // #endif
// for (auto const &link : links)
// {
// poseMsg = msg.add_pose();
// poseMsg->set_name(link->GetScopedName());
// poseMsg->set_id(link->GetId());
// msgs::Set(poseMsg, link->GetRelativePose().Ign());
// }
// // add all nested models to the queue
// Model_V models = m->NestedModels();
// for (auto const &n : models)
// modelList.push_back(n);
// }
// }
// }
//Added by zenglei@2018-12-10-------End
//////////////////////////////////////////////////
void World::ProcessMessages()
{
{
{ //Closed by zenglei@2018-12-10
boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex);
if ((this->dataPtr->posePub && this->dataPtr->posePub->HasConnections()) ||
@ -2477,10 +3099,68 @@ void World::ProcessMessages()
// Time stamp this PosesStamped message
msgs::Set(msg.mutable_time(), this->GetSimTime());
// if(!this->dataPtr->publishModelPoses.empty())
// {
// // #ifdef USE_COUNT_TIME //Added by zenglei@2018-12-10
// // struct timeval tv;
// // double cur_time;
// // gettimeofday(&tv,NULL);
// // cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
// // #endif
// // std::vector<boost::thread> threadVec;
// std::vector<ModelPtr> modelVec;
// for (auto const &model : this->dataPtr->publishModelPoses)
// {
// modelVec.push_back(model);
// }
// // int startModel = 0;
// // int stepSize = 2;
// // for(int index = 0; index < 8; index++)
// // {
// // threadVec.push_back(boost::thread(boost::bind(&World::processMessageThreadFunc, this, modelVec, index,8,msg)));
// boost::thread
// // }
// // for(auto const &thr: threadVec)
// // {
// // thr.join();
// // }
// }
// if(!this->dataPtr->publishLightPoses.empty())
// {
// for (auto const &light : this->dataPtr->publishLightPoses)
// {
// msgs::Pose *poseMsg = msg.add_pose();
// // Publish the light's pose
// poseMsg->set_name(light->GetScopedName());
// // \todo Change to relative once lights can be attached to links
// // on the rendering side
// // \todo Hack: we use empty id to indicate it's pose of a light
// // Need to add an id field to light.proto
// // poseMsg->set_id(light->GetId());
// msgs::Set(poseMsg, light->GetWorldPose().Ign());
// }
// if (this->dataPtr->posePub && this->dataPtr->posePub->HasConnections())
// this->dataPtr->posePub->Publish(msg);
// }
// if (this->dataPtr->poseLocalPub &&
// this->dataPtr->poseLocalPub->HasConnections())
// {
// // rendering::Scene depends on this timestamp, which is used by
// // rendering sensors to time stamp their data
// this->dataPtr->poseLocalPub->Publish(msg);
// }
if (!this->dataPtr->publishModelPoses.empty() ||
!this->dataPtr->publishLightPoses.empty())
{
// #ifdef USE_COUNT_TIME //Added by zenglei@2018-12-10
// gettimeofday(&tv,NULL);
// publishModelPoses_time += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
// #endif
for (auto const &model : this->dataPtr->publishModelPoses)
{
std::list<ModelPtr> modelList;
@ -2498,6 +3178,11 @@ void World::ProcessMessages()
// Publish each of the model's child links relative poses
Link_V links = m->GetLinks();
// #ifdef USE_COUNT_TIME //Added by zenglei@2018-12-10
// std::cout<<"links.size: "<<links.size()<<std::endl;
// #endif
for (auto const &link : links)
{
poseMsg = msg.add_pose();
@ -2513,6 +3198,10 @@ void World::ProcessMessages()
}
}
// #ifdef USE_COUNT_TIME //Added by zenglei@2018-12-10
// std::cout<<"this->dataPtr->publishLightPoses.size: "<<this->dataPtr->publishLightPoses.size()<<std::endl;
// #endif
for (auto const &light : this->dataPtr->publishLightPoses)
{
msgs::Pose *poseMsg = msg.add_pose();
@ -2526,11 +3215,16 @@ void World::ProcessMessages()
// poseMsg->set_id(light->GetId());
msgs::Set(poseMsg, light->GetWorldPose().Ign());
}
if (this->dataPtr->posePub && this->dataPtr->posePub->HasConnections())
this->dataPtr->posePub->Publish(msg);
}
// #ifdef USE_COUNT_TIME //Added by zenglei@2018-12-10
// struct timeval tv;
// double cur_time;
// gettimeofday(&tv,NULL);
// cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
// #endif
if (this->dataPtr->poseLocalPub &&
this->dataPtr->poseLocalPub->HasConnections())
{
@ -2538,11 +3232,15 @@ void World::ProcessMessages()
// rendering sensors to time stamp their data
this->dataPtr->poseLocalPub->Publish(msg);
}
// #ifdef USE_COUNT_TIME //Added by zenglei@2018-12-10
// gettimeofday(&tv,NULL);
// publishModelPoses_time += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
// #endif
}
this->dataPtr->publishModelPoses.clear();
this->dataPtr->publishLightPoses.clear();
}
{
boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex);
@ -2550,6 +3248,11 @@ void World::ProcessMessages()
{
if (!this->dataPtr->publishModelScales.empty())
{
// #ifdef USE_COUNT_TIME //Added by zenglei@2018-12-10
// std::cout<<"this->dataPtr->publishModelScales.size: "<<this->dataPtr->publishModelScales.size()<<std::endl;
// #endif
for (auto const &model : this->dataPtr->publishModelScales)
{
std::list<ModelPtr> modelList;
@ -2583,6 +3286,11 @@ void World::ProcessMessages()
if (common::Time::GetWallTime() - this->dataPtr->prevProcessMsgsTime >
this->dataPtr->processMsgsPeriod)
{
// #ifdef USE_COUNT_TIME
// std::cout<<"-----Here-----"<<std::endl;
// #endif
this->ProcessPlaybackControlMsgs();
this->ProcessEntityMsgs();
this->ProcessRequestMsgs();

421
gazebo/gazebo/physics/World.hh
View File

@ -47,12 +47,402 @@
#include "gazebo/physics/WorldState.hh"
#include "gazebo/util/system.hh"
//Added by zenglei@2019-01-22 begin
#include <string>
#include <errno.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
// #include <fcntl.h>
#include <unistd.h>
#define QUEUE 1
#define BUF_SIZE 8
//Added by zenglei@2019-01-22 end
// #ifdef USE_COUNT_TIME //Added by zenglei@2018-12-06
// extern double quickStepperTimeValue;
// extern double computeBeforeLcpValue;
// extern double lcpTimeValue;
// extern double updateBodyValue;
// extern double dxReallocateWorldProcessContextValue; //Added by zenglei@2018-12-06
// extern double dxProcessIslandsValue;
// extern double physicsStepFuncTime;
// #endif
//Added by zenglei@20190219--begin
// #include "HelloWorldPublisher.h"
// #include "HelloWorldSubscriber.h"
// #include <fastrtps/Domain.h>
// #include <fastrtps/utils/eClock.h>
// // #include <fastrtps/log/Log.h>
// using namespace eprosima;
// using namespace fastrtps;
// using namespace rtps;
//Added by zenglei@20190219--end
namespace gazebo
{
namespace physics
{
/// Forward declare private data class.
class WorldPrivate;
//Added by zenglei@2019-01-23---begin
class TcpServer
{
public:
TcpServer():client_fd(-1){
server_fd = socket(AF_INET, SOCK_STREAM, 0);
memset(&server_sockaddr, 0 , sizeof(struct sockaddr_in));
memset(&client_addr, 0, sizeof(struct sockaddr_in));
}
~TcpServer(){
if(-1 != client_fd)
close(client_fd);
close(server_fd);
}
void StartServer()
{
printf("Start to Tcp Server!\n");
server_sockaddr.sin_family = AF_INET;
server_sockaddr.sin_port = htons(port);
server_sockaddr.sin_addr.s_addr = htonl(INADDR_ANY);
unsigned int value = 0x1;
setsockopt(server_fd, SOL_SOCKET, SO_REUSEADDR, (void *)&value, sizeof(value));
if(bind(server_fd, (struct sockaddr* ) &server_sockaddr, sizeof(server_sockaddr))==-1) {
perror("bind\n");
return;
}
if(listen(server_fd, QUEUE) == -1) {
perror("listen\n");
return;
}
socklen_t length = sizeof(client_addr);
///成功返回非负描述字，出错返回-1
client_fd = accept(server_fd, (struct sockaddr*)&client_addr, &length);
if( client_fd < 0 ) {
perror("connect\n");
return;
}
// int flag = fcntl(client_fd, F_GETFL, 0);
// fcntl(client_fd, F_SETFL, flag | O_NONBLOCK);
printf("A client connected Succeed!\n");
}
bool sendData(uint64_t nStep)
{
memset(buff, 0, BUF_SIZE);
memcpy(buff, &nStep, sizeof(uint64_t));
// std::cout<<"send data: "<<nStep<<std::endl;
if(send(client_fd, buff, sizeof(uint64_t), 0) > 0)
{
// std::cout<<"Send Data Succeed!"<<std::endl;
return true;
}
else
{
// std::cout<<"Send Data Failed!"<<std::endl;
return false;
}
}
bool recvData(uint64_t &nStep)
{
memset(buff, 0, BUF_SIZE);
// std::cout<<"recv data: "<<std::endl;
if(recv(client_fd, buff, sizeof(uint64_t), 0) > 0)
{
memcpy(&nStep, buff, sizeof(uint64_t));
// std::cout<<"Recv Data "<< nStep<<"Succeed!"<<std::endl;
return true;
}
else
{
// std::cout<<"Recv Data Failed!"<<std::endl;
return false;
}
}
void SetPort(int port_)
{
port = port_;
}
void Close()
{
close(client_fd);
close(server_fd);
}
void CloseClient()
{
close(client_fd);
}
private:
int server_fd;
int client_fd;
int port;
struct sockaddr_in server_sockaddr;
struct sockaddr_in client_addr;
char buff[BUF_SIZE];
};
class TcpClient
{
public:
TcpClient(){
conn_fd = socket(AF_INET,SOCK_STREAM, 0);
memset(&servaddr, 0, sizeof(servaddr));
}
~TcpClient(){
close(conn_fd);
}
void ConnectServer()
{
servaddr.sin_family = AF_INET;
servaddr.sin_port = htons(port); ///服务器端口
servaddr.sin_addr.s_addr = inet_addr(ip.c_str()); ///服务器ip
while(1)
{
if (connect(conn_fd, (struct sockaddr *)&servaddr, sizeof(servaddr)) >= 0)
{
break;
}
perror("connect error!\n");
}
// int flag = fcntl(conn_fd, F_GETFL, 0);
// fcntl(conn_fd, F_SETFL, flag | O_NONBLOCK);
}
bool sendData(uint64_t nStep)
{
memset(buff, 0, BUF_SIZE);
// *((int*)buff) = nStep;
memcpy(buff, &nStep, sizeof(uint64_t));
// std::cout<<"send data: "<<nStep<<std::endl;
if(send(conn_fd, buff, sizeof(uint64_t), 0) > 0)
{
// std::cout<<"Send Data Succeed!"<<std::endl;
return true;
}
else
{
// std::cout<<"Send Data Failed!"<<std::endl;
return false;
}
}
bool recvData(uint64_t &nStep)
{
memset(buff, 0, BUF_SIZE);
// std::cout<<"recv data: "<<std::endl;
if(recv(conn_fd, buff, sizeof(uint64_t), 0) > 0)
{
memcpy(&nStep, buff, sizeof(uint64_t));
// std::cout<<"Recv Data "<< nStep<<"Succeed!"<<std::endl;
return true;
}
else
{
// std::cout<<"Recv Data Failed!"<<std::endl;
return false;
}
}
void SetIp(std::string ip_)
{
ip = ip_;
}
void SetPort(int port_)
{
port = port_;
}
void Close()
{
close(conn_fd);
}
private:
int conn_fd;
int port;
std::string ip;
struct sockaddr_in servaddr;
char buff[BUF_SIZE];
};
//Added by zenglei@2019-01-23
// //Class : tcp to send signal (zhangshuai@2019.01.04)
// class TcpSendSignal
// {
// public:
// TcpSendSignal(){
// };
// ~TcpSendSignal(){
// };
// void connectServer()
// {
// if ((client_sockfd = socket(AF_INET, SOCK_STREAM, 0)) < 0)
// {
// printf("create socket error: %s(errno: %d)\n", strerror(errno), errno);
// }
// memset(&remote_addr, 0, sizeof(remote_addr));
// remote_addr.sin_family = AF_INET;
// remote_addr.sin_port = htons(remote_port);
// if (inet_pton(AF_INET, gazebo_address.c_str(), &remote_addr.sin_addr) <= 0)
// {
// printf("inet_pton error for %s\n", gazebo_address.c_str());
// }
// if (connect(client_sockfd, (struct sockaddr *)&remote_addr, sizeof(remote_addr)) < 0)
// {
// printf("connect error: %s(errno: %d)\n", strerror(errno), errno);
// while (connect(client_sockfd, (struct sockaddr *)&remote_addr, sizeof(remote_addr)) < 0)
// {
// if (connect(client_sockfd, (struct sockaddr *)&remote_addr, sizeof(remote_addr)) >= 0)
// {
// break;
// }
// }
// }
// };
// void sendSignal()
// {
// int loop_signal = 1;
// // fgets(sendbuf, BUFSIZ, stdin);
// *sendbuf = loop_signal;
// printf("sendbuf: %d\n", *sendbuf);
// if (send(client_sockfd, sendbuf, strlen(sendbuf), 0) < 0)
// {
// printf("send msg error: %s(errno: %d)\n", strerror(errno), errno);
// }
// recv(client_sockfd, recvbuf, sizeof(recvbuf),0); ///接收
// printf("recvbuf: %d\n", *recvbuf);
// memset(sendbuf, 0, sizeof(sendbuf));
// memset(recvbuf, 0, sizeof(recvbuf));
// }
// void setAddress(std::string address)
// {
// gazebo_address = address;
// };
// void setPort(int port)
// {
// remote_port = port;
// };
// void tcpClose()
// {
// close(client_sockfd);
// };
// private:
// std::string gazebo_address;
// int remote_port;
// int client_sockfd;
// struct sockaddr_in remote_addr; //服务器端网络地址结构体
// // int sin_size;
// char sendbuf[BUFSIZ]; //数据传送的缓冲区
// char recvbuf[BUFSIZ]; //数据传送的缓冲区
// };
// //Class end: tcp to send signal (zhangshuai@2019.01.04)
// //Class : tcp to receive signal (zhangshuai@2019.01.04)
// class TcpReceiveSignal
// {
// public:
// TcpReceiveSignal(){
// };
// ~TcpReceiveSignal(){
// };
// void receiveSignal()
// {
// int loop_signal = 0;
// if ((listenfd = socket(AF_INET, SOCK_STREAM, 0)) == -1)
// {
// printf("create socket error: %s(errno: %d)\n", strerror(errno), errno);
// }
// memset(&servaddr, 0, sizeof(servaddr));
// servaddr.sin_family = AF_INET;
// servaddr.sin_addr.s_addr = htonl(INADDR_ANY);
// servaddr.sin_port = htons(remote_port);
// if (bind(listenfd, (struct sockaddr *)&servaddr, sizeof(servaddr)) == -1)
// {
// printf("bind socket error: %s(errno: %d)\n", strerror(errno), errno);
// }
// if (listen(listenfd, 10) == -1)
// {
// printf("listen socket error: %s(errno: %d)\n", strerror(errno), errno);
// }
// printf("======waiting for client's request======\n");
// while (1)
// {
// if ((connfd = accept(listenfd, (struct sockaddr *)NULL, NULL)) == -1)
// {
// printf("accept socket error: %s(errno: %d)", strerror(errno), errno);
// continue;
// }
// n = recv(connfd, buff, BUFSIZ, 0);
// buff[n] = '\0';
// memcpy(&loop_signal, buff, sizeof(loop_signal));
// printf("recv msg from client: %d\n", loop_signal);
// if (loop_signal == 1)
// {
// close(connfd);
// break;
// }
// }
// };
// void setPort(int port)
// {
// remote_port = port;
// };
// void tcpClose()
// {
// close(listenfd);
// };
// private:
// int listenfd, connfd;
// int remote_port;
// struct sockaddr_in servaddr;
// char buff[BUFSIZ];
// int n;
// };
// //Class end : tcp to receive signal (zhangshuai@2019.01.04)
/// \addtogroup gazebo_physics
/// \{
@ -60,10 +450,10 @@ namespace gazebo
/// \class World World.hh physics/physics.hh
/// \brief The world provides access to all other object within a simulated
/// environment.
///
/// The World is the container for all models and their components
/// (links, joints, sensors, plugins, etc), and WorldPlugin instances.
/// Many core function are also handled in the World, including physics
///#include <sys/types.h>
///#include <sys/socket.h> l models and their components
///#include <netinet/in.h> etc), and WorldPlugin instances.
///#include <arpa/inet.h> ed in the World, including physics
/// update, model updates, and message processing.
class GZ_PHYSICS_VISIBLE World :
public boost::enable_shared_from_this<World>
@ -459,12 +849,18 @@ namespace gazebo
/// \brief Step the world once.
private: void Step();
//\brief rewrite the function by zenglei@2018-12-05 modified by zhangshuai@2018-12-14
private: void Step(double &updateTime,double &beforemodelUpdateTime,double &modelUpdateTime,double &updateCollisionTime,double &updatePhysicsTime);
/// \brief Step the world once by reading from a log file.
private: void LogStep();
/// \brief Update the world.
private: void Update();
/// \brief rewrite the Update Function by zenglei@2018-12-05 modified by zhangshuai@2018-12-14
private: void Update(double &beforemodelUpdateTime,double &modelUpdateTime,double &updateCollisionTime,double &updatePhysicsTime);
/// \brief Pause callback.
/// \param[in] _p True if paused.
private: void OnPause(bool _p);
@ -587,6 +983,23 @@ namespace gazebo
/// Friend SimbodyPhysics so that it has access to dataPtr->dirtyPoses
private: friend class SimbodyPhysics;
//Added by zenglei@2019-01-22
// private: int sock_fd;
// private: struct sockaddr_in addr_serv;
// private: int len;
// private: char recv_buf[BUFSIZE];
// private: HelloWorldPublisher mypub; //Added by zenglei@20190219
private: TcpClient gazeboSend;
private: TcpServer gazeboRecv;
private: std::string ip;
private: int type_; //0:server; 1:client
private: int port;
private: int flag;
// /// brief Start thread for ProcessMessages //Added by zenglei@2018-12-11
// private: boost::mutex mu_process; //定义互斥锁
// private: void processMessageThreadFunc(std::vector<ModelPtr> &vecModel, int startIndex, int stepSize, msgs::PosesStamped &msg); //定义ProcessMessage模块多线程处理逻辑
};
/// \}
}

2
gazebo/gazebo/physics/ode/ODELink.cc
View File

@ -580,6 +580,7 @@ void ODELink::AddRelativeForce(const math::Vector3 &_force)
{
if (this->linkId)
{
// if(!GetEnabled()) //Added by zenglei@20190322
this->SetEnabled(true);
dBodyAddRelForce(this->linkId, _force.x, _force.y, _force.z);
}
@ -667,6 +668,7 @@ void ODELink::AddRelativeTorque(const math::Vector3 &_torque)
{
if (this->linkId)
{
// if(!GetEnabled()) //Added by zenglei@20190322
this->SetEnabled(true);
dBodyAddRelTorque(this->linkId, _torque.x, _torque.y, _torque.z);
}

53
gazebo/gazebo/physics/ode/ODEPhysics.cc
View File

@ -32,6 +32,7 @@
#include <utility>
#include <vector>
// #include "quickstep.h" //Added by zenglei@2018-11-27
#include "gazebo/util/Diagnostics.hh"
#include "gazebo/common/Assert.hh"
#include "gazebo/common/Console.hh"
@ -79,6 +80,9 @@
#include "gazebo/physics/ode/ODEPhysicsPrivate.hh"
#include "gazebo/countTime.hh" //Added by zenglei@2018-12-06
// #include <fstream> //Added by zenglei@2018-12-06
using namespace gazebo;
using namespace physics;
@ -401,6 +405,21 @@ void ODEPhysics::UpdateCollision()
DIAG_TIMER_STOP("ODEPhysics::UpdateCollision");
}
#ifdef USE_COUNT_TIME
double physicsStepFuncTime = 0; //Added by zenglei@2018-12-06
double contactFeedbackTime = 0; //Added by zenglei@20190322
double dxReallocateWorldProcessContextValue = 0;
double dxProcessIslandsValue = 0;
// extern double quickStepperTimeValue; //Added by zenglei@2018-12-06
// double tmpQuick = quickStepperTimeValue;
// extern double quickStepperTimeValue; //Added by zenglei@2018-12-06
// extern double computeBeforeLcpValue;
// extern double lcpTimeValue;
// extern double updateBodyValue;
// extern double dxReallocateWorldProcessContextValue;
// extern double dxProcessIslandsValue;
#endif
//////////////////////////////////////////////////
void ODEPhysics::UpdatePhysics()
{
@ -409,13 +428,33 @@ void ODEPhysics::UpdatePhysics()
// need to lock, otherwise might conflict with world resetting
{
boost::recursive_mutex::scoped_lock lock(*this->physicsUpdateMutex);
#ifdef USE_COUNT_TIME //Added by zenglei@2018-12-06
struct timeval tv; //Added by zenglei@2018-12-06
double start_time;
gettimeofday(&tv,NULL);
start_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
// Update the dynamical model
(*(this->dataPtr->physicsStepFunc))
(*(this->dataPtr->physicsStepFunc))
(this->dataPtr->worldId, this->maxStepSize);
math::Vector3 f1, f2, t1, t2;
#ifdef USE_COUNT_TIME //Added by zenglei@2018-12-06
gettimeofday(&tv,NULL);
physicsStepFuncTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - start_time;
// std::cout<<"physicsStepFuncTime: "<<physicsStepFuncTime<<std::endl;
// std::ofstream outputPhysics("/tmp/physicsStepFuncTime");
// outputPhysics<<physicsStepFuncTime;
// outputPhysics.close();
#endif
math::Vector3 f1, f2, t1, t2;
#ifdef USE_COUNT_TIME //Added by zenglei@20190322
gettimeofday(&tv,NULL);
start_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
//Add by zenglei@2018-11-26
// std::cout<<"************************this->dataPtr->jointFeedbackIndex: "<<this->dataPtr->jointFeedbackIndex<<"**********************"<<std::endl;
// Set the joint contact feedback for each contact.
for (unsigned int i = 0; i < this->dataPtr->jointFeedbackIndex; ++i)
{
@ -445,6 +484,10 @@ void ODEPhysics::UpdatePhysics()
col2->GetLink()->GetWorldPose().rot.RotateVectorReverse(t2);
}
}
#ifdef USE_COUNT_TIME //Added by zenglei@20190322
gettimeofday(&tv,NULL);
contactFeedbackTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - start_time;
#endif
}
DIAG_TIMER_STOP("ODEPhysics::UpdatePhysics");
@ -913,7 +956,9 @@ void ODEPhysics::SetStepType(const std::string &_type)
// Set the physics update function
if (this->dataPtr->stepType == "quick")
this->dataPtr->physicsStepFunc = &dWorldQuickStep;
this->dataPtr->physicsStepFunc = &dWorldQuickStep; //Modified by zenglei@2018-11-27
// this->dataPtr->physicsStepFunc1 = &dWorldQuickStep1;
// this->dataPtr->physicsStepFunc = &dWorldParallelQuickStep;
else if (this->dataPtr->stepType == "world")
this->dataPtr->physicsStepFunc = &dWorldStep;
else

5
gazebo/gazebo/physics/ode/ODEPhysics.hh
View File

@ -32,6 +32,11 @@
#include "gazebo/gazebo_config.h"
#include "gazebo/util/system.hh"
// #ifdef USE_COUNT_TIME //Added by zenglei@2018-12-06
// double dxReallocateWorldProcessContextValue = 0;
// double dxProcessIslandsValue = 0;
// #endif
namespace gazebo
{
namespace physics

2
gazebo/gazebo/physics/ode/ODEPhysicsPrivate.hh
View File

@ -64,7 +64,7 @@ namespace gazebo
/// \brief Physics step function.
public: int (*physicsStepFunc)(dxWorld*, dReal);
// public: int (*physicsStepFunc1)(dxWorld*, dReal, double&, double&); //Added by zenglei@20190322
/// \brief All the collsiion spaces.
public: std::map<std::string, dSpaceID> spaces;

2
gazebo/gazebo/physics/simbody/SimbodyLink.cc
View File

@ -138,6 +138,7 @@ void SimbodyLink::SetGravityMode(bool _mode)
//////////////////////////////////////////////////
void SimbodyLink::ProcessSetGravityMode()
{
std::cout<<"=====================SimbodyLink::ProcessSetGravityMode================"<<std::endl; //Added by zenglei@20190306
if (this->gravityModeDirty)
{
if (this->physicsInitialized)
@ -305,6 +306,7 @@ void SimbodyLink::SetLinkStatic(bool _static)
//////////////////////////////////////////////////
void SimbodyLink::ProcessSetLinkStatic()
{
std::cout<<"=====================SimbodyLink::ProcessSetLinkStatic================"<<std::endl; //Added by zenglei@20190306
if (this->masterMobod.isEmptyHandle())
return;

1
gazebo/gazebo/sensors/SensorManager.cc
View File

@ -746,6 +746,7 @@ void SimTimeEventHandler::AddRelativeEvent(const common::Time &_time,
/////////////////////////////////////////////////
void SimTimeEventHandler::OnUpdate(const common::UpdateInfo &_info)
{
// std::cout<<"=====================SimTimeEventHandler::OnUpdate================"<<std::endl; //Added by zenglei@20190306
boost::mutex::scoped_lock timingLock(g_sensorTimingMutex);
boost::mutex::scoped_lock lock(this->mutex);

45
gazebo/gazebo/server_main.cc
View File

@ -19,25 +19,62 @@
#include "gazebo/common/Console.hh"
#include "gazebo/Server.hh"
#include "gazebo/countTime.hh" //Added by zenglei@2018-12-04
//////////////////////////////////////////////////
int main(int argc, char **argv)
{
{
#ifdef USE_COUNT_TIME //Added by zenglei@2018-12-04
struct timeval tv;
double start_time,cur_time,tmp_time;
gettimeofday(&tv,NULL);
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
start_time = cur_time;
#endif
gazebo::Server *server = NULL;
try
{
// Initialize the informational logger. This will log warnings, and
// errors.
gzLogInit("server-", "gzserver.log");
// gzLogInit("server-", "gzserver.log"); //Closed by zenglei@2018-11-26
// Initialize the data logger. This will log state information.
gazebo::util::LogRecord::Instance()->Init("gzserver");
// gazebo::util::LogRecord::Instance()->Init("gzserver"); //Closed by zenglei@2018-11-26
//Add Test Debug Info by zenglei@2018-11-26
//std::cout<<"---------------------------------Test Debug Info--------------------------------\n";
server = new gazebo::Server();
#ifdef USE_COUNT_TIME //Added by zenglei@2018-12-04
gettimeofday(&tv, NULL); //Added by zenglei@2018-12-04
tmp_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
std::cout<<"new gazebo::Server cost time: "<<tmp_time - cur_time<<std::endl;
cur_time = tmp_time;
#endif
if (!server->ParseArgs(argc, argv))
return -1;
#ifdef USE_COUNT_TIME //Added by zenglei@2018-12-04
gettimeofday(&tv, NULL); //Added by zenglei@2018-12-04
tmp_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
std::cout<<"server->ParseArgs cost time: "<<tmp_time - cur_time<<std::endl;
cur_time = tmp_time;
#endif
server->Run();
#ifdef USE_COUNT_TIME //Added by zenglei@2018-12-04
gettimeofday(&tv, NULL); //Added by zenglei@2018-12-04
tmp_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
std::cout<<"server->Run cost time: "<<tmp_time - cur_time<<std::endl;
std::cout<<"server main cost time: "<<tmp_time - start_time<<std::endl;
#endif
server->Fini();
delete server;

2
gazebo/gazebo/transport/Connection.hh
View File

@ -19,6 +19,7 @@
#include <tbb/task.h>
#include <google/protobuf/message.h>
// #include <tbb/task_scheduler_init.h> //Added by zenglei@20190222
#include <boost/asio.hpp>
#include <boost/bind.hpp>
@ -309,6 +310,7 @@ namespace gazebo
if (!_e && !transport::is_stopped())
{
// tbb::task_scheduler_init connectInit(10); //Added by zenglei@20190222
ConnectionReadTask *task = new(tbb::task::allocate_root())
ConnectionReadTask(boost::get<0>(_handler), data);
tbb::task::enqueue(*task);

3
gazebo/gazebo/transport/ConnectionManager.cc
View File

@ -20,6 +20,8 @@
#include <Winsock2.h>
#endif
// #include <tbb/task_scheduler_init.h> //Added by zenglei@20190222
#include <boost/bind.hpp>
#include "gazebo/msgs/msgs.hh"
@ -412,6 +414,7 @@ void ConnectionManager::ProcessMessage(const std::string &_data)
if (pub.host() != this->serverConn->GetLocalAddress() ||
pub.port() != this->serverConn->GetLocalPort())
{
// tbb::task_scheduler_init TopicInit(10); //Added by zenglei@20190222
TopicManagerConnectionTask *task = new(tbb::task::allocate_root())
TopicManagerConnectionTask(pub);
tbb::task::enqueue(*task);

3
gazebo/gazebo/transport/Node.hh
View File

@ -26,6 +26,8 @@
#include <string>
#include <vector>
// #include <tbb/task_scheduler_init.h> //Added by zenglei@20190222
#include "gazebo/transport/TransportTypes.hh"
#include "gazebo/transport/TopicManager.hh"
#include "gazebo/util/system.hh"
@ -158,6 +160,7 @@ namespace gazebo
void Publish(const std::string &_topic,
const google::protobuf::Message &_message)
{
// tbb::task_scheduler_init nodeInit(10); //Added by zenglei@20190222
transport::PublisherPtr pub = this->Advertise<M>(_topic);
PublishTask *task = new(tbb::task::allocate_root())
PublishTask(pub, _message);

1
gazebo/gazebo/transport/TopicManager.cc
View File

@ -24,6 +24,7 @@
#include <tbb/parallel_for.h>
#include <tbb/blocked_range.h>
#include <boost/function.hpp>
#include "gazebo/msgs/msgs.hh"
#include "gazebo/transport/Node.hh"

1
gazebo/gazebo/util/Diagnostics.cc
View File

@ -117,6 +117,7 @@ boost::filesystem::path DiagnosticManager::LogPath() const
//////////////////////////////////////////////////
void DiagnosticManager::Update(const common::UpdateInfo &_info)
{
// std::cout<<"=====================DiagnosticManager::Update================"<<std::endl; //Added by zenglei@20190306
if (_info.realTime > common::Time::Zero)
{
this->dataPtr->msg.set_real_time_factor(

1
gazebo/plugins/ActuatorPlugin.cc
View File

@ -164,6 +164,7 @@ void ActuatorPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
//////////////////////////////////////////////////
void ActuatorPlugin::WorldUpdateCallback()
{
std::cout<<"=====================ActuatorPlugin::WorldUpdateCallback================"<<std::endl; //Added by zenglei@20190306
// Update the stored joints according to the desired model.
for (unsigned int i = 0; i < this->joints.size(); i++)
{

1
gazebo/plugins/BreakableJointPlugin.cc
View File

@ -69,6 +69,7 @@ void BreakableJointPlugin::OnUpdate(msgs::WrenchStamped _msg)
/////////////////////////////////////////////////
void BreakableJointPlugin::OnWorldUpdate()
{
std::cout<<"=====================BreakableJointPlugin::OnWorldUpdate================"<<std::endl; //Added by zenglei@20190306
this->parentSensor->SetActive(false);
this->parentJoint->Detach();
this->parentJoint->SetProvideFeedback(false);

1
gazebo/plugins/BuoyancyPlugin.cc
View File

@ -155,6 +155,7 @@ void BuoyancyPlugin::Init()
/////////////////////////////////////////////////
void BuoyancyPlugin::OnUpdate()
{
std::cout<<"=====================BuoyancyPlugin::OnUpdate================"<<std::endl; //Added by zenglei@20190306
for (auto link : this->model->GetLinks())
{
VolumeProperties volumeProperties = this->volPropsMap[link->GetId()];

1
gazebo/plugins/CartDemoPlugin.cc
View File

@ -107,6 +107,7 @@ void CartDemoPlugin::Init()
/////////////////////////////////////////////////
void CartDemoPlugin::OnUpdate()
{
std::cout<<"=====================CartDemoPlugin::OnUpdate================"<<std::endl; //Added by zenglei@20190306
common::Time currTime = this->model->GetWorld()->GetSimTime();
common::Time stepTime = currTime - this->prevUpdateTime;
this->prevUpdateTime = currTime;

1
gazebo/plugins/CessnaPlugin.cc
View File

@ -151,6 +151,7 @@ void CessnaPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
/////////////////////////////////////////////////
void CessnaPlugin::Update(const common::UpdateInfo &/*_info*/)
{
std::cout<<"=====================CessnaPlugin::Update================"<<std::endl; //Added by zenglei@20190306
std::lock_guard<std::mutex> lock(this->mutex);
gazebo::common::Time curTime = this->model->GetWorld()->GetSimTime();

1
gazebo/plugins/ContainPlugin.cc
View File

@ -222,6 +222,7 @@ void ContainPlugin::Enable(ConstIntPtr &_msg)
/////////////////////////////////////////////////
void ContainPlugin::OnUpdate(const common::UpdateInfo &/*_info*/)
{
std::cout<<"=====================ContainPlugin::OnUpdate================"<<std::endl; //Added by zenglei@20190306
// Only get the entity once
physics::EntityPtr entity = this->dataPtr->entity.lock();
if (!entity)

2
gazebo/plugins/DiffDrivePlugin.cc
View File

@ -109,7 +109,7 @@ void DiffDrivePlugin::OnUpdate()
common::Time currTime = this->model->GetWorld()->GetSimTime();
common::Time stepTime = currTime - this->prevUpdateTime;
*/
std::cout<<"=====================DiffDrivePlugin::OnUpdate================"<<std::endl; //Added by zenglei@20190306
double leftVelDesired = (this->wheelSpeed[LEFT] / this->wheelRadius);
double rightVelDesired = (this->wheelSpeed[RIGHT] / this->wheelRadius);

1
gazebo/plugins/ElevatorPlugin.cc
View File

@ -175,6 +175,7 @@ void ElevatorPlugin::MoveToFloor(const int _floor)
/////////////////////////////////////////////////
void ElevatorPlugin::Update(const common::UpdateInfo &_info)
{
std::cout<<"=====================ElevatorPlugin::Update================"<<std::endl; //Added by zenglei@20190306
std::lock_guard<std::mutex> lock(this->dataPtr->stateMutex);
// Process the states

1
gazebo/plugins/FollowerPlugin.cc
View File

@ -252,6 +252,7 @@ bool FollowerPlugin::FindSensor(const physics::ModelPtr &_model)
/////////////////////////////////////////////////
void FollowerPlugin::OnUpdate()
{
std::cout<<"=====================FollowerPlugin::OnUpdate================"<<std::endl; //Added by zenglei@20190306
std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
// Update follower.

1
gazebo/plugins/HarnessPlugin.cc
View File

@ -216,6 +216,7 @@ void HarnessPlugin::Init()
/////////////////////////////////////////////////
void HarnessPlugin::OnUpdate(const common::UpdateInfo &_info)
{
std::cout<<"=====================HarnessPlugin::OnUpdate================"<<std::endl; //Added by zenglei@20190306
// Bootstrap the time.
if (this->prevSimTime == common::Time::Zero)
{

1
gazebo/plugins/HydraDemoPlugin.cc
View File

@ -64,6 +64,7 @@ void HydraDemoPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
/////////////////////////////////////////////////
void HydraDemoPlugin::Update(const common::UpdateInfo & /*_info*/)
{
std::cout<<"=====================HydraDemoPlugin::Update================"<<std::endl; //Added by zenglei@20190306
boost::mutex::scoped_lock lock(this->msgMutex);
// Return if we don't have messages yet

1
gazebo/plugins/HydraPlugin.cc
View File

@ -191,6 +191,7 @@ void RazerHydra::Load(physics::WorldPtr _world, sdf::ElementPtr /*_sdf*/)
/////////////////////////////////////////////////
void RazerHydra::Update(const common::UpdateInfo & /*_info*/)
{
std::cout<<"=====================RazerHydra::Update================"<<std::endl; //Added by zenglei@20190306
boost::mutex::scoped_lock lock(this->mutex);
math::Pose origRight(this->pos[1], this->quat[1]);

1
gazebo/plugins/JointTrajectoryPlugin.cc
View File

@ -82,6 +82,7 @@ void JointTrajectoryPlugin::Load(physics::ModelPtr _parent,
/////////////////////////////////////////////////
void JointTrajectoryPlugin::UpdateStates(const common::UpdateInfo & /*_info*/)
{
std::cout<<"=====================JointTrajectoryPlugin::UpdateStates================"<<std::endl; //Added by zenglei@20190306
common::Time cur_time = this->world->GetSimTime();
// for (physics::Joint_V::const_iterator j = this->model->GetJoints().begin();

1
gazebo/plugins/LiftDragPlugin.cc
View File

@ -157,6 +157,7 @@ void LiftDragPlugin::Load(physics::ModelPtr _model,
/////////////////////////////////////////////////
void LiftDragPlugin::OnUpdate()
{
std::cout<<"=====================LiftDragPlugin::OnUpdate================"<<std::endl; //Added by zenglei@20190306
GZ_ASSERT(this->link, "Link was NULL");
// get linear velocity at cp in inertial frame
math::Vector3 velI = this->link->GetWorldLinearVel(this->cp);

1
gazebo/plugins/ModelPropShop.cc
View File

@ -141,6 +141,7 @@ void ModelPropShop::OnWorldCreated()
/////////////////////////////////////////////
void ModelPropShop::Update()
{
std::cout<<"=====================ModelPropShop::Update================"<<std::endl; //Added by zenglei@20190306
// Make sure to initialize the rendering engine in the same thread that will
// capture images.
if (!this->scene)

1
gazebo/plugins/MudPlugin.cc
View File

@ -173,6 +173,7 @@ void MudPlugin::OnContact(ConstContactsPtr &_msg)
/////////////////////////////////////////////////
void MudPlugin::OnUpdate()
{
std::cout<<"=====================MudPlugin::OnUpdate================"<<std::endl; //Added by zenglei@20190306
double dt = this->physics->GetMaxStepSize();
if (dt < 1e-6)
dt = 1e-6;

1
gazebo/plugins/RandomVelocityPlugin.cc
View File

@ -142,6 +142,7 @@ void RandomVelocityPlugin::Reset()
/////////////////////////////////////////////////
void RandomVelocityPlugin::Update(const common::UpdateInfo &_info)
{
std::cout<<"=====================RandomVelocityPlugin::Update(================"<<std::endl; //Added by zenglei@20190306
GZ_ASSERT(this->dataPtr->link, "<link> in RandomVelocity plugin is null");
// Short-circuit in case the link is invalid.

1
gazebo/plugins/SphereAtlasDemoPlugin.cc
View File

@ -180,6 +180,7 @@ void SphereAtlasDemoPlugin::Reset()
/////////////////////////////////////////////////
void SphereAtlasDemoPlugin::OnUpdate()
{
std::cout<<"=====================SphereAtlasDemoPlugin::OnUpdate================"<<std::endl; //Added by zenglei@20190306
common::Time currTime = this->model->GetWorld()->GetSimTime();
common::Time stepTime = currTime - this->prevUpdateTime;
this->prevUpdateTime = currTime;

1
gazebo/plugins/TouchPlugin.cc
View File

@ -146,6 +146,7 @@ void TouchPlugin::Enable(ConstIntPtr &_msg)
/////////////////////////////////////////////////
void TouchPlugin::OnUpdate(const common::UpdateInfo &_info)
{
std::cout<<"=====================TouchPlugin::OnUpdate================"<<std::endl; //Added by zenglei@20190306
// Get all contacts across all sensors
msgs::Contacts contacts;

1
gazebo/plugins/TransporterPlugin.cc
View File

@ -146,6 +146,7 @@ void TransporterPlugin::OnActivation(ConstGzStringPtr &_msg)
/////////////////////////////////////////////////
void TransporterPlugin::Update()
{
std::cout<<"=====================TransporterPlugin::Update================"<<std::endl; //Added by zenglei@20190306
// Get all the models
physics::Model_V models = this->dataPtr->world->GetModels();

1
gazebo/plugins/VehiclePlugin.cc
View File

@ -182,6 +182,7 @@ void VehiclePlugin::Init()
/////////////////////////////////////////////////
void VehiclePlugin::OnUpdate()
{
std::cout<<"=====================VehiclePlugin::OnUpdate================"<<std::endl; //Added by zenglei@20190306
// Get the normalized gas and brake amount
double gas = this->gasJoint->GetAngle(0).Radian() / this->maxGas;
double brake = this->brakeJoint->GetAngle(0).Radian() / this->maxBrake;

1
gazebo/plugins/events/InRegionEventSource.cc
View File

@ -94,6 +94,7 @@ void InRegionEventSource::Info() const
////////////////////////////////////////////////////////////////////////////////
void InRegionEventSource::Update()
{
std::cout<<"=====================InRegionEventSource::Update================"<<std::endl; //Added by zenglei@20190306
// model must exist
if (!this->model)
return;

1
gazebo/plugins/events/JointEventSource.cc
View File

@ -204,6 +204,7 @@ bool JointEventSource::LookupJoint()
////////////////////////////////////////////////////////////////////////////////
void JointEventSource::Update()
{
std::cout<<"=====================JointEventSource::Update================"<<std::endl; //Added by zenglei@20190306
if (!this->LookupJoint())
return;

1
gazebo/plugins/events/OccupiedEventSource.cc
View File

@ -107,6 +107,7 @@ void OccupiedEventSource::Load(const sdf::ElementPtr _sdf)
/////////////////////////////////////////////////
void OccupiedEventSource::Update()
{
std::cout<<"=====================OccupiedEventSource::Update================"<<std::endl; //Added by zenglei@20190306
// Get all the models.
physics::Model_V models = this->world->GetModels();

1
gazebo/plugins/events/RegionEventBoxPlugin.cc
View File

@ -101,6 +101,7 @@ void RegionEventBoxPlugin::OnModelMsg(ConstModelPtr &_msg)
//////////////////////////////////////////////////
void RegionEventBoxPlugin::OnUpdate(const common::UpdateInfo &_info)
{
std::cout<<"=====================RegionEventBoxPlugin::OnUpdate================"<<std::endl; //Added by zenglei@20190306
// check to see if the box visual geometry has changed size or pose
// due to user interaction via the gui, if so update region dimensions
{

1
gazebo/plugins/events/SimStateEventSource.cc
View File

@ -51,6 +51,7 @@ void SimStateEventSource::Load(const sdf::ElementPtr _sdf)
////////////////////////////////////////////////////////////////////////////////
void SimStateEventSource::OnUpdate(const common::UpdateInfo &_info)
{
std::cout<<"=====================SimStateEventSource::OnUpdate================"<<std::endl; //Added by zenglei@20190306
if (_info.simTime < this->simTime)
{
std::string json;

1
gazebo/test/plugins/ForceTorqueModelRemovalTestPlugin.cc
View File

@ -63,6 +63,7 @@ void ForceTorqueModelRemovalTestPlugin::Load(sensors::SensorPtr _sensor,
void ForceTorqueModelRemovalTestPlugin::onUpdate(
const gazebo::common::UpdateInfo & /*_info*/)
{
std::cout<<"=====================ForceTorqueModelRemovalTestPlugin::onUpdate================"<<std::endl; //Added by zenglei@20190306
ignition::math::Vector3d force;
ignition::math::Vector3d torque;

1
gazebo/test/plugins/ModelTrajectoryTestPlugin.cc
View File

@ -59,6 +59,7 @@ void ModelTrajectoryTestPlugin::OnPoseTrajectoryMsg(
/////////////////////////////////////////////////
void ModelTrajectoryTestPlugin::OnUpdate()
{
std::cout<<"=====================ModelTrajectoryTestPlugin::OnUpdate================"<<std::endl; //Added by zenglei@20190306
common::Time currTime = this->model->GetWorld()->GetSimTime();
common::Time stepTime = currTime - this->prevUpdateTime;
this->prevUpdateTime = currTime;

1
gazebo/test/plugins/SpringTestPlugin.cc
View File

@ -63,6 +63,7 @@ void SpringTestPlugin::Init()
/////////////////////////////////////////////////
void SpringTestPlugin::ExplicitUpdate()
{
std::cout<<"=====================SpringTestPlugin::ExplicitUpdate================"<<std::endl; //Added by zenglei@20190306
common::Time currTime = this->model->GetWorld()->GetSimTime();
common::Time stepTime = currTime - this->prevUpdateTime;
this->prevUpdateTime = currTime;