Rep mocap

.github/workflows/rep_mocap.yaml

@@ -0,0 +1,39 @@
+name: rep-mocap
+
+on:
+  pull_request:
+    branches:
+      - rep-mocap
+  push:
+    branches:
+      - rep-mocap
+
+jobs:
+  build-and-test:
+    runs-on: ${{ matrix.os }}
+    strategy:
+      matrix:
+        os: [ubuntu-24.04]
+      fail-fast: false
+    steps:
+      - name: Repo checkout
+        uses: actions/checkout@v2
+
+      - name: Setup ROS 2
+        uses: ros-tooling/setup-ros@0.7.9
+        with:
+          required-ros-distributions: rolling
+      - name: build and test
+        uses: ros-tooling/action-ros-ci@0.3.15
+        with:
+          package-name: mocap4r2_control mocap4r2_control_msgs rqt_mocap4r2_control mocap4r2_marker_publisher mocap4r2_marker_viz mocap4r2_marker_viz_srvs
+          target-ros2-distro: rolling
+          vcs-repo-file-url: ${GITHUB_WORKSPACE}/dependency_repos.repos
+      - name: Codecov
+        uses: codecov/codecov-action@v1.1.0
+        with:
+          file: ros_ws/lcov/total_coverage.info
+          flags: unittests
+          name: codecov-umbrella
+          # yml: ./codecov.yml
+          fail_ci_if_error: false

dependency_repos.repos

@@ -1,5 +1,5 @@
 repositories:
-  mocap4r2_msgs:
+  mocap_interfaces:
     type: git
-    url: https://github.com/MOCAP4ROS2-Project/mocap4r2_msgs.git
-    version: humble-devel
+    url: https://github.com/MOCAP4ROS2-Project/mocap_interfaces.git
+    version: rolling

mocap4r2_control/mocap4r2_control/test/test_auxiliar.cpp

@@ -85,7 +85,5 @@ int main(int argc, char ** argv)
 
   auto result = RUN_ALL_TESTS();
 
-  rclcpp::shutdown();
-  rclcpp::Rate(1).sleep();
   return result;
 }

mocap4r2_control/mocap4r2_control/test/test_control.cpp

@@ -200,7 +200,5 @@ int main(int argc, char ** argv)
 
   auto result = RUN_ALL_TESTS();
 
-  rclcpp::shutdown();
-  rclcpp::Rate(1).sleep();
   return result;
 }

mocap4r2_control/rqt_mocap4r2_control/include/rqt_mocap4r2_control/mocap4r2_control.hpp

@@ -15,7 +15,7 @@
 #ifndef RQT_MOCAP4R2_CONTROL__MOCAP4R2_CONTROL__HPP_
 #define RQT_MOCAP4R2_CONTROL__MOCAP4R2_CONTROL__HPP_
 
-#include <rqt_gui_cpp/plugin.h>
+#include <rqt_gui_cpp/plugin.hpp>
 
 #include <ui_mocap4r2_control.h>
 

mocap4r2_dummy_driver/CMakeLists.txt

@@ -14,13 +14,13 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
-find_package(mocap4r2_msgs REQUIRED)
+find_package(mocap_interfaces REQUIRED)
 find_package(mocap4r2_control REQUIRED)
 
 set(dependencies
   rclcpp
   rclcpp_lifecycle
-  mocap4r2_msgs
+  mocap_interfaces
   mocap4r2_control
 )
 

mocap4r2_dummy_driver/include/mocap4r2_dummy_driver/mocap4r2_dummy_driver.hpp

@@ -21,10 +21,10 @@
 #ifndef MOCAP4R2_DUMMY_DRIVER__MOCAP4R2_DUMMY_DRIVER_HPP_
 #define MOCAP4R2_DUMMY_DRIVER__MOCAP4R2_DUMMY_DRIVER_HPP_
 
-#include "mocap4r2_msgs/msg/marker.hpp"
-#include "mocap4r2_msgs/msg/markers.hpp"
-#include "mocap4r2_msgs/msg/rigid_body.hpp"
-#include "mocap4r2_msgs/msg/rigid_bodies.hpp"
+#include "mocap_interfaces/msg/marker.hpp"
+#include "mocap_interfaces/msg/marker_array.hpp"
+#include "mocap_interfaces/msg/rigid_body.hpp"
+#include "mocap_interfaces/msg/rigid_body_array.hpp"
 
 
 #include "rclcpp/rclcpp.hpp"
@@ -62,14 +62,14 @@ class DummyDriverNode : public mocap4r2_control::ControlledLifecycleNode
   void control_start(const mocap4r2_control_msgs::msg::Control::SharedPtr msg) override;
   void control_stop(const mocap4r2_control_msgs::msg::Control::SharedPtr msg) override;
 
-  rclcpp_lifecycle::LifecyclePublisher<mocap4r2_msgs::msg::Markers>::SharedPtr
+  rclcpp_lifecycle::LifecyclePublisher<mocap_interfaces::msg::MarkerArray>::SharedPtr
     mocap4r2_markers_pub_;
-  rclcpp_lifecycle::LifecyclePublisher<mocap4r2_msgs::msg::RigidBodies>::SharedPtr
+  rclcpp_lifecycle::LifecyclePublisher<mocap_interfaces::msg::RigidBodyArray>::SharedPtr
     mocap4r2_rigid_body_pub_;
 
   rclcpp::TimerBase::SharedPtr timer_;
 
-  uint32_t frame_number_{0};
+  uint32_t seq_{0};
 };
 
 }  // namespace mocap4r2_dummy_driver

mocap4r2_dummy_driver/package.xml

@@ -14,7 +14,7 @@
 
   <depend>rclcpp</depend>
   <depend>rclcpp_lifecycle</depend>
-  <depend>mocap4r2_msgs</depend>
+  <depend>mocap_interfaces</depend>
   <depend>mocap4r2_control</depend>
 
   <test_depend>ament_lint_common</test_depend>

mocap4r2_dummy_driver/src/mocap4r2_dummy_driver/mocap4r2_dummy_driver.cpp

@@ -19,8 +19,8 @@
 // Author: Francisco Martín <fmrico@urjc.es>
 
 
-#include "mocap4r2_msgs/msg/marker.hpp"
-#include "mocap4r2_msgs/msg/markers.hpp"
+#include "mocap_interfaces/msg/marker.hpp"
+#include "mocap_interfaces/msg/marker_array.hpp"
 
 #include "mocap4r2_dummy_driver/mocap4r2_dummy_driver.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
@@ -56,17 +56,18 @@ DummyDriverNode::control_stop(const mocap4r2_control_msgs::msg::Control::SharedP
 void
 DummyDriverNode::publish_data()
 {
-  frame_number_++;
+  seq_++;
 
   // Markers
   if (mocap4r2_markers_pub_->get_subscription_count() > 0) {
-    mocap4r2_msgs::msg::Markers msg;
+    mocap_interfaces::msg::MarkerArray msg;
     msg.header.stamp = now();
     msg.header.frame_id = "mocap";
-    msg.frame_number = frame_number_;
+    msg.seq = seq_;
 
-    mocap4r2_msgs::msg::Marker marker;
-    marker.id_type = mocap4r2_msgs::msg::Marker::USE_INDEX;
+    mocap_interfaces::msg::Marker marker;
+    marker.header.stamp = this->now();
+    marker.id_type = mocap_interfaces::msg::Marker::USE_INDEX;
     marker.marker_index = 0;
     marker.translation.x = 0.0;
     marker.translation.y = 0.0;
@@ -89,12 +90,13 @@ DummyDriverNode::publish_data()
   }
 
   if (mocap4r2_rigid_body_pub_->get_subscription_count() > 0) {
-    mocap4r2_msgs::msg::RigidBodies msg_rb;
+    mocap_interfaces::msg::RigidBodyArray msg_rb;
     msg_rb.header.stamp = now();
     msg_rb.header.frame_id = "mocap";
-    msg_rb.frame_number = frame_number_;
+    msg_rb.seq = seq_;
 
-    mocap4r2_msgs::msg::RigidBody rb;
+    mocap_interfaces::msg::RigidBody rb;
+    rb.header.stamp = this->now();
     rb.rigid_body_name = "rigid_body_0";
     rb.pose.position.x = 0.0;
     rb.pose.position.y = 0.0;
@@ -104,8 +106,9 @@ DummyDriverNode::publish_data()
     rb.pose.orientation.z = 0.0;
     rb.pose.orientation.w = 1.0;
 
-    mocap4r2_msgs::msg::Marker marker;
-    marker.id_type = mocap4r2_msgs::msg::Marker::USE_INDEX;
+    mocap_interfaces::msg::Marker marker;
+    marker.header.stamp = this->now();
+    marker.id_type = mocap_interfaces::msg::Marker::USE_INDEX;
     marker.marker_index = 0;
     marker.translation.x = 0.0;
     marker.translation.y = 0.0;
@@ -124,7 +127,7 @@ DummyDriverNode::publish_data()
     marker.translation.z = 0.0;
     rb.markers.push_back(marker);
 
-    msg_rb.rigidbodies.push_back(rb);
+    msg_rb.rigid_bodies.push_back(rb);
 
     mocap4r2_rigid_body_pub_->publish(msg_rb);
   }
@@ -138,9 +141,9 @@ using CallbackReturnT =
 CallbackReturnT
 DummyDriverNode::on_configure(const rclcpp_lifecycle::State & state)
 {
-  mocap4r2_markers_pub_ = create_publisher<mocap4r2_msgs::msg::Markers>(
+  mocap4r2_markers_pub_ = create_publisher<mocap_interfaces::msg::MarkerArray>(
     "markers", rclcpp::QoS(1000));
-  mocap4r2_rigid_body_pub_ = create_publisher<mocap4r2_msgs::msg::RigidBodies>(
+  mocap4r2_rigid_body_pub_ = create_publisher<mocap_interfaces::msg::RigidBodyArray>(
     "rigid_bodies", rclcpp::QoS(1000));
 
   // Connect with the mocap system

mocap4r2_marker_viz/mocap4r2_marker_publisher/CMakeLists.txt

@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(mocap4r2_marker_publisher)
 set(version 2.0)
 
-set (CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD 17)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)

mocap4r2_marker_viz/mocap4r2_marker_publisher/package.xml

@@ -1,4 +1,4 @@
-<?xml version="2.0"?>
+<?xml version="1.0"?>
 <package format="3">
   <name>mocap4r2_marker_publisher</name>
   <version>0.0.7</version>

mocap4r2_marker_viz/mocap4r2_marker_viz/mocap4r2_marker_publisher/CMakeLists.txt

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.5)
+project(mocap4r2_marker_publisher)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+find_package(rclcpp REQUIRED)
+find_package(mocap_interfaces REQUIRED)
+
+add_executable(mocap4r2_marker_publisher src/mocap4r2_marker_publisher.cpp)
+
+ament_target_dependencies(mocap4r2_marker_publisher rclcpp mocap_interfaces)
+
+install(TARGETS
+  mocap4r2_marker_publisher
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

mocap4r2_marker_viz/mocap4r2_marker_viz/mocap4r2_marker_publisher/package.xml

@@ -0,0 +1,20 @@
+<?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>mocap4r2_marker_publisher</name>
+  <version>0.0.7</version>
+  <description>Node for publishing some simple marker data for testing purposes</description>
+  <maintainer email="fmrico@gmail.com">Francisco Martín</maintainer>
+  <license>BSD</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <depend>rclcpp</depend>
+  <depend>mocap_interfaces</depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

mocap4r2_marker_viz/mocap4r2_marker_viz/mocap4r2_marker_publisher/src/mocap4r2_marker_publisher.cpp

@@ -0,0 +1,66 @@
+// Copyright 2019 Intelligent Robotics Lab
+//
+// 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.
+//
+// Author: David Vargas Frutos <david.vargas@urjc.es>
+
+#include <chrono>
+#include <memory>
+#include <iostream>
+
+#include "rclcpp/rclcpp.hpp"
+#include "mocap_interfaces/msg/marker.hpp"
+#include "mocap_interfaces/msg/marker_array.hpp"
+
+using namespace std::chrono_literals;
+
+class MarkerPublisher : public rclcpp::Node
+{
+public:
+  MarkerPublisher()
+  : Node("mocap4r2_marker_publisher")
+  {
+    publisher_ = this->create_publisher<mocap_interfaces::msg::MarkerArray>("markers", 10);
+    timer_ = this->create_wall_timer(1000ms, std::bind(&MarkerPublisher::timer_callback, this));
+  }
+
+  void timer_callback()
+  {
+    mocap_interfaces::msg::MarkerArray markers;
+    for (int i = 0; i < 10; i++) {
+      mocap_interfaces::msg::Marker marker;
+      marker.header.stamp = this->now();
+      marker.id_type = mocap_interfaces::msg::Marker::USE_INDEX;
+      marker.marker_index = i;
+      marker.translation.x = 0;
+      marker.translation.y = 0;
+      marker.translation.z = 0.1 * i;
+      markers.markers.push_back(marker);
+    }
+    publisher_->publish(markers);
+  }
+
+private:
+  rclcpp::Publisher<mocap_interfaces::msg::MarkerArray>::SharedPtr publisher_;
+  rclcpp::TimerBase::SharedPtr timer_;
+};
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<MarkerPublisher>();
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+
+  return 0;
+}

mocap4r2_marker_viz/mocap4r2_marker_viz/package.xml

@@ -1,4 +1,4 @@
-<?xml version="2.0"?>
+<?xml version="1.0"?>
 <package format="3">
   <name>mocap4r2_marker_viz</name>
   <version>0.0.7</version>
@@ -9,11 +9,10 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <depend>mocap4r2_msgs</depend>
+  <depend>mocap_interfaces</depend>
   <depend>visualization_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>mocap4r2_marker_viz_srvs</depend>
-  <depend>mocap_interfaces</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

mocap4r2_robot_gt/mocap4r2_robot_gt/CMakeLists.txt

@@ -9,7 +9,7 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
-find_package(mocap4r2_msgs REQUIRED)
+find_package(mocap_interfaces REQUIRED)
 find_package(mocap4r2_robot_gt_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
@@ -18,7 +18,7 @@ find_package(tf2_geometry_msgs REQUIRED)
 set(dependencies
     rclcpp
     rclcpp_components
-    mocap4r2_msgs
+    mocap_interfaces
     mocap4r2_robot_gt_msgs
     tf2_ros
     geometry_msgs