From cb35012c7d1ba6ed5d7550d7b2a559bba56f35b3 Mon Sep 17 00:00:00 2001
From: zhangshuai <zhangshuai9999@outlook.com>
Date: Thu, 30 May 2019 10:06:18 +0800
Subject: [PATCH] MPI_ALLGATHERV test3

---
 .../gazebo/physics/World.cc                   | 30 ++++++++++++-------
 1 file changed, 19 insertions(+), 11 deletions(-)

diff --git a/Gazebo_Distributed_MPI/gazebo/physics/World.cc b/Gazebo_Distributed_MPI/gazebo/physics/World.cc
index 15675d7..523af92 100644
--- a/Gazebo_Distributed_MPI/gazebo/physics/World.cc
+++ b/Gazebo_Distributed_MPI/gazebo/physics/World.cc
@@ -920,8 +920,9 @@ void World::Update()
 
     char *sendBuffer;
     char *receiveBuffer;
-    int bufferLen[tmp_gazebo_count];
-    int displs[tmp_gazebo_count];
+    int *bufferLen = new int[tmp_gazebo_count];
+    int *displs = new int[tmp_gazebo_count];
+
     int receiveBufferLen = 0;
 
     for (int tmp_gazebo_id = 0; tmp_gazebo_id < tmp_gazebo_count; tmp_gazebo_id++)
@@ -937,14 +938,14 @@ void World::Update()
         modelPose->set_name(tmp_model_name);
         // if (this->gazeboLocalID == tmp_gazebo_id)
         // {
-          modelPose->set_id(this->GetModel(tmp_model_name)->GetLink("canonical")->GetId());
-          modelPose->mutable_position()->set_x(this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose().pos.x);
-          modelPose->mutable_position()->set_y(this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose().pos.y);
-          modelPose->mutable_position()->set_z(this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose().pos.z);
-          modelPose->mutable_orientation()->set_x(this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose().rot.x);
-          modelPose->mutable_orientation()->set_y(this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose().rot.y);
-          modelPose->mutable_orientation()->set_z(this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose().rot.z);
-          modelPose->mutable_orientation()->set_w(this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose().rot.w);
+        modelPose->set_id(this->GetModel(tmp_model_name)->GetLink("canonical")->GetId());
+        modelPose->mutable_position()->set_x(this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose().pos.x);
+        modelPose->mutable_position()->set_y(this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose().pos.y);
+        modelPose->mutable_position()->set_z(this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose().pos.z);
+        modelPose->mutable_orientation()->set_x(this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose().rot.x);
+        modelPose->mutable_orientation()->set_y(this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose().rot.y);
+        modelPose->mutable_orientation()->set_z(this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose().rot.z);
+        modelPose->mutable_orientation()->set_w(this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose().rot.w);
         // }
 
         tmp_model_name.clear();
@@ -964,6 +965,9 @@ void World::Update()
       for (int k = 0; k < tmp_gazebo_id; k++)
         displs[tmp_gazebo_id] = displs[tmp_gazebo_id] + bufferLen[k];
 
+      std::cout << "*************$$$$$$$$$ bufferLen:" << bufferLen[tmp_gazebo_id] << " $$$$$$$$$$***************" << std::endl;
+      std::cout << "*************$$$$$$$$$ displs: " << displs[tmp_gazebo_id] << " $$$$$$$$$$***************" << std::endl;
+
       if (this->gazeboLocalID == tmp_gazebo_id)
       {
         sendBuffer = new char[bufferLen[tmp_gazebo_id] + 1];
@@ -980,6 +984,8 @@ void World::Update()
     receiveBuffer = new char[receiveBufferLen + 1];
     receiveBuffer[receiveBufferLen] = '\0';
 
+    std::cout << "*************$$$$$$$$$ bufferLen:" << receiveBufferLen << " $$$$$$$$$$***************" << std::endl;
+
     // Added by zhangshuai 2019.04.03 for count time ----Begin
 #ifdef USE_COUNT_TIME
     gettimeofday(&tv, NULL);
@@ -1031,7 +1037,7 @@ void World::Update()
         {
           math::Pose tmp_pose;
           std::string tmp_model_name = modelPoseListReceive.pose(i).name();
-          // std::string tmp_model_name = this->distribution->GetGazeboIDPtr(1)->GetModelName(i);
+          std::cout << "*************$$$$$$$$$ model_name:" << tmp_model_name << " $$$$$$$$$$***************" << std::endl;
           tmp_pose.pos.x = modelPoseListReceive.pose(i).position().x();
           tmp_pose.pos.y = modelPoseListReceive.pose(i).position().y();
           tmp_pose.pos.z = modelPoseListReceive.pose(i).position().z();
@@ -1047,6 +1053,8 @@ void World::Update()
 
     delete[] sendBuffer;
     delete[] receiveBuffer;
+    delete[] bufferLen;
+    delete[] displs;
 
     // Added by zhangshuai 2019.04.03 for count time ----Begin
 #ifdef USE_COUNT_TIME