modiy
This commit is contained in:
zhangshuai
parent
7e41222cfa
commit
85c423e5ea
|
|
@ -275,8 +275,14 @@ void World::Load(sdf::ElementPtr _sdf)
|
|||
if (this->dataPtr->sdf->HasElement("distribution"))
|
||||
{
|
||||
sdf::ElementPtr distributionElem = this->dataPtr->sdf->GetElement("distribution");
|
||||
// int myid = 0;
|
||||
int nproc;
|
||||
char proc_name[MPI_MAX_PROCESSOR_NAME];
|
||||
int nameLength;
|
||||
MPI_Comm_rank(MPI_COMM_WORLD, &this->gazeboLocalID);
|
||||
MPI_Comm_size(MPI_COMM_WORLD, &nproc);
|
||||
MPI_Get_processor_name(proc_name, &nameLength);
|
||||
printf("Gazebo, Iam host %s with rank %d of %d\n", proc_name, this->gazeboLocalID, nproc);
|
||||
|
||||
this->flag = distributionElem->Get<int>("flag");
|
||||
|
||||
std::cout << "================= gazebo_local_ID: " << this->gazeboLocalID << "\t Flag: " << this->flag << " =================" << std::endl;
|
||||
|
|
@ -898,150 +904,150 @@ void World::Update()
|
|||
#endif
|
||||
// Added by zhangshuai 2019.04.03 for count time ----End
|
||||
|
||||
// Added by zhangshuai 2019.04.23 for MPI communication----Begin
|
||||
if (1 == this->flag)
|
||||
// Added by zhangshuai 2019.04.23 for MPI communication----Begin
|
||||
if (1 == this->flag)
|
||||
{
|
||||
// Added by zhangshuai 2019.04.03 for count time ----Begin
|
||||
#ifdef USE_COUNT_TIME
|
||||
gettimeofday(&tv, NULL);
|
||||
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
|
||||
#endif
|
||||
// Added by zhangshuai 2019.04.03 for count time ----End
|
||||
|
||||
MPI_Status *status = (MPI_Status *)malloc(sizeof(MPI_Status) * (this->distribution->GetGazeboCount()));
|
||||
MPI_Request *request = (MPI_Request *)malloc(sizeof(MPI_Request) * (this->distribution->GetGazeboCount()));
|
||||
|
||||
msgs::Pose_V modelPoseListSend;
|
||||
|
||||
for (unsigned int i = 0; i < this->distribution->GetGazeboIDPtr(this->gazeboLocalID)->GetModelCount(); i++)
|
||||
{
|
||||
// Added by zhangshuai 2019.04.03 for count time ----Begin
|
||||
#ifdef USE_COUNT_TIME
|
||||
gettimeofday(&tv, NULL);
|
||||
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
|
||||
#endif
|
||||
// Added by zhangshuai 2019.04.03 for count time ----End
|
||||
msgs::Pose *modelPose;
|
||||
modelPose = modelPoseListSend.add_pose();
|
||||
|
||||
MPI_Status *status = (MPI_Status *)malloc(sizeof(MPI_Status) * (this->distribution->GetGazeboCount()));
|
||||
MPI_Request *request = (MPI_Request *)malloc(sizeof(MPI_Request) * (this->distribution->GetGazeboCount()));
|
||||
std::string tmp_model_name = this->distribution->GetGazeboIDPtr(this->gazeboLocalID)->GetModelName(i);
|
||||
|
||||
msgs::Pose_V modelPoseListSend;
|
||||
modelPose->set_name(tmp_model_name);
|
||||
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);
|
||||
|
||||
for (unsigned int i = 0; i < this->distribution->GetGazeboIDPtr(this->gazeboLocalID)->GetModelCount(); i++)
|
||||
{
|
||||
msgs::Pose *modelPose;
|
||||
modelPose = modelPoseListSend.add_pose();
|
||||
|
||||
std::string tmp_model_name = this->distribution->GetGazeboIDPtr(this->gazeboLocalID)->GetModelName(i);
|
||||
|
||||
modelPose->set_name(tmp_model_name);
|
||||
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();
|
||||
}
|
||||
|
||||
// Added by zhangshuai 2019.04.03 for count time ----Begin
|
||||
#ifdef USE_COUNT_TIME
|
||||
gettimeofday(&tv, NULL);
|
||||
before_tcpTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
|
||||
#endif
|
||||
// Added by zhangshuai 2019.04.03 for count time ----End
|
||||
|
||||
// Added by zhangshuai 2019.04.03 for count time ----Begin
|
||||
#ifdef USE_COUNT_TIME
|
||||
gettimeofday(&tv, NULL);
|
||||
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
|
||||
#endif
|
||||
// Added by zhangshuai 2019.04.03 for count time ----End
|
||||
|
||||
//send poses to another process
|
||||
for (int tmp_gazebo_id = 0; tmp_gazebo_id < this->distribution->GetGazeboCount(); tmp_gazebo_id++)
|
||||
{
|
||||
if (this->gazeboLocalID != tmp_gazebo_id)
|
||||
{
|
||||
std::string send_messages = "";
|
||||
if (modelPoseListSend.pose_size() > 0)
|
||||
{
|
||||
modelPoseListSend.SerializeToString(&send_messages);
|
||||
modelPoseListSend.clear_pose();
|
||||
}
|
||||
|
||||
int bufferLen = send_messages.size();
|
||||
char *sendBuffer = new char[bufferLen + 1];
|
||||
for (int i = 0; i < bufferLen; i++)
|
||||
{
|
||||
sendBuffer[i] = send_messages[i];
|
||||
}
|
||||
sendBuffer[bufferLen] = '\0';
|
||||
|
||||
send_messages.clear();
|
||||
|
||||
MPI_Isend(sendBuffer, bufferLen, MPI_CHAR, tmp_gazebo_id, 99, MPI_COMM_WORLD, &request[tmp_gazebo_id]);
|
||||
}
|
||||
}
|
||||
|
||||
// Added by zhangshuai 2019.04.03 for count time ----Begin
|
||||
#ifdef USE_COUNT_TIME
|
||||
gettimeofday(&tv, NULL);
|
||||
tcpTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
|
||||
#endif
|
||||
// Added by zhangshuai 2019.04.03 for count time ----End
|
||||
|
||||
// Added by zhangshuai 2019.04.03 for count time ----Begin
|
||||
#ifdef USE_COUNT_TIME
|
||||
gettimeofday(&tv, NULL);
|
||||
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
|
||||
#endif
|
||||
// Added by zhangshuai 2019.04.03 for count time ----End
|
||||
|
||||
//receive and get information from other processes
|
||||
for (int tmp_gazebo_id = 0; tmp_gazebo_id < this->distribution->GetGazeboCount(); tmp_gazebo_id++)
|
||||
{
|
||||
if (this->gazeboLocalID != tmp_gazebo_id)
|
||||
{
|
||||
MPI_Probe(tmp_gazebo_id, 99, MPI_COMM_WORLD, &status[tmp_gazebo_id]);
|
||||
int size;
|
||||
MPI_Get_count(&status[tmp_gazebo_id], MPI_CHAR, &size);
|
||||
char *receiveBuffer = (char *)malloc(sizeof(char) * (size));
|
||||
MPI_Recv(receiveBuffer, size, MPI_CHAR, tmp_gazebo_id, 99, MPI_COMM_WORLD, &status[tmp_gazebo_id]);
|
||||
|
||||
if (size > 0)
|
||||
{
|
||||
std::string receive_messages;
|
||||
for (int j = 0; j < size; j++)
|
||||
{
|
||||
receive_messages.push_back(receiveBuffer[j]);
|
||||
}
|
||||
|
||||
msgs::Pose_V modelPoseListReceive;
|
||||
modelPoseListReceive.ParseFromString(receive_messages);
|
||||
receive_messages.clear();
|
||||
|
||||
//insert models according to name,pose,vel from process 0
|
||||
for (int i = 0; i < modelPoseListReceive.pose_size(); i++)
|
||||
{
|
||||
math::Pose tmp_pose;
|
||||
std::string tmp_model_name = modelPoseListReceive.pose(i).name();
|
||||
// std::string tmp_model_name = this->distribution->GetGazeboIDPtr(1)->GetModelName(i);
|
||||
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();
|
||||
tmp_pose.rot.x = modelPoseListReceive.pose(i).orientation().x();
|
||||
tmp_pose.rot.y = modelPoseListReceive.pose(i).orientation().y();
|
||||
tmp_pose.rot.z = modelPoseListReceive.pose(i).orientation().z();
|
||||
tmp_pose.rot.w = modelPoseListReceive.pose(i).orientation().w();
|
||||
|
||||
this->GetModel(tmp_model_name)->GetLink("canonical")->SetWorldPose(tmp_pose);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Added by zhangshuai 2019.04.03 for count time ----Begin
|
||||
#ifdef USE_COUNT_TIME
|
||||
gettimeofday(&tv, NULL);
|
||||
after_tcpTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
|
||||
#endif
|
||||
// Added by zhangshuai 2019.04.03 for count time ----End
|
||||
|
||||
// MPI_Waitall(this->distribution->GetGazeboCount(), request, status);
|
||||
// MPI_Barrier(MPI_COMM_WORLD);
|
||||
tmp_model_name.clear();
|
||||
}
|
||||
MPI_Barrier(MPI_COMM_WORLD);
|
||||
// Added by zhangshuai 2019.04.23 for MPI communication ----End
|
||||
|
||||
// Added by zhangshuai 2019.04.03 for count time ----Begin
|
||||
#ifdef USE_COUNT_TIME
|
||||
gettimeofday(&tv, NULL);
|
||||
before_tcpTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
|
||||
#endif
|
||||
// Added by zhangshuai 2019.04.03 for count time ----End
|
||||
|
||||
// Added by zhangshuai 2019.04.03 for count time ----Begin
|
||||
#ifdef USE_COUNT_TIME
|
||||
gettimeofday(&tv, NULL);
|
||||
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
|
||||
#endif
|
||||
// Added by zhangshuai 2019.04.03 for count time ----End
|
||||
|
||||
//send poses to another process
|
||||
for (int tmp_gazebo_id = 0; tmp_gazebo_id < this->distribution->GetGazeboCount(); tmp_gazebo_id++)
|
||||
{
|
||||
if (this->gazeboLocalID != tmp_gazebo_id)
|
||||
{
|
||||
std::string send_messages = "";
|
||||
if (modelPoseListSend.pose_size() > 0)
|
||||
{
|
||||
modelPoseListSend.SerializeToString(&send_messages);
|
||||
modelPoseListSend.clear_pose();
|
||||
}
|
||||
|
||||
int bufferLen = send_messages.size();
|
||||
char *sendBuffer = new char[bufferLen + 1];
|
||||
for (int i = 0; i < bufferLen; i++)
|
||||
{
|
||||
sendBuffer[i] = send_messages[i];
|
||||
}
|
||||
sendBuffer[bufferLen] = '\0';
|
||||
|
||||
send_messages.clear();
|
||||
|
||||
MPI_Isend(sendBuffer, bufferLen, MPI_CHAR, tmp_gazebo_id, 99, MPI_COMM_WORLD, &request[tmp_gazebo_id]);
|
||||
}
|
||||
}
|
||||
|
||||
// Added by zhangshuai 2019.04.03 for count time ----Begin
|
||||
#ifdef USE_COUNT_TIME
|
||||
gettimeofday(&tv, NULL);
|
||||
tcpTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
|
||||
#endif
|
||||
// Added by zhangshuai 2019.04.03 for count time ----End
|
||||
|
||||
// Added by zhangshuai 2019.04.03 for count time ----Begin
|
||||
#ifdef USE_COUNT_TIME
|
||||
gettimeofday(&tv, NULL);
|
||||
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
|
||||
#endif
|
||||
// Added by zhangshuai 2019.04.03 for count time ----End
|
||||
|
||||
//receive and get information from other processes
|
||||
for (int tmp_gazebo_id = 0; tmp_gazebo_id < this->distribution->GetGazeboCount(); tmp_gazebo_id++)
|
||||
{
|
||||
if (this->gazeboLocalID != tmp_gazebo_id)
|
||||
{
|
||||
MPI_Probe(tmp_gazebo_id, 99, MPI_COMM_WORLD, &status[tmp_gazebo_id]);
|
||||
int size;
|
||||
MPI_Get_count(&status[tmp_gazebo_id], MPI_CHAR, &size);
|
||||
char *receiveBuffer = (char *)malloc(sizeof(char) * (size));
|
||||
MPI_Recv(receiveBuffer, size, MPI_CHAR, tmp_gazebo_id, 99, MPI_COMM_WORLD, &status[tmp_gazebo_id]);
|
||||
|
||||
if (size > 0)
|
||||
{
|
||||
std::string receive_messages;
|
||||
for (int j = 0; j < size; j++)
|
||||
{
|
||||
receive_messages.push_back(receiveBuffer[j]);
|
||||
}
|
||||
|
||||
msgs::Pose_V modelPoseListReceive;
|
||||
modelPoseListReceive.ParseFromString(receive_messages);
|
||||
receive_messages.clear();
|
||||
|
||||
//insert models according to name,pose,vel from process 0
|
||||
for (int i = 0; i < modelPoseListReceive.pose_size(); i++)
|
||||
{
|
||||
math::Pose tmp_pose;
|
||||
std::string tmp_model_name = modelPoseListReceive.pose(i).name();
|
||||
// std::string tmp_model_name = this->distribution->GetGazeboIDPtr(1)->GetModelName(i);
|
||||
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();
|
||||
tmp_pose.rot.x = modelPoseListReceive.pose(i).orientation().x();
|
||||
tmp_pose.rot.y = modelPoseListReceive.pose(i).orientation().y();
|
||||
tmp_pose.rot.z = modelPoseListReceive.pose(i).orientation().z();
|
||||
tmp_pose.rot.w = modelPoseListReceive.pose(i).orientation().w();
|
||||
|
||||
this->GetModel(tmp_model_name)->GetLink("canonical")->SetWorldPose(tmp_pose);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Added by zhangshuai 2019.04.03 for count time ----Begin
|
||||
#ifdef USE_COUNT_TIME
|
||||
gettimeofday(&tv, NULL);
|
||||
after_tcpTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
|
||||
#endif
|
||||
// Added by zhangshuai 2019.04.03 for count time ----End
|
||||
|
||||
// MPI_Waitall(this->distribution->GetGazeboCount(), request, status);
|
||||
// MPI_Barrier(MPI_COMM_WORLD);
|
||||
}
|
||||
MPI_Barrier(MPI_COMM_WORLD);
|
||||
// Added by zhangshuai 2019.04.23 for MPI communication ----End
|
||||
|
||||
// Only update state information if logging data.
|
||||
if (util::LogRecord::Instance()->Running())
|
||||
|
|
|
|||
Loading…
Reference in New Issue