diff --git a/.github/workflows/nexus_integration_tests.yaml b/.github/workflows/nexus_integration_tests.yaml index f9aadcd..0629357 100644 --- a/.github/workflows/nexus_integration_tests.yaml +++ b/.github/workflows/nexus_integration_tests.yaml @@ -40,7 +40,7 @@ jobs: rosdep update rosdep install --from-paths . -yir - name: build - run: /ros_entrypoint.sh colcon build --packages-up-to nexus_integration_tests nexus_motion_planner --mixin release lld --cmake-args -DCMAKE_CXX_COMPILER_LAUNCHER=ccache + run: /ros_entrypoint.sh colcon build --packages-up-to nexus_gazebo nexus_integration_tests nexus_motion_planner --mixin release lld --cmake-args -DCMAKE_CXX_COMPILER_LAUNCHER=ccache - name: Test - Unit Tests run: . ./install/setup.bash && RMW_IMPLEMENTATION=rmw_cyclonedds_cpp /ros_entrypoint.sh colcon test --packages-select nexus_motion_planner --event-handlers=console_direct+ - name: Test - Integration Test diff --git a/nexus_gazebo/CHANGELOG.rst b/nexus_gazebo/CHANGELOG.rst new file mode 100644 index 0000000..72e003f --- /dev/null +++ b/nexus_gazebo/CHANGELOG.rst @@ -0,0 +1,10 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package nexus_gazebo +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.1 (2023-11-22) +------------------ + +0.1.0 (2023-11-06) +------------------ +* Provides a motion capture plugin for Gazebo to track rigid bodies and broadcast their poses over VRPN. diff --git a/nexus_gazebo/CMakeLists.txt b/nexus_gazebo/CMakeLists.txt new file mode 100644 index 0000000..6fed223 --- /dev/null +++ b/nexus_gazebo/CMakeLists.txt @@ -0,0 +1,51 @@ +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) +project(nexus_gazebo) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(ignition-gazebo6 REQUIRED) +find_package(VRPN REQUIRED) + +#=============================================================================== +add_library(${PROJECT_NAME} SHARED + src/MotionCaptureSystem.cc + src/MotionCaptureRigidBody.cc +) + +target_link_libraries(${PROJECT_NAME} + ignition-gazebo6::core + ${VRPN_LIBRARIES} +) + +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ + ${VRPN_INCLUDE_DIRS} +) + +ament_target_dependencies(${PROJECT_NAME} + ignition-gazebo6 + VRPN) + + +#=============================================================================== +if(BUILD_TESTING) + +endif() + +#=============================================================================== +install(DIRECTORY models/motion_capture_system DESTINATION share/${PROJECT_NAME}/models) +install(DIRECTORY models/motion_capture_rigid_body DESTINATION share/${PROJECT_NAME}/models) +install(DIRECTORY include/nexus_gazebo DESTINATION include/${PROJECT_NAME}) +install(TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + RUNTIME DESTINATION lib/${PROJECT_NAME} + INCLUDES DESTINATION include/${PROJECT_NAME} +) + +ament_environment_hooks("nexus_gazebo_gazebo_paths.dsv.in") + +ament_package() diff --git a/nexus_gazebo/include/nexus_gazebo/Components.hh b/nexus_gazebo/include/nexus_gazebo/Components.hh new file mode 100644 index 0000000..51eab80 --- /dev/null +++ b/nexus_gazebo/include/nexus_gazebo/Components.hh @@ -0,0 +1,36 @@ +// Copyright 2023 Johnson & Johnson +// +// 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. + +#ifndef NEXUS_GAZEBO__COMPONENTS_HH_ +#define NEXUS_GAZEBO__COMPONENTS_HH_ + +#include +#include +#include +#include +#include + +namespace nexus_gazebo::components { +using MotionCaptureRigidBody = ignition::gazebo::components::Component< + std::string, + class MotionCaptureRigidBodyTag, + ignition::gazebo::serializers::StringSerializer>; + +IGN_GAZEBO_REGISTER_COMPONENT( + "nexus_gazebo.components.MotionCaptureRigidBody", + MotionCaptureRigidBody) + +} // namespace nexus_gazebo::components + +#endif // NEXUS_GAZEBO__COMPONENTS_HH_ diff --git a/nexus_gazebo/include/nexus_gazebo/MotionCaptureRigidBody.hh b/nexus_gazebo/include/nexus_gazebo/MotionCaptureRigidBody.hh new file mode 100644 index 0000000..b1a6c6f --- /dev/null +++ b/nexus_gazebo/include/nexus_gazebo/MotionCaptureRigidBody.hh @@ -0,0 +1,51 @@ +// Copyright 2023 Johnson & Johnson +// +// 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. + +#ifndef NEXUS_GAZEBO__MOTIONCAPTURERIGIDBODY_HH_ +#define NEXUS_GAZEBO__MOTIONCAPTURERIGIDBODY_HH_ + +#include +#include + +#include +#include + +namespace nexus_gazebo { + +using Entity = ignition::gazebo::Entity; +using EntityComponentManager = ignition::gazebo::EntityComponentManager; +using EventManager = ignition::gazebo::EventManager; +using ISystemConfigure = ignition::gazebo::ISystemConfigure; +using System = ignition::gazebo::System; + +/// \class MotionCaptureRigidBody +class MotionCaptureRigidBody : + public System, + public ISystemConfigure +{ +public: + MotionCaptureRigidBody(); + + void Configure( + const Entity& _entity, + const std::shared_ptr& _sdf, + EntityComponentManager& _ecm, + EventManager& _eventMgr) override; + +private: + std::string rigid_body_label; +}; +} // namespace nexus_gazebo + +#endif // NEXUS_GAZEBO__MOTIONCAPTURERIGIDBODY_HH_ diff --git a/nexus_gazebo/include/nexus_gazebo/MotionCaptureSystem.hh b/nexus_gazebo/include/nexus_gazebo/MotionCaptureSystem.hh new file mode 100644 index 0000000..14d77ad --- /dev/null +++ b/nexus_gazebo/include/nexus_gazebo/MotionCaptureSystem.hh @@ -0,0 +1,106 @@ +// Copyright 2023 Johnson & Johnson +// +// 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. + +#ifndef NEXUS_GAZEBO__MOTIONCAPTURESYSTEM_HH_ +#define NEXUS_GAZEBO__MOTIONCAPTURESYSTEM_HH_ + +#include +#include +#include +#include + +#include +#include + +#include "vrpn_Connection.h" +#include "vrpn_ConnectionPtr.h" +#include "vrpn_Tracker.h" + +namespace nexus_gazebo { + +using Entity = ignition::gazebo::Entity; +using EntityComponentManager = ignition::gazebo::EntityComponentManager; +using EventManager = ignition::gazebo::EventManager; +using ISystemConfigure = ignition::gazebo::ISystemConfigure; +using ISystemPostUpdate = ignition::gazebo::ISystemPostUpdate; +using Pose3d = ignition::math::Pose3d; +using System = ignition::gazebo::System; +using UpdateInfo = ignition::gazebo::UpdateInfo; + +class RigidBodyTracker : public vrpn_Tracker +{ +public: + explicit RigidBodyTracker( + const std::string& _tracker_name, + vrpn_Connection* _c = nullptr); + + ~RigidBodyTracker() override = default; + + void mainloop() override; + + void updatePose( + const std::chrono::steady_clock::duration& _sim_time, + const Pose3d& pose); + +private: + struct timeval timestamp; + Pose3d pose; +}; + +/// \class MotionCaptureSystem +class MotionCaptureSystem : + public System, + public ISystemConfigure, + public ISystemPostUpdate +{ +public: + /// \brief Constructor + MotionCaptureSystem(); + + /// \brief Destructor + ~MotionCaptureSystem() override; + + /// Configure the system + void Configure( + const Entity& _entity, + const std::shared_ptr& _sdf, + EntityComponentManager& _ecm, + EventManager& _eventMgr) override; + + /// Post-update callback + void PostUpdate( + const UpdateInfo& _info, + const EntityComponentManager& _ecm) override; + +private: + int port {3883}; + bool stream_rigid_bodies {true}; + bool stream_labeled_markers {true}; + bool stream_unlabeled_markers {true}; + double update_frequency {50.0}; + + /// Pointer to the VRPN connection + vrpn_ConnectionPtr connection {nullptr}; + + Entity entity {0}; + + std::chrono::steady_clock::duration last_pose_pub_time {0}; + + /// Collection of VRPN trackers + std::unordered_map> trackers; +}; + +} // namespace nexus_gazebo +#endif // NEXUS_GAZEBO__MOTIONCAPTURESYSTEM_HH_ diff --git a/nexus_gazebo/models/motion_capture_rigid_body/model.config b/nexus_gazebo/models/motion_capture_rigid_body/model.config new file mode 100644 index 0000000..88dd2f6 --- /dev/null +++ b/nexus_gazebo/models/motion_capture_rigid_body/model.config @@ -0,0 +1,15 @@ + + + motion_capture_rigid_body + 1.0 + motion_capture_rigid_body.sdf + + + Michael Carroll + michael@openrobotics.org + + + + A group of motion capture markers tracked as a rigid body + + diff --git a/nexus_gazebo/models/motion_capture_rigid_body/motion_capture_rigid_body.sdf b/nexus_gazebo/models/motion_capture_rigid_body/motion_capture_rigid_body.sdf new file mode 100644 index 0000000..61bd5b0 --- /dev/null +++ b/nexus_gazebo/models/motion_capture_rigid_body/motion_capture_rigid_body.sdf @@ -0,0 +1,26 @@ + + + + + + + + 0.0032 + + + + 0.9 0.9 0.9 1.0 + + + + + + + 0.0032 + + + + + + + diff --git a/nexus_gazebo/models/motion_capture_system/model.config b/nexus_gazebo/models/motion_capture_system/model.config new file mode 100644 index 0000000..8029750 --- /dev/null +++ b/nexus_gazebo/models/motion_capture_system/model.config @@ -0,0 +1,15 @@ + + + motion_capture_system + 1.0 + motion_capture_system.sdf + + + Michael Carroll + michael@openrobotics.org + + + + A motion capture system that publishes the poses of markers and rigid bodies over VRPN. + + diff --git a/nexus_gazebo/models/motion_capture_system/motion_capture_system.sdf b/nexus_gazebo/models/motion_capture_system/motion_capture_system.sdf new file mode 100644 index 0000000..34486e8 --- /dev/null +++ b/nexus_gazebo/models/motion_capture_system/motion_capture_system.sdf @@ -0,0 +1,13 @@ + + + true + + + 3883 + 50.0 + true + true + true + + + diff --git a/nexus_gazebo/nexus_gazebo_gazebo_paths.dsv.in b/nexus_gazebo/nexus_gazebo_gazebo_paths.dsv.in new file mode 100644 index 0000000..3b934f0 --- /dev/null +++ b/nexus_gazebo/nexus_gazebo_gazebo_paths.dsv.in @@ -0,0 +1,4 @@ +prepend-non-duplicate;GZ_SIM_RESOURCE_PATH;@CMAKE_INSTALL_PREFIX@/share/@PROJECT_NAME@/models +prepend-non-duplicate;GZ_SIM_SYSTEM_PLUGIN_PATH;@CMAKE_INSTALL_PREFIX@/lib +prepend-non-duplicate;IGN_GAZEBO_RESOURCE_PATH;@CMAKE_INSTALL_PREFIX@/share/@PROJECT_NAME@/models +prepend-non-duplicate;IGN_GAZEBO_SYSTEM_PLUGIN_PATH;@CMAKE_INSTALL_PREFIX@/lib diff --git a/nexus_gazebo/package.xml b/nexus_gazebo/package.xml new file mode 100644 index 0000000..dfa81ea --- /dev/null +++ b/nexus_gazebo/package.xml @@ -0,0 +1,25 @@ + + + + nexus_gazebo + 0.1.1 + Nexus gazebo simulation assets + + Michael Carroll + + Apache License 2.0 + + Michael Carroll + + ament_cmake + + rclcpp + vrpn + + ignition-gazebo6 + ignition-math6 + + + ament_cmake + + diff --git a/nexus_gazebo/src/MotionCaptureRigidBody.cc b/nexus_gazebo/src/MotionCaptureRigidBody.cc new file mode 100644 index 0000000..8a19ab8 --- /dev/null +++ b/nexus_gazebo/src/MotionCaptureRigidBody.cc @@ -0,0 +1,55 @@ +// Copyright 2023 Johnson & Johnson +// +// 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 "nexus_gazebo/MotionCaptureRigidBody.hh" +#include "nexus_gazebo/Components.hh" + +#include +#include +#include + +constexpr const char* kRigidBodyLabel = "rigid_body_label"; + +namespace nexus_gazebo { + +////////////////////////////////////////////////// +MotionCaptureRigidBody::MotionCaptureRigidBody() = default; + +void MotionCaptureRigidBody::Configure( + const Entity& _entity, + const std::shared_ptr& _sdf, + EntityComponentManager& _ecm, + EventManager& /*_eventMgr*/) +{ + // Read SDF Configuration + this->rigid_body_label = _sdf->Get( + kRigidBodyLabel, this->rigid_body_label).first; + + if (this->rigid_body_label.empty()) + { + // In the case that the user hasn't overridden the label, then use + // the name of the entity we are attached to. + this->rigid_body_label = + _ecm.Component(_entity)->Data(); + } + + _ecm.CreateComponent(_entity, + components::MotionCaptureRigidBody(this->rigid_body_label)); +} +} // namespace nexus_gazebo + +IGNITION_ADD_PLUGIN( + nexus_gazebo::MotionCaptureRigidBody, + nexus_gazebo::System, + nexus_gazebo::MotionCaptureRigidBody::ISystemConfigure); diff --git a/nexus_gazebo/src/MotionCaptureSystem.cc b/nexus_gazebo/src/MotionCaptureSystem.cc new file mode 100644 index 0000000..cf30753 --- /dev/null +++ b/nexus_gazebo/src/MotionCaptureSystem.cc @@ -0,0 +1,172 @@ +// Copyright 2023 Johnson & Johnson +// +// 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 "nexus_gazebo/MotionCaptureSystem.hh" +#include "nexus_gazebo/Components.hh" + +#include + +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/Util.hh" + +constexpr const char* kPort = "port"; +constexpr const char* kSdfStreamRigid = "stream_rigid_bodies"; +constexpr const char* kSdfStreamLabeled = "stream_labeled_markers"; +constexpr const char* kSdfStreamUnlabeled = "stream_unlabeled_markers"; +constexpr const char* kUpdateFrequency = "update_frequency"; + +namespace nexus_gazebo { + +RigidBodyTracker::RigidBodyTracker(const std::string& _tracker_name, + vrpn_Connection* c) +: vrpn_Tracker(_tracker_name.c_str(), c) +{ +} + +void RigidBodyTracker::mainloop() +{ + this->pos[0] = this->pose.X(); + this->pos[1] = this->pose.Y(); + this->pos[2] = this->pose.Z(); + + this->d_quat[0] = this->pose.Rot().X(); + this->d_quat[1] = this->pose.Rot().Y(); + this->d_quat[2] = this->pose.Rot().Z(); + this->d_quat[3] = this->pose.Rot().W(); + + std::array msgbuf {}; + int len = vrpn_Tracker::encode_to(msgbuf.data()); + + if (!static_cast(this->d_connection->connected())) + return; + + if (0 > this->d_connection->pack_message( + len, + this->timestamp, + position_m_id, + d_sender_id, msgbuf.data(), + vrpn_CONNECTION_LOW_LATENCY)) + { + return; + } + + server_mainloop(); +} + +void RigidBodyTracker::updatePose( + const std::chrono::steady_clock::duration& _sim_time, + const Pose3d& _pose) +{ + this->pose = _pose; + this->timestamp.tv_sec = _sim_time.count() / 1000; + this->timestamp.tv_usec = (_sim_time.count() % 1000) * 1000; +} + +////////////////////////////////////////////////// +MotionCaptureSystem::MotionCaptureSystem() = default; + +////////////////////////////////////////////////// +MotionCaptureSystem::~MotionCaptureSystem() = default; + +////////////////////////////////////////////////// +void MotionCaptureSystem::Configure( + const Entity& _entity, + const std::shared_ptr& _sdf, + EntityComponentManager& /*_ecm*/, + EventManager& /*_eventMgr*/) +{ + this->entity = _entity; + + // Read SDF configuration + this->port = _sdf->Get(kPort, this->port).first; + this->stream_rigid_bodies = + _sdf->Get(kSdfStreamRigid, this->stream_rigid_bodies).first; + this->stream_labeled_markers = + _sdf->Get(kSdfStreamLabeled, this->stream_labeled_markers).first; + this->stream_unlabeled_markers = + _sdf->Get(kSdfStreamUnlabeled, this->stream_unlabeled_markers).first; + this->update_frequency = + _sdf->Get(kUpdateFrequency, this->update_frequency).first; + + // Configure the VRPN server + this->connection = vrpn_ConnectionPtr::create_server_connection(this->port); +} + +////////////////////////////////////////////////// +void MotionCaptureSystem::PostUpdate( + const UpdateInfo& _info, + const EntityComponentManager& _ecm) +{ + if (_info.dt < std::chrono::steady_clock::duration::zero()) + { + ignwarn << "Detected jump back in time" << std::endl; + } + + if (_info.paused) + { + return; + } + + auto diff = _info.simTime - this->last_pose_pub_time; + if ((diff > std::chrono::steady_clock::duration::zero()) && + (diff < std::chrono::duration {1.0 / this->update_frequency})) + { + return; + } + + // Tracker pose in world frame + const auto pose_WT = ignition::gazebo::worldPose(this->entity, _ecm); + + if (this->stream_rigid_bodies) + { + _ecm.Each( + [&](const Entity& _entity, + const components::MotionCaptureRigidBody* _rigid_body)->bool + { + // Rigid body pose in world frame + const auto pose_WB = ignition::gazebo::worldPose(_entity, _ecm); + + // Rigid body pose in tracker frame (pose_TB = pose_TW * pose_WB) + const auto pose_TB = pose_WT.Inverse() * pose_WB; + + if (this->trackers.count(_entity) == 0) + { + ignmsg << "Creating Tracker: " << _rigid_body->Data() << std::endl; + this->trackers.insert( + { + _entity, + std::make_unique( + _rigid_body->Data(), + this->connection.get()) + }); + } + this->trackers[_entity]->updatePose(_info.simTime, pose_TB); + return true; + }); + } + + for (auto&[_, tracker] : this->trackers) + { + tracker->mainloop(); + } + this->connection->mainloop(); +} + +} // namespace nexus_gazebo + +IGNITION_ADD_PLUGIN( + nexus_gazebo::MotionCaptureSystem, + nexus_gazebo::System, + nexus_gazebo::MotionCaptureSystem::ISystemConfigure, + nexus_gazebo::MotionCaptureSystem::ISystemPostUpdate) diff --git a/nexus_gazebo/worlds/example.sdf b/nexus_gazebo/worlds/example.sdf new file mode 100644 index 0000000..c24c7ae --- /dev/null +++ b/nexus_gazebo/worlds/example.sdf @@ -0,0 +1,38 @@ + + + + origin_mocap + model://motion_capture_system + 0 0 0 0 0 0 + true + + + + rotated_mocap + model://motion_capture_system + 0 0 0 0 0 1.5707 + true + + + + model://motion_capture_rigid_body + calibration_link_x + 1 0 0 0 0 0 + true + + + + model://motion_capture_rigid_body + calibration_link_y + 0 1 0 0 0 0 + true + + + + model://motion_capture_rigid_body + calibration_link_z + 0 1 0 0 0 0 + true + + +