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

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 10 commits
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
99 changes: 99 additions & 0 deletions source/ignition_live/FUSDLayerNoticeListener.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,99 @@
/*
* Copyright (C) 2022 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/

#include "FUSDLayerNoticeListener.hpp"

#include <memory>
#include <string>

#include <ignition/transport/Node.hh>

namespace ignition
{
namespace omniverse
{
class FUSDLayerNoticeListener::Implementation
{
public:
std::shared_ptr<ThreadSafe<pxr::UsdStageRefPtr>> stage;
std::string worldName;
std::shared_ptr<ignition::transport::Node> node;
};

FUSDLayerNoticeListener::FUSDLayerNoticeListener(
std::shared_ptr<ThreadSafe<pxr::UsdStageRefPtr>> _stage,
const std::string& _worldName)
: dataPtr(ignition::utils::MakeImpl<Implementation>())
{
this->dataPtr->stage = _stage;
this->dataPtr->worldName = _worldName;
this->dataPtr->node = std::make_shared<ignition::transport::Node>();
}

void FUSDLayerNoticeListener::HandleGlobalLayerReload(
const pxr::SdfNotice::LayerDidReloadContent& n)
{
igndbg << "HandleGlobalLayerReload called" << std::endl;
}

// Print some interesting info about the LayerNotice
void FUSDLayerNoticeListener::HandleRootOrSubLayerChange(
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;

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->dataPtr->node->Request(
"/world/" + this->dataPtr->worldName + "/remove",
req, timeout, rep, result);
if (executed)
{
if (rep.data())
{
igndbg << "Model was removed [" << sdfPath.GetName() << "]"
<< std::endl;
this->dataPtr->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;
}
}
}

}
}
201 changes: 15 additions & 186 deletions source/ignition_live/FUSDLayerNoticeListener.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,13 @@

#include "Scene.hpp"

#include "ThreadSafe.hpp"

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

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

#include <ignition/utils/ImplPtr.hh>

namespace ignition
{
Expand All @@ -29,196 +34,20 @@ 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);

void HandleGlobalLayerReload(const pxr::SdfNotice::LayerDidReloadContent& n)
{
igndbg << "HandleGlobalLayerReload called" << std::endl;
}
void HandleGlobalLayerReload(const pxr::SdfNotice::LayerDidReloadContent& n);

// Print some interesting info about the LayerNotice
void HandleRootOrSubLayerChange(
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';
// // }
// }
// }
// }
// }
}
const class pxr::SdfNotice::LayersDidChangeSentPerLayer& _layerNotice,
const pxr::TfWeakPtr<pxr::SdfLayer>& _sender);

Scene& scene;
std::string worldName;
ignition::transport::Node node;
ignition::common::WorkerPool pool;
/// \internal
/// \brief Private data pointer
IGN_UTILS_IMPL_PTR(dataPtr)
};
} // namespace omniverse
} // namespace ignition
Expand Down
Loading