Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
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
66 changes: 66 additions & 0 deletions .github/workflows/industrial_ci.yml
Original file line number Diff line number Diff line change
@@ -0,0 +1,66 @@
name: CI

# This determines when this workflow is run
# on: [push, pull_request] # on all pushes and PRs

# or more fine-grained
on:
# When master or melodic-devel branch is pushed to
push:
branches:
- master
- feature/**
- fix/**
paths-ignore:
- doc/**
- captures/**
- lib/**
- '**.md'
- LICENSE
# When there is a pull request against master
pull_request:
branches:
- master
- feature/**
- fix/**
paths-ignore:
- doc/**
- captures/**
- lib/**
- '**.md'
- LICENSE

jobs:
industrial_ci:
strategy:
matrix:
env:
- ROS_REPO: ros
BUILDER: colcon
ANALYZER: sonarqube
TEST_COVERAGE: true
UPSTREAM_WORKSPACE: 'github:kroshu/kroshu_ros2_core#master'
CMAKE_ARGS: '-DMOCK_FRI=ON'
ROS_DISTRO: foxy
env:
CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci)
EVENT_NAME: ${{ github.event_name }}
BRANCH: ${{ github.event.ref }}
PR_BRANCH: ${{ github.event.pull_request.head.ref }}
PR_BASE: ${{ github.event.pull_request.base.ref }}
PR_NUMBER: ${{ github.event.number }}
ANALYZER_TOKEN: ${{ secrets.ANALYZER_TOKEN }}
DEBUG_BASH: true
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v2
with:
fetch-depth: 0
# This step will fetch/store the directory used by ccache before/after the ci run
- uses: actions/cache@v2
with:
path: ${{ env.CCACHE_DIR }}
key: ccache-${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }}
# Run industrial_ci
- uses: 'kroshu/industrial_ci@master'
env: ${{ matrix.env }}
45 changes: 0 additions & 45 deletions .travis.yml

This file was deleted.

18 changes: 15 additions & 3 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,15 @@ find_package(sensor_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(kroshu_ros2_core REQUIRED)
find_package(cortex_interfaces REQUIRED)

find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/FrameOfData.msg"
"srv/CortexRequestInt.srv"
"srv/CortexRequestFloat.srv"
"srv/CortexRequestEmpty.srv"
"srv/CortexRequestEmptyWithArg.srv"
"srv/CortexRequestFod.srv")

add_library(cortex_client SHARED
src/CortexMock.cpp
Expand All @@ -34,17 +42,21 @@ add_executable(simple_fod_printer
src/SimpleFodPrinter.cpp
src/CortexClientNode.cpp)
ament_target_dependencies(simple_fod_printer rclcpp rclcpp_lifecycle
lifecycle_msgs kroshu_ros2_core cortex_interfaces)
lifecycle_msgs kroshu_ros2_core)
target_link_libraries(simple_fod_printer
cortex_client ${kroshu_ros2_core_LIBRARIES})
rosidl_target_interfaces(simple_fod_printer
${PROJECT_NAME} "rosidl_typesupport_cpp")

add_executable(marker_publisher
src/MarkerPublisher.cpp
src/CortexClientNode.cpp)
ament_target_dependencies(marker_publisher rclcpp rclcpp_lifecycle visualization_msgs
lifecycle_msgs kroshu_ros2_core cortex_interfaces)
lifecycle_msgs kroshu_ros2_core)
target_link_libraries(marker_publisher
cortex_client ${kroshu_ros2_core_LIBRARIES})
rosidl_target_interfaces(marker_publisher
${PROJECT_NAME} "rosidl_typesupport_cpp")

add_executable(motion_tracker
src/MotionTracker.cpp)
Expand Down
4 changes: 4 additions & 0 deletions README.md
Original file line number Diff line number Diff line change
@@ -1,6 +1,10 @@
# ROS2 interface for Cortex MoCap system
Repository of ROS2 interface for Cortex motion capture system.

Github CI | SonarCloud
------------| ---------------
[![Build Status](https://github.com/kroshu/ros2_cortex/workflows/CI/badge.svg?branch=master)](https://github.com/kroshu/ros2_cortex/actions) | [![Quality Gate Status](https://sonarcloud.io/api/project_badges/measure?project=kroshu_ros2_cortex&metric=alert_status)](https://sonarcloud.io/dashboard?id=kroshu_ros2_cortex)

The repository contains a mock version of the Cortex SDK. The CortexClient class provides a C++ wrapper, which can function with both the SDK and the mock, depending on what the user compiles. The CortexClientNode class creates a ROS2 node which uses the Cortex system through the wrapper. Deriving from this class and overriding the dataHandlerFunc_ funtion, the user is able to create its own ROS2 node with the custom frame of data handler function. The base class provides parameters and services which cover the requests provided by the SDK.

One example is SimpleFodPrinter, which simply prints the frame id and the number of unidentified markers of the actual frame of data. Another example is MarkerPublisher, which publishes visualization_msgs/MarkerArray ROS2 messages, converting every frame of data to marker data. The MotionTracker node subscribes to the topic published py MarkerPublisher and controls a robot accordingly. Both the CortexClientNode classes and the MotionTracker class is derived from the ROS2BaseNode class implemented in https://github.com/kroshu/kroshu_ros2_core.
Expand Down
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes
51 changes: 26 additions & 25 deletions include/ros2_cortex/CortexClientNode.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,17 +26,17 @@

#include "Cortex.h"
#include "ros2_cortex/CortexClient.hpp"
#include "kroshu_ros2_core/ROS2BaseNode.hpp"
#include "cortex_interfaces/srv/cortex_request_int.hpp"
#include "cortex_interfaces/srv/cortex_request_float.hpp"
#include "cortex_interfaces/srv/cortex_request_empty.hpp"
#include "cortex_interfaces/srv/cortex_request_empty_with_arg.hpp"
#include "cortex_interfaces/srv/cortex_request_fod.hpp"
#include "kroshu_ros2_core/ROS2BaseLCNode.hpp"
#include "ros2_cortex/srv/cortex_request_int.hpp"
#include "ros2_cortex/srv/cortex_request_float.hpp"
#include "ros2_cortex/srv/cortex_request_empty.hpp"
#include "ros2_cortex/srv/cortex_request_empty_with_arg.hpp"
#include "ros2_cortex/srv/cortex_request_fod.hpp"

namespace ros2_cortex
{

class CortexClientNode : public kroshu_ros2_core::ROS2BaseNode
class CortexClientNode : public kroshu_ros2_core::ROS2BaseLCNode
{
public:
explicit CortexClientNode(const std::string & node_name);
Expand All @@ -47,46 +47,47 @@ class CortexClientNode : public kroshu_ros2_core::ROS2BaseNode
const std::string & log_message);

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
on_configure(const rclcpp_lifecycle::State & state) override;
on_configure(const rclcpp_lifecycle::State &) override;

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
on_activate(const rclcpp_lifecycle::State & state) override;
on_activate(const rclcpp_lifecycle::State &) override;

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
on_deactivate(const rclcpp_lifecycle::State & state) override;
on_deactivate(const rclcpp_lifecycle::State &) override;

private:
void setServices();
void setIntServices();
void setFloatServices();
void setEmptyServices();
void setEmptyWithArgServices();
bool onOutputFilenameChangeRequest(const std::string & new_value) const;
bool onPlayModeChangeRequest(const std::string & new_value) const;
bool onOutputFilenameChangeRequest(const std::string & new_value);
bool onPlayModeChangeRequest(const std::string & new_value);

rclcpp::Service<cortex_interfaces::srv::CortexRequestInt>::SharedPtr post_get_play_mode_service;
rclcpp::Service<cortex_interfaces::srv::CortexRequestInt>::SharedPtr
rclcpp::Service<ros2_cortex::srv::CortexRequestInt>::SharedPtr post_get_play_mode_service;
rclcpp::Service<ros2_cortex::srv::CortexRequestInt>::SharedPtr
get_context_analog_bit_depth_service;
rclcpp::Service<cortex_interfaces::srv::CortexRequestInt>::SharedPtr get_up_axis_service;
rclcpp::Service<ros2_cortex::srv::CortexRequestInt>::SharedPtr get_up_axis_service;

rclcpp::Service<cortex_interfaces::srv::CortexRequestFloat>::SharedPtr
rclcpp::Service<ros2_cortex::srv::CortexRequestFloat>::SharedPtr
get_context_frame_rate_service;
rclcpp::Service<cortex_interfaces::srv::CortexRequestFloat>::SharedPtr
rclcpp::Service<ros2_cortex::srv::CortexRequestFloat>::SharedPtr
get_context_analog_sample_rate_service;
rclcpp::Service<cortex_interfaces::srv::CortexRequestFloat>::SharedPtr
rclcpp::Service<ros2_cortex::srv::CortexRequestFloat>::SharedPtr
get_conversion_to_millimeters_service;

rclcpp::Service<cortex_interfaces::srv::CortexRequestEmpty>::SharedPtr start_recording_service;
rclcpp::Service<cortex_interfaces::srv::CortexRequestEmpty>::SharedPtr stop_recording_service;
rclcpp::Service<ros2_cortex::srv::CortexRequestEmpty>::SharedPtr start_recording_service;
rclcpp::Service<ros2_cortex::srv::CortexRequestEmpty>::SharedPtr stop_recording_service;

rclcpp::Service<cortex_interfaces::srv::CortexRequestEmptyWithArg>::SharedPtr reset_ids_service;
rclcpp::Service<ros2_cortex::srv::CortexRequestEmptyWithArg>::SharedPtr reset_ids_service;

rclcpp::Service<cortex_interfaces::srv::CortexRequestFod>::SharedPtr get_frame_of_data_service;
rclcpp::Service<ros2_cortex::srv::CortexRequestFod>::SharedPtr get_frame_of_data_service;

sFrameOfData current_fod_;
std::shared_ptr<Parameter<std::string>> play_mode_;
std::shared_ptr<Parameter<std::string>> output_filename_;
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback;
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_;

std::string play_mode_;
std::string output_filename_;

protected:
std::shared_ptr<CortexClient> cortex_client_ = CortexClient::getInstance();
Expand Down
6 changes: 3 additions & 3 deletions include/ros2_cortex/MotionTracker.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@
namespace ros2_cortex
{

class MotionTracker : public kroshu_ros2_core::ROS2BaseNode
class MotionTracker : public kroshu_ros2_core::ROS2BaseLCNode
{
public:
MotionTracker();
Expand Down Expand Up @@ -77,8 +77,8 @@ class MotionTracker : public kroshu_ros2_core::ROS2BaseNode
const geometry_msgs::msg::Point & second);
bool onLowerLimitsChangeRequest(const std::vector<double> & new_value);
bool onUpperLimitsChangeRequest(const std::vector<double> & new_value);
std::shared_ptr<Parameter<std::vector<double>>> lower_limits_param_;
std::shared_ptr<Parameter<std::vector<double>>> upper_limits_param_;
std::vector<double> lower_limits_param_;
std::vector<double> upper_limits_param_;
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback;
};

Expand Down
2 changes: 2 additions & 0 deletions msg/FrameOfData.msg
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
int64 frame_id
int64 num_of_ui_markers
6 changes: 4 additions & 2 deletions package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@
<build_depend>std_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>kroshu_ros2_core</build_depend>
<build_depend>cortex_interfaces</build_depend>
<build_depend>rosidl_default_generators</build_depend>

<exec_depend>rclcpp</exec_depend>
<exec_depend>rclcpp_lifecycle</exec_depend>
Expand All @@ -28,7 +28,7 @@
<exec_depend>std_msgs</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>kroshu_ros2_core</exec_depend>
<exec_depend>cortex_interfaces</exec_depend>
<exec_depend>rosidl_default_runtime</exec_depend>

<test_depend>ament_cmake_copyright</test_depend>
<test_depend>ament_cmake_cppcheck</test_depend>
Expand All @@ -38,6 +38,8 @@
<test_depend>ament_cmake_lint_cmake</test_depend>
<test_depend>ament_cmake_xmllint</test_depend>

<member_of_group>rosidl_interface_packages</member_of_group>

<export>
<build_type>ament_cmake</build_type>
</export>
Expand Down
Loading