Merge pull request #3 from Juancams/main

Coresense Instrumentation Implemented

.github/workflows/main.yaml

@@ -0,0 +1,36 @@
+name: main
+
+on:
+  push:
+    branches:
+      - '*'
+
+jobs:
+  ROS2:
+    runs-on: ubuntu-22.04
+    steps:
+      - name: Checkout
+        uses: actions/checkout@v2
+
+      - name: Setup ROS 2 environment
+        uses: ros-tooling/[email protected]
+        with:
+          ros-distro: humble
+
+      - name: Build ROS 2 package
+        uses: ros-tooling/[email protected]
+        with:
+          package-name: coresense_instrumentation_driver coresense_instrumentation_interfaces
+          target-ros2-distro: humble
+          colcon-defaults: |
+            {
+              "build": {
+                "mixin": ["coverage-gcc"]
+              }
+            }
+          colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/1ddb69bedfd1f04c2f000e95452f7c24a4d6176b/index.yaml
+
+      - name: Upload coverage reports to Codecov
+        uses: codecov/codecov-action@v3
+        env:
+          CODECOV_TOKEN: ${{ secrets.CODECOV_TOKEN }}

README.md

@@ -1 +1,90 @@
-# coresense_instrumentation
+# Coresense Instrumentation
+
+![distro](https://img.shields.io/badge/Ubuntu%2022-Jammy%20Jellyfish-green)
+![distro](https://img.shields.io/badge/ROS2-Humble-blue)
+[![main](https://github.com/Juancams/coresense_instrumentation/actions/workflows/main.yaml/badge.svg?branch=main)](https://github.com/Juancams/coresense_instrumentation/actions/workflows/main.yaml)
+[![codecov](https://codecov.io/gh/Juancams/coresense_instrumentation/graph/badge.svg?token=EvUIoImzzh)](https://codecov.io/gh/Juancams/coresense_instrumentation)
+
+## Installation
+
+```bash
+cd ~/ros2_ws/src
+git clone https://github.com/CoreSenseEU/coresense_instrumentation
+cd ~/ros2_ws
+colcon build --symlink-install --packages-up-to coresense_instrumention
+```
+
+## Usage
+### Coresense Instrumentation Driver
+<details>
+<summary>Click to expand</summary>
+
+```python
+def generate_launch_description():
+
+    names = ['scan_raw',
+             'nav_vel',
+             'image_raw']
+
+    topics = ['/scan_raw',
+              '/nav_vel',
+              '/head_front_camera/rgb/image_raw']
+
+    msgs = ['sensor_msgs::msg::LaserScan',
+            'geometry_msgs::msg::Twist',
+            'sensor_msgs::msg::Image']
+
+    node_types = ['Producer',
+                  'Consumer',
+                  'Producer']
+
+    ns = ''
+
+    composable_nodes = []
+    for topic, msg, name, node_type in zip(topics, msgs, names, node_types):
+        composable_node = ComposableNode(
+            package='coresense_instrumentation_driver',
+            plugin='coresense_instrumentation_driver::Instrumentation'
+                    + node_type + '<' + msg + '>',
+            name=name + '_node',
+            namespace=ns,
+            parameters=[{'topic': topic,
+                         'topic_type': msg,
+                         'type': node_type}],
+        )
+        composable_nodes.append(composable_node)
+
+    container = ComposableNodeContainer(
+        name='coresense_container',
+        namespace=ns,
+        package='rclcpp_components',
+        executable='component_container',
+        composable_node_descriptions=composable_nodes,
+        output='screen',
+    )
+```
+You have to indicate the name you will give to your nodes, the topic you subscribe to, the type of message and whether you are going to subscribe or publish to the topic.
+
+</details>
+
+```shell
+ros2 launch coresense_instrumentation_driver coresense_instrumentation_driver.launch.py
+```
+
+### Coresense Instrumentation Rviz
+```shell
+ros2 launch coresense_instrumentation_rviz coresense_instrumentation_rviz.launch.py
+```
+
+<details>
+<summary>Click to expand</summary>
+
+
+![image](https://github.com/Juancams/coresense_instrumentation/assets/44479765/fc2403ed-f39d-4b4d-ac86-57477d56342e)
+![image](https://github.com/Juancams/coresense_instrumentation/assets/44479765/186e220d-ec4b-4071-9ecd-bc408d6e9725)
+
+When you launch rviz, you will see on your right a panel like this, but with your nodes. Through the following buttons, you can activate/deactivate your nodes and create/delete your publishers/subscribers
+</details>
+
+## Demo
+[instrumentation_driver_demo](https://github.com/Juancams/coresense_instrumentation/assets/44479765/e6cada5c-5071-4a41-b226-5dc5c18a37aa)

coresense_instrumentation/CHANGELOG.rst

@@ -0,0 +1,11 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package coresense_instrumentation
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.2.0 (2023-11-30)
+------------------
+* Added new type: sensor_msgs/msg/Image with specialiced template
+
+0.1.0 (2023-11-29)
+------------------
+* Driver works correctly

coresense_instrumentation/CMakeLists.txt

@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 3.5)
+project(coresense_instrumentation)
+find_package(ament_cmake REQUIRED)
+ament_package()

coresense_instrumentation/package.xml

@@ -0,0 +1,19 @@
+<?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>coresense_instrumentation</name>
+  <version>1.0.0</version>
+  <description>Coresense Instrumentation</description>
+  <maintainer email="[email protected]">Juan Carlos Manzanares Serrano</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>coresense_instrumentation_interfaces</depend>
+  <depend>coresense_instrumentation_driver</depend>
+  <depend>coresense_instrumentation_rviz</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

coresense_instrumentation_driver/CHANGELOG.rst

@@ -0,0 +1,11 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package coresense_instrumentation_drived
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.2.0 (2023-11-30)
+------------------
+* Added new type: sensor_msgs/msg/Image with specialiced template
+
+0.1.0 (2023-11-29)
+------------------
+* Driver works correctly

coresense_instrumentation_driver/CMakeLists.txt

@@ -0,0 +1,78 @@
+cmake_minimum_required(VERSION 3.5)
+project(coresense_instrumentation_driver)
+
+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)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(image_transport REQUIRED)
+find_package(coresense_instrumentation_interfaces REQUIRED)
+
+set(dependencies
+  rclcpp
+  rclcpp_components
+  rclcpp_lifecycle
+  std_msgs
+  sensor_msgs
+  geometry_msgs
+  image_transport
+  coresense_instrumentation_interfaces
+)
+
+include_directories(include)
+
+add_library(${PROJECT_NAME} SHARED
+  src/coresense_instrumentation_driver/InstrumentationProducerGeneric.cpp
+  src/coresense_instrumentation_driver/InstrumentationProducerImage.cpp
+  src/coresense_instrumentation_driver/InstrumentationConsumerGeneric.cpp
+)
+
+ament_target_dependencies(${PROJECT_NAME} ${dependencies})
+
+rclcpp_components_register_nodes(${PROJECT_NAME}
+  "coresense_instrumentation_driver::InstrumentationProducer<std_msgs::msg::String>"
+  "coresense_instrumentation_driver::InstrumentationProducer<sensor_msgs::msg::LaserScan>"
+  "coresense_instrumentation_driver::InstrumentationProducer<sensor_msgs::msg::Image>"
+  "coresense_instrumentation_driver::InstrumentationProducer<geometry_msgs::msg::Twist>"
+  "coresense_instrumentation_driver::InstrumentationConsumer<std_msgs::msg::String>"
+  "coresense_instrumentation_driver::InstrumentationConsumer<sensor_msgs::msg::LaserScan>"
+  "coresense_instrumentation_driver::InstrumentationConsumer<sensor_msgs::msg::Image>"
+  "coresense_instrumentation_driver::InstrumentationConsumer<geometry_msgs::msg::Twist>"
+)
+
+install(TARGETS
+  ${PROJECT_NAME}
+  DESTINATION lib
+)
+
+install(DIRECTORY include/
+  DESTINATION include
+)
+
+install(TARGETS
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME})
+
+ament_export_include_directories(include)
+ament_export_dependencies(${dependencies})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  set(ament_cmake_copyright_FOUND TRUE)
+  set(ament_cmake_cpplint_FOUND TRUE)
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+  add_subdirectory(test)
+endif()
+
+ament_package()

coresense_instrumentation_driver/config/params.yaml

@@ -0,0 +1,4 @@
+coresense_instrumentation_driver_node:
+  ros__parameters:
+    topics: ["scan", "image"]
+    topic_types: [sensor_msgs/msg/LaserScan, sensor_msgs/msg/Image]

coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationConsumer.hpp

@@ -0,0 +1,90 @@
+// Copyright 2023 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.
+
+#ifndef INSTRUMENTATION_CONSUMER_HPP
+#define INSTRUMENTATION_CONSUMER_HPP
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "lifecycle_msgs/msg/state.hpp"
+#include "std_msgs/msg/string.hpp"
+#include "sensor_msgs/msg/laser_scan.hpp"
+#include "sensor_msgs/msg/image.hpp"
+#include "image_transport/image_transport.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "coresense_instrumentation_interfaces/srv/create_publisher.hpp"
+#include "coresense_instrumentation_interfaces/srv/delete_publisher.hpp"
+#include "coresense_instrumentation_interfaces/srv/create_subscriber.hpp"
+#include "coresense_instrumentation_interfaces/srv/delete_subscriber.hpp"
+#include "coresense_instrumentation_interfaces/msg/node_info.hpp"
+#include <cxxabi.h>
+
+namespace coresense_instrumentation_driver
+{
+
+using std::placeholders::_1;
+
+template<typename TopicT>
+class InstrumentationConsumer : public rclcpp_lifecycle::LifecycleNode
+{
+public:
+  InstrumentationConsumer(
+    const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+
+  virtual ~InstrumentationConsumer();
+
+  std::string get_topic();
+  std::string get_topic_type();
+
+  using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+  CallbackReturnT on_configure(const rclcpp_lifecycle::State &) override;
+  CallbackReturnT on_activate(const rclcpp_lifecycle::State &) override;
+  CallbackReturnT on_deactivate(const rclcpp_lifecycle::State &) override;
+  CallbackReturnT on_cleanup(const rclcpp_lifecycle::State &) override;
+  CallbackReturnT on_shutdown(const rclcpp_lifecycle::State &) override;
+
+private:
+  typename rclcpp_lifecycle::LifecyclePublisher<TopicT>::SharedPtr pub_;
+  rclcpp::Publisher<coresense_instrumentation_interfaces::msg::NodeInfo>::SharedPtr status_pub_;
+  rclcpp::TimerBase::SharedPtr status_timer_;
+
+  void handleCreateSubscriberRequest(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<coresense_instrumentation_interfaces::srv::CreateSubscriber::Request> request,
+    const std::shared_ptr<coresense_instrumentation_interfaces::srv::CreateSubscriber::Response> response);
+
+  void handleDeleteSubscriberRequest(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<coresense_instrumentation_interfaces::srv::DeleteSubscriber::Request> request,
+    const std::shared_ptr<coresense_instrumentation_interfaces::srv::DeleteSubscriber::Response> response);
+
+  void publish_status();
+
+  rclcpp::Service<coresense_instrumentation_interfaces::srv::CreateSubscriber>::SharedPtr
+    create_subscriber_service_;
+  rclcpp::Service<coresense_instrumentation_interfaces::srv::DeleteSubscriber>::SharedPtr
+    delete_subscriber_service_;
+
+  std::unordered_map<std::string,
+    typename rclcpp::Subscription<TopicT>::SharedPtr> subscriptions_;
+
+  std::string topic_;
+  std::string topic_type_;
+  std::string type_;
+};
+
+} // namespace coresense_instrumentation_driver
+
+#endif // INSTRUMENTATION_CONSUMER_HPP