From a194b029358f16386bef204b5d11ad62131a6dbc Mon Sep 17 00:00:00 2001 From: XunMeng2017 Date: Mon, 25 Mar 2019 11:35:23 +0800 Subject: [PATCH] =?UTF-8?q?=E5=88=A9=E7=94=A8OpenMp=E3=80=81TBB=E3=80=81?= =?UTF-8?q?=20C++Thread=E4=BC=98=E5=8C=96=E6=A8=A1=E5=9E=8B=E9=A2=84?= =?UTF-8?q?=E5=A4=84=E7=90=86=E6=A8=A1=E5=9D=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README | 54 +- gazebo/.vscode/settings.json | 59 + gazebo/CMakeLists.txt | 2 +- gazebo/deps/CMakeLists.txt | 2 +- gazebo/deps/opende/include/ode/common.h | 5 + gazebo/deps/opende/include/ode/objects.h | 2 +- gazebo/deps/opende/src/ode.cpp | 57 +- gazebo/deps/opende/src/quickstep.cpp | 78 +- gazebo/deps/opende/src/quickstep_cg_lcp.cpp | 6 +- gazebo/deps/opende/src/quickstep_pgs_lcp.cpp | 3 +- gazebo/deps/opende/src/quickstep_util.cpp | 64 +- gazebo/deps/opende/src/quickstep_util.h | 2 +- gazebo/deps/opende/src/util.cpp | 27 +- gazebo/deps/parallel_quickstep/CMakeLists.txt | 1 + .../parallel_quickstep/src/openmp_kernels.cpp | 6 +- .../parallel_quickstep/src/openmp_utils.h | 6 +- gazebo/examples/plugins/camera/camera_move.cc | 1 + .../custom_messages/custom_messages.cc | 1 + .../examples/plugins/model_push/model_push.cc | 1 + gazebo/examples/plugins/pr2_pose_test.cc | 1 + .../examples/plugins/projector/projector.cc | 1 + gazebo/examples/plugins/ray_test.cc | 1 + gazebo/gazebo/CMakeLists.txt | 5 + gazebo/gazebo/Server.cc | 64 +- gazebo/gazebo/common/Event.hh | 1658 +++++++++++------ gazebo/gazebo/common/Event_1.hh | 771 ++++++++ gazebo/gazebo/common/Event_2.hh | 771 ++++++++ gazebo/gazebo/common/Event_backup.hh | 1549 +++++++++++++++ gazebo/gazebo/common/Events.cc | 8 +- gazebo/gazebo/common/Events.hh | 4 +- gazebo/gazebo/countTime.hh | 13 + gazebo/gazebo/physics/CMakeLists.txt | 14 +- gazebo/gazebo/physics/Entity.cc | 1 + gazebo/gazebo/physics/HelloWorld.cc | 129 ++ gazebo/gazebo/physics/HelloWorld.h | 225 +++ .../gazebo/physics/HelloWorldPubSubTypes.cc | 121 ++ gazebo/gazebo/physics/HelloWorldPubSubTypes.h | 52 + gazebo/gazebo/physics/HelloWorldPublisher.cc | 170 ++ gazebo/gazebo/physics/HelloWorldPublisher.h | 62 + gazebo/gazebo/physics/HelloWorldPublisher.xml | 13 + gazebo/gazebo/physics/HelloWorldSubscriber.cc | 142 ++ gazebo/gazebo/physics/HelloWorldSubscriber.h | 65 + .../gazebo/physics/HelloWorldSubscriber.xml | 13 + gazebo/gazebo/physics/Link.cc | 1 + gazebo/gazebo/physics/World.cc | 756 +++++++- gazebo/gazebo/physics/World.hh | 421 ++++- gazebo/gazebo/physics/ode/ODELink.cc | 2 + gazebo/gazebo/physics/ode/ODEPhysics.cc | 53 +- gazebo/gazebo/physics/ode/ODEPhysics.hh | 5 + .../gazebo/physics/ode/ODEPhysicsPrivate.hh | 2 +- gazebo/gazebo/physics/simbody/SimbodyLink.cc | 2 + gazebo/gazebo/sensors/SensorManager.cc | 1 + gazebo/gazebo/server_main.cc | 45 +- gazebo/gazebo/transport/Connection.hh | 2 + gazebo/gazebo/transport/ConnectionManager.cc | 3 + gazebo/gazebo/transport/Node.hh | 3 + gazebo/gazebo/transport/TopicManager.cc | 1 + gazebo/gazebo/util/Diagnostics.cc | 1 + gazebo/plugins/ActuatorPlugin.cc | 1 + gazebo/plugins/BreakableJointPlugin.cc | 1 + gazebo/plugins/BuoyancyPlugin.cc | 1 + gazebo/plugins/CartDemoPlugin.cc | 1 + gazebo/plugins/CessnaPlugin.cc | 1 + gazebo/plugins/ContainPlugin.cc | 1 + gazebo/plugins/DiffDrivePlugin.cc | 2 +- gazebo/plugins/ElevatorPlugin.cc | 1 + gazebo/plugins/FollowerPlugin.cc | 1 + gazebo/plugins/HarnessPlugin.cc | 1 + gazebo/plugins/HydraDemoPlugin.cc | 1 + gazebo/plugins/HydraPlugin.cc | 1 + gazebo/plugins/JointTrajectoryPlugin.cc | 1 + gazebo/plugins/LiftDragPlugin.cc | 1 + gazebo/plugins/ModelPropShop.cc | 1 + gazebo/plugins/MudPlugin.cc | 1 + gazebo/plugins/RandomVelocityPlugin.cc | 1 + gazebo/plugins/SphereAtlasDemoPlugin.cc | 1 + gazebo/plugins/TouchPlugin.cc | 1 + gazebo/plugins/TransporterPlugin.cc | 1 + gazebo/plugins/VehiclePlugin.cc | 1 + gazebo/plugins/events/InRegionEventSource.cc | 1 + gazebo/plugins/events/JointEventSource.cc | 1 + gazebo/plugins/events/OccupiedEventSource.cc | 1 + gazebo/plugins/events/RegionEventBoxPlugin.cc | 1 + gazebo/plugins/events/SimStateEventSource.cc | 1 + .../ForceTorqueModelRemovalTestPlugin.cc | 1 + .../test/plugins/ModelTrajectoryTestPlugin.cc | 1 + gazebo/test/plugins/SpringTestPlugin.cc | 1 + 87 files changed, 6873 insertions(+), 685 deletions(-) create mode 100644 gazebo/.vscode/settings.json create mode 100644 gazebo/gazebo/common/Event_1.hh create mode 100644 gazebo/gazebo/common/Event_2.hh create mode 100644 gazebo/gazebo/common/Event_backup.hh create mode 100644 gazebo/gazebo/countTime.hh create mode 100644 gazebo/gazebo/physics/HelloWorld.cc create mode 100644 gazebo/gazebo/physics/HelloWorld.h create mode 100644 gazebo/gazebo/physics/HelloWorldPubSubTypes.cc create mode 100644 gazebo/gazebo/physics/HelloWorldPubSubTypes.h create mode 100644 gazebo/gazebo/physics/HelloWorldPublisher.cc create mode 100644 gazebo/gazebo/physics/HelloWorldPublisher.h create mode 100644 gazebo/gazebo/physics/HelloWorldPublisher.xml create mode 100644 gazebo/gazebo/physics/HelloWorldSubscriber.cc create mode 100644 gazebo/gazebo/physics/HelloWorldSubscriber.h create mode 100644 gazebo/gazebo/physics/HelloWorldSubscriber.xml diff --git a/README b/README index edf29a3..f2b0ef0 100644 --- a/README +++ b/README @@ -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 - \ No newline at end of file +注意事项： + 1、针对G+R架构下Gazebo7.14上运行30架Hector四旋翼无人机达不到1的情况，基于程序插桩技术，得到Gazebo仿真运行瓶颈——模型预处理模块，该模块由Gazebo事件机制实现。 + 2、针对该模块采用并行技术进行优化，并行手段包括OpenMp、TBB、线程池、C++11Thtead。在sdformat包中增加标签用于设置并行参数。其中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标签各属性含义如下： + 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或源码方式进行缺失依赖库的安装。 diff --git a/gazebo/.vscode/settings.json b/gazebo/.vscode/settings.json new file mode 100644 index 0000000..c9ddbff --- /dev/null +++ b/gazebo/.vscode/settings.json @@ -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" + } +} \ No newline at end of file diff --git a/gazebo/CMakeLists.txt b/gazebo/CMakeLists.txt index fae1488..e807bcb 100644 --- a/gazebo/CMakeLists.txt +++ b/gazebo/CMakeLists.txt @@ -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 diff --git a/gazebo/deps/CMakeLists.txt b/gazebo/deps/CMakeLists.txt index 2d2080c..1372dfb 100644 --- a/gazebo/deps/CMakeLists.txt +++ b/gazebo/deps/CMakeLists.txt @@ -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() diff --git a/gazebo/deps/opende/include/ode/common.h b/gazebo/deps/opende/include/ode/common.h index c0602fe..d2003ee 100644 --- a/gazebo/deps/opende/include/ode/common.h +++ b/gazebo/deps/opende/include/ode/common.h @@ -27,6 +27,11 @@ #include +//Add by zenglei@2018-11-21 +//#ifndef USE_TPROW +//#define USE_TPROW +//#endif + #ifdef __cplusplus extern "C" { #endif diff --git a/gazebo/deps/opende/include/ode/objects.h b/gazebo/deps/opende/include/ode/objects.h index 610c44c..b12b5cd 100644 --- a/gazebo/deps/opende/include/ode/objects.h +++ b/gazebo/deps/opende/include/ode/objects.h @@ -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. diff --git a/gazebo/deps/opende/src/ode.cpp b/gazebo/deps/opende/src/ode.cpp index 9bff73c..13dc3aa 100644 --- a/gazebo/deps/opende/src/ode.cpp +++ b/gazebo/deps/opende/src/ode.cpp @@ -41,12 +41,23 @@ #include "odetls.h" #include "robuststep.h" +#include "gazebo/countTime.hh" //Added by zenglei@2018-12-06 +// #include //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"< 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"< 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: "< 0,"stepsize must be > 0"); bool result = false; + + if (dxReallocateWorldProcessContext (w, stepsize, &dxEstimateQuickStepMemoryRequirements)) { dxProcessIslands (w, stepsize, &dxQuickStepper); - result = true; } diff --git a/gazebo/deps/opende/src/quickstep.cpp b/gazebo/deps/opende/src/quickstep.cpp index 31a5f7e..8c358e4 100644 --- a/gazebo/deps/opende/src/quickstep.cpp +++ b/gazebo/deps/opende/src/quickstep.cpp @@ -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 //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-----------------"<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: "< 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: "< -#endif +// #ifndef _WIN32 //Added by zenglei@2018-12-10 +// #include +// #endif #include "quickstep_util.h" #include "quickstep_cg_lcp.h" diff --git a/gazebo/deps/opende/src/quickstep_pgs_lcp.cpp b/gazebo/deps/opende/src/quickstep_pgs_lcp.cpp index a11abfd..6713a97 100644 --- a/gazebo/deps/opende/src/quickstep_pgs_lcp.cpp +++ b/gazebo/deps/opende/src/quickstep_pgs_lcp.cpp @@ -51,6 +51,7 @@ using namespace ode; static void* ComputeRows(void *p) { + // std::cout<<"---------------------ComputeRows---------------------"<AllocateArray (m*12); compute_invM_JT (m,J,iMJ,jb,body,invMOI); diff --git a/gazebo/deps/opende/src/quickstep_util.cpp b/gazebo/deps/opende/src/quickstep_util.cpp index 74510a9..4b47515 100644 --- a/gazebo/deps/opende/src/quickstep_util.cpp +++ b/gazebo/deps/opende/src/quickstep_util.cpp @@ -30,10 +30,11 @@ #include "objects.h" #include "joints/joint.h" #include "util.h" +//#include //Add by zenglei@2018-12-03 -#ifndef _WIN32 - #include -#endif +// #ifndef _WIN32 //Added by zenglei@2018-12-10 +// #include +// #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 #include +// #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< 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(); } @@ -38,7 +38,7 @@ void ompPGSReduce( typename vec4::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::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; diff --git a/gazebo/deps/parallel_quickstep/src/openmp_utils.h b/gazebo/deps/parallel_quickstep/src/openmp_utils.h index eb636d8..6368959 100644 --- a/gazebo/deps/parallel_quickstep/src/openmp_utils.h +++ b/gazebo/deps/parallel_quickstep/src/openmp_utils.h @@ -9,13 +9,13 @@ static inline void myAtomicVecAdd(typename vec4::Type& a, typename vec4::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; */ } diff --git a/gazebo/examples/plugins/camera/camera_move.cc b/gazebo/examples/plugins/camera/camera_move.cc index eb7c15e..0bd4110 100644 --- a/gazebo/examples/plugins/camera/camera_move.cc +++ b/gazebo/examples/plugins/camera/camera_move.cc @@ -38,6 +38,7 @@ namespace gazebo // Called by the world update start event public: void OnUpdate() { + std::cout<<"====================CameraMove::OnUpdate================="<model->GetWorldPose(); v = pose.rot * v; diff --git a/gazebo/examples/plugins/custom_messages/custom_messages.cc b/gazebo/examples/plugins/custom_messages/custom_messages.cc index a8d91e3..d91e96f 100644 --- a/gazebo/examples/plugins/custom_messages/custom_messages.cc +++ b/gazebo/examples/plugins/custom_messages/custom_messages.cc @@ -39,6 +39,7 @@ namespace gazebo // Called by the world update start event public: void OnUpdate() { + std::cout<<"=====================CustomMessages::OnUpdate===================="<pub->Publish(msg); diff --git a/gazebo/examples/plugins/model_push/model_push.cc b/gazebo/examples/plugins/model_push/model_push.cc index 10473bd..01328f1 100644 --- a/gazebo/examples/plugins/model_push/model_push.cc +++ b/gazebo/examples/plugins/model_push/model_push.cc @@ -38,6 +38,7 @@ namespace gazebo // Called by the world update start event public: void OnUpdate(const common::UpdateInfo & /*_info*/) { + std::cout<<"======================ModelPush::OnUpdate==================="<model->SetLinearVel(math::Vector3(.3, 0, 0)); } diff --git a/gazebo/examples/plugins/pr2_pose_test.cc b/gazebo/examples/plugins/pr2_pose_test.cc index 953cb72..9dc60e0 100644 --- a/gazebo/examples/plugins/pr2_pose_test.cc +++ b/gazebo/examples/plugins/pr2_pose_test.cc @@ -58,6 +58,7 @@ namespace gazebo // Called by the world update start event public: void OnUpdate() { + std::cout<<"=======================PR2PoseTest::OnUpdate===================="<simTime = this->world->GetSimTime(); math::Pose orig_pose = this->model->GetWorldPose(); diff --git a/gazebo/examples/plugins/projector/projector.cc b/gazebo/examples/plugins/projector/projector.cc index a0b32e9..b8eebe5 100644 --- a/gazebo/examples/plugins/projector/projector.cc +++ b/gazebo/examples/plugins/projector/projector.cc @@ -50,6 +50,7 @@ namespace gazebo ////////////////////////////////////////////////// public: void OnUpdate() { + std::cout<<"=====================ProjectorPlugin::OnUpdate==================="<prevTime > common::Time(2, 0)) { this->state = !this->state; diff --git a/gazebo/examples/plugins/ray_test.cc b/gazebo/examples/plugins/ray_test.cc index 894137a..915ce97 100644 --- a/gazebo/examples/plugins/ray_test.cc +++ b/gazebo/examples/plugins/ray_test.cc @@ -58,6 +58,7 @@ namespace gazebo // Called by the world update start event public: void OnUpdate() { + std::cout<<"=====================RayTest::OnUpdate================"<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: "<GetElement("world")->HasElement("event")) + if(_elem->GetElement("world")->HasElement("parallel")) + { + sdf::ElementPtr eventElem = _elem->GetElement("world")->GetElement("parallel"); + event::Events::worldUpdateBegin.SetThreadInfo(eventElem->Get("method"),eventElem->Get("numbers_of_thread"), eventElem->Get("size_of_block"), eventElem->Get("type")); + // if(6 == eventElem->Get("method")) + // event::Events::worldUpdateBegin.InitThreadPool(eventElem->Get("numbers_of_thread")); + // std::cout<<"=======numbers_of_thread: "<Get("numbers_of_thread")<<"\t size_of_block: "<Get("size_of_block")<<"============="<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: "<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: "< #include +#include //Added by zenglei@2019-01-09 +// #include //Added by zenglei@2019-01-09 +// #include //Added by zenglei@2018-12-20 +#include //Added by zenglei@2018-12-25 +#include //Added by zenglei@2018-12-25 +// #include //Added by zenglei@2018-12-25 +#include //Added by zenglei@2018-12-25 +#include //Added by zenglei@2018-12-26 +#include //Added by zenglei@2018-12-26 +// #include //Added by zenglei@2019-01-03 +#include //Added by zenglei@2019-01-08 +#include //Added by zenglei@2019-01-15 +#define CALLBACK_NUM 217 //Added by zenglei@2019-03-11 #include #include #include #include #include "gazebo/util/system.hh" +#include "gazebo/countTime.hh" //Addded by zenglei@2019-01-04 + +#include "gazebo/countTime.hh" //Added by zhangshuai@2018-12-17 +// #include //Added by zhangshuai@2018-12-17 + +// #ifdef USE_COUNT_TIME +// double timeCallBack = 0; +// #endif 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. - /// \{ +/// \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(); +/// \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 + /// \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(); +/// \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 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] _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 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 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 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; - }; + /// \brief Data pointer. +protected: + EventPrivate *dataPtr; +}; - /// \cond - // Private data members for Connection class. - class GZ_COMMON_VISIBLE ConnectionPrivate - { - /// \brief Constructor. - public: ConnectionPrivate(); +/// \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 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 event for this connection - public: Event *event; + /// \brief the id set in the constructor +public: + int id; +public: + common::Time creationTime; +}; +/// \endcond - /// \brief the id set in the constructor - public: int id; +/// \brief A class that encapsulates a connection. +class GZ_COMMON_VISIBLE Connection +{ + /// \brief Constructor. +public: + Connection(); - /// \brief set during the constructor - public: common::Time creationTime; - }; - /// \endcond + /// \brief Constructor. + /// \param[in] _e Event pointer to connect with. + /// \param[in] _i Unique id. +public: + Connection(Event *_e, int _i); - /// \brief A class that encapsulates a connection. - class GZ_COMMON_VISIBLE Connection - { - /// \brief Constructor. - public: Connection(); + /// \brief Destructor. +public: + ~Connection(); - /// \brief Constructor. - /// \param[in] _e Event pointer to connect with. - /// \param[in] _i Unique id. - public: Connection(Event *_e, int _i); + /// \brief Get the id of this connection. + /// \return The id of this connection. +public: + int GetId() const; - /// \brief Destructor. - public: ~Connection(); + /// \brief Private data pointer. +private: + ConnectionPrivate *dataPtr; - /// \brief Get the id of this connection. - /// \return The id of this connection. - public: int GetId() const; + /// \brief Friend class. +public: + template + friend class EventT; +}; - /// \brief Private data pointer. - private: ConnectionPrivate *dataPtr; - - /// \brief Friend class. - public: template friend class EventT; - }; - - /// \internal - template - class EventConnection - { - /// \brief Constructor - public: EventConnection(const bool _on, +/// \internal +template +class EventConnection +{ + /// \brief Constructor +public: + EventConnection(const bool _on, boost::function *_cb) - : callback(_cb) - { - // Windows Visual Studio 2012 does not have atomic_bool constructor, - // so we have to set "on" using operator= - this->on = _on; - } + : 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 On/off value for the event callback +public: + std::atomic_bool on; - /// \brief Callback function - public: std::shared_ptr > callback; - }; + /// \brief Callback function +public: + std::shared_ptr> callback; +}; - /// \cond - // Private data members for EventT class. - template< typename T> - class EventTPrivate : public EventPrivate - { - /// \def EvtConnectionMap - /// \brief Event Connection map typedef. - typedef std::map > > - EvtConnectionMap; +/// \cond +// Private data members for EventT class. +template +class EventTPrivate : public EventPrivate +{ + /// \def EvtConnectionMap + /// \brief Event Connection map typedef. + typedef std::map>> + EvtConnectionMap; - /// \brief Array of connection callbacks. - public: EvtConnectionMap connections; + /// \brief Array of connection callbacks. +public: + EvtConnectionMap connections; - /// \brief A thread lock. - public: std::mutex mutex; + /// \brief A thread lock. +public: + std::mutex mutex; - /// \brief List of connections to remove - public: std::list - connectionsToRemove; - }; - /// \endcond + /// \brief List of connections to remove +public: + std::list + 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(); + //Added by zenglei@2019-01-03----Start + // template + // struct ThreadInfomation{ + // int start; + // int end; + // std::vector > > &vecCallback; + // P p; + // }; - /// \brief Destructor. - public: virtual ~EventT(); + // template + // void *thread_run(void *args) + // { + // P *thrInfo = (P*)args; + // for(int i = thrInfo->start; i < thrInfo->end; i++) + // { + // (*thrInfo->vecCallback[i])(thrInfo->p); + // } + // delete thrInfo; + // pthread_exit(NULL); + // } + //Added by zenglei@2019-01-03----End - /// \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 &_subscriber); +/// \class EventT Event.hh common/common.hh +/// \brief A class for event processing. +template +class EventT : public Event +{ + /// \brief Constructor. +public: + EventT(); - /// \brief Disconnect a callback to this event. - /// \param[in] _c The connection to disconnect. - public: virtual void Disconnect(ConnectionPtr _c); + /// \brief Destructor. +public: + virtual ~EventT(); - /// \brief Disconnect a callback to this event. - /// \param[in] _id The id of the connection to disconnect. - public: virtual void Disconnect(int _id); + /// \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 &_subscriber); - /// \brief Get the number of connections. - /// \return Number of connection to this Event. - public: unsigned int ConnectionCount() const; + /// \brief Disconnect a callback to this event. + /// \param[in] _c The connection to disconnect. +public: + virtual void Disconnect(ConnectionPtr _c); - /// \brief Access the signal. - public: void operator()() - {this->Signal();} + /// \brief Disconnect a callback to this event. + /// \param[in] _id The id of the connection to disconnect. +public: + virtual void Disconnect(int _id); - /// \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 Get the number of connections. + /// \return Number of connection to this Event. +public: + unsigned int ConnectionCount() const; - /// \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); - } +public: //Added by zenglei@2018-12-25 + void SetThreadInfo(int method,int thread_num, int block_size, int type); //Added by zenglei@2018-12-25 + // void InitThreadPool(int thread_num); //Added by zenglei@2019-01-09 + /// \brief Access the signal. +public: + void operator()() + { + this->Signal(); + } - /// \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 one parameter. + /// \param[in] _p the parameter. +public: + template + void operator()(const P &_p) + { + this->Signal(_p); + } - /// \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 two parameters. + /// \param[in] _p1 the first parameter. + /// \param[in] _p2 the second parameter. +public: + template + void operator()(const P1 &_p1, const P2 &_p2) + { + this->Signal(_p1, _p2); + } - /// \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 three parameters. + /// \param[in] _p1 the first parameter. + /// \param[in] _p2 the second parameter. + /// \param[in] _p3 the second parameter. +public: + template + void operator()(const P1 &_p1, const P2 &_p2, const P3 &_p3) + { + this->Signal(_p1, _p2, _p3); + } - /// \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 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 + 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 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 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 + 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 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() - { - 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) - { - 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) - { - 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) - { - 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 - void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3, - const P4 &_p4) - { - 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 - void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3, - const P4 &_p4, const P5 &_p5) - { - 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 - void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3, + /// \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 + void operator()(const P1 &_p1, const P2 &_p2, const P3 &_p3, const P4 &_p4, const P5 &_p5, const P6 &_p6) - { - this->Cleanup(); + { + this->Signal(_p1, _p2, _p3, _p4, _p5, _p6); + } - this->myDataPtr->signaled = true; - for (auto iter: this->myDataPtr->connections) + /// \brief Signal the event with seven parameters. + /// \param[in] _p1 the first parameter.EventConnection + /// \param[in] _p2 the second parameterEventConnection. + /// \param[in] _p3 the second parameterEventConnection. + /// \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 + 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 + 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 + /// \param[in] _p9 the ninth parameter.ColladaLoader.cc:2 +public: + template + 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 paramete_pr. + /// \param[in] _p3 the second paramete_pr. + /// \param[in] _p4 the first parameter_p. + /// \param[in] _p5 the fifth parameter_p. + /// \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 + 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-----------------"<Cleanup(); + + this->myDataPtr->signaled = true; + //Added by zenglei@2018-12-20-----------------Start + // std::cout<<"==============The count of Connects: "<myDataPtr->connections.size()<<"==============="<myDataPtr->connections.size() > 20) + // { + // for(auto iter : this->myDataPtr->connections) + // { + // if(iter.second->on) + // { + // if(this->callbackPool && this->callbackPool->size() > 0) + // { + // this->callbackPool->schedule([&iter](){ + // (*iter.second->callback)(); + // }); + // } + // else + // (*iter.second->callback)(); + // } + // } + // if(this->callbackPool && this->callbackPool->size() > 0) + // this->callbackPool->wait(); + // } + // else + // { + // for(auto iter : this->myDataPtr->connections) + // { + // if(iter.second->on) + // (*iter.second->callback)(); + // } + // } + + //Added by zenglei@2018-12-20------------------End + // auto const &iter = this->myDataPtr->connections.rbegin(); + // int index = iter->first + 1; + // std::cout<<"==============count: "<myDataPtr->connections) + { + if (iter.second->on) + (*iter.second->callback)(); + } + } + + //Added by zenglei@2019-01-03 + // template + // struct ThreadInfo{ + // int start; + // int end; + // T p; + // }; + /// \brief Signal the event with one parameter. + /// \param[in] _p parameter. +public: + template + void Signal(const P &_p) + { + // std::cout<<"------------------Signal1-----------------"<Cleanup(); + + this->myDataPtr->signaled = true; + + // static int steps = 0; + // static double time8 = 0; + // static double timeCallBack = 0; + // static double timeBefore = 0; + // static double timeMiddle = 0; + // static double timeAfter = 0; + // static double timeCallbacks[CALLBACK_NUM]; + // static double prevTime = 0; + // for (auto iter : this->myDataPtr->connections) //Closed by zenglei@2018-12-25 + // { + // if (iter.second->on) + // (*iter.second->callback)(_p); + // } + //Added by zenglei@2018-12-25 -----------------Begin + // #ifdef USE_COUNT_TIME //Added by zenglei@2018-12-04 + // struct timeval tv; + // double cur_time; + // double tmp_time; + // double start_time; + // gettimeofday(&tv,NULL); + // start_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6; + // #endif + // std::cout<<"=======Step: "<myDataPtr->connections.size()<<"======="< > > vecCallback; //Added by zenglei@2019-01-04 + // std::vector > > vecCBArray[30]; //Added by zenglei@20190312 + // std::vector > > vecCallback1; //Added by zenglei@2019-01-04 + // std::vector > > vecCallback2; //Added by zenglei@2019-01-04 + int count = 0; + for (auto iter : this->myDataPtr->connections) + { + if (iter.second->on) + { + // if((count - 8) % 8 != 6 ) + if(count < 7) { - if (iter.second->on) - (*iter.second->callback)(_p1, _p2, _p3, _p4, _p5, _p6); + // std::cout<<"\n===============callback Start=============="<callback)(_p); + // vecCallback.push_back(iter.second->callback); + // #ifdef USE_COUNT_TIME + // gettimeofday(&tv, NULL); + // if(steps > 2000) + // time8 += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time; + // if(steps == 3999) + // std::cout<<"------Cost Time: "<callback); + // vecCallback.push_back(iter.second->callback); } - } - - /// \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 - void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3, - const P4 &_p4, const P5 &_p5, const P6 &_p6, const P7 &_p7) - { - this->Cleanup(); - - this->myDataPtr->signaled = true; - for (auto iter: this->myDataPtr->connections.begin()) + else { - if (iter.second->on) - (*iter.second->callback)(_p1, _p2, _p3, _p4, _p5, _p6, _p7); + // vecCallback2.push_back(iter.second->callback); //Added by Howe@20190112 + vecCallback.push_back(iter.second->callback); + // vecCBArray[count/7 - 1].push_back(iter.second->callback); //Added by zenglei@20190312 } + count++; } - - /// \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 - 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) + } + // #ifdef USE_COUNT_TIME //Added by zenglei@20190311 + // gettimeofday(&tv, NULL); + // prevTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - start_time; + // #endif + int vecLength = vecCallback.size(); + if(vecLength <= 0) + return; + // if(count <= 7 ) + // return; + // steps++; + // std::cout<<"==========The length of callback queue: "<Cleanup(); + // nCount++; + // std::cout<<"\n===============callback Start=============="<myDataPtr->signaled = true; - for (auto iter: this->myDataPtr->connections) + // #ifdef USE_COUNT_TIME //Added by zenglei@2018-12-04 + // gettimeofday(&tv,NULL); + // cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6; + // #endif + + // std::cout<<"-----------------CallBack "<size() > 0) + // { + // callbackPool->schedule([&vecCallback, start, end,_p]{ + // for(int index = start; index < start + 8 && index < end; index++) + // (*vecCallback[index])(_p); + // }); + // } + // } + // if(callbackPool && callbackPool->size() > 0) + // callbackPool->wait(); + // } + else if(7 == method_) //C++11 Thread优化 + { + vecThread.clear(); + int length = vecLength / thread_num_; + for(int i = 0; i < thread_num_; i++) { - if (iter.second->on) + int start = i * length; + int end = start + length; + vecThread.push_back(std::thread([&vecCallback, start, end, vecLength, _p](){ + for(int index = start; index < end && index < vecLength; index++) + (*vecCallback[index])(_p); + })); + } + for(auto &thr: vecThread) + thr.join(); + } + else if(8 == method_) + { + switch(type_) + { + case 0: { - (*iter.second->callback)(_p1, _p2, _p3, _p4, _p5, _p6, _p7, _p8); + parallel_for(tbb::blocked_range(0, vecCallback.size(), block_size_), + [&vecCallback, &_p](const tbb::blocked_range &_r){ + for (size_t i = _r.begin(); i != _r.end(); i++) + { + (*vecCallback[i])(_p); + } + }, tbb::auto_partitioner() + ); + break; } - } - } - - /// \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) - { - this->Cleanup(); - - this->myDataPtr->signaled = true; - for (auto iter: this->myDataPtr->connections) - { - if (iter.second->on) + case 1: { - (*iter.second->callback)( - _p1, _p2, _p3, _p4, _p5, _p6, _p7, _p8, _p9); + tbb::affinity_partitioner ap; + parallel_for(tbb::blocked_range(0, vecCallback.size(), block_size_), + [&vecCallback, &_p](const tbb::blocked_range &_r){ + for (size_t i = _r.begin(); i != _r.end(); i++) + { + (*vecCallback[i])(_p); + } + }, ap + ); + break; } - } - } - - /// \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) - { - this->Cleanup(); - - this->myDataPtr->signaled = true; - for (auto iter: this->myDataPtr->connections) - { - if (iter.second->on) + case 2: { - (*iter.second->callback)( - _p1, _p2, _p3, _p4, _p5, _p6, _p7, _p8, _p9, _p10); + parallel_for(tbb::blocked_range(0, vecCallback.size(), block_size_), + [&vecCallback, &_p](const tbb::blocked_range &_r){ + for (size_t i = _r.begin(); i != _r.end(); i++) + { + (*vecCallback[i])(_p); + } + }, tbb::simple_partitioner() + ); + break; } + default: + break; } } + } + // #ifdef USE_COUNT_TIME + // gettimeofday(&tv,NULL); + // timeCallBack += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time; + // #endif + // #ifdef USE_COUNT_TIME + // if(steps == 300000) + // { + // std::cout<<"prevTime: "< *myDataPtr; - }; + /// \brief Signal the event with two parameter. + /// \param[in] _p1 the first parameter. + /// \param[in] _p2 the second parameter. +public: + template + void Signal(const P1 &_p1, const P2 &_p2) + { + // std::cout<<"------------------Signal2-----------------"<Cleanup(); - /// \brief Constructor. - template - EventT::EventT() + 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 + void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3) + { + // std::cout<<"------------------Signal3-----------------"<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 + void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3, + const P4 &_p4) + { + // std::cout<<"------------------Signal4-----------------"<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 + void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3, + const P4 &_p4, const P5 &_p5) + { + // std::cout<<"------------------Signal5-----------------"<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 + void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3, + const P4 &_p4, const P5 &_p5, const P6 &_p6) + { + // std::cout<<"------------------Signal6-----------------"<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 + 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-----------------"<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 + 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-----------------"<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 + 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-----------------"<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 + 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-----------------"<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 *myDataPtr; + int thread_num_; //Added by zenglei@2018-12-25 + int method_; //Added by zenglei@2019-03-04 + int block_size_; //Added by zenglei@2018-12-25 + int type_; //Added by zenglei@2018-12-26 + +// private: //Added by zenglei@2018-12-20 + // boost::threadpool::pool *callbackPool; + std::vector vecThread; //Added by zenglei@2019-01-04 + // boost::threadpool::thread_pool< > *callbackPool; //Added by zenglei@2019-01-09 + +}; + +/// \brief Constructor. +template +EventT::EventT() : Event(*(new EventTPrivate())) - { - this->myDataPtr = static_cast*>(this->dataPtr); - } +{ + this->myDataPtr = static_cast *>(this->dataPtr); + // callbackPool = new boost::threadpool::pool(6); //Added by zenglei@2018-12-20 + // callbackPool = NULL; //Added by zenglei@2019-01-09 + method_ = 0; //Addded by zenglei@2019-03-04 + thread_num_ = 0; //Added by zenglei@2018-12-25 + block_size_ = 8; //Added by zenglei@2018-12-25 + type_ = 0; //Added by zenglei@2019-01-15 +} - /// \brief Destructor. Deletes all the associated connections. - template - EventT::~EventT() - { - this->myDataPtr->connections.clear(); - } +/// \brief Destructor. Deletes all the associated connections. +template +EventT::~EventT() +{ + this->myDataPtr->connections.clear(); + // if(this->callbackPool) //Added by zenglei@2018-12-20 + // { + // this->callbackPool->wait(); + // delete this->callbackPool; + // this->callbackPool = NULL; + // } + // if(callbackPool) + // { + // callbackPool->wait(); + // delete callbackPool; + // callbackPool = NULL; + // } +} - /// \brief Adds a connection. - /// \param[in] _subscriber the subscriber to connect. - template - ConnectionPtr EventT::Connect(const boost::function &_subscriber) - { - 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(true, - new boost::function(_subscriber))); - return ConnectionPtr(new Connection(this, index)); - } +/// \brief Adds a connection. +/// \param[in] _subscriber the subscriber to connect. +template +ConnectionPtr EventT::Connect(const boost::function &_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-----------------"<myDataPtr->connections.empty()) + { + auto const &iter = this->myDataPtr->connections.rbegin(); + index = iter->first + 1; + } + this->myDataPtr->connections[index].reset(new EventConnection(true, + new boost::function(_subscriber))); + return ConnectionPtr(new Connection(this, index)); - /// \brief Removes a connection. - /// \param[in] _c the connection. - template - void EventT::Disconnect(ConnectionPtr _c) - { - if (!_c) - return; +// #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 +} - this->Disconnect(_c->GetId()); - _c->dataPtr->event = NULL; - _c->dataPtr->id = -1; - } +/// \brief Removes a connection. +/// \param[in] _c the connection. +template +void EventT::Disconnect(ConnectionPtr _c) +{ + if (!_c) + return; - /// \brief Get the number of connections. - /// \return Number of connections. - template - unsigned int EventT::ConnectionCount() const - { - return this->myDataPtr->connections.size(); - } + this->Disconnect(_c->GetId()); + _c->dataPtr->event = NULL; + _c->dataPtr->id = -1; +} - /// \brief Removes a connection. - /// \param[in] _id the connection index. - template - void EventT::Disconnect(int _id) - { - // Find the connection - auto const &it = this->myDataPtr->connections.find(_id); +/// \brief Get the number of connections. +/// \return Number of connections. +template +unsigned int EventT::ConnectionCount() const +{ + return this->myDataPtr->connections.size(); +} +//Added by zenglei@2018-12-25----Begin +template +void EventT::SetThreadInfo(int method,int thread_num, int block_size, int type) +{ + method_ = method; + thread_num_ = thread_num; + block_size_ = block_size; + type_ = type; + // if(thread_num > 0) + // tbb::task_scheduler_init init(thread_num); +} +//Added by zenglei@2018-12-25----End - if (it != this->myDataPtr->connections.end()) - { - it->second->on = false; - this->myDataPtr->connectionsToRemove.push_back(it); - } - } +//Added by zenglei@2019-01-09----Begin +// template +// void EventT::InitThreadPool(int thread_num) +// { +// if(callbackPool == NULL) +// { +// callbackPool = new boost::threadpool::thread_pool< >(thread_num); +// } +// } - ///////////////////////////////////////////// - template - void EventT::Cleanup() - { - std::lock_guard lock(this->myDataPtr->mutex); - // Remove all queue connections. - for (auto &conn : this->myDataPtr->connectionsToRemove) - this->myDataPtr->connections.erase(conn); - this->myDataPtr->connectionsToRemove.clear(); - } +//Added by zenglei@2019-01-09----End - /// \} +/// \brief Removes a connection. +/// \param[in] _id the connection index. +template +void EventT::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 +void EventT::Cleanup() +{ + std::lock_guard 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 diff --git a/gazebo/gazebo/common/Event_1.hh b/gazebo/gazebo/common/Event_1.hh new file mode 100644 index 0000000..ae5f1d5 --- /dev/null +++ b/gazebo/gazebo/common/Event_1.hh @@ -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 +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include "gazebo/util/system.hh" + +// #include "gazebo/countTime.hh" //Added by zhangshuai@2018-12-17 +// #include //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 + friend class EventT; +}; + +/// \internal +template +class EventConnection +{ + /// \brief Constructor +public: + EventConnection(const bool _on, + boost::function *_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> callback; +}; + +/// \cond +// Private data members for EventT class. +template +class EventTPrivate : public EventPrivate +{ + /// \def EvtConnectionMap + /// \brief Event Connection map typedef. + typedef std::map>> + 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 + connectionsToRemove; +}; +/// \endcond + +/// \class EventT Event.hh common/common.hh +/// \brief A class for event processing. +template +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 &_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 + 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 + 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 + 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 + 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 + 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 + 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 + 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 + 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 + 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 + 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-----------------"<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 + void Signal(const P &_p) + { + // std::cout<<"------------------Signal1-----------------"<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 + void Signal(const P1 &_p1, const P2 &_p2) + { + // std::cout<<"------------------Signal2-----------------"<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 + void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3) + { + // std::cout<<"------------------Signal3-----------------"<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 + void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3, + const P4 &_p4) + { + // std::cout<<"------------------Signal4-----------------"<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 + void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3, + const P4 &_p4, const P5 &_p5) + { + // std::cout<<"------------------Signal5-----------------"<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 + void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3, + const P4 &_p4, const P5 &_p5, const P6 &_p6) + { + // std::cout<<"------------------Signal6-----------------"<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 + 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-----------------"<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 + 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-----------------"<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 + 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-----------------"<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 + 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-----------------"<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 *myDataPtr; +}; + +/// \brief Constructor. +template +EventT::EventT() + : Event(*(new EventTPrivate())) +{ + this->myDataPtr = static_cast *>(this->dataPtr); +} + +/// \brief Destructor. Deletes all the associated connections. +template +EventT::~EventT() +{ + this->myDataPtr->connections.clear(); +} + +/// \brief Adds a connection. +/// \param[in] _subscriber the subscriber to connect. +template +ConnectionPtr EventT::Connect(const boost::function &_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-----------------"<myDataPtr->connections.empty()) + { + auto const &iter = this->myDataPtr->connections.rbegin(); + index = iter->first + 1; + } + this->myDataPtr->connections[index].reset(new EventConnection(true, + new boost::function(_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 +void EventT::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 +unsigned int EventT::ConnectionCount() const +{ + return this->myDataPtr->connections.size(); +} + +/// \brief Removes a connection. +/// \param[in] _id the connection index. +template +void EventT::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 +void EventT::Cleanup() +{ + std::lock_guard 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 diff --git a/gazebo/gazebo/common/Event_2.hh b/gazebo/gazebo/common/Event_2.hh new file mode 100644 index 0000000..ae5f1d5 --- /dev/null +++ b/gazebo/gazebo/common/Event_2.hh @@ -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 +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include "gazebo/util/system.hh" + +// #include "gazebo/countTime.hh" //Added by zhangshuai@2018-12-17 +// #include //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 + friend class EventT; +}; + +/// \internal +template +class EventConnection +{ + /// \brief Constructor +public: + EventConnection(const bool _on, + boost::function *_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> callback; +}; + +/// \cond +// Private data members for EventT class. +template +class EventTPrivate : public EventPrivate +{ + /// \def EvtConnectionMap + /// \brief Event Connection map typedef. + typedef std::map>> + 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 + connectionsToRemove; +}; +/// \endcond + +/// \class EventT Event.hh common/common.hh +/// \brief A class for event processing. +template +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 &_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 + 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 + 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 + 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 + 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 + 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 + 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 + 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 + 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 + 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 + 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-----------------"<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 + void Signal(const P &_p) + { + // std::cout<<"------------------Signal1-----------------"<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 + void Signal(const P1 &_p1, const P2 &_p2) + { + // std::cout<<"------------------Signal2-----------------"<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 + void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3) + { + // std::cout<<"------------------Signal3-----------------"<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 + void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3, + const P4 &_p4) + { + // std::cout<<"------------------Signal4-----------------"<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 + void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3, + const P4 &_p4, const P5 &_p5) + { + // std::cout<<"------------------Signal5-----------------"<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 + void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3, + const P4 &_p4, const P5 &_p5, const P6 &_p6) + { + // std::cout<<"------------------Signal6-----------------"<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 + 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-----------------"<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 + 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-----------------"<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 + 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-----------------"<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 + 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-----------------"<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 *myDataPtr; +}; + +/// \brief Constructor. +template +EventT::EventT() + : Event(*(new EventTPrivate())) +{ + this->myDataPtr = static_cast *>(this->dataPtr); +} + +/// \brief Destructor. Deletes all the associated connections. +template +EventT::~EventT() +{ + this->myDataPtr->connections.clear(); +} + +/// \brief Adds a connection. +/// \param[in] _subscriber the subscriber to connect. +template +ConnectionPtr EventT::Connect(const boost::function &_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-----------------"<myDataPtr->connections.empty()) + { + auto const &iter = this->myDataPtr->connections.rbegin(); + index = iter->first + 1; + } + this->myDataPtr->connections[index].reset(new EventConnection(true, + new boost::function(_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 +void EventT::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 +unsigned int EventT::ConnectionCount() const +{ + return this->myDataPtr->connections.size(); +} + +/// \brief Removes a connection. +/// \param[in] _id the connection index. +template +void EventT::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 +void EventT::Cleanup() +{ + std::lock_guard 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 diff --git a/gazebo/gazebo/common/Event_backup.hh b/gazebo/gazebo/common/Event_backup.hh new file mode 100644 index 0000000..4774365 --- /dev/null +++ b/gazebo/gazebo/common/Event_backup.hh @@ -0,0 +1,1549 @@ +/* + * 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 +#include +#include +#include +#include +#include +#include + +#include +#include +#include //Added by zenglei@2019-01-09 +#include //Added by zenglei@2019-01-09 +// #include //Added by zenglei@2018-12-20 +#include //Added by zenglei@2018-12-25 +#include //Added by zenglei@2018-12-25 +// #include //Added by zenglei@2018-12-25 +#include //Added by zenglei@2018-12-25 +#include //Added by zenglei@2018-12-26 +#include //Added by zenglei@2018-12-26 +// #include //Added by zenglei@2019-01-03 +#include //Added by zenglei@2019-01-08 +#include //Added by zenglei@2019-01-15 + +#include +#include +#include +#include +#include "gazebo/util/system.hh" +#include "gazebo/countTime.hh" //Addded by zenglei@2019-01-04 + +#include "gazebo/countTime.hh" //Added by zhangshuai@2018-12-17 +// #include //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; +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 + friend class EventT; +}; + +/// \internal +template +class EventConnection +{ + /// \brief Constructor +public: + EventConnection(const bool _on, + boost::function *_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> callback; +}; + +/// \cond +// Private data members for EventT class. +template +class EventTPrivate : public EventPrivate +{ + /// \def EvtConnectionMap + /// \brief Event Connection map typedef. + typedef std::map>> + 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 + connectionsToRemove; +}; +/// \endcond + + //Added by zenglei@2019-01-03----Start + // template + // struct ThreadInfomation{ + // int start; + // int end; + // std::vector > > &vecCallback; + // P p; + // }; + + // template + // void *thread_run(void *args) + // { + // P *thrInfo = (P*)args; + // for(int i = thrInfo->start; i < thrInfo->end; i++) + // { + // (*thrInfo->vecCallback[i])(thrInfo->p); + // } + // delete thrInfo; + // pthread_exit(NULL); + // } + //Added by zenglei@2019-01-03----End + +/// \class EventT Event.hh common/common.hh +/// \brief A class for event processing. +template +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 &_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; + +public: //Added by zenglei@2018-12-25 + void SetThreadInfo(int thread_num, int block_size, int type); //Added by zenglei@2018-12-25 + void InitThreadPool(int thread_num); //Added by zenglei@2019-01-09 + /// \brief Access the signal. +public: + void operator()() + { + this->Signal(); + } + + /// \brief Signal the event with one parameter. + /// \param[in] _p the parameter. +public: + template + 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 + 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 + 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 + 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 + 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 + 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.EventConnection + /// \param[in] _p2 the second parameterEventConnection. + /// \param[in] _p3 the second parameterEventConnection. + /// \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 + 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 + 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 + /// \param[in] _p9 the ninth parameter.ColladaLoader.cc:2 +public: + template + 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 paramete_pr. + /// \param[in] _p3 the second paramete_pr. + /// \param[in] _p4 the first parameter_p. + /// \param[in] _p5 the fifth parameter_p. + /// \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 + 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-----------------"<Cleanup(); + + this->myDataPtr->signaled = true; + //Added by zenglei@2018-12-20-----------------Start + // std::cout<<"==============The count of Connects: "<myDataPtr->connections.size()<<"==============="<myDataPtr->connections.size() > 20) + // { + // for(auto iter : this->myDataPtr->connections) + // { + // if(iter.second->on) + // { + // if(this->callbackPool && this->callbackPool->size() > 0) + // { + // this->callbackPool->schedule([&iter](){ + // (*iter.second->callback)(); + // }); + // } + // else + // (*iter.second->callback)(); + // } + // } + // if(this->callbackPool && this->callbackPool->size() > 0) + // this->callbackPool->wait(); + // } + // else + // { + // for(auto iter : this->myDataPtr->connections) + // { + // if(iter.second->on) + // (*iter.second->callback)(); + // } + // } + + //Added by zenglei@2018-12-20------------------End + // auto const &iter = this->myDataPtr->connections.rbegin(); + // int index = iter->first + 1; + // std::cout<<"==============count: "<myDataPtr->connections) + { + if (iter.second->on) + (*iter.second->callback)(); + } + } + + //Added by zenglei@2019-01-03 + // template + // struct ThreadInfo{ + // int start; + // int end; + // T p; + // }; + /// \brief Signal the event with one parameter. + /// \param[in] _p parameter. +public: + template + void Signal(const P &_p) + { + // std::cout<<"------------------Signal1-----------------"<Cleanup(); + + this->myDataPtr->signaled = true; + + static int steps = 0; + steps++; + static double time8 = 0; + static double timeCallBack = 0; + static double timeJoin = 0; + // static double timePreHalf = 0; + // static double timeHalf = 0; + static double totalTime = 0; + // for (auto iter : this->myDataPtr->connections) //Closed by zenglei@2018-12-25 + // { + // if (iter.second->on) + // (*iter.second->callback)(_p); + // } + //Added by zenglei@2018-12-25 -----------------Begin + #ifdef USE_COUNT_TIME //Added by zenglei@2018-12-04 + struct timeval tv; + double cur_time; + double start_time; + // gettimeofday(&tv,NULL); + // start_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6; + #endif + // std::cout<<"=======Step: "<myDataPtr->connections.size()<<"======="< > > vecCallback; //Added by zenglei@2019-01-04 + std::vector > > vecCallback1; //Added by zenglei@2019-01-04 + std::vector > > vecCallback2; //Added by zenglei@2019-01-04 + int count = 0; + for (auto iter : this->myDataPtr->connections) + { + if (iter.second->on) + { + count++; + // if((count - 8) % 8 != 6 ) + if(count <= 8) + { + // std::cout<<"\n===============callback Start=============="<callback)(_p); + // vecCallback.push_back(iter.second->callback); + #ifdef USE_COUNT_TIME + gettimeofday(&tv, NULL); + if(steps > 2000) + time8 += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time; + if(steps == 3999) + std::cout<<"------Cost Time: "<callback); + // vecCallback.push_back(iter.second->callback); + // } + else + { + // vecCallback2.push_back(iter.second->callback); //Added by Howe@20190112 + vecCallback.push_back(iter.second->callback); + } + } + } + // std::cout<<"====block_size: "< 4) + // { + // std::cout<<"======================================="< 2000) + { + std::thread thr1([]{ + + }); + for(int i = 0; i < end; i++) + (*vecCallback[i])(_p); + thr1.join(); + }else + { + for(int i = 0; i < end; i++) + (*vecCallback[i])(_p); + } + } + else if(2 == block_size_) + { + std::thread thr2([vecCallback, length, end, _p](){ + for(int i = length; i < end; i++ ) + (*vecCallback[i])(_p); + }); + thr2.detach(); + for(int i = 0; i < length; i++) + (*vecCallback[i])(_p); + // thr2.join(); + } + else if(3 == block_size_) + { + for(int i = 0; i < length; i++) + (*vecCallback[i])(_p); + if(steps < 2000) + { + for(int j = length; j < end; j++ ) + (*vecCallback[j])(_p); + } + } + else if(4 == block_size_) + { + if(steps < 2000) + { + for(int j = length; j < end; j++ ) + (*vecCallback[j])(_p); + for(int i = 0; i < length; i++) + (*vecCallback[i])(_p); + } + else + { + std::thread thr2([vecCallback, length, end, _p](){ + // for(int i = length; i < end; i++ ) + // (*vecCallback[i])(_p); + }); + for(int i = 0; i < length; i++) + (*vecCallback[i])(_p); + thr2.join(); + } + }else if(5 == block_size_) + { + std::cout<<"====Thread ID0: "< ft1 = async(std::launch::async, [&vecCallback, length, _p]{ + for(int i = 0; i < length; i+=2) + { + (*vecCallback[i])(_p); + (*vecCallback[i+1])(_p); + } + }); + std::future ft2 = async(std::launch::async, [&vecCallback, length, end, _p]{ + for(int i = length; i < end; i+=2) + { + (*vecCallback[i])(_p); + (*vecCallback[i+1])(_p); + } + + }); + + ft1.wait(); + ft2.wait(); + } + else if(7 == block_size_) + { + std::cout<<"====Thread ID0: "<size() > 0) + { + callbackPool->schedule([&vecCallback, start,_p]{ + for(int index = start; index < start + 8; index++) + (*vecCallback[index])(_p); + }); + } + } + if(callbackPool && callbackPool->size() > 0) + callbackPool->wait(); + } + else if(13 == block_size_) + { + #ifdef USE_COUNT_TIME //Added by zenglei@2018-12-04 + gettimeofday(&tv,NULL); + cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6; + #endif + for(int i = 0; i < end; i++) + { + (*vecCallback[i])(_p); + } + + #ifdef USE_COUNT_TIME + gettimeofday(&tv, NULL); + if(steps > 2000) + timeCallBack += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time; + if(steps == 3999) + std::cout<<"------timeCallBack: "< 2000) + timeJoin += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time; + if(steps == 3999) + std::cout<<"------timeJoin: "< > callBack = vecCallback[i]; + if(callbackPool && callbackPool->size() > 0) + { + callbackPool->schedule([callBack, _p]{ + (*callBack)(_p); + }); + } + } + if(callbackPool && callbackPool->size() > 0) + callbackPool->wait(); + } + else if(21 == block_size_) //Added by Howe@20190112 + { + std::thread thr1([&vecCallback1,_p](){ + for(auto iter1: vecCallback1) + (*iter1)(_p); + }); + std::thread thr2([&vecCallback2,_p](){ + for(auto iter2: vecCallback2) + (*iter2)(_p); + }); + + if(thr1.joinable()) + thr1.join(); + if(thr2.joinable()) + thr2.join(); + } + // #ifdef USE_COUNT_TIME + // gettimeofday(&tv, NULL); + // if(steps > 1000) + // timePreHalf += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time; + // if(steps == 1999) + // std::cout<<"------Cost Time PreHalf: "< 1000) + // timeHalf += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time; + // if(steps == 1999) + // std::cout<<"------Cost Time Half: "< * thrInf = new struct ThreadInfomation; + // thrInf.start = start; + // thrInf.end = end; + // thrInf.vecCallback = vecCallback; + // thrInf.p = _p; + // pthread_create(&pThArray[i], NULL, &thread_run >, (void)thrInf); + // // vecThread.push_back(std::thread([&vecCallback, start, end, _p](){ + // // for(int index = start; index < end; index++) + // // { + // // (*vecCallback[index])(_p); + // // } + // // })); + // } + // for(int k = 0; k < num; k++) + // pthread_join(pThArray[k],NULL); + // delete []pThArray; + } + else + { + switch(type_) + { + case 0: + { + parallel_for(tbb::blocked_range(0, vecCallback.size(), block_size_), + [&vecCallback, &_p](const tbb::blocked_range &_r){ + for (size_t i = _r.begin(); i != _r.end(); i++) + { + (*vecCallback[i])(_p); + } + }, tbb::auto_partitioner() + ); + break; + } + case 1: + { + tbb::affinity_partitioner ap; + parallel_for(tbb::blocked_range(0, vecCallback.size(), block_size_), + [&vecCallback, &_p](const tbb::blocked_range &_r){ + for (size_t i = _r.begin(); i != _r.end(); i++) + { + (*vecCallback[i])(_p); + } + }, ap + ); + break; + } + case 2: + { + parallel_for(tbb::blocked_range(0, vecCallback.size(), block_size_), + [&vecCallback, &_p](const tbb::blocked_range &_r){ + for (size_t i = _r.begin(); i != _r.end(); i++) + { + (*vecCallback[i])(_p); + } + }, tbb::simple_partitioner() + ); + break; + } + default: + break; + } + } + } + #ifdef USE_COUNT_TIME + gettimeofday(&tv, NULL); + if(steps > 2000) + totalTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - start_time; + if(steps == 3999) + std::cout<<"------Total Time: "< + void Signal(const P1 &_p1, const P2 &_p2) + { + // std::cout<<"------------------Signal2-----------------"<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 + void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3) + { + // std::cout<<"------------------Signal3-----------------"<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 + void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3, + const P4 &_p4) + { + // std::cout<<"------------------Signal4-----------------"<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 + void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3, + const P4 &_p4, const P5 &_p5) + { + // std::cout<<"------------------Signal5-----------------"<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 + void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3, + const P4 &_p4, const P5 &_p5, const P6 &_p6) + { + // std::cout<<"------------------Signal6-----------------"<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 + 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-----------------"<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 + 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-----------------"<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 + 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-----------------"<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 + 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-----------------"<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 *myDataPtr; + int thread_num_; //Added by zenglei@2018-12-25 + int block_size_; //Added by zenglei@2018-12-25 + int type_; //Added by zenglei@2018-12-26 + +// private: //Added by zenglei@2018-12-20 + // boost::threadpool::pool *callbackPool; + std::vector vecThread; //Added by zenglei@2019-01-04 + boost::threadpool::thread_pool< > *callbackPool; //Added by zenglei@2019-01-09 + +}; + +/// \brief Constructor. +template +EventT::EventT() + : Event(*(new EventTPrivate())) +{ + this->myDataPtr = static_cast *>(this->dataPtr); + // callbackPool = new boost::threadpool::pool(6); //Added by zenglei@2018-12-20 + callbackPool = NULL; //Added by zenglei@2019-01-09 + thread_num_ = 0; //Added by zenglei@2018-12-25 + block_size_ = 8; //Added by zenglei@2018-12-25 + type_ = 0; //Added by zenglei@2019-01-15 +} + +/// \brief Destructor. Deletes all the associated connections. +template +EventT::~EventT() +{ + this->myDataPtr->connections.clear(); + // if(this->callbackPool) //Added by zenglei@2018-12-20 + // { + // this->callbackPool->wait(); + // delete this->callbackPool; + // this->callbackPool = NULL; + // } + if(callbackPool) + { + callbackPool->wait(); + delete callbackPool; + callbackPool = NULL; + } +} + +/// \brief Adds a connection. +/// \param[in] _subscriber the subscriber to connect. +template +ConnectionPtr EventT::Connect(const boost::function &_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-----------------"<myDataPtr->connections.empty()) + { + auto const &iter = this->myDataPtr->connections.rbegin(); + index = iter->first + 1; + } + this->myDataPtr->connections[index].reset(new EventConnection(true, + new boost::function(_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 +void EventT::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 +unsigned int EventT::ConnectionCount() const +{ + return this->myDataPtr->connections.size(); +} +//Added by zenglei@2018-12-25----Begin +template +void EventT::SetThreadInfo(int thread_num, int block_size, int type) +{ + thread_num_ = thread_num; + block_size_ = block_size; + type_ = type; + // if(thread_num > 0) + // tbb::task_scheduler_init init(thread_num); +} +//Added by zenglei@2018-12-25----End + +//Added by zenglei@2019-01-09----Begin +template +void EventT::InitThreadPool(int thread_num) +{ + if(callbackPool == NULL) + { + callbackPool = new boost::threadpool::thread_pool< >(thread_num); + } +} + +//Added by zenglei@2019-01-09----End + +/// \brief Removes a connection. +/// \param[in] _id the connection index. +template +void EventT::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 +void EventT::Cleanup() +{ + std::lock_guard 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 diff --git a/gazebo/gazebo/common/Events.cc b/gazebo/gazebo/common/Events.cc index 2193ca7..5276a3b 100644 --- a/gazebo/gazebo/common/Events.cc +++ b/gazebo/gazebo/common/Events.cc @@ -16,8 +16,13 @@ */ #include "gazebo/common/Events.hh" +// #include "gazebo/countTime.hh" //Added by zhangshuai@2018-12-17 +// #include //Added by zhangshuai@2018-12-17 +// #include //Added by zhangshuai@2018-12-17 + using namespace gazebo; using namespace event; +// using namespace std; //Added by zhangshuai@2018-12-17 EventT Events::pause; EventT Events::step; @@ -29,8 +34,9 @@ EventT Events::entityCreated; EventT Events::setSelectedEntity; EventT Events::addEntity; EventT Events::deleteEntity; - EventT Events::worldUpdateBegin; + + EventT Events::beforePhysicsUpdate; EventT Events::worldUpdateEnd; diff --git a/gazebo/gazebo/common/Events.hh b/gazebo/gazebo/common/Events.hh index 32a1e56..509ccca 100644 --- a/gazebo/gazebo/common/Events.hh +++ b/gazebo/gazebo/common/Events.hh @@ -136,7 +136,9 @@ namespace gazebo /// \return a connection public: template static ConnectionPtr ConnectWorldUpdateBegin(T _subscriber) - { return worldUpdateBegin.Connect(_subscriber); } + { + // std::cout<<"------------------ConnectWorldUpdateBegin-----------------"< +#endif + +#ifndef USE_COUNT_TIME + #define USE_COUNT_TIME +#endif + + +#endif diff --git a/gazebo/gazebo/physics/CMakeLists.txt b/gazebo/gazebo/physics/CMakeLists.txt index 3d82b0f..c98bb91 100644 --- a/gazebo/gazebo/physics/CMakeLists.txt +++ b/gazebo/gazebo/physics/CMakeLists.txt @@ -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} ) diff --git a/gazebo/gazebo/physics/Entity.cc b/gazebo/gazebo/physics/Entity.cc index 6623773..40406e3 100644 --- a/gazebo/gazebo/physics/Entity.cc +++ b/gazebo/gazebo/physics/Entity.cc @@ -650,6 +650,7 @@ void Entity::UpdateParameters(sdf::ElementPtr _sdf) ////////////////////////////////////////////////// void Entity::UpdateAnimation(const common::UpdateInfo &_info) { + std::cout<<"=====================Entity::UpdateAnimation================"<animation->AddTime((_info.simTime - this->prevAnimationTime).Double()); diff --git a/gazebo/gazebo/physics/HelloWorld.cc b/gazebo/gazebo/physics/HelloWorld.cc new file mode 100644 index 0000000..8d7d2db --- /dev/null +++ b/gazebo/gazebo/physics/HelloWorld.cc @@ -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 + +using namespace eprosima::fastcdr::exception; + +#include + +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 +{ + + +} diff --git a/gazebo/gazebo/physics/HelloWorld.h b/gazebo/gazebo/physics/HelloWorld.h new file mode 100644 index 0000000..ce01bb9 --- /dev/null +++ b/gazebo/gazebo/physics/HelloWorld.h @@ -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 +#include +#include +#include +#include +#include +#include + +#if defined(_WIN32) +#if defined(EPROSIMA_USER_DLL_EXPORT) +#define eProsima_user_DllExport __declspec( dllexport ) +#else +#define eProsima_user_DllExport +#endif +#else +#include +#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_ \ No newline at end of file diff --git a/gazebo/gazebo/physics/HelloWorldPubSubTypes.cc b/gazebo/gazebo/physics/HelloWorldPubSubTypes.cc new file mode 100644 index 0000000..2114e93 --- /dev/null +++ b/gazebo/gazebo/physics/HelloWorldPubSubTypes.cc @@ -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 +#include + +#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 HelloWorldPubSubType::getSerializedSizeProvider(void* data) { + return [data]() -> uint32_t { + return (uint32_t)type::getCdrSerializedSize(*static_cast(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; +} diff --git a/gazebo/gazebo/physics/HelloWorldPubSubTypes.h b/gazebo/gazebo/physics/HelloWorldPubSubTypes.h new file mode 100644 index 0000000..4a01ad0 --- /dev/null +++ b/gazebo/gazebo/physics/HelloWorldPubSubTypes.h @@ -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 + + + +#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 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_ \ No newline at end of file diff --git a/gazebo/gazebo/physics/HelloWorldPublisher.cc b/gazebo/gazebo/physics/HelloWorldPublisher.cc new file mode 100644 index 0000000..1ba1dd1 --- /dev/null +++ b/gazebo/gazebo/physics/HelloWorldPublisher.cc @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include + +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 descriptor = std::make_shared(); + 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; +} diff --git a/gazebo/gazebo/physics/HelloWorldPublisher.h b/gazebo/gazebo/physics/HelloWorldPublisher.h new file mode 100644 index 0000000..6c05dd2 --- /dev/null +++ b/gazebo/gazebo/physics/HelloWorldPublisher.h @@ -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 +#include +#include + + +#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_ */ diff --git a/gazebo/gazebo/physics/HelloWorldPublisher.xml b/gazebo/gazebo/physics/HelloWorldPublisher.xml new file mode 100644 index 0000000..551b336 --- /dev/null +++ b/gazebo/gazebo/physics/HelloWorldPublisher.xml @@ -0,0 +1,13 @@ + + + HelloWorldPublisher + + 1 + 2 + HelloWorldTopic1 + HelloWorld + NO_KEY + RELIABLE_RELIABILITY_QOS + + + diff --git a/gazebo/gazebo/physics/HelloWorldSubscriber.cc b/gazebo/gazebo/physics/HelloWorldSubscriber.cc new file mode 100644 index 0000000..522d920 --- /dev/null +++ b/gazebo/gazebo/physics/HelloWorldSubscriber.cc @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include + +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 descriptor = std::make_shared(); + 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); +} diff --git a/gazebo/gazebo/physics/HelloWorldSubscriber.h b/gazebo/gazebo/physics/HelloWorldSubscriber.h new file mode 100644 index 0000000..aff48b3 --- /dev/null +++ b/gazebo/gazebo/physics/HelloWorldSubscriber.h @@ -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 +#include +#include +#include + + + + +#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_ */ diff --git a/gazebo/gazebo/physics/HelloWorldSubscriber.xml b/gazebo/gazebo/physics/HelloWorldSubscriber.xml new file mode 100644 index 0000000..7d9d1c6 --- /dev/null +++ b/gazebo/gazebo/physics/HelloWorldSubscriber.xml @@ -0,0 +1,13 @@ + + + HelloWorldSubscriber + + 3 + 4 + HelloWorldTopic1 + HelloWorld + NO_KEY + RELIABLE_RELIABILITY_QOS + + + diff --git a/gazebo/gazebo/physics/Link.cc b/gazebo/gazebo/physics/Link.cc index 6a70e79..4455545 100644 --- a/gazebo/gazebo/physics/Link.cc +++ b/gazebo/gazebo/physics/Link.cc @@ -493,6 +493,7 @@ void Link::SetLaserRetro(float _retro) ////////////////////////////////////////////////// void Link::Update(const common::UpdateInfo & /*_info*/) { + // std::cout<<"=====================Link::Update================"<audioSink) { diff --git a/gazebo/gazebo/physics/World.cc b/gazebo/gazebo/physics/World.cc index 1e47fff..b4573d0 100644 --- a/gazebo/gazebo/physics/World.cc +++ b/gazebo/gazebo/physics/World.cc @@ -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 //Added by zenglei@2018-12-10 +// #include //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( "~/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("type"); + ip = eventElem->Get("ip"); + port = eventElem->Get("port"); + flag = eventElem->Get("flag"); + // std::cout<<"Server IP: "<Get("numbers_of_thread")<<"\t size_of_block: "<Get("size_of_block")<<"============="<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: "<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: "<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: "<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<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: "<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: "< 0) + { + common::Time::Sleep(sleepTime); + actualSleep = common::Time::GetWallTime() - tmpTime; + // std::cout<<"sleepTime2: "<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"<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"<dataPtr->iterations != recvIter) + // { + // std::cout<<"================Wait to aync==============\n"<dataPtr->iterations) < 0) + // continue; + // if(gazeboSend.receiveSignal(nStepIter) < 0) + // continue; + // if(nStepIter == this->dataPtr->iterations) + // break; + // } + // std::cout<<"============Succeed!!===========\n"<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 = "<dataPtr->iterations<dataPtr->iterations) + { + gazeboSend.sendData(this->dataPtr->iterations); + } + else + continue; + // std::cout<<"recv tmp: "<dataPtr->iterations<dataPtr->iterations<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: "<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: "<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: "<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************"<dataPtr->updateInfo); + // std::cout<<"*************event::Events::worldUpdateBegin----End************"<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************"<dataPtr->updateInfo); + // std::cout<<"*************event::Events::beforePhysicsUpdate----End************"<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(0, -// this->dataPtr->models.size(), 10), +// this->dataPtr->models.size(), 25), // ModelUpdate_TBB(&this->dataPtr->models)); +// // tbb::parallel_for (tbb::blocked_range(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 &vecModel, int startIndex, int stepSize, msgs::PosesStamped &msg) +// { +// boost::mutex::scoped_lock lock(mu_process); +// unsigned int size = vecModel.size(); +// std::list 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: "<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 threadVec; + // std::vector 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 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: "<dataPtr->publishLightPoses.size: "<dataPtr->publishLightPoses.size()<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: "<dataPtr->publishModelScales.size()<dataPtr->publishModelScales) { std::list 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-----"<ProcessPlaybackControlMsgs(); this->ProcessEntityMsgs(); this->ProcessRequestMsgs(); diff --git a/gazebo/gazebo/physics/World.hh b/gazebo/gazebo/physics/World.hh index 11ecf42..a271793 100644 --- a/gazebo/gazebo/physics/World.hh +++ b/gazebo/gazebo/physics/World.hh @@ -47,12 +47,402 @@ #include "gazebo/physics/WorldState.hh" #include "gazebo/util/system.hh" +//Added by zenglei@2019-01-22 begin +#include +#include +#include +#include +#include +#include +// #include +#include +#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 + +// #include +// // #include + +// 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: "< 0) + { + // std::cout<<"Send Data Succeed!"< 0) + { + memcpy(&nStep, buff, sizeof(uint64_t)); + // std::cout<<"Recv Data "<< nStep<<"Succeed!"<= 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: "< 0) + { + // std::cout<<"Send Data Succeed!"< 0) + { + memcpy(&nStep, buff, sizeof(uint64_t)); + // std::cout<<"Recv Data "<< nStep<<"Succeed!"<= 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 + ///#include l models and their components + ///#include etc), and WorldPlugin instances. + ///#include ed in the World, including physics /// update, model updates, and message processing. class GZ_PHYSICS_VISIBLE World : public boost::enable_shared_from_this @@ -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 &vecModel, int startIndex, int stepSize, msgs::PosesStamped &msg); //定义ProcessMessage模块多线程处理逻辑 + }; /// \} } diff --git a/gazebo/gazebo/physics/ode/ODELink.cc b/gazebo/gazebo/physics/ode/ODELink.cc index e8accae..831181b 100644 --- a/gazebo/gazebo/physics/ode/ODELink.cc +++ b/gazebo/gazebo/physics/ode/ODELink.cc @@ -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); } diff --git a/gazebo/gazebo/physics/ode/ODEPhysics.cc b/gazebo/gazebo/physics/ode/ODEPhysics.cc index 6a36171..ce5f823 100644 --- a/gazebo/gazebo/physics/ode/ODEPhysics.cc +++ b/gazebo/gazebo/physics/ode/ODEPhysics.cc @@ -32,6 +32,7 @@ #include #include +// #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 //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: "<dataPtr->jointFeedbackIndex: "<dataPtr->jointFeedbackIndex<<"**********************"<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 diff --git a/gazebo/gazebo/physics/ode/ODEPhysics.hh b/gazebo/gazebo/physics/ode/ODEPhysics.hh index a979c85..d936ec8 100644 --- a/gazebo/gazebo/physics/ode/ODEPhysics.hh +++ b/gazebo/gazebo/physics/ode/ODEPhysics.hh @@ -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 diff --git a/gazebo/gazebo/physics/ode/ODEPhysicsPrivate.hh b/gazebo/gazebo/physics/ode/ODEPhysicsPrivate.hh index 048ee99..181ef5a 100644 --- a/gazebo/gazebo/physics/ode/ODEPhysicsPrivate.hh +++ b/gazebo/gazebo/physics/ode/ODEPhysicsPrivate.hh @@ -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 spaces; diff --git a/gazebo/gazebo/physics/simbody/SimbodyLink.cc b/gazebo/gazebo/physics/simbody/SimbodyLink.cc index 9c3444a..f59971b 100644 --- a/gazebo/gazebo/physics/simbody/SimbodyLink.cc +++ b/gazebo/gazebo/physics/simbody/SimbodyLink.cc @@ -138,6 +138,7 @@ void SimbodyLink::SetGravityMode(bool _mode) ////////////////////////////////////////////////// void SimbodyLink::ProcessSetGravityMode() { + std::cout<<"=====================SimbodyLink::ProcessSetGravityMode================"<gravityModeDirty) { if (this->physicsInitialized) @@ -305,6 +306,7 @@ void SimbodyLink::SetLinkStatic(bool _static) ////////////////////////////////////////////////// void SimbodyLink::ProcessSetLinkStatic() { + std::cout<<"=====================SimbodyLink::ProcessSetLinkStatic================"<masterMobod.isEmptyHandle()) return; diff --git a/gazebo/gazebo/sensors/SensorManager.cc b/gazebo/gazebo/sensors/SensorManager.cc index 6f031cb..7bbded3 100644 --- a/gazebo/gazebo/sensors/SensorManager.cc +++ b/gazebo/gazebo/sensors/SensorManager.cc @@ -746,6 +746,7 @@ void SimTimeEventHandler::AddRelativeEvent(const common::Time &_time, ///////////////////////////////////////////////// void SimTimeEventHandler::OnUpdate(const common::UpdateInfo &_info) { + // std::cout<<"=====================SimTimeEventHandler::OnUpdate================"<mutex); diff --git a/gazebo/gazebo/server_main.cc b/gazebo/gazebo/server_main.cc index 6115982..913cf07 100644 --- a/gazebo/gazebo/server_main.cc +++ b/gazebo/gazebo/server_main.cc @@ -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: "<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: "<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: "<Fini(); delete server; diff --git a/gazebo/gazebo/transport/Connection.hh b/gazebo/gazebo/transport/Connection.hh index f65a796..2f93fd9 100644 --- a/gazebo/gazebo/transport/Connection.hh +++ b/gazebo/gazebo/transport/Connection.hh @@ -19,6 +19,7 @@ #include #include +// #include //Added by zenglei@20190222 #include #include @@ -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); diff --git a/gazebo/gazebo/transport/ConnectionManager.cc b/gazebo/gazebo/transport/ConnectionManager.cc index c0e16d6..869ef1f 100644 --- a/gazebo/gazebo/transport/ConnectionManager.cc +++ b/gazebo/gazebo/transport/ConnectionManager.cc @@ -20,6 +20,8 @@ #include #endif +// #include //Added by zenglei@20190222 + #include #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); diff --git a/gazebo/gazebo/transport/Node.hh b/gazebo/gazebo/transport/Node.hh index 71a2e2c..b48a581 100644 --- a/gazebo/gazebo/transport/Node.hh +++ b/gazebo/gazebo/transport/Node.hh @@ -26,6 +26,8 @@ #include #include +// #include //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(_topic); PublishTask *task = new(tbb::task::allocate_root()) PublishTask(pub, _message); diff --git a/gazebo/gazebo/transport/TopicManager.cc b/gazebo/gazebo/transport/TopicManager.cc index aad153a..f5065bf 100644 --- a/gazebo/gazebo/transport/TopicManager.cc +++ b/gazebo/gazebo/transport/TopicManager.cc @@ -24,6 +24,7 @@ #include #include + #include #include "gazebo/msgs/msgs.hh" #include "gazebo/transport/Node.hh" diff --git a/gazebo/gazebo/util/Diagnostics.cc b/gazebo/gazebo/util/Diagnostics.cc index f8677f3..111d7a8 100644 --- a/gazebo/gazebo/util/Diagnostics.cc +++ b/gazebo/gazebo/util/Diagnostics.cc @@ -117,6 +117,7 @@ boost::filesystem::path DiagnosticManager::LogPath() const ////////////////////////////////////////////////// void DiagnosticManager::Update(const common::UpdateInfo &_info) { + // std::cout<<"=====================DiagnosticManager::Update================"< common::Time::Zero) { this->dataPtr->msg.set_real_time_factor( diff --git a/gazebo/plugins/ActuatorPlugin.cc b/gazebo/plugins/ActuatorPlugin.cc index bb6863a..db0c4aa 100644 --- a/gazebo/plugins/ActuatorPlugin.cc +++ b/gazebo/plugins/ActuatorPlugin.cc @@ -164,6 +164,7 @@ void ActuatorPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) ////////////////////////////////////////////////// void ActuatorPlugin::WorldUpdateCallback() { + std::cout<<"=====================ActuatorPlugin::WorldUpdateCallback================"<joints.size(); i++) { diff --git a/gazebo/plugins/BreakableJointPlugin.cc b/gazebo/plugins/BreakableJointPlugin.cc index 3067ab0..e894609 100644 --- a/gazebo/plugins/BreakableJointPlugin.cc +++ b/gazebo/plugins/BreakableJointPlugin.cc @@ -69,6 +69,7 @@ void BreakableJointPlugin::OnUpdate(msgs::WrenchStamped _msg) ///////////////////////////////////////////////// void BreakableJointPlugin::OnWorldUpdate() { + std::cout<<"=====================BreakableJointPlugin::OnWorldUpdate================"<parentSensor->SetActive(false); this->parentJoint->Detach(); this->parentJoint->SetProvideFeedback(false); diff --git a/gazebo/plugins/BuoyancyPlugin.cc b/gazebo/plugins/BuoyancyPlugin.cc index ade7814..946383f 100644 --- a/gazebo/plugins/BuoyancyPlugin.cc +++ b/gazebo/plugins/BuoyancyPlugin.cc @@ -155,6 +155,7 @@ void BuoyancyPlugin::Init() ///////////////////////////////////////////////// void BuoyancyPlugin::OnUpdate() { + std::cout<<"=====================BuoyancyPlugin::OnUpdate================"<model->GetLinks()) { VolumeProperties volumeProperties = this->volPropsMap[link->GetId()]; diff --git a/gazebo/plugins/CartDemoPlugin.cc b/gazebo/plugins/CartDemoPlugin.cc index e5f97ce..3857378 100644 --- a/gazebo/plugins/CartDemoPlugin.cc +++ b/gazebo/plugins/CartDemoPlugin.cc @@ -107,6 +107,7 @@ void CartDemoPlugin::Init() ///////////////////////////////////////////////// void CartDemoPlugin::OnUpdate() { + std::cout<<"=====================CartDemoPlugin::OnUpdate================"<model->GetWorld()->GetSimTime(); common::Time stepTime = currTime - this->prevUpdateTime; this->prevUpdateTime = currTime; diff --git a/gazebo/plugins/CessnaPlugin.cc b/gazebo/plugins/CessnaPlugin.cc index 397f1f7..2e1df06 100644 --- a/gazebo/plugins/CessnaPlugin.cc +++ b/gazebo/plugins/CessnaPlugin.cc @@ -151,6 +151,7 @@ void CessnaPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) ///////////////////////////////////////////////// void CessnaPlugin::Update(const common::UpdateInfo &/*_info*/) { + std::cout<<"=====================CessnaPlugin::Update================"< lock(this->mutex); gazebo::common::Time curTime = this->model->GetWorld()->GetSimTime(); diff --git a/gazebo/plugins/ContainPlugin.cc b/gazebo/plugins/ContainPlugin.cc index c225a69..ff162e7 100644 --- a/gazebo/plugins/ContainPlugin.cc +++ b/gazebo/plugins/ContainPlugin.cc @@ -222,6 +222,7 @@ void ContainPlugin::Enable(ConstIntPtr &_msg) ///////////////////////////////////////////////// void ContainPlugin::OnUpdate(const common::UpdateInfo &/*_info*/) { + std::cout<<"=====================ContainPlugin::OnUpdate================"<dataPtr->entity.lock(); if (!entity) diff --git a/gazebo/plugins/DiffDrivePlugin.cc b/gazebo/plugins/DiffDrivePlugin.cc index 116ac80..a03bfe5 100644 --- a/gazebo/plugins/DiffDrivePlugin.cc +++ b/gazebo/plugins/DiffDrivePlugin.cc @@ -109,7 +109,7 @@ void DiffDrivePlugin::OnUpdate() common::Time currTime = this->model->GetWorld()->GetSimTime(); common::Time stepTime = currTime - this->prevUpdateTime; */ - + std::cout<<"=====================DiffDrivePlugin::OnUpdate================"<wheelSpeed[LEFT] / this->wheelRadius); double rightVelDesired = (this->wheelSpeed[RIGHT] / this->wheelRadius); diff --git a/gazebo/plugins/ElevatorPlugin.cc b/gazebo/plugins/ElevatorPlugin.cc index 4530b5f..f79cd0e 100644 --- a/gazebo/plugins/ElevatorPlugin.cc +++ b/gazebo/plugins/ElevatorPlugin.cc @@ -175,6 +175,7 @@ void ElevatorPlugin::MoveToFloor(const int _floor) ///////////////////////////////////////////////// void ElevatorPlugin::Update(const common::UpdateInfo &_info) { + std::cout<<"=====================ElevatorPlugin::Update================"< lock(this->dataPtr->stateMutex); // Process the states diff --git a/gazebo/plugins/FollowerPlugin.cc b/gazebo/plugins/FollowerPlugin.cc index efab7cc..70e156e 100644 --- a/gazebo/plugins/FollowerPlugin.cc +++ b/gazebo/plugins/FollowerPlugin.cc @@ -252,6 +252,7 @@ bool FollowerPlugin::FindSensor(const physics::ModelPtr &_model) ///////////////////////////////////////////////// void FollowerPlugin::OnUpdate() { + std::cout<<"=====================FollowerPlugin::OnUpdate================"< lock(this->dataPtr->mutex); // Update follower. diff --git a/gazebo/plugins/HarnessPlugin.cc b/gazebo/plugins/HarnessPlugin.cc index 84532b5..a686822 100644 --- a/gazebo/plugins/HarnessPlugin.cc +++ b/gazebo/plugins/HarnessPlugin.cc @@ -216,6 +216,7 @@ void HarnessPlugin::Init() ///////////////////////////////////////////////// void HarnessPlugin::OnUpdate(const common::UpdateInfo &_info) { + std::cout<<"=====================HarnessPlugin::OnUpdate================"<prevSimTime == common::Time::Zero) { diff --git a/gazebo/plugins/HydraDemoPlugin.cc b/gazebo/plugins/HydraDemoPlugin.cc index 54cba11..eb4b953 100644 --- a/gazebo/plugins/HydraDemoPlugin.cc +++ b/gazebo/plugins/HydraDemoPlugin.cc @@ -64,6 +64,7 @@ void HydraDemoPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) ///////////////////////////////////////////////// void HydraDemoPlugin::Update(const common::UpdateInfo & /*_info*/) { + std::cout<<"=====================HydraDemoPlugin::Update================"<msgMutex); // Return if we don't have messages yet diff --git a/gazebo/plugins/HydraPlugin.cc b/gazebo/plugins/HydraPlugin.cc index 8fa0adc..cce705b 100644 --- a/gazebo/plugins/HydraPlugin.cc +++ b/gazebo/plugins/HydraPlugin.cc @@ -191,6 +191,7 @@ void RazerHydra::Load(physics::WorldPtr _world, sdf::ElementPtr /*_sdf*/) ///////////////////////////////////////////////// void RazerHydra::Update(const common::UpdateInfo & /*_info*/) { + std::cout<<"=====================RazerHydra::Update================"<mutex); math::Pose origRight(this->pos[1], this->quat[1]); diff --git a/gazebo/plugins/JointTrajectoryPlugin.cc b/gazebo/plugins/JointTrajectoryPlugin.cc index 312521e..7422af5 100644 --- a/gazebo/plugins/JointTrajectoryPlugin.cc +++ b/gazebo/plugins/JointTrajectoryPlugin.cc @@ -82,6 +82,7 @@ void JointTrajectoryPlugin::Load(physics::ModelPtr _parent, ///////////////////////////////////////////////// void JointTrajectoryPlugin::UpdateStates(const common::UpdateInfo & /*_info*/) { + std::cout<<"=====================JointTrajectoryPlugin::UpdateStates================"<world->GetSimTime(); // for (physics::Joint_V::const_iterator j = this->model->GetJoints().begin(); diff --git a/gazebo/plugins/LiftDragPlugin.cc b/gazebo/plugins/LiftDragPlugin.cc index 1475b70..3122188 100644 --- a/gazebo/plugins/LiftDragPlugin.cc +++ b/gazebo/plugins/LiftDragPlugin.cc @@ -157,6 +157,7 @@ void LiftDragPlugin::Load(physics::ModelPtr _model, ///////////////////////////////////////////////// void LiftDragPlugin::OnUpdate() { + std::cout<<"=====================LiftDragPlugin::OnUpdate================"<link, "Link was NULL"); // get linear velocity at cp in inertial frame math::Vector3 velI = this->link->GetWorldLinearVel(this->cp); diff --git a/gazebo/plugins/ModelPropShop.cc b/gazebo/plugins/ModelPropShop.cc index 999ed6a..2643cd6 100644 --- a/gazebo/plugins/ModelPropShop.cc +++ b/gazebo/plugins/ModelPropShop.cc @@ -141,6 +141,7 @@ void ModelPropShop::OnWorldCreated() ///////////////////////////////////////////// void ModelPropShop::Update() { + std::cout<<"=====================ModelPropShop::Update================"<scene) diff --git a/gazebo/plugins/MudPlugin.cc b/gazebo/plugins/MudPlugin.cc index 5125d1a..bd05b38 100644 --- a/gazebo/plugins/MudPlugin.cc +++ b/gazebo/plugins/MudPlugin.cc @@ -173,6 +173,7 @@ void MudPlugin::OnContact(ConstContactsPtr &_msg) ///////////////////////////////////////////////// void MudPlugin::OnUpdate() { + std::cout<<"=====================MudPlugin::OnUpdate================"<physics->GetMaxStepSize(); if (dt < 1e-6) dt = 1e-6; diff --git a/gazebo/plugins/RandomVelocityPlugin.cc b/gazebo/plugins/RandomVelocityPlugin.cc index c340b89..2366ae9 100644 --- a/gazebo/plugins/RandomVelocityPlugin.cc +++ b/gazebo/plugins/RandomVelocityPlugin.cc @@ -142,6 +142,7 @@ void RandomVelocityPlugin::Reset() ///////////////////////////////////////////////// void RandomVelocityPlugin::Update(const common::UpdateInfo &_info) { + std::cout<<"=====================RandomVelocityPlugin::Update(================"<dataPtr->link, " in RandomVelocity plugin is null"); // Short-circuit in case the link is invalid. diff --git a/gazebo/plugins/SphereAtlasDemoPlugin.cc b/gazebo/plugins/SphereAtlasDemoPlugin.cc index d855770..c08bfe9 100644 --- a/gazebo/plugins/SphereAtlasDemoPlugin.cc +++ b/gazebo/plugins/SphereAtlasDemoPlugin.cc @@ -180,6 +180,7 @@ void SphereAtlasDemoPlugin::Reset() ///////////////////////////////////////////////// void SphereAtlasDemoPlugin::OnUpdate() { + std::cout<<"=====================SphereAtlasDemoPlugin::OnUpdate================"<model->GetWorld()->GetSimTime(); common::Time stepTime = currTime - this->prevUpdateTime; this->prevUpdateTime = currTime; diff --git a/gazebo/plugins/TouchPlugin.cc b/gazebo/plugins/TouchPlugin.cc index 4cb50e1..f00a0f4 100644 --- a/gazebo/plugins/TouchPlugin.cc +++ b/gazebo/plugins/TouchPlugin.cc @@ -146,6 +146,7 @@ void TouchPlugin::Enable(ConstIntPtr &_msg) ///////////////////////////////////////////////// void TouchPlugin::OnUpdate(const common::UpdateInfo &_info) { + std::cout<<"=====================TouchPlugin::OnUpdate================"<dataPtr->world->GetModels(); diff --git a/gazebo/plugins/VehiclePlugin.cc b/gazebo/plugins/VehiclePlugin.cc index a27f1e9..127bd57 100644 --- a/gazebo/plugins/VehiclePlugin.cc +++ b/gazebo/plugins/VehiclePlugin.cc @@ -182,6 +182,7 @@ void VehiclePlugin::Init() ///////////////////////////////////////////////// void VehiclePlugin::OnUpdate() { + std::cout<<"=====================VehiclePlugin::OnUpdate================"<gasJoint->GetAngle(0).Radian() / this->maxGas; double brake = this->brakeJoint->GetAngle(0).Radian() / this->maxBrake; diff --git a/gazebo/plugins/events/InRegionEventSource.cc b/gazebo/plugins/events/InRegionEventSource.cc index 1459e3c..4cba584 100644 --- a/gazebo/plugins/events/InRegionEventSource.cc +++ b/gazebo/plugins/events/InRegionEventSource.cc @@ -94,6 +94,7 @@ void InRegionEventSource::Info() const //////////////////////////////////////////////////////////////////////////////// void InRegionEventSource::Update() { + std::cout<<"=====================InRegionEventSource::Update================"<model) return; diff --git a/gazebo/plugins/events/JointEventSource.cc b/gazebo/plugins/events/JointEventSource.cc index 688b999..1e2d70e 100644 --- a/gazebo/plugins/events/JointEventSource.cc +++ b/gazebo/plugins/events/JointEventSource.cc @@ -204,6 +204,7 @@ bool JointEventSource::LookupJoint() //////////////////////////////////////////////////////////////////////////////// void JointEventSource::Update() { + std::cout<<"=====================JointEventSource::Update================"<LookupJoint()) return; diff --git a/gazebo/plugins/events/OccupiedEventSource.cc b/gazebo/plugins/events/OccupiedEventSource.cc index 98499fa..20fec17 100644 --- a/gazebo/plugins/events/OccupiedEventSource.cc +++ b/gazebo/plugins/events/OccupiedEventSource.cc @@ -107,6 +107,7 @@ void OccupiedEventSource::Load(const sdf::ElementPtr _sdf) ///////////////////////////////////////////////// void OccupiedEventSource::Update() { + std::cout<<"=====================OccupiedEventSource::Update================"<world->GetModels(); diff --git a/gazebo/plugins/events/RegionEventBoxPlugin.cc b/gazebo/plugins/events/RegionEventBoxPlugin.cc index 3f4ad86..cbfb8a7 100644 --- a/gazebo/plugins/events/RegionEventBoxPlugin.cc +++ b/gazebo/plugins/events/RegionEventBoxPlugin.cc @@ -101,6 +101,7 @@ void RegionEventBoxPlugin::OnModelMsg(ConstModelPtr &_msg) ////////////////////////////////////////////////// void RegionEventBoxPlugin::OnUpdate(const common::UpdateInfo &_info) { + std::cout<<"=====================RegionEventBoxPlugin::OnUpdate================"<simTime) { std::string json; diff --git a/gazebo/test/plugins/ForceTorqueModelRemovalTestPlugin.cc b/gazebo/test/plugins/ForceTorqueModelRemovalTestPlugin.cc index bc77c20..b04e627 100644 --- a/gazebo/test/plugins/ForceTorqueModelRemovalTestPlugin.cc +++ b/gazebo/test/plugins/ForceTorqueModelRemovalTestPlugin.cc @@ -63,6 +63,7 @@ void ForceTorqueModelRemovalTestPlugin::Load(sensors::SensorPtr _sensor, void ForceTorqueModelRemovalTestPlugin::onUpdate( const gazebo::common::UpdateInfo & /*_info*/) { + std::cout<<"=====================ForceTorqueModelRemovalTestPlugin::onUpdate================"<model->GetWorld()->GetSimTime(); common::Time stepTime = currTime - this->prevUpdateTime; this->prevUpdateTime = currTime; diff --git a/gazebo/test/plugins/SpringTestPlugin.cc b/gazebo/test/plugins/SpringTestPlugin.cc index ef21324..87d9db9 100644 --- a/gazebo/test/plugins/SpringTestPlugin.cc +++ b/gazebo/test/plugins/SpringTestPlugin.cc @@ -63,6 +63,7 @@ void SpringTestPlugin::Init() ///////////////////////////////////////////////// void SpringTestPlugin::ExplicitUpdate() { + std::cout<<"=====================SpringTestPlugin::ExplicitUpdate================"<model->GetWorld()->GetSimTime(); common::Time stepTime = currTime - this->prevUpdateTime; this->prevUpdateTime = currTime;