diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml new file mode 100644 index 0000000..d5bb7a5 --- /dev/null +++ b/.github/workflows/industrial_ci.yml @@ -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 }} \ No newline at end of file diff --git a/.travis.yml b/.travis.yml deleted file mode 100644 index 7b832a0..0000000 --- a/.travis.yml +++ /dev/null @@ -1,45 +0,0 @@ -# This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git). -# For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst) - -language: cpp # optional, just removes the language badge -compiler: - - gcc -os: - - linux -branches: - only: - - master - - /^feature\/.*$/ - -# include the following block if the C/C++ build artifacts should get cached by Travis, -# CCACHE_DIR needs to get set as well to actually fill the cache -#cache: -# directories: -# - $HOME/.ccache - -git: - quiet: true # optional, silences the cloning of the target repository - -# configure the build environment(s) -# https://github.com/ros-industrial/industrial_ci/blob/master/doc/index.rst#variables-you-can-configure -env: - global: # global settings for all jobs - - ROS_REPO=main - - CCACHE_DIR=$HOME/.ccache # enables C/C++ caching in industrial_ci - - UPSTREAM_WORKSPACE='github:kroshu/kroshu_ros2_core#master github:kroshu/ros2_cortex_interfaces#master' - - BUILDER='colcon' - - SONARQUBE='true' - - TEST_COVERAGE='true' - matrix: # each line is a job - - ROS_DISTRO='foxy' - -notifications: - email: false - -# clone and run industrial_ci -install: - - git clone --quiet --depth 1 https://github.com/kroshu/industrial_ci.git .industrial_ci - - curl "https://raw.githubusercontent.com/davetcoleman/roscpp_code_format/master/.clang-format" -o .clang-format - -script: -- .industrial_ci/travis.sh diff --git a/CMakeLists.txt b/CMakeLists.txt index 1b9f56c..d9dc840 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 @@ -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) diff --git a/README.md b/README.md index 1041fcc..df04ced 100644 --- a/README.md +++ b/README.md @@ -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. diff --git a/CaptureWithPlots1.json b/captures/CaptureWithPlots1.json similarity index 100% rename from CaptureWithPlots1.json rename to captures/CaptureWithPlots1.json diff --git a/CaptureWithPlots2.json b/captures/CaptureWithPlots2.json similarity index 100% rename from CaptureWithPlots2.json rename to captures/CaptureWithPlots2.json diff --git a/CaptureWithPlots3.json b/captures/CaptureWithPlots3.json similarity index 100% rename from CaptureWithPlots3.json rename to captures/CaptureWithPlots3.json diff --git a/CaptureWithPlots4.json b/captures/CaptureWithPlots4.json similarity index 100% rename from CaptureWithPlots4.json rename to captures/CaptureWithPlots4.json diff --git a/comp_model_ros.png b/doc/comp_model_ros.png similarity index 100% rename from comp_model_ros.png rename to doc/comp_model_ros.png diff --git a/include/ros2_cortex/CortexClientNode.hpp b/include/ros2_cortex/CortexClientNode.hpp index bb5e7b5..ba1bd32 100644 --- a/include/ros2_cortex/CortexClientNode.hpp +++ b/include/ros2_cortex/CortexClientNode.hpp @@ -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); @@ -47,13 +47,13 @@ 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(); @@ -61,32 +61,33 @@ class CortexClientNode : public kroshu_ros2_core::ROS2BaseNode 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::SharedPtr post_get_play_mode_service; - rclcpp::Service::SharedPtr + rclcpp::Service::SharedPtr post_get_play_mode_service; + rclcpp::Service::SharedPtr get_context_analog_bit_depth_service; - rclcpp::Service::SharedPtr get_up_axis_service; + rclcpp::Service::SharedPtr get_up_axis_service; - rclcpp::Service::SharedPtr + rclcpp::Service::SharedPtr get_context_frame_rate_service; - rclcpp::Service::SharedPtr + rclcpp::Service::SharedPtr get_context_analog_sample_rate_service; - rclcpp::Service::SharedPtr + rclcpp::Service::SharedPtr get_conversion_to_millimeters_service; - rclcpp::Service::SharedPtr start_recording_service; - rclcpp::Service::SharedPtr stop_recording_service; + rclcpp::Service::SharedPtr start_recording_service; + rclcpp::Service::SharedPtr stop_recording_service; - rclcpp::Service::SharedPtr reset_ids_service; + rclcpp::Service::SharedPtr reset_ids_service; - rclcpp::Service::SharedPtr get_frame_of_data_service; + rclcpp::Service::SharedPtr get_frame_of_data_service; sFrameOfData current_fod_; - std::shared_ptr> play_mode_; - std::shared_ptr> 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 cortex_client_ = CortexClient::getInstance(); diff --git a/include/ros2_cortex/MotionTracker.hpp b/include/ros2_cortex/MotionTracker.hpp index 2da62bc..aa029bb 100644 --- a/include/ros2_cortex/MotionTracker.hpp +++ b/include/ros2_cortex/MotionTracker.hpp @@ -31,7 +31,7 @@ namespace ros2_cortex { -class MotionTracker : public kroshu_ros2_core::ROS2BaseNode +class MotionTracker : public kroshu_ros2_core::ROS2BaseLCNode { public: MotionTracker(); @@ -77,8 +77,8 @@ class MotionTracker : public kroshu_ros2_core::ROS2BaseNode const geometry_msgs::msg::Point & second); bool onLowerLimitsChangeRequest(const std::vector & new_value); bool onUpperLimitsChangeRequest(const std::vector & new_value); - std::shared_ptr>> lower_limits_param_; - std::shared_ptr>> upper_limits_param_; + std::vector lower_limits_param_; + std::vector upper_limits_param_; rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback; }; diff --git a/msg/FrameOfData.msg b/msg/FrameOfData.msg new file mode 100644 index 0000000..b07ebb2 --- /dev/null +++ b/msg/FrameOfData.msg @@ -0,0 +1,2 @@ +int64 frame_id +int64 num_of_ui_markers diff --git a/package.xml b/package.xml index 5ffb772..dfcef00 100644 --- a/package.xml +++ b/package.xml @@ -18,7 +18,7 @@ std_msgs geometry_msgs kroshu_ros2_core - cortex_interfaces + rosidl_default_generators rclcpp rclcpp_lifecycle @@ -28,7 +28,7 @@ std_msgs geometry_msgs kroshu_ros2_core - cortex_interfaces + rosidl_default_runtime ament_cmake_copyright ament_cmake_cppcheck @@ -38,6 +38,8 @@ ament_cmake_lint_cmake ament_cmake_xmllint + rosidl_interface_packages + ament_cmake diff --git a/src/CortexClientNode.cpp b/src/CortexClientNode.cpp index 647c056..8f22c3d 100644 --- a/src/CortexClientNode.cpp +++ b/src/CortexClientNode.cpp @@ -23,32 +23,27 @@ #include #include "rclcpp/rclcpp.hpp" -#include "kroshu_ros2_core/ROS2BaseNode.hpp" +#include "kroshu_ros2_core/ROS2BaseLCNode.hpp" #include "ros2_cortex/CortexClient.hpp" namespace ros2_cortex { CortexClientNode::CortexClientNode(const std::string & node_name) -: kroshu_ros2_core::ROS2BaseNode(node_name), - play_mode_(std::make_shared>( - "play_mode", "live", - ParameterSetAccessRights { - true, true, true, false}, - [this](const std::string & new_value) { - return this->onPlayModeChangeRequest(new_value); - }, *this)), - output_filename_( - std::make_shared>( - "output_filename", "CortexDefaultOutput.cap", - ParameterSetAccessRights { - true, true, false, false}, - [this](const std::string & new_value) { - return this->onOutputFilenameChangeRequest(new_value); - }, *this)) +: kroshu_ros2_core::ROS2BaseLCNode(node_name) { - ROS2BaseNode::registerParameter(play_mode_); - ROS2BaseNode::registerParameter(output_filename_); + registerParameter( + "play_mode", "live", kroshu_ros2_core::ParameterSetAccessRights {true, true, + false, false}, [this](const std::string & new_value) { + return this->onPlayModeChangeRequest(new_value); + }); + registerParameter( + "output_filename", "CortexDefaultOutput.cap", + kroshu_ros2_core::ParameterSetAccessRights { + true, true, false, false}, + [this](const std::string & new_value) { + return this->onOutputFilenameChangeRequest(new_value); + }); using namespace std::placeholders; cortex_client_->setDataHandlerFunc( @@ -61,9 +56,10 @@ CortexClientNode::CortexClientNode(const std::string & node_name) CortexClientNode::errorMsgHandlerFunc_(log_level, log_message); }); - param_callback = this->add_on_set_parameters_callback( - [this](const std::vector & parameters) - {return onParamChange(parameters);}); + param_callback_ = this->add_on_set_parameters_callback( + [this](const std::vector & parameters) { + return getParameterHandler().onParamChange(parameters); + }); } CortexClientNode::~CortexClientNode() @@ -73,40 +69,30 @@ CortexClientNode::~CortexClientNode() } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -CortexClientNode::on_configure(const rclcpp_lifecycle::State & state) +CortexClientNode::on_configure(const rclcpp_lifecycle::State &) { const std::string empty_str = ""; cortex_client_->initialize(empty_str, empty_str); setServices(); - return kroshu_ros2_core::ROS2BaseNode::SUCCESS; + return kroshu_ros2_core::ROS2BaseLCNode::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -CortexClientNode::on_activate(const rclcpp_lifecycle::State & state) +CortexClientNode::on_activate(const rclcpp_lifecycle::State &) { - std::string play_mode_str = "live"; - bool success = play_mode_->getValue(play_mode_str); - if (!success) { - return kroshu_ros2_core::ROS2BaseNode::ERROR; - } - onPlayModeChangeRequest(play_mode_str); - return kroshu_ros2_core::ROS2BaseNode::SUCCESS; + onPlayModeChangeRequest(play_mode_); + return kroshu_ros2_core::ROS2BaseLCNode::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -CortexClientNode::on_deactivate(const rclcpp_lifecycle::State & state) +CortexClientNode::on_deactivate(const rclcpp_lifecycle::State &) { - std::string play_mode_str = "live"; - bool success = play_mode_->getValue(play_mode_str); - if (!success) { - return kroshu_ros2_core::ROS2BaseNode::ERROR; - } - if (play_mode_str == "live") { + if (play_mode_ == "live") { cortex_client_->pause(); } else { cortex_client_->postPause(); } - return kroshu_ros2_core::ROS2BaseNode::SUCCESS; + return kroshu_ros2_core::ROS2BaseLCNode::SUCCESS; } void CortexClientNode::setServices() @@ -117,10 +103,10 @@ void CortexClientNode::setServices() setEmptyWithArgServices(); get_frame_of_data_service = - this->create_service( + this->create_service( "get_frame_of_data", [this]( - const std::shared_ptr request, - std::shared_ptr response) { + const std::shared_ptr request, + std::shared_ptr response) { sFrameOfData fod; this->cortex_client_->getFrameOfData(fod, request->base_positions); response->fod.frame_id = fod.iFrame; @@ -131,26 +117,26 @@ void CortexClientNode::setServices() void CortexClientNode::setIntServices() { post_get_play_mode_service = - this->create_service( + this->create_service( "post_get_play_mode", [this]( - const std::shared_ptr request, - std::shared_ptr response) { + const std::shared_ptr, + std::shared_ptr response) { response->response = this->cortex_client_->postGetPlayMode(); }); get_context_analog_bit_depth_service = - this->create_service( + this->create_service( "get_context_analog_bit_depth", [this]( - const std::shared_ptr request, - std::shared_ptr response) { + const std::shared_ptr, + std::shared_ptr response) { response->response = this->cortex_client_->getContextAnalogBitDepth(); }); get_up_axis_service = - this->create_service( + this->create_service( "get_up_axis", [this]( - const std::shared_ptr request, - std::shared_ptr response) { + const std::shared_ptr, + std::shared_ptr response) { response->response = this->cortex_client_->getUpAxis(); }); } @@ -158,26 +144,26 @@ void CortexClientNode::setIntServices() void CortexClientNode::setFloatServices() { get_context_frame_rate_service = - this->create_service( + this->create_service( "get_context_frame_rate", [this]( - const std::shared_ptr request, - std::shared_ptr response) { + const std::shared_ptr, + std::shared_ptr response) { response->response = this->cortex_client_->getContextFrameRate(); }); get_context_analog_sample_rate_service = - this->create_service( + this->create_service( "get_context_analog_sample_rate", [this]( - const std::shared_ptr request, - std::shared_ptr response) { + const std::shared_ptr, + std::shared_ptr response) { response->response = this->cortex_client_->getContextAnalogSampleRate(); }); get_conversion_to_millimeters_service = - this->create_service( + this->create_service( "get_conversion_to_millimeters", [this]( - const std::shared_ptr request, - std::shared_ptr response) { + const std::shared_ptr, + std::shared_ptr response) { response->response = this->cortex_client_->getConversionToMillimeters(); }); } @@ -185,18 +171,18 @@ void CortexClientNode::setFloatServices() void CortexClientNode::setEmptyServices() { start_recording_service = - this->create_service( + this->create_service( "start_recording", [this]( - const std::shared_ptr request, - std::shared_ptr response) { + const std::shared_ptr, + std::shared_ptr) { this->cortex_client_->startRecording(); }); stop_recording_service = - this->create_service( + this->create_service( "stop_recording", [this]( - const std::shared_ptr request, - std::shared_ptr response) { + const std::shared_ptr, + std::shared_ptr) { this->cortex_client_->stopRecording(); }); } @@ -204,23 +190,24 @@ void CortexClientNode::setEmptyServices() void CortexClientNode::setEmptyWithArgServices() { reset_ids_service = - this->create_service( + this->create_service( "reset_ids", [this]( - const std::shared_ptr request, - std::shared_ptr response) { + const std::shared_ptr request, + std::shared_ptr) { this->cortex_client_->resetIds(request->arg); }); } bool CortexClientNode::onOutputFilenameChangeRequest( - const std::string & new_value) const + const std::string & new_value) { cortex_client_->setOutputName(new_value); + output_filename_ = new_value; return true; } bool CortexClientNode::onPlayModeChangeRequest( - const std::string & new_value) const + const std::string & new_value) { if (new_value == "live") {cortex_client_->liveMode();} else if (new_value == "post_forward") { cortex_client_->postForward(); @@ -228,6 +215,7 @@ bool CortexClientNode::onPlayModeChangeRequest( RCLCPP_ERROR(get_logger(), "Invalid parameter value for play_mode"); return false; } + play_mode_ = new_value; return true; } diff --git a/src/CortexMock.cpp b/src/CortexMock.cpp index e2660a9..adee888 100755 --- a/src/CortexMock.cpp +++ b/src/CortexMock.cpp @@ -579,7 +579,7 @@ void CortexMock::run() } const char capture_file_path[] = - "/home/rosdeveloper/ros2_ws/src/ros2_cortex/CaptureWithPlots1.json"; + "./../captures/CaptureWithPlots1.json"; CortexMock mock(capture_file_path); int Cortex_GetSdkVersion(unsigned char version[4]) diff --git a/src/MotionTracker.cpp b/src/MotionTracker.cpp index 0d3cdcc..79ae423 100644 --- a/src/MotionTracker.cpp +++ b/src/MotionTracker.cpp @@ -21,7 +21,7 @@ #include "rclcpp/message_memory_strategy.hpp" #include "rclcpp/rclcpp.hpp" -#include "kroshu_ros2_core/ROS2BaseNode.hpp" +#include "kroshu_ros2_core/ROS2BaseLCNode.hpp" #include "ros2_cortex/MotionTracker.hpp" namespace ros2_cortex @@ -33,27 +33,22 @@ double d2r(double degrees) } MotionTracker::MotionTracker() -: kroshu_ros2_core::ROS2BaseNode("motion_tracker"), +: kroshu_ros2_core::ROS2BaseLCNode("motion_tracker"), lower_limits_rad_(joint_num_), upper_limits_rad_(joint_num_), - original_joint_points_(joint_num_), - lower_limits_param_(std::make_shared>>( - "lower_limits_deg", - lower_limits_deg_default_, - ParameterSetAccessRights { - true, true, true, false}, - [this](const std::vector & new_value) { - return this->onLowerLimitsChangeRequest(new_value); - }, *this)), - upper_limits_param_( - std::make_shared>>( - "upper_limits_deg", - upper_limits_deg_default_, - ParameterSetAccessRights { - true, true, true, false}, - [this](const std::vector & new_value) { - return this->onUpperLimitsChangeRequest(new_value); - }, *this)) + original_joint_points_(joint_num_) { + registerParameter>( + "lower_limits_deg", lower_limits_deg_default_, kroshu_ros2_core::ParameterSetAccessRights {true, + true, + false, false}, [this](const std::vector & new_value) { + return this->onLowerLimitsChangeRequest(new_value); + }); + registerParameter>( + "upper_limits_deg", upper_limits_deg_default_, kroshu_ros2_core::ParameterSetAccessRights {true, + true, + false, false}, [this](const std::vector & new_value) { + return this->onUpperLimitsChangeRequest(new_value); + }); original_joint_points_[0].x = 0.0; original_joint_points_[0].y = 0.0; original_joint_points_[0].z = segment_lengths_[0]; @@ -91,7 +86,7 @@ MotionTracker::MotionTracker() param_callback = this->add_on_set_parameters_callback( [this](const std::vector & parameters) - {return onParamChange(parameters);}); + {return getParameterHandler().onParamChange(parameters);}); } double MotionTracker::distBetweenPoints( @@ -161,7 +156,7 @@ MotionTracker::on_cleanup(const rclcpp_lifecycle::State & state) { reference_joint_state_->position.assign(joint_num_, 0); active_joint_msg_->data = 1; - return kroshu_ros2_core::ROS2BaseNode::SUCCESS; + return kroshu_ros2_core::ROS2BaseLCNode::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn @@ -169,7 +164,7 @@ MotionTracker::on_activate(const rclcpp_lifecycle::State & state) { reference_joint_state_publisher_->on_activate(); active_axis_changed_publisher_->on_activate(); - return kroshu_ros2_core::ROS2BaseNode::SUCCESS; + return kroshu_ros2_core::ROS2BaseLCNode::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn @@ -178,7 +173,7 @@ MotionTracker::on_deactivate(const rclcpp_lifecycle::State & state) marker_array_subscriber_.reset(); reference_joint_state_publisher_->on_deactivate(); active_axis_changed_publisher_->on_deactivate(); - return kroshu_ros2_core::ROS2BaseNode::SUCCESS; + return kroshu_ros2_core::ROS2BaseLCNode::SUCCESS; } bool MotionTracker::onLowerLimitsChangeRequest( diff --git a/srv/CortexRequestEmpty.srv b/srv/CortexRequestEmpty.srv new file mode 100644 index 0000000..ed97d53 --- /dev/null +++ b/srv/CortexRequestEmpty.srv @@ -0,0 +1 @@ +--- diff --git a/srv/CortexRequestEmptyWithArg.srv b/srv/CortexRequestEmptyWithArg.srv new file mode 100644 index 0000000..e95ea6b --- /dev/null +++ b/srv/CortexRequestEmptyWithArg.srv @@ -0,0 +1,2 @@ +string arg +--- diff --git a/srv/CortexRequestFloat.srv b/srv/CortexRequestFloat.srv new file mode 100644 index 0000000..1ae939d --- /dev/null +++ b/srv/CortexRequestFloat.srv @@ -0,0 +1,2 @@ +--- +float64 response diff --git a/srv/CortexRequestFod.srv b/srv/CortexRequestFod.srv new file mode 100644 index 0000000..7ab5389 --- /dev/null +++ b/srv/CortexRequestFod.srv @@ -0,0 +1,3 @@ +bool base_positions +--- +FrameOfData fod diff --git a/srv/CortexRequestInt.srv b/srv/CortexRequestInt.srv new file mode 100644 index 0000000..e918742 --- /dev/null +++ b/srv/CortexRequestInt.srv @@ -0,0 +1,2 @@ +--- +int64 response