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

Connector: IssacSim -> Ignition #16

Merged
merged 15 commits into from
Mar 31, 2022
Merged

Connector: IssacSim -> Ignition #16

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
15 commits
Select commit Hold shift + click to select a range
c90eea6
Connector: IssacSim -> Ignition
ahcorde Feb 24, 2022
c5d1fc1
Merge remote-tracking branch 'origin/main' into ahcorde/issacsim_2_ig…
ahcorde Mar 1, 2022
ffdd1f0
Added feedback
ahcorde Mar 1, 2022
f858a4d
Workaround model sync (#19)
Mar 16, 2022
7edfc7d
Added some fixes
ahcorde Mar 16, 2022
91f76eb
Some fixes
ahcorde Mar 17, 2022
24dc155
Joints
ahcorde Mar 17, 2022
e7bc26c
improve joint and whem we should add a link or visual
ahcorde Mar 18, 2022
d572087
include some improvements
ahcorde Mar 24, 2022
b23402e
Improve implemenetation
ahcorde Mar 24, 2022
5ac2ab0
Added feedback
ahcorde Mar 28, 2022
78e43db
Merge remote-tracking branch 'origin/main' into ahcorde/issacsim_2_ig…
ahcorde Mar 28, 2022
c9f346a
Fix joint issue
ahcorde Mar 29, 2022
4d6daa0
Restructure tutorials
ahcorde Mar 29, 2022
e952b99
Fix some issues
ahcorde Mar 30, 2022
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
219 changes: 47 additions & 172 deletions source/ignition_live/FUSDLayerNoticeListener.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,10 @@

#include "Scene.hpp"

#include "ThreadSafe.hpp"

#include <pxr/usd/usd/stage.h>

#include <ignition/common/Console.hh>
#include <ignition/common/WorkerPool.hh>

Expand All @@ -29,9 +33,10 @@ namespace omniverse
class FUSDLayerNoticeListener : public pxr::TfWeakBase
{
public:
FUSDLayerNoticeListener(Scene& _scene,
const std::string& _worldName)
: scene(_scene), worldName(_worldName)
FUSDLayerNoticeListener(
std::shared_ptr<ThreadSafe<pxr::UsdStageRefPtr>> _stage,
const std::string& _worldName)
: stage(_stage), worldName(_worldName)
{
}

Expand All @@ -45,177 +50,47 @@ class FUSDLayerNoticeListener : public pxr::TfWeakBase
const class pxr::SdfNotice::LayersDidChangeSentPerLayer& LayerNotice,
const pxr::TfWeakPtr<pxr::SdfLayer>& Sender)
{
// auto Iter = LayerNotice.find(Sender);
// for (auto& ChangeEntry : Iter->second.GetEntryList())
// {
// const pxr::SdfPath& sdfPath = ChangeEntry.first;

// pxr::SdfPathVector vectorPath = sdfPath.GetPrefixes();

// // for (auto &v : vectorPath)
// // {
// // igndbg << "v " << v.GetName() << '\n';
// // }

// if (ChangeEntry.second.flags.didRemoveNonInertPrim)
// {
// std::unordered_map<std::string, IgnitionModel> models;
// models = this->scene->GetModels();
// ignition::msgs::Entity req;

// auto it = models.find(sdfPath.GetName());
// if (it == models.end())
// {
// ignwarn << "Not able to remove [" << sdfPath.GetName() << "]"
// << std::endl;
// }

// req.set_id(it->second.id);
// req.set_name(it->second.name);
// // req.set_name(ignition::msgs::Entity::Type::MODEL);

// ignition::msgs::Boolean rep;
// bool result;
// unsigned int timeout = 5000;
// bool executed = node.Request("/world/" + this->worldName + "/remove",
// req, timeout, rep, result);
// if (executed)
// {
// if (rep.data())
// {
// igndbg << "model was removed [" << sdfPath.GetName() << "]"
// << std::endl;
// this->scene->RemoveModel(sdfPath.GetName());
// }
// else
// {
// ignerr << "Error model was not removed [" << sdfPath.GetName()
// << "]" << std::endl;
// }
// }
// // ignmsg << " deleted " << sdfPath.GetName() << std::endl;
// }
// else if (ChangeEntry.second.flags.didAddNonInertPrim)
// {
// ignmsg << " added" << sdfPath.GetName() << std::endl;
// }

// for (auto Info : ChangeEntry.second.infoChanged)
// {
// if (Info.second.second.IsArrayValued() &&
// Info.second.second.GetArraySize() > 4)
// {
// ignmsg << " : " << Info.second.second.GetTypeName() << "["
// << Info.second.second.GetArraySize() << "]" << std::endl;
// }
// else
// {
// std::unordered_map<std::string, IgnitionModel> models;
// models = this->scene->GetModels();

// IgnitionModel ignitionModel;
// auto it = models.find(vectorPath[1].GetName());
// if (it == models.end())
// {
// return;
// }
// ignitionModel = it->second;

// // Prepare the input parameters.
// ignition::msgs::Pose req;
// ignition::msgs::Boolean rep;

// req.set_name(vectorPath[1].GetName());
// req.set_id(ignitionModel.id);

// if (sdfPath.GetName() == "xformOp:translate")
// {
// // std::cerr << "Info.second.second.GetTypeName() " <<
// // Info.second.second.GetTypeName() << '\n';
// pxr::GfVec3f translate;
// if (Info.second.second.GetTypeName() == "GfVec3d")
// {
// pxr::GfVec3d translated = Info.second.second.Get<pxr::GfVec3d>();
// translate[0] = translated[0];
// translate[1] = translated[1];
// translate[2] = translated[2];
// }
// else if (Info.second.second.GetTypeName() == "GfVec3f")
// {
// translate = Info.second.second.Get<pxr::GfVec3f>();
// }
// req.mutable_position()->set_x(translate[0]);
// req.mutable_position()->set_y(translate[1]);
// req.mutable_position()->set_z(translate[2]);
// }
// else
// {
// req.mutable_position()->set_x(ignitionModel.pose.Pos().X());
// req.mutable_position()->set_y(ignitionModel.pose.Pos().Y());
// req.mutable_position()->set_z(ignitionModel.pose.Pos().Z());
// }

// if (sdfPath.GetName() == "xformOp:orientation")
// {
// }
// else
// {
// req.mutable_orientation()->set_x(ignitionModel.pose.Rot().X());
// req.mutable_orientation()->set_y(ignitionModel.pose.Rot().Y());
// req.mutable_orientation()->set_z(ignitionModel.pose.Rot().Z());
// req.mutable_orientation()->set_w(ignitionModel.pose.Rot().W());
// }

// ignmsg << " : " << Info.second.first << " -> " << Info.second.second
// << std::endl;

// {
// // pool.AddWork([&] ()
// // {
// bool result;
// unsigned int timeout = 5000;
// bool executed = node.Request("/world/" + worldName + "/set_pose",
// req, timeout, rep, result);
// if (executed)
// {
// if (rep.data())
// {
// igndbg << "The position was setted fine" << std::endl;
// }
// else
// {
// ignerr << "The position was not setted fine" << std::endl;
// }
// }
// // });
// // bool result;
// // unsigned int timeout = 500;
// // bool executed = this->node.Request(
// // "/world/" + worldName + "/set_pose", req, timeout, rep, result);

// // this->scene->SetModelPose(vectorPath[1].GetName(),
// // ignition::math::Pose3d(
// // ignition::math::Vector3d(
// // req.mutable_position()->x(),
// // req.mutable_position()->y(),
// // req.mutable_position()->z()
// // ),
// // ignition::math::Quaterniond(
// // req.mutable_orientation()->w(),
// // req.mutable_orientation()->x(),
// // req.mutable_orientation()->y(),
// // req.mutable_orientation()->z())));
// // if (executed)
// // {
// // std::cerr << "rep " << rep.data() << '\n';
// // }
// }
// }
// }
// }
auto Iter = LayerNotice.find(Sender);
for (auto& ChangeEntry : Iter->second.GetEntryList())
ahcorde marked this conversation as resolved.
Show resolved Hide resolved
{
const pxr::SdfPath& sdfPath = ChangeEntry.first;

if (ChangeEntry.second.flags.didRemoveNonInertPrim)
{
ignition::msgs::Entity req;
req.set_name(sdfPath.GetName());
req.set_type(ignition::msgs::Entity::MODEL);

ignition::msgs::Boolean rep;
bool result;
unsigned int timeout = 5000;
bool executed = this->node.Request(
"/world/" + this->worldName + "/remove",
req, timeout, rep, result);
if (executed)
{
if (rep.data())
{
igndbg << "Model was removed [" << sdfPath.GetName() << "]"
<< std::endl;
this->stage->Lock()->RemovePrim(sdfPath);
}
else
{
ignerr << "Error model was not removed [" << sdfPath.GetName()
<< "]" << std::endl;
}
}
ignmsg << "Deleted " << sdfPath.GetName() << std::endl;
}
else if (ChangeEntry.second.flags.didAddNonInertPrim)
{
ignmsg << "Added" << sdfPath.GetName() << std::endl;
}
}
}

Scene& scene;
std::shared_ptr<ThreadSafe<pxr::UsdStageRefPtr>> stage;
std::string worldName;
ignition::transport::Node node;
ignition::common::WorkerPool pool;
Expand Down
Loading