Skip to content

Handle gpu articulation in ROS2 joint publisher #293

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

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
+25 −13
Open

Handle gpu articulation in ROS2 joint publisher #293

Show file tree
Hide file tree
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
4 changes: 3 additions & 1 deletion source/extensions/isaacsim.ros2.bridge/include/isaacsim/ros2/bridge/Ros2Message.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -427,6 +427,7 @@ class Ros2JointStateMessage : public Ros2Message
* @param[in] jointEfforts Joint efforts.
* @param[in] dofTypes Articulation DOF types.
* @param[in] stageUnits Unit scale of the stage.
* @param[in] cudaDeviceIndex Index of the device where the data lives (-1 for host data)
*/
virtual void writeData(const double& timeStamp,
omni::physics::tensors::IArticulationView* articulation,
Expand All @@ -435,7 +436,8 @@ class Ros2JointStateMessage : public Ros2Message
std::vector<float>& jointVelocities,
std::vector<float>& jointEfforts,
std::vector<uint8_t>& dofTypes,
const double& stageUnits) = 0;
const double& stageUnits,
const int& cudaDeviceIndex) = 0;

/**
* @brief Read the message field values.
Expand Down
4 changes: 3 additions & 1 deletion source/extensions/isaacsim.ros2.bridge/library/backend/Ros2Impl.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -450,6 +450,7 @@ class Ros2JointStateMessageImpl : public Ros2JointStateMessage, Ros2MessageInter
* @param[in] jointEfforts Vector of joint efforts
* @param[in] dofTypes Vector of DOF types
* @param[in] stageUnits Stage unit scale factor
* @param[in] cudaDeviceIndex Index of the device where the data lives (-1 for host data)
*/
virtual void writeData(const double& timeStamp,
omni::physics::tensors::IArticulationView* articulation,
Expand All @@ -458,7 +459,8 @@ class Ros2JointStateMessageImpl : public Ros2JointStateMessage, Ros2MessageInter
std::vector<float>& jointVelocities,
std::vector<float>& jointEfforts,
std::vector<uint8_t>& dofTypes,
const double& stageUnits);
const double& stageUnits,
const int& cudaDeviceIndex);

/**
* @brief Reads the joint state data
Expand Down
17 changes: 10 additions & 7 deletions source/extensions/isaacsim.ros2.bridge/library/backend/Ros2Message.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -998,15 +998,16 @@ template <typename T>
static void createTensorDesc(omni::physics::tensors::TensorDesc& tensorDesc,
std::vector<T>& buffer,
int numElements,
omni::physics::tensors::TensorDataType type)
omni::physics::tensors::TensorDataType type,
int cudaDeviceIndex)
{
buffer.resize(numElements);
tensorDesc.dtype = type;
tensorDesc.numDims = 1;
tensorDesc.dims[0] = numElements;
tensorDesc.data = buffer.data();
tensorDesc.ownData = true;
tensorDesc.device = -1;
tensorDesc.device = cudaDeviceIndex;
}

void Ros2JointStateMessageImpl::writeData(const double& timeStamp,
Expand All @@ -1016,7 +1017,8 @@ void Ros2JointStateMessageImpl::writeData(const double& timeStamp,
std::vector<float>& jointVelocities,
std::vector<float>& jointEfforts,
std::vector<uint8_t>& dofTypes,
const double& stageUnits)
const double& stageUnits,
const int& cudaDeviceIndex)
{
if (!m_msg)
{
Expand All @@ -1030,10 +1032,11 @@ void Ros2JointStateMessageImpl::writeData(const double& timeStamp,
omni::physics::tensors::TensorDesc velocityTensor;
omni::physics::tensors::TensorDesc effortTensor;
omni::physics::tensors::TensorDesc dofTypeTensor;
createTensorDesc(positionTensor, jointPositions, numDofs, omni::physics::tensors::TensorDataType::eFloat32);
createTensorDesc(velocityTensor, jointVelocities, numDofs, omni::physics::tensors::TensorDataType::eFloat32);
createTensorDesc(effortTensor, jointEfforts, numDofs, omni::physics::tensors::TensorDataType::eFloat32);
createTensorDesc(dofTypeTensor, dofTypes, numDofs, omni::physics::tensors::TensorDataType::eUint8);
createTensorDesc(positionTensor, jointPositions, numDofs, omni::physics::tensors::TensorDataType::eFloat32, cudaDeviceIndex);
createTensorDesc(velocityTensor, jointVelocities, numDofs, omni::physics::tensors::TensorDataType::eFloat32, cudaDeviceIndex);
createTensorDesc(effortTensor, jointEfforts, numDofs, omni::physics::tensors::TensorDataType::eFloat32, cudaDeviceIndex);
// The DOF type data always lives on host.
createTensorDesc(dofTypeTensor, dofTypes, numDofs, omni::physics::tensors::TensorDataType::eUint8, -1);
bool hasDofStates = true;
if (!articulation->getDofPositions(&positionTensor))
{
Expand Down
2 changes: 1 addition & 1 deletion source/extensions/isaacsim.ros2.bridge/nodes/OgnROS2PublishJointState.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -164,7 +164,7 @@ class OgnROS2PublishJointState : public Ros2Node
m_stage = pxr::UsdUtilsStageCache::Get().Find(pxr::UsdStageCache::Id::FromLongInt(stageId));

state.m_message->writeData(db.inputs.timeStamp(), m_articulation, m_stage, m_jointPositions, m_jointVelocities,
m_jointEfforts, m_dofTypes, stageUnits);
m_jointEfforts, m_dofTypes, stageUnits, db.inputs.cudaDeviceIndex());
state.m_publisher.get()->publish(state.m_message->getPtr());
return true;
}
Expand Down
11 changes: 8 additions & 3 deletions source/extensions/isaacsim.ros2.bridge/nodes/OgnROS2PublishJointState.ogn
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
{
"ROS2PublishJointState":{
"version": 1,
"version": 2,
"icon": "icons/isaac-sim.svg",
"description": [
"This node publishes joint states of a robot in ROS2 JointState message"
Expand Down Expand Up @@ -49,10 +49,15 @@
"description": "ROS2 Timestamp in seconds",
"uiName": "Timestamp",
"default": 0.0


},
"cudaDeviceIndex": {
"type": "int",
"description": "Index of the device where the data lives (-1 for host data)",
"default": -1
}
}
}


}
}