Resolve merge conflicts

Signed-off-by: Yadunund <[email protected]>

.github/workflows/nexus_integration_tests.yaml

@@ -40,7 +40,11 @@ jobs:
         rosdep update
         rosdep install --from-paths . -yir
     - name: build
-      run: /ros_entrypoint.sh colcon build --packages-up-to nexus_calibration nexus_integration_tests nexus_motion_planner --mixin release lld --cmake-args -DCMAKE_CXX_COMPILER_LAUNCHER=ccache
+<<<<<<< HEAD
+      run: /ros_entrypoint.sh colcon build --packages-up-to nexus_calibration nexus_gazebo 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_calibration nexus_gazebo nexus_integration_tests nexus_motion_planner --mixin release lld --cmake-args -DCMAKE_CXX_COMPILER_LAUNCHER=ccache
+>>>>>>> main
     - 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

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.

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
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/include/${PROJECT_NAME}>
+  ${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()

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 <string>
+#include <ignition/gazebo/components/Component.hh>
+#include <ignition/gazebo/components/Factory.hh>
+#include <ignition/gazebo/components/Serialization.hh>
+#include <ignition/gazebo/config.hh>
+
+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_

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 <memory>
+#include <string>
+
+#include <ignition/gazebo/config.hh>
+#include <ignition/gazebo/System.hh>
+
+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<const sdf::Element>& _sdf,
+    EntityComponentManager& _ecm,
+    EventManager& _eventMgr) override;
+
+private:
+  std::string rigid_body_label;
+};
+}  // namespace nexus_gazebo
+
+#endif  // NEXUS_GAZEBO__MOTIONCAPTURERIGIDBODY_HH_

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 <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#include <ignition/gazebo/config.hh>
+#include <ignition/gazebo/System.hh>
+
+#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<const sdf::Element>& _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<Entity,
+    std::unique_ptr<RigidBodyTracker>> trackers;
+};
+
+}  // namespace nexus_gazebo
+#endif  // NEXUS_GAZEBO__MOTIONCAPTURESYSTEM_HH_

nexus_gazebo/models/motion_capture_rigid_body/model.config

@@ -0,0 +1,15 @@
+<?xml version='1.0' encoding='utf-8'?>
+<model>
+  <name>motion_capture_rigid_body</name>
+  <version>1.0</version>
+  <sdf version="1.9">motion_capture_rigid_body.sdf</sdf>
+
+  <author>
+    <name>Michael Carroll</name>
+    <email>[email protected]</email>
+  </author>
+
+  <description>
+    A group of motion capture markers tracked as a rigid body
+  </description>
+</model>

nexus_gazebo/models/motion_capture_rigid_body/motion_capture_rigid_body.sdf

@@ -0,0 +1,26 @@
+<sdf version="1.9">
+    <model name="motion_capture_rigid_body">
+        <link name="motion_capture_rigid_body">
+            <visual name="visual0">
+              <geometry>
+                <sphere>
+                  <!-- 1/4" marker bead -->
+                  <radius>0.0032</radius>
+                </sphere>
+              </geometry>
+              <material>
+                <diffuse>0.9 0.9 0.9 1.0</diffuse>
+              </material>
+            </visual>
+            <collision name="collision0">
+              <geometry>
+                <sphere>
+                  <!-- 1/4" marker bead -->
+                  <radius>0.0032</radius>
+                </sphere>
+              </geometry>
+            </collision>
+        </link>
+        <plugin filename="nexus_gazebo" name="nexus_gazebo::MotionCaptureRigidBody"/>
+    </model>
+</sdf>

nexus_gazebo/models/motion_capture_system/model.config

@@ -0,0 +1,15 @@
+<?xml version='1.0' encoding='utf-8'?>
+<model>
+  <name>motion_capture_system</name>
+  <version>1.0</version>
+  <sdf version="1.9">motion_capture_system.sdf</sdf>
+
+  <author>
+    <name>Michael Carroll</name>
+    <email>[email protected]</email>
+  </author>
+
+  <description>
+  A motion capture system that publishes the poses of markers and rigid bodies over VRPN.
+  </description>
+</model>

nexus_gazebo/models/motion_capture_system/motion_capture_system.sdf

@@ -0,0 +1,13 @@
+<sdf version="1.9">
+    <model name="motion_capture_system">
+        <static>true</static>
+        <link name="motion_capture_origin"/>
+        <plugin filename="nexus_gazebo" name="nexus_gazebo::MotionCaptureSystem">
+          <port>3883</port>
+          <update_frequency>50.0</update_frequency>
+          <stream_rigid_bodies>true</stream_rigid_bodies>
+          <stream_labeled_markers>true</stream_labeled_markers>
+          <stream_unlabeled_markers>true</stream_unlabeled_markers>
+        </plugin>
+    </model>
+</sdf>

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

nexus_gazebo/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nexus_gazebo</name>
+  <version>0.1.1</version>
+  <description>Nexus gazebo simulation assets</description>
+
+  <maintainer email="[email protected]">Michael Carroll</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <author email="[email protected]">Michael Carroll</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>vrpn</depend>
+
+  <depend condition="$GZ_VERSION == '' and $IGNITION_VERSION == ''">ignition-gazebo6</depend>
+  <depend condition="$GZ_VERSION == '' and $IGNITION_VERSION == ''">ignition-math6</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>