Skip to content
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

Jump to bottom

Refactor: Trajectory loading seperated into external function #1744

Merged
merged 1 commit into from
Nov 1, 2022
Merged

Refactor: Trajectory loading seperated into external function #1744

Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
187 changes: 103 additions & 84 deletions src/rendering/SceneManager.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -129,6 +129,16 @@ class gz::sim::SceneManagerPrivate
public: AnimationUpdateData ActorTrajectoryAt(
Entity _id, const std::chrono::steady_clock::duration &_time) const;

/// \brief Load Actor trajectories
/// \param[in] _actor Actor
/// \param[in] _mapAnimNameId Animation name to id map
/// \param[in] _skel Mesh skeleton
/// \return Trajectory vector
public: std::vector<common::TrajectoryInfo>
LoadTrajectories(const sdf::Actor &_actor,
std::unordered_map<std::string, unsigned int> &_mapAnimNameId,
common::SkeletonPtr _skel);

/// \brief Holds the spherical coordinates from the world.
public: math::SphericalCoordinates sphericalCoordinates;
};
Expand Down Expand Up @@ -1035,90 +1045,8 @@ rendering::VisualPtr SceneManager::CreateActor(Entity _id,
return nullptr;

this->dataPtr->actorSkeletons[_id] = meshSkel;

std::vector<common::TrajectoryInfo> trajectories;
if (_actor.TrajectoryCount() != 0)
{
// Load all trajectories specified in sdf
for (unsigned i = 0; i < _actor.TrajectoryCount(); i++)
{
const sdf::Trajectory *trajSdf = _actor.TrajectoryByIndex(i);
if (nullptr == trajSdf)
{
gzerr << "Null trajectory SDF for [" << _actor.Name() << "]"
<< std::endl;
continue;
}

common::TrajectoryInfo trajInfo;
trajInfo.SetId(trajSdf->Id());
trajInfo.SetAnimIndex(mapAnimNameId[trajSdf->Type()]);

if (trajSdf->WaypointCount() != 0)
{
std::map<TP, math::Pose3d> waypoints;
for (unsigned j = 0; j < trajSdf->WaypointCount(); j++)
{
auto point = trajSdf->WaypointByIndex(j);
TP pointTp(std::chrono::milliseconds(
static_cast<int>(point->Time()*1000)));
waypoints[pointTp] = point->Pose();
}
trajInfo.SetWaypoints(waypoints, trajSdf->Tension());
// Animations are offset by 1 because index 0 is taken by the mesh name
auto animation = _actor.AnimationByIndex(trajInfo.AnimIndex()-1);

if (animation && animation->InterpolateX())
{
// warn if no x displacement can be interpolated
// warn only once per mesh
static std::unordered_set<std::string> animInterpolateCheck;
if (animInterpolateCheck.count(animation->Filename()) == 0)
{
std::string rootNodeName = meshSkel->RootNode()->Name();
common::SkeletonAnimation *skelAnim =
meshSkel->Animation(trajInfo.AnimIndex());
common::NodeAnimation *rootNode = skelAnim->NodeAnimationByName(
rootNodeName);
math::Matrix4d lastPos = rootNode->KeyFrame(
rootNode->FrameCount() - 1).second;
math::Matrix4d firstPos = rootNode->KeyFrame(0).second;
if (!math::equal(firstPos.Translation().X(),
lastPos.Translation().X()))
{
trajInfo.Waypoints()->SetInterpolateX(animation->InterpolateX());
}
else
{
gzwarn << "Animation has no x displacement. "
<< "Ignoring <interpolate_x> for the animation in '"
<< animation->Filename() << "'." << std::endl;
}
animInterpolateCheck.insert(animation->Filename());
}
}
}
else
{
trajInfo.SetTranslated(false);
}
trajectories.push_back(trajInfo);
}
}
// if there are no trajectories, but there are animations, add a trajectory
else
{
auto skel = this->dataPtr->actorSkeletons[_id];
common::TrajectoryInfo trajInfo;
trajInfo.SetId(0);
trajInfo.SetAnimIndex(0);
trajInfo.SetStartTime(TP(0ms));
auto timepoint = std::chrono::milliseconds(
static_cast<int>(skel->Animation(0)->Length() * 1000));
trajInfo.SetEndTime(TP(timepoint));
trajInfo.SetTranslated(false);
trajectories.push_back(trajInfo);
}
auto trajectories = this->dataPtr->LoadTrajectories(_actor, mapAnimNameId,
meshSkel);

// sequencing all trajectories
auto delayStartTime = std::chrono::milliseconds(
Expand Down Expand Up @@ -2402,3 +2330,94 @@ SceneManager::LoadAnimations(const sdf::Actor &_actor)
}
return mapAnimNameId;
}

/////////////////////////////////////////////////
std::vector<common::TrajectoryInfo>
SceneManagerPrivate::LoadTrajectories(const sdf::Actor &_actor,
std::unordered_map<std::string, unsigned int> &_mapAnimNameId,
common::SkeletonPtr _meshSkel)
{
std::vector<common::TrajectoryInfo> trajectories;
if (_actor.TrajectoryCount() != 0)
{
// Load all trajectories specified in sdf
for (unsigned i = 0; i < _actor.TrajectoryCount(); ++i)
{
const sdf::Trajectory *trajSdf = _actor.TrajectoryByIndex(i);
if (nullptr == trajSdf)
{
ignerr << "Null trajectory SDF for [" << _actor.Name() << "]"
<< std::endl;
continue;
}
common::TrajectoryInfo trajInfo;
trajInfo.SetId(trajSdf->Id());
trajInfo.SetAnimIndex(_mapAnimNameId[trajSdf->Type()]);

if (trajSdf->WaypointCount() != 0)
{
std::map<TP, math::Pose3d> waypoints;
for (unsigned j = 0; j < trajSdf->WaypointCount(); ++j)
{
auto point = trajSdf->WaypointByIndex(j);
TP pointTp(std::chrono::milliseconds(
static_cast<int>(point->Time() * 1000)));
waypoints[pointTp] = point->Pose();
}
trajInfo.SetWaypoints(waypoints, trajSdf->Tension());
// Animations are offset by 1 because index 0 is taken by the mesh name
auto animation = _actor.AnimationByIndex(trajInfo.AnimIndex() - 1);

if (animation && animation->InterpolateX())
{
// warn if no x displacement can be interpolated
// warn only once per mesh
static std::unordered_set<std::string> animInterpolateCheck;
if (animInterpolateCheck.count(animation->Filename()) == 0)
{
std::string rootNodeName = _meshSkel->RootNode()->Name();
common::SkeletonAnimation *skelAnim =
_meshSkel->Animation(trajInfo.AnimIndex());
common::NodeAnimation *rootNode = skelAnim->NodeAnimationByName(
rootNodeName);
math::Matrix4d lastPos = rootNode->KeyFrame(
rootNode->FrameCount() - 1).second;
math::Matrix4d firstPos = rootNode->KeyFrame(0).second;
if (!math::equal(firstPos.Translation().X(),
lastPos.Translation().X()))
{
trajInfo.Waypoints()->SetInterpolateX(animation->InterpolateX());
}
else
{
ignwarn << "Animation has no x displacement. "
<< "Ignoring <interpolate_x> for the animation in '"
<< animation->Filename() << "'." << std::endl;
}
animInterpolateCheck.insert(animation->Filename());
}
}
}
else
{
trajInfo.SetTranslated(false);
}
trajectories.push_back(trajInfo);
}
}
// if there are no trajectories, but there are animations, add a trajectory
else
{
common::TrajectoryInfo trajInfo;
trajInfo.SetId(0);
trajInfo.SetAnimIndex(0);
trajInfo.SetStartTime(TP(0ms));

auto timepoint = std::chrono::milliseconds(
static_cast<int>(_meshSkel->Animation(0)->Length() * 1000));
trajInfo.SetEndTime(TP(timepoint));
trajInfo.SetTranslated(false);
trajectories.push_back(trajInfo);
}
return trajectories;
}