Skip to content

Update the robot pose transmission logic #744

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 2 commits into from
Jan 15, 2021
Merged

Update the robot pose transmission logic #744

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
6514b45
Update the robot pose transmission logic
Jan 7, 2021
f49a1e6
Also compute robot distances using the 1m/1s rule
Jan 7, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
154 changes: 78 additions & 76 deletions subt_ign/src/GameLogicPlugin.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1387,9 +1387,12 @@ void GameLogicPlugin::PostUpdate(
this->dataPtr->robotPrevPose[name] = pose;
return true;
}
// Send robot pose information if the robot has traveled more then
// 1m or 1second of simulation time has elapsed.
else if (!robotPoseDataIt->second.empty() &&
robotPoseDataIt->second.back().second.Pos().Distance(
pose.Pos()) > 1.0)
(robotPoseDataIt->second.back().second.Pos().Distance(
pose.Pos()) > 1.0 ||
t - robotPoseDataIt->second.back().first.count() * 1e-9 > 1.0))
{
// time passed since last pose sample
double prevT = robotPoseDataIt->second.back().first.count() * 1e-9;
Expand Down Expand Up @@ -1468,95 +1471,94 @@ void GameLogicPlugin::PostUpdate(
}

robotPoseDataIt->second.push_back(std::make_pair(tDur, pose));
}

// compute and log greatest / total distance traveled and
// elevation changes
// compute and log greatest / total distance traveled and
// elevation changes

// distance traveled by this robot
double distanceDiff =
this->dataPtr->robotPrevPose[name].Pos().Distance(pose.Pos());
double distanceTraveled = this->dataPtr->robotDistance[name] +
distanceDiff;
this->dataPtr->robotDistance[name] = distanceTraveled;
// distance traveled by this robot
double distanceDiff =
this->dataPtr->robotPrevPose[name].Pos().Distance(pose.Pos());
double distanceTraveled = this->dataPtr->robotDistance[name] +
distanceDiff;
this->dataPtr->robotDistance[name] = distanceTraveled;

// greatest distance traveled by a robot
if (distanceTraveled > this->dataPtr->maxRobotDistance.second)
{
this->dataPtr->maxRobotDistance.first = name;
this->dataPtr->maxRobotDistance.second = distanceTraveled;
}
// greatest distance traveled by a robot
if (distanceTraveled > this->dataPtr->maxRobotDistance.second)
{
this->dataPtr->maxRobotDistance.first = name;
this->dataPtr->maxRobotDistance.second = distanceTraveled;
}

// max euclidean from starting pose for this robot
double euclideanDist =
pose.Pos().Distance(this->dataPtr->robotStartPose[name].Pos());
if (euclideanDist > this->dataPtr->robotMaxEuclideanDistance[name])
this->dataPtr->robotMaxEuclideanDistance[name] = euclideanDist;
// max euclidean from starting pose for this robot
double euclideanDist =
pose.Pos().Distance(this->dataPtr->robotStartPose[name].Pos());
if (euclideanDist > this->dataPtr->robotMaxEuclideanDistance[name])
this->dataPtr->robotMaxEuclideanDistance[name] = euclideanDist;

// greatest euclidean distance traveled by a robot
if (euclideanDist > this->dataPtr->maxRobotEuclideanDistance.second)
{
this->dataPtr->maxRobotEuclideanDistance.first = name;
this->dataPtr->maxRobotEuclideanDistance.second = euclideanDist;
}
// greatest euclidean distance traveled by a robot
if (euclideanDist > this->dataPtr->maxRobotEuclideanDistance.second)
{
this->dataPtr->maxRobotEuclideanDistance.first = name;
this->dataPtr->maxRobotEuclideanDistance.second = euclideanDist;
}

// total distance traveled by all robots
this->dataPtr->robotsTotalDistance += distanceDiff;
// total distance traveled by all robots
this->dataPtr->robotsTotalDistance += distanceDiff;

// greatest elevation gain / loss
// Elevations are rounded down to nearest mulitple of the elevation
// step size
double elevationDiff = this->dataPtr->FloorMultiple(
pose.Pos().Z(), this->dataPtr->elevationStepSize) -
this->dataPtr->FloorMultiple(
this->dataPtr->robotPrevPose[name].Pos().Z(),
this->dataPtr->elevationStepSize);
// greatest elevation gain / loss
// Elevations are rounded down to nearest mulitple of the elevation
// step size
double elevationDiff = this->dataPtr->FloorMultiple(
pose.Pos().Z(), this->dataPtr->elevationStepSize) -
this->dataPtr->FloorMultiple(
this->dataPtr->robotPrevPose[name].Pos().Z(),
this->dataPtr->elevationStepSize);

if (elevationDiff > 0)
{
double elevationGain = this->dataPtr->robotElevationGain[name]
+ elevationDiff;
this->dataPtr->robotElevationGain[name] = elevationGain;
if (elevationGain > this->dataPtr->maxRobotElevationGain.second)
if (elevationDiff > 0)
{
this->dataPtr->maxRobotElevationGain.first = name;
this->dataPtr->maxRobotElevationGain.second = elevationGain;
double elevationGain = this->dataPtr->robotElevationGain[name]
+ elevationDiff;
this->dataPtr->robotElevationGain[name] = elevationGain;
if (elevationGain > this->dataPtr->maxRobotElevationGain.second)
{
this->dataPtr->maxRobotElevationGain.first = name;
this->dataPtr->maxRobotElevationGain.second = elevationGain;
}
// total elevation gain by all robots
this->dataPtr->robotsTotalElevationGain += elevationDiff;
}
// total elevation gain by all robots
this->dataPtr->robotsTotalElevationGain += elevationDiff;
}
else
{
double elevationLoss = this->dataPtr->robotElevationLoss[name]
+ elevationDiff;
this->dataPtr->robotElevationLoss[name] = elevationLoss;
if (elevationLoss < this->dataPtr->maxRobotElevationLoss.second)
else
{
this->dataPtr->maxRobotElevationLoss.first = name;
this->dataPtr->maxRobotElevationLoss.second = elevationLoss;
double elevationLoss = this->dataPtr->robotElevationLoss[name]
+ elevationDiff;
this->dataPtr->robotElevationLoss[name] = elevationLoss;
if (elevationLoss < this->dataPtr->maxRobotElevationLoss.second)
{
this->dataPtr->maxRobotElevationLoss.first = name;
this->dataPtr->maxRobotElevationLoss.second = elevationLoss;
}
// total elevation loss by all robots
this->dataPtr->robotsTotalElevationLoss += elevationDiff;
}
// total elevation loss by all robots
this->dataPtr->robotsTotalElevationLoss += elevationDiff;
}

// min / max elevation reached
double elevation = this->dataPtr->FloorMultiple(pose.Pos().Z(),
this->dataPtr->elevationStepSize);
if (elevation > this->dataPtr->maxRobotElevation.second ||
this->dataPtr->maxRobotElevation.first.empty())
{
this->dataPtr->maxRobotElevation.first = name;
this->dataPtr->maxRobotElevation.second = elevation;
}
// min / max elevation reached
double elevation = this->dataPtr->FloorMultiple(pose.Pos().Z(),
this->dataPtr->elevationStepSize);
if (elevation > this->dataPtr->maxRobotElevation.second ||
this->dataPtr->maxRobotElevation.first.empty())
{
this->dataPtr->maxRobotElevation.first = name;
this->dataPtr->maxRobotElevation.second = elevation;
}

if (elevation < this->dataPtr->minRobotElevation.second ||
this->dataPtr->minRobotElevation.first.empty())
{
this->dataPtr->minRobotElevation.first = name;
this->dataPtr->minRobotElevation.second = elevation;
if (elevation < this->dataPtr->minRobotElevation.second ||
this->dataPtr->minRobotElevation.first.empty())
{
this->dataPtr->minRobotElevation.first = name;
this->dataPtr->minRobotElevation.second = elevation;
}
this->dataPtr->robotPrevPose[name] = pose;
}

this->dataPtr->robotPrevPose[name] = pose;
return true;
});

Expand Down