From 209e99a4b3f13db84f75911100e3511aef6dd500 Mon Sep 17 00:00:00 2001
From: Roman Andrianov <rom.andrianov1984@gmail.com>
Date: Tue, 14 Mar 2023 05:42:00 +0000
Subject: [PATCH] Added camera component

---
 .../camera_component/CMakeLists.txt           | 68 +++++++++++++++++++
 env_components/camera_component/package.xml   | 30 ++++++++
 .../src/ign_camera_component.cpp              | 42 ++++++++++++
 env_manager/CMakeLists.txt                    |  6 +-
 env_manager/config/config.lua                 |  2 +-
 .../config/environments/ground_true.lua       |  7 +-
 env_manager/config/environments/simulator.lua |  7 +-
 env_manager/config/nodes.lua                  |  8 ++-
 rbs.repos                                     |  6 +-
 9 files changed, 168 insertions(+), 8 deletions(-)
 create mode 100644 env_components/camera_component/CMakeLists.txt
 create mode 100644 env_components/camera_component/package.xml
 create mode 100644 env_components/camera_component/src/ign_camera_component.cpp

diff --git a/env_components/camera_component/CMakeLists.txt b/env_components/camera_component/CMakeLists.txt
new file mode 100644
index 0000000..13e27e7
--- /dev/null
+++ b/env_components/camera_component/CMakeLists.txt
@@ -0,0 +1,68 @@
+cmake_minimum_required(VERSION 3.5)
+project(camera_component)
+
+if(NOT CMAKE_C_STANDARD)
+    set(CMAKE_C_STANDARD 99)
+endif()
+
+if(NOT CMAKE_CXX_STANDARD)
+    set(CMAKE_CXX_STANDARD 17)
+endif()
+
+set(CMAKE_CXX_STANDARD_REQUIRED True)
+
+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(ament_cmake_ros REQUIRED)
+find_package(image_transport REQUIRED)
+find_package(ros_gz_bridge REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(env_manager REQUIRED)
+
+find_package(ignition-transport11 REQUIRED)
+set(GZ_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR})
+
+find_package(ignition-msgs8 REQUIRED)
+set(GZ_MSGS_VER ${ignition-msgs8_VERSION_MAJOR})
+
+set(GZ_TARGET_PREFIX ignition)
+
+add_library(ign_camera_component SHARED
+    src/ign_camera_component.cpp)
+target_compile_definitions(ign_camera_component
+    PRIVATE "COMPOSITION_BUILDING_DLL")
+target_link_libraries(ign_camera_component
+    ${GZ_TARGET_PREFIX}-msgs${GZ_MSGS_VER}::core
+    ${GZ_TARGET_PREFIX}-transport${GZ_TRANSPORT_VER}::core
+    )
+ament_target_dependencies(ign_camera_component
+    "image_transport"
+    "ros_gz_bridge"
+    "rclcpp"
+    "rclcpp_components"
+    "sensor_msgs"
+    "env_manager")
+rclcpp_components_register_nodes(ign_camera_component "camera_component::IgnCameraComponent")
+set(node_plugins "${node_plugins}camera_component::IgnCameraComponent;$<TARGET_FILE:camera_component>\n")
+
+install(TARGETS ign_camera_component
+    DESTINATION lib)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
diff --git a/env_components/camera_component/package.xml b/env_components/camera_component/package.xml
new file mode 100644
index 0000000..5f37451
--- /dev/null
+++ b/env_components/camera_component/package.xml
@@ -0,0 +1,30 @@
+<?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>camera_component</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="rom.andrianov1984@gmail.com">splinter1984</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>pkg-config</buildtool_depend>
+
+  <depend>image_transport</depend>
+  <depend>ros_gz_bridge</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+  <depend>env_manager</depend>
+
+  <depend condition="$GZ_VERSION == fortress or $IGNITION_VERSION == fortress">ignition-msgs8</depend>
+  <depend condition="$GZ_VERSION == fortress or $IGNITION_VERSION == fortress">ignition-transport11</depend>
+  <depend condition="$GZ_VERSION == '' and $IGNITION_VERSION == ''">ignition-msgs8</depend>
+  <depend condition="$GZ_VERSION == '' and $IGNITION_VERSION == ''">ignition-transport11</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/env_components/camera_component/src/ign_camera_component.cpp b/env_components/camera_component/src/ign_camera_component.cpp
new file mode 100644
index 0000000..bbf7d69
--- /dev/null
+++ b/env_components/camera_component/src/ign_camera_component.cpp
@@ -0,0 +1,42 @@
+#include <component_manager/publisher_component.hpp>
+
+#include <iostream>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <ignition/transport/Node.hh>
+#include <rclcpp/rclcpp.hpp>
+#include <image_transport/image_transport.hpp>
+#include <ros_gz_bridge/convert.hpp>
+
+#include <sensor_msgs/msg/image.hpp>
+
+namespace camera_component
+{
+
+class IgnCameraComponent: public env_manager::component_manager::PublisherComponent<sensor_msgs::msg::Image>
+{
+public:
+    explicit IgnCameraComponent(const rclcpp::NodeOptions &options)
+        : env_manager::component_manager::PublisherComponent<sensor_msgs::msg::Image>(options)
+    {
+        auto topic_name = "/camera";
+        _ign_node = std::make_shared<ignition::transport::Node>();
+        _ign_node->Subscribe(topic_name, &IgnCameraComponent::on_image, this);
+    }
+
+private:
+    void on_image(const ignition::msgs::Image& img)
+    {
+        sensor_msgs::msg::Image rimg;
+        ros_gz_bridge::convert_gz_to_ros(img, rimg);
+        populate_publication(rimg);
+    }
+
+    std::shared_ptr<ignition::transport::Node> _ign_node;
+};
+} // namespace pub_component
+#include "rclcpp_components/register_node_macro.hpp"
+
+RCLCPP_COMPONENTS_REGISTER_NODE(camera_component::IgnCameraComponent)
diff --git a/env_manager/CMakeLists.txt b/env_manager/CMakeLists.txt
index 4a58e66..bfd7962 100644
--- a/env_manager/CMakeLists.txt
+++ b/env_manager/CMakeLists.txt
@@ -6,7 +6,7 @@ if(NOT CMAKE_C_STANDARD)
   set(CMAKE_C_STANDARD 99)
 endif()
 
-# Default to C++14
+# Default to C++17
 if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 17)
 endif()
@@ -31,7 +31,7 @@ find_package(rcutils REQUIRED)
 find_package(glog 0.4.0 REQUIRED)
 find_package(Lua 5.3 REQUIRED)
 
-add_library(env_manager STATIC 
+add_library(env_manager SHARED 
     src/component_manager/component_manager.cpp
     src/config/config_file_resolver.cpp
     src/config/lua_file_resolver.cpp
@@ -79,7 +79,7 @@ ament_target_dependencies(
     rclcpp
     glog
     Lua
-    )
+)
 
 install(DIRECTORY config DESTINATION config)
 
diff --git a/env_manager/config/config.lua b/env_manager/config/config.lua
index be8ce07..a778f16 100644
--- a/env_manager/config/config.lua
+++ b/env_manager/config/config.lua
@@ -9,7 +9,7 @@ include "environments/ground_true.lua"
 -- env_manager configuration
 ENV_MANAGER = {
     environments = { 
-        SIMULATOR, GROUND_TRUE 
+        GROUND_TRUE 
     },
     nodes = NODES 
 }
diff --git a/env_manager/config/environments/ground_true.lua b/env_manager/config/environments/ground_true.lua
index 29e4e22..38c7f06 100644
--- a/env_manager/config/environments/ground_true.lua
+++ b/env_manager/config/environments/ground_true.lua
@@ -2,6 +2,7 @@
 GROUND_TRUE = {
     namespace = "ground_true",
     components = {
+        --[[
         talker_node = {
             lib = "libpub_component.so",
             class = "pub_component::Publisher",
@@ -9,6 +10,10 @@ GROUND_TRUE = {
         service_node = {
             lib = "libsrv_component.so",
             class = "srv_component::Service"
-        }
+        },]]--
+        camera_node = {
+            lib = "libign_camera_component.so",
+            class = "camera_component::IgnCameraComponent"
+        },
     }
 }
diff --git a/env_manager/config/environments/simulator.lua b/env_manager/config/environments/simulator.lua
index 3bc4d1a..3e36699 100644
--- a/env_manager/config/environments/simulator.lua
+++ b/env_manager/config/environments/simulator.lua
@@ -2,6 +2,7 @@
 SIMULATOR = {
     namespace = "simulator_env",
     components = {
+        --[[
         talker_node = {
             lib = "libpub_component.so",
             class = "pub_component::Publisher",
@@ -9,6 +10,10 @@ SIMULATOR = {
         service_node = {
             lib = "libsrv_component.so",
             class = "srv_component::Service"
-        }
+        },]]--
+        camera_node = {
+            lib = "libign_camera_component.so",
+            class = "camera_component::IgnCameraComponent"
+        },
     }
 }
diff --git a/env_manager/config/nodes.lua b/env_manager/config/nodes.lua
index f3ad7de..128dc53 100644
--- a/env_manager/config/nodes.lua
+++ b/env_manager/config/nodes.lua
@@ -1,4 +1,5 @@
 NODES = {
+    --[[
     talker_node = {
         name = "talker",
         type = "Publisher",
@@ -8,5 +9,10 @@ NODES = {
         name = "add_two_ints",
         type = "Service",
         msg_type = "example_interfaces/AddTwoInts"
-    }
+    },]]--
+    camera_node = {
+        name = "camera_node",
+        type = "Publisher",
+        msg_type = "sensor_msgs/Image"
+    },
 }
diff --git a/rbs.repos b/rbs.repos
index 4c83886..3f43d85 100644
--- a/rbs.repos
+++ b/rbs.repos
@@ -14,4 +14,8 @@ repositories:
   behavior_tree:
     type: git
     url: https://github.com/solid-sinusoid/behavior_tree.git
-    version: master
\ No newline at end of file
+    version: master
+  ros_gz:
+    type: git
+    url: https://github.com/gazebosim/ros_gz.git
+    version: humble