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"))
|
if (this->dataPtr->sdf->HasElement("distribution"))
|
||||||
{
|
{
|
||||||
sdf::ElementPtr distributionElem = this->dataPtr->sdf->GetElement("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_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");
|
this->flag = distributionElem->Get<int>("flag");
|
||||||
|
|
||||||
std::cout << "================= gazebo_local_ID: " << this->gazeboLocalID << "\t Flag: " << this->flag << " =================" << std::endl;
|
std::cout << "================= gazebo_local_ID: " << this->gazeboLocalID << "\t Flag: " << this->flag << " =================" << std::endl;
|
||||||
|
|
@ -898,150 +904,150 @@ void World::Update()
|
||||||
#endif
|
#endif
|
||||||
// Added by zhangshuai 2019.04.03 for count time ----End
|
// Added by zhangshuai 2019.04.03 for count time ----End
|
||||||
|
|
||||||
// Added by zhangshuai 2019.04.23 for MPI communication----Begin
|
// Added by zhangshuai 2019.04.23 for MPI communication----Begin
|
||||||
if (1 == this->flag)
|
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
|
msgs::Pose *modelPose;
|
||||||
#ifdef USE_COUNT_TIME
|
modelPose = modelPoseListSend.add_pose();
|
||||||
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()));
|
std::string tmp_model_name = this->distribution->GetGazeboIDPtr(this->gazeboLocalID)->GetModelName(i);
|
||||||
MPI_Request *request = (MPI_Request *)malloc(sizeof(MPI_Request) * (this->distribution->GetGazeboCount()));
|
|
||||||
|
|
||||||
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++)
|
tmp_model_name.clear();
|
||||||
{
|
|
||||||
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);
|
|
||||||
}
|
}
|
||||||
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.
|
// Only update state information if logging data.
|
||||||
if (util::LogRecord::Instance()->Running())
|
if (util::LogRecord::Instance()->Running())
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue