From cfda68d74e2beec7f0ae309476af130a01f97db9 Mon Sep 17 00:00:00 2001 From: Hyunseok Date: Wed, 14 Feb 2024 16:34:25 +0900 Subject: [PATCH 01/12] Update README.md --- cloisim_ros_bringup/README.md | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/cloisim_ros_bringup/README.md b/cloisim_ros_bringup/README.md index 8967153f..7ce6619a 100644 --- a/cloisim_ros_bringup/README.md +++ b/cloisim_ros_bringup/README.md @@ -59,3 +59,11 @@ or ```shell ros2 launch cloisim_ros_bringup bringup.launch.py single_mode:=True ``` + +or + +specific target model without namespace + +```shell +ros2 launch cloisim_ros_bringup bringup.launch.py single_mode:=True target_model:=cloi0 +``` From 1665804c2c1a974c891923e2c8dd53a7090b9d35 Mon Sep 17 00:00:00 2001 From: "jamie.lee" Date: Fri, 23 Feb 2024 15:43:52 +0900 Subject: [PATCH 02/12] Rename imu, gps, odom topics by user in bringup --- cloisim_ros_bringup/launch/bringup.launch.py | 27 ++++++++++++++++++-- 1 file changed, 25 insertions(+), 2 deletions(-) diff --git a/cloisim_ros_bringup/launch/bringup.launch.py b/cloisim_ros_bringup/launch/bringup.launch.py index 4e2a76e8..977d1336 100644 --- a/cloisim_ros_bringup/launch/bringup.launch.py +++ b/cloisim_ros_bringup/launch/bringup.launch.py @@ -23,14 +23,19 @@ def generate_launch_description(): _target_parts_name = LaunchConfiguration('target_parts_name') _scan = LaunchConfiguration('scan') _cmd_vel = LaunchConfiguration('cmd_vel') - + _odom = LaunchConfiguration('odom') + _imu = LaunchConfiguration('imu') + _navsatfix = LaunchConfiguration('navsatfix') cloisim_ros_cmd = Node( package="cloisim_ros_bringup", executable="bringup", output='screen', remappings=[('scan', _scan), - ('cmd_vel', _cmd_vel)], + ('cmd_vel', _cmd_vel), + ('odom', _odom), + ('imu', _imu), + ('navsatfix', _navsatfix)], parameters=[{'single_mode': _single_mode, 'target_model': ParameterValue(_target_model, value_type=str), 'target_parts_type': ParameterValue(_target_parts_type, value_type=str), @@ -66,6 +71,21 @@ def generate_launch_description(): 'cmd_vel', default_value='cmd_vel', description='specify cmd_vel topic you want') + + declare_launch_argument_odom = DeclareLaunchArgument( + 'odom', + default_value='odom', + description='specify odom topic you want') + + declare_launch_argument_imu = DeclareLaunchArgument( + 'imu', + default_value='imu', + description='specify imu/data topic you want') + + declare_launch_argument_navsatfix = DeclareLaunchArgument( + 'navsatfix', + default_value='navsatfix', + description='specify navsatfix topic you want') stdout_log_use_stdout_envvar = SetEnvironmentVariable( 'RCUTILS_LOGGING_USE_STDOUT', '1') @@ -85,6 +105,9 @@ def generate_launch_description(): ld.add_action(declare_launch_argument_tpn) ld.add_action(declare_launch_argument_sc) ld.add_action(declare_launch_argument_cmdvel) + ld.add_action(declare_launch_argument_odom) + ld.add_action(declare_launch_argument_imu) + ld.add_action(declare_launch_argument_navsatfix) ld.add_action(cloisim_ros_cmd) return ld From 88693bd01811637b09a1ce8996dfd08f97479b7e Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Fri, 1 Mar 2024 14:55:34 +0900 Subject: [PATCH 03/12] Add new cloisim_ros package - cloisim_ros_segmentationcamera Modify cloisim_ros_bringup --- cloisim_ros_bringup/CMakeLists.txt | 2 + cloisim_ros_bringup/package.xml | 1 + cloisim_ros_bringup/src/bringup.cpp | 8 ++ .../src/bringup_param.cpp | 5 +- cloisim_ros_segmentationcamera/CMakeLists.txt | 74 +++++++++++++++++++ cloisim_ros_segmentationcamera/README.md | 19 +++++ .../segmentation_camera.hpp | 51 +++++++++++++ cloisim_ros_segmentationcamera/package.xml | 21 ++++++ cloisim_ros_segmentationcamera/src/main.cpp | 53 +++++++++++++ .../src/segmentation_camera.cpp | 59 +++++++++++++++ 10 files changed, 291 insertions(+), 2 deletions(-) create mode 100644 cloisim_ros_segmentationcamera/CMakeLists.txt create mode 100644 cloisim_ros_segmentationcamera/README.md create mode 100644 cloisim_ros_segmentationcamera/include/cloisim_ros_segmentationcamera/segmentation_camera.hpp create mode 100644 cloisim_ros_segmentationcamera/package.xml create mode 100644 cloisim_ros_segmentationcamera/src/main.cpp create mode 100644 cloisim_ros_segmentationcamera/src/segmentation_camera.cpp diff --git a/cloisim_ros_bringup/CMakeLists.txt b/cloisim_ros_bringup/CMakeLists.txt index cd984c88..c21ef047 100644 --- a/cloisim_ros_bringup/CMakeLists.txt +++ b/cloisim_ros_bringup/CMakeLists.txt @@ -23,6 +23,7 @@ find_package(cloisim_ros_lidar REQUIRED) find_package(cloisim_ros_camera REQUIRED) find_package(cloisim_ros_depthcamera REQUIRED) find_package(cloisim_ros_multicamera REQUIRED) +find_package(cloisim_ros_segmentationcamera REQUIRED) find_package(cloisim_ros_realsense REQUIRED) find_package(cloisim_ros_gps REQUIRED) find_package(cloisim_ros_imu REQUIRED) @@ -43,6 +44,7 @@ set(dependencies cloisim_ros_camera cloisim_ros_depthcamera cloisim_ros_multicamera + cloisim_ros_segmentationcamera cloisim_ros_realsense cloisim_ros_gps cloisim_ros_imu diff --git a/cloisim_ros_bringup/package.xml b/cloisim_ros_bringup/package.xml index 7449cf9f..8c3074d3 100644 --- a/cloisim_ros_bringup/package.xml +++ b/cloisim_ros_bringup/package.xml @@ -24,6 +24,7 @@ cloisim_ros_camera cloisim_ros_depthcamera cloisim_ros_multicamera + cloisim_ros_segmentationcamera cloisim_ros_realsense cloisim_ros_gps cloisim_ros_imu diff --git a/cloisim_ros_bringup/src/bringup.cpp b/cloisim_ros_bringup/src/bringup.cpp index 7f9f4af4..f9537ed2 100644 --- a/cloisim_ros_bringup/src/bringup.cpp +++ b/cloisim_ros_bringup/src/bringup.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -96,6 +97,13 @@ static shared_ptr make_device_node( else node = std::make_shared(node_options, node_name, model_name); } + else if (!node_type.compare("SEGMENTCAMERA")) + { + if (enable_single_mode) + node = std::make_shared(node_options, node_name); + else + node = std::make_shared(node_options, node_name, model_name); + } else if (!node_type.compare("GPS")) { if (enable_single_mode) diff --git a/cloisim_ros_bringup_param/src/bringup_param.cpp b/cloisim_ros_bringup_param/src/bringup_param.cpp index 86e2f6f9..4ab1e83e 100644 --- a/cloisim_ros_bringup_param/src/bringup_param.cpp +++ b/cloisim_ros_bringup_param/src/bringup_param.cpp @@ -22,7 +22,8 @@ bool BringUpParam::IsRobotSpecificType(const string node_type) return (!node_type.compare("MICOM") || !node_type.compare("JOINTCONTROL") || !node_type.compare("LIDAR") || !node_type.compare("LASER") || !node_type.compare("CAMERA") || !node_type.compare("DEPTHCAMERA") || - !node_type.compare("MULTICAMERA") || !node_type.compare("REALSENSE") || + !node_type.compare("MULTICAMERA") || !node_type.compare("SEGMENTCAMERA") || + !node_type.compare("REALSENSE") || !node_type.compare("GPS") || !node_type.compare("IMU") || !node_type.compare("SONAR")); } @@ -204,4 +205,4 @@ void BringUpParam::StoreFilteredInfoAsParameters( cloisim_ros::BringUpParam::StoreBridgeInfosAsParameters(bridge_infos, node_options); } -} // namespace cloisim_ros +} // namespace cloisim_ros \ No newline at end of file diff --git a/cloisim_ros_segmentationcamera/CMakeLists.txt b/cloisim_ros_segmentationcamera/CMakeLists.txt new file mode 100644 index 00000000..5d998a5a --- /dev/null +++ b/cloisim_ros_segmentationcamera/CMakeLists.txt @@ -0,0 +1,74 @@ +############################################################################### +# Set minimum required version of cmake, project name and compile options +################################################################################ +cmake_minimum_required(VERSION 3.5) +project(cloisim_ros_segmentationcamera) +set(EXEC_NAME "standalone") + +include("../cloisim_ros_base/cmake/cloisim_ros_package.cmake") +cloisim_ros_package() + +################################################################################ +# Find colcon packages and libraries for colcon and system dependencies +################################################################################ +find_package(ament_cmake REQUIRED) +find_package(cloisim_ros_camera REQUIRED) + +set(dependencies +cloisim_ros_camera +) + +################################################################################ +# Build +################################################################################ +add_library( + ${PROJECT_NAME}_core SHARED + src/segmentation_camera.cpp +) + +target_include_directories( + ${PROJECT_NAME}_core PUBLIC + $ + $ +) + +ament_target_dependencies( + ${PROJECT_NAME}_core + ${dependencies} +) + +add_executable( + ${STANDALONE_EXEC_NAME} + src/main.cpp +) + +target_link_libraries( + ${STANDALONE_EXEC_NAME} PRIVATE + ${PROJECT_NAME}_core +) + +################################################################################ +# Install +################################################################################ +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS ${PROJECT_NAME}_core + EXPORT export_${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + INCLUDES DESTINATION include +) + +install( + TARGETS ${STANDALONE_EXEC_NAME} + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${dependencies}) + +ament_package() \ No newline at end of file diff --git a/cloisim_ros_segmentationcamera/README.md b/cloisim_ros_segmentationcamera/README.md new file mode 100644 index 00000000..8cbb74d7 --- /dev/null +++ b/cloisim_ros_segmentationcamera/README.md @@ -0,0 +1,19 @@ +# CLOiSim-ROS Segmentation Camera + +support ros remapping, --ros-args -r /test:=test + +```shell +ros2 run cloisim_ros_segmentationcamera standalone +``` + +or + +```shell +ros2 run cloisim_ros_segmentationcamera standalone --ros-args -p single_mode:=True -p target_model:=cloi -p target_parts_name:=segmentationcamera +``` + +or + +```shell +ros2 run cloisim_ros_segmentationcamera standalone --ros-args -p target_model:=cloi -p target_parts_name:=segmentationcamera +``` diff --git a/cloisim_ros_segmentationcamera/include/cloisim_ros_segmentationcamera/segmentation_camera.hpp b/cloisim_ros_segmentationcamera/include/cloisim_ros_segmentationcamera/segmentation_camera.hpp new file mode 100644 index 00000000..003ddca9 --- /dev/null +++ b/cloisim_ros_segmentationcamera/include/cloisim_ros_segmentationcamera/segmentation_camera.hpp @@ -0,0 +1,51 @@ +/** + * @file segmentation_camera.hpp + * @date 2024-03-01 + * @author hyunseok Yang + * @brief + * ROS2 Segmentation Camera class for simulator + * @remark + * @warning + * LGE Advanced Robotics Laboratory + * Copyright(C) 2024 LG Electronics Co., LTD., Seoul, Korea + * All Rights are Reserved. + * SPDX-License-Identifier: MIT + */ + +#ifndef CLOISIM_ROS_SEGMENTATIONCAMERA__SEGMENTATION_CAMERA_HPP_ +#define CLOISIM_ROS_SEGMENTATIONCAMERA__SEGMENTATION_CAMERA_HPP_ + +#include + +#include +#include + +namespace cloisim_ros +{ +class SegmentationCamera : public Camera +{ + public: + explicit SegmentationCamera(const rclcpp::NodeOptions &options_, const std::string node_name, const std::string namespace_ = ""); + explicit SegmentationCamera(const std::string node_name = "cloisim_ros_segmentationcamera", const std::string namespace_ = ""); + virtual ~SegmentationCamera(); + + private: + void Initialize() override; + void Deinitialize() override; + + private: + void PublishData(const std::string &buffer); + + private: + // Camera info publisher + // rclcpp::Publisher::SharedPtr pubDepthCameraInfo{nullptr}; + + // Store current point cloud. + // sensor_msgs::msg::PointCloud2 msg_pc2; + + // Point cloud publisher. + // rclcpp::Publisher::SharedPtr pubPointCloud; +}; +} // namespace cloisim_ros + +#endif // CLOISIM_ROS_SEGMENTATIONCAMERA__SEGMENTATION_CAMERA_HPP_ \ No newline at end of file diff --git a/cloisim_ros_segmentationcamera/package.xml b/cloisim_ros_segmentationcamera/package.xml new file mode 100644 index 00000000..37dd4ee8 --- /dev/null +++ b/cloisim_ros_segmentationcamera/package.xml @@ -0,0 +1,21 @@ + + + + cloisim_ros_segmentationcamera + 3.4.3 + virtual segmentation camera for simulator + Sungkyu Kang + Hyunseok Yang + Sungkyu Kang + MIT + + ament_cmake + + rclcpp + sensor_msgs + cloisim_ros_camera + + + ament_cmake + + \ No newline at end of file diff --git a/cloisim_ros_segmentationcamera/src/main.cpp b/cloisim_ros_segmentationcamera/src/main.cpp new file mode 100644 index 00000000..6eeb776d --- /dev/null +++ b/cloisim_ros_segmentationcamera/src/main.cpp @@ -0,0 +1,53 @@ +/** + * @file main.cpp + * @date 2024-03-01 + * @author hyunseok Yang + * @brief + * ROS2 Segmentation Camera class for simulator + * @remark + * @warning + * LGE Advanced Robotics Laboratory + * Copyright(C) 2024 LG Electronics Co., LTD., Seoul, Korea + * All Rights are Reserved. + * SPDX-License-Identifier: MIT + */ + +#include "cloisim_ros_segmentationcamera/segmentation_camera.hpp" +#include + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::executors::SingleThreadedExecutor executor; + + const auto bringup_param_node = std::make_shared("cloisim_ros_segmentationcamera"); + bringup_param_node->TargetPartsType("SEGMENTCAMERA"); + executor.add_node(bringup_param_node); + + const auto filtered_result = bringup_param_node->GetBringUpList(true); + + std::shared_ptr node = nullptr; + if (!filtered_result.empty()) + { + rclcpp::NodeOptions node_options; + bringup_param_node->StoreFilteredInfoAsParameters(filtered_result, node_options); + + const auto is_single_mode = bringup_param_node->IsSingleMode(); + const auto model_name = bringup_param_node->TargetModel(); + const auto node_name = bringup_param_node->TargetPartsName(); + + if (is_single_mode) + node = std::make_shared(node_options, node_name); + else + node = std::make_shared(node_options, node_name, model_name); + } + + if (node != nullptr) + { + executor.add_node(node); + } + + executor.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/cloisim_ros_segmentationcamera/src/segmentation_camera.cpp b/cloisim_ros_segmentationcamera/src/segmentation_camera.cpp new file mode 100644 index 00000000..7c98419d --- /dev/null +++ b/cloisim_ros_segmentationcamera/src/segmentation_camera.cpp @@ -0,0 +1,59 @@ +/** + * @file segmentation_camera.cpp + * @date 2024-03-01 + * @author hyunseok Yang + * @brief + * ROS2 Segmentation Camera class for simulator + * @remark + * @warning + * LGE Advanced Robotics Laboratory + * Copyright(C) 2024 LG Electronics Co., LTD., Seoul, Korea + * All Rights are Reserved. + * SPDX-License-Identifier: MIT + */ + +#include "cloisim_ros_segmentationcamera/segmentation_camera.hpp" + +using namespace std; + +namespace cloisim_ros +{ + +SegmentationCamera::SegmentationCamera( + const rclcpp::NodeOptions &options_, const string node_name, const string namespace_) + : Camera(options_, node_name, namespace_) +{ +} + +SegmentationCamera::SegmentationCamera( + const string node_name, const string namespace_) + : SegmentationCamera(rclcpp::NodeOptions(), node_name, namespace_) +{ +} + +SegmentationCamera::~SegmentationCamera() +{ + // DBG_SIM_INFO("Delete"); +} + +void SegmentationCamera::Initialize() +{ + Camera::Initialize(); + + DBG_SIM_INFO("SegmentationCamera Initlaization"); + // pubPointCloud = create_publisher(topic_base_name_ + "/points", rclcpp::QoS(rclcpp::KeepLast(1))); +} + +void SegmentationCamera::Deinitialize() +{ + Camera::Deinitialize(); + + DBG_SIM_INFO("SegmentationCamera Deinitialization"); +} + +void SegmentationCamera::PublishData(const string &buffer) +{ + Camera::PublishData(buffer); +} + +} // namespace cloisim_ros From dd28c81d61c10f7e393b1119aebe05c3ebf0e1b2 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Tue, 5 Mar 2024 10:15:54 +0900 Subject: [PATCH 04/12] add new protobuf messages - segmentation - vision_class --- cloisim_ros_protobuf_msgs/CMakeLists.txt | 1 + .../msgs/segmentation.proto | 25 ++++++++++++++++++ .../msgs/vision_class.proto | 26 +++++++++++++++++++ 3 files changed, 52 insertions(+) create mode 100644 cloisim_ros_protobuf_msgs/msgs/segmentation.proto create mode 100644 cloisim_ros_protobuf_msgs/msgs/vision_class.proto diff --git a/cloisim_ros_protobuf_msgs/CMakeLists.txt b/cloisim_ros_protobuf_msgs/CMakeLists.txt index 69f7f17e..de5a498f 100644 --- a/cloisim_ros_protobuf_msgs/CMakeLists.txt +++ b/cloisim_ros_protobuf_msgs/CMakeLists.txt @@ -19,6 +19,7 @@ set(PROTOBUF_MSGS_DIRECTORY "msgs") list(APPEND PROTOBUF_DEFINITION_FILES "any;param;param_v;color") list(APPEND PROTOBUF_DEFINITION_FILES "header;time;vector2d;vector3d;quaternion;pose;twist") list(APPEND PROTOBUF_DEFINITION_FILES "imu;image;images_stamped;image_stamped;camerasensor;distortion") +list(APPEND PROTOBUF_DEFINITION_FILES "segmentation;vision_class") list(APPEND PROTOBUF_DEFINITION_FILES "sonar;sonar_stamped") list(APPEND PROTOBUF_DEFINITION_FILES "laserscan;laserscan_stamped") list(APPEND PROTOBUF_DEFINITION_FILES "micom;battery;pointcloud;gps") diff --git a/cloisim_ros_protobuf_msgs/msgs/segmentation.proto b/cloisim_ros_protobuf_msgs/msgs/segmentation.proto new file mode 100644 index 00000000..564adf4b --- /dev/null +++ b/cloisim_ros_protobuf_msgs/msgs/segmentation.proto @@ -0,0 +1,25 @@ +/** + * @file segmentation.proto + * @date 2024-03-04 + * @author hyunseok Yang + * @warning + * LGE Advanced Robotics Laboratory + * Copyright(C) 2024 LG Electronics Co., LTD., Seoul, Korea + * All Rights are Reserved. + */ + +syntax = "proto2"; +package cloisim.msgs; + +/// \ingroup cloisim_msgs +/// \interface Segmentation +/// \brief Message for Segmentation + +import "image_stamped.proto"; +import "vision_class.proto"; + +message Segmentation +{ + required ImageStamped image_stamped = 1; + repeated VisionClass class_map = 2; +} \ No newline at end of file diff --git a/cloisim_ros_protobuf_msgs/msgs/vision_class.proto b/cloisim_ros_protobuf_msgs/msgs/vision_class.proto new file mode 100644 index 00000000..8192baf3 --- /dev/null +++ b/cloisim_ros_protobuf_msgs/msgs/vision_class.proto @@ -0,0 +1,26 @@ +/** + * @file vision_class.proto + * @date 2024-03-04 + * @author hyunseok Yang + * @warning + * LGE Advanced Robotics Laboratory + * Copyright(C) 2024 LG Electronics Co., LTD., Seoul, Korea + * All Rights are Reserved. + */ + +syntax = "proto2"; +package cloisim.msgs; + +/// \ingroup cloisim_msgs +/// \interface VisionClass +/// \brief Message for VisionClass + +message VisionClass +{ + // The int value that identifies the class. + required uint32 class_id = 1; + + // The name of the class represented by the class_id + // The string value that identifies the class + required string class_name = 2; +} \ No newline at end of file From 6f18b35780cbae4b8d1ff759cad26d77412ad338 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Tue, 5 Mar 2024 10:18:18 +0900 Subject: [PATCH 05/12] Modify .vscode/c_cpp_properties - include header --- .vscode/c_cpp_properties.json | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 21b254fa..bb6a82e1 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -8,7 +8,11 @@ "${workspaceFolder}/../../install/perception_msgs/include/", "${workspaceFolder}/../../install/elevator_system_msgs/include/", "${workspaceFolder}/../../install/cloisim_ros_protobuf_msgs/include/", - "${workspaceFolder}/../../install/cloisim_ros_msgs/include/" + "${workspaceFolder}/../../install/cloisim_ros_msgs/include/", + "${workspaceFolder}/../../../install/perception_msgs/include/", + "${workspaceFolder}/../../../install/elevator_system_msgs/include/", + "${workspaceFolder}/../../../install/cloisim_ros_protobuf_msgs/include/", + "${workspaceFolder}/../../../install/cloisim_ros_msgs/include/" ], "defines": [], "compilerPath": "/usr/bin/gcc", From 64b7bb384b12666a005971661bd5dcb2bc58db30 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Fri, 8 Mar 2024 10:42:25 +0900 Subject: [PATCH 06/12] Combine Three type of camera package cloisim_ros_camera => cloisim_ros_camera + cloisim_ros_depthcamera + cloisim_ros_segmentationcamera Create camera_base class --- cloisim_ros_bringup/CMakeLists.txt | 4 - cloisim_ros_bringup/package.xml | 2 - cloisim_ros_bringup/src/bringup.cpp | 4 +- cloisim_ros_camera/CMakeLists.txt | 47 +++++- cloisim_ros_camera/README.md | 46 ++++- .../include/cloisim_ros_camera/camera.hpp | 26 +-- .../cloisim_ros_camera/camera_base.hpp | 66 ++++++++ .../cloisim_ros_camera/depth_camera.hpp | 24 +-- .../segmentation_camera.hpp | 60 +++++++ cloisim_ros_camera/package.xml | 5 +- cloisim_ros_camera/src/camera.cpp | 102 ++---------- cloisim_ros_camera/src/camera_base.cpp | 157 ++++++++++++++++++ cloisim_ros_camera/src/depth_camera.cpp | 44 +++++ .../src/{main.cpp => main_cam.cpp} | 0 .../src/main_depthcam.cpp | 2 +- .../src/main_segcam.cpp | 4 +- .../src/segmentation_camera.cpp | 81 +++++++++ cloisim_ros_depthcamera/CMakeLists.txt | 74 --------- cloisim_ros_depthcamera/README.md | 19 --- cloisim_ros_depthcamera/package.xml | 21 --- cloisim_ros_depthcamera/src/depthcamera.cpp | 57 ------- cloisim_ros_segmentationcamera/CMakeLists.txt | 74 --------- cloisim_ros_segmentationcamera/README.md | 19 --- .../segmentation_camera.hpp | 51 ------ cloisim_ros_segmentationcamera/package.xml | 21 --- .../src/segmentation_camera.cpp | 59 ------- 26 files changed, 527 insertions(+), 542 deletions(-) create mode 100644 cloisim_ros_camera/include/cloisim_ros_camera/camera_base.hpp rename cloisim_ros_depthcamera/include/cloisim_ros_depthcamera/depthcamera.hpp => cloisim_ros_camera/include/cloisim_ros_camera/depth_camera.hpp (57%) create mode 100644 cloisim_ros_camera/include/cloisim_ros_camera/segmentation_camera.hpp create mode 100644 cloisim_ros_camera/src/camera_base.cpp create mode 100644 cloisim_ros_camera/src/depth_camera.cpp rename cloisim_ros_camera/src/{main.cpp => main_cam.cpp} (100%) rename cloisim_ros_depthcamera/src/main.cpp => cloisim_ros_camera/src/main_depthcam.cpp (96%) rename cloisim_ros_segmentationcamera/src/main.cpp => cloisim_ros_camera/src/main_segcam.cpp (94%) create mode 100644 cloisim_ros_camera/src/segmentation_camera.cpp delete mode 100644 cloisim_ros_depthcamera/CMakeLists.txt delete mode 100644 cloisim_ros_depthcamera/README.md delete mode 100644 cloisim_ros_depthcamera/package.xml delete mode 100644 cloisim_ros_depthcamera/src/depthcamera.cpp delete mode 100644 cloisim_ros_segmentationcamera/CMakeLists.txt delete mode 100644 cloisim_ros_segmentationcamera/README.md delete mode 100644 cloisim_ros_segmentationcamera/include/cloisim_ros_segmentationcamera/segmentation_camera.hpp delete mode 100644 cloisim_ros_segmentationcamera/package.xml delete mode 100644 cloisim_ros_segmentationcamera/src/segmentation_camera.cpp diff --git a/cloisim_ros_bringup/CMakeLists.txt b/cloisim_ros_bringup/CMakeLists.txt index c21ef047..39420773 100644 --- a/cloisim_ros_bringup/CMakeLists.txt +++ b/cloisim_ros_bringup/CMakeLists.txt @@ -21,9 +21,7 @@ find_package(cloisim_ros_camera REQUIRED) find_package(cloisim_ros_micom REQUIRED) find_package(cloisim_ros_lidar REQUIRED) find_package(cloisim_ros_camera REQUIRED) -find_package(cloisim_ros_depthcamera REQUIRED) find_package(cloisim_ros_multicamera REQUIRED) -find_package(cloisim_ros_segmentationcamera REQUIRED) find_package(cloisim_ros_realsense REQUIRED) find_package(cloisim_ros_gps REQUIRED) find_package(cloisim_ros_imu REQUIRED) @@ -42,9 +40,7 @@ set(dependencies cloisim_ros_micom cloisim_ros_lidar cloisim_ros_camera - cloisim_ros_depthcamera cloisim_ros_multicamera - cloisim_ros_segmentationcamera cloisim_ros_realsense cloisim_ros_gps cloisim_ros_imu diff --git a/cloisim_ros_bringup/package.xml b/cloisim_ros_bringup/package.xml index 8c3074d3..749f69bd 100644 --- a/cloisim_ros_bringup/package.xml +++ b/cloisim_ros_bringup/package.xml @@ -22,9 +22,7 @@ cloisim_ros_micom cloisim_ros_lidar cloisim_ros_camera - cloisim_ros_depthcamera cloisim_ros_multicamera - cloisim_ros_segmentationcamera cloisim_ros_realsense cloisim_ros_gps cloisim_ros_imu diff --git a/cloisim_ros_bringup/src/bringup.cpp b/cloisim_ros_bringup/src/bringup.cpp index f9537ed2..7fb1f54a 100644 --- a/cloisim_ros_bringup/src/bringup.cpp +++ b/cloisim_ros_bringup/src/bringup.cpp @@ -15,7 +15,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -25,7 +26,6 @@ #include #include #include -#include #include #include diff --git a/cloisim_ros_camera/CMakeLists.txt b/cloisim_ros_camera/CMakeLists.txt index 59ea56da..d7e236ed 100644 --- a/cloisim_ros_camera/CMakeLists.txt +++ b/cloisim_ros_camera/CMakeLists.txt @@ -7,6 +7,10 @@ project(cloisim_ros_camera) include("../cloisim_ros_base/cmake/cloisim_ros_package.cmake") cloisim_ros_package() +set(STANDALONE_CAMERA "camera") +set(STANDALONE_DEPTHCAMERA "depth_camera") +set(STANDALONE_SEGMENTATIONCAMERA "segmentation_camera") + ################################################################################ # Find colcon packages and libraries for colcon and system dependencies ################################################################################ @@ -16,6 +20,7 @@ find_package(camera_info_manager REQUIRED) find_package(image_transport REQUIRED) find_package(cloisim_ros_base REQUIRED) find_package(cloisim_ros_bringup_param REQUIRED) +find_package(vision_msgs REQUIRED) set(dependencies sensor_msgs @@ -23,13 +28,17 @@ set(dependencies image_transport cloisim_ros_base cloisim_ros_bringup_param + vision_msgs ) ################################################################################ # Build ################################################################################ add_library(${PROJECT_NAME}_core SHARED + src/camera_base.cpp src/camera.cpp + src/depth_camera.cpp + src/segmentation_camera.cpp ) target_include_directories( @@ -44,12 +53,32 @@ ament_target_dependencies( ) add_executable( - ${STANDALONE_EXEC_NAME} - src/main.cpp + ${STANDALONE_CAMERA} + src/main_cam.cpp +) + +target_link_libraries( + ${STANDALONE_CAMERA} PRIVATE + ${PROJECT_NAME}_core +) + +add_executable( + ${STANDALONE_DEPTHCAMERA} + src/main_depthcam.cpp ) target_link_libraries( - ${STANDALONE_EXEC_NAME} PRIVATE + ${STANDALONE_DEPTHCAMERA} PRIVATE + ${PROJECT_NAME}_core +) + +add_executable( + ${STANDALONE_SEGMENTATIONCAMERA} + src/main_segcam.cpp +) + +target_link_libraries( + ${STANDALONE_SEGMENTATIONCAMERA} PRIVATE ${PROJECT_NAME}_core ) @@ -70,7 +99,17 @@ install( ) install( - TARGETS ${STANDALONE_EXEC_NAME} + TARGETS ${STANDALONE_CAMERA} + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install( + TARGETS ${STANDALONE_DEPTHCAMERA} + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install( + TARGETS ${STANDALONE_SEGMENTATIONCAMERA} RUNTIME DESTINATION lib/${PROJECT_NAME} ) diff --git a/cloisim_ros_camera/README.md b/cloisim_ros_camera/README.md index aa384be8..86fecea0 100644 --- a/cloisim_ros_camera/README.md +++ b/cloisim_ros_camera/README.md @@ -25,20 +25,58 @@ This filename in `` element enables to connect cloisim_ros_camera. ``` - ## how to run + +### Camera + +```shell +ros2 run cloisim_ros_camera camera +``` + +or + +```shell +ros2 run cloisim_ros_camera camera --ros-args -p single_mode:=True -p target_model:=cloi1 -p target_parts_name:=camera +``` + +or + +```shell +ros2 run cloisim_ros_camera camera --ros-args -p target_model:=cloi1 -p target_parts_name:=camera +``` + +### Depth Camera + +```shell +ros2 run cloisim_ros_camera depth_camera +``` + +or + +```shell +ros2 run cloisim_ros_camera depth_camera --ros-args -p single_mode:=True -p target_model:=cloi1 -p target_parts_name:=depthcamera +``` + +or + +```shell +ros2 run cloisim_ros_camera depth_camera --ros-args -p target_model:=cloi1 -p target_parts_name:=depthcamera +``` + +### Segmentation Camera + ```shell -ros2 run cloisim_ros_camera standalone +ros2 run cloisim_ros_camera segmentation_camera ``` or ```shell -ros2 run cloisim_ros_camera standalone --ros-args -p single_mode:=True -p target_model:=cloi1 -p target_parts_name:=camera +ros2 run cloisim_ros_camera segmentation_camera --ros-args -p single_mode:=True -p target_model:=cloi -p target_parts_name:=segmentationcamera ``` or ```shell -ros2 run cloisim_ros_camera standalone --ros-args -p target_model:=cloi1 -p target_parts_name:=camera +ros2 run cloisim_ros_camera segmentation_camera --ros-args -p target_model:=cloi -p target_parts_name:=segmentationcamera ``` diff --git a/cloisim_ros_camera/include/cloisim_ros_camera/camera.hpp b/cloisim_ros_camera/include/cloisim_ros_camera/camera.hpp index b438910c..055da515 100644 --- a/cloisim_ros_camera/include/cloisim_ros_camera/camera.hpp +++ b/cloisim_ros_camera/include/cloisim_ros_camera/camera.hpp @@ -22,38 +22,18 @@ #include #include -#include +#include #include #include namespace cloisim_ros { -class Camera : public Base +class Camera : public CameraBase { public: explicit Camera(const rclcpp::NodeOptions &options_, const std::string node_name = "cloisim_ros_camera", const std::string namespace_ = ""); explicit Camera(const std::string node_name = "cloisim_ros_camera", const std::string namespace_ = ""); virtual ~Camera(); - - protected: - void Initialize() override; - void Deinitialize() override; - - protected: - void PublishData(const std::string &buffer); - - private: - // image buffer from simulator - cloisim::msgs::ImageStamped pb_img_; - - // message for ROS2 communictaion - sensor_msgs::msg::Image msg_img_; - - // Image publisher - image_transport::CameraPublisher pub_; - - // Camera info manager - std::shared_ptr camera_info_manager_; }; } // namespace cloisim_ros -#endif // CLOISIM_ROS_CAMERA__CAMERA_HPP_ +#endif // CLOISIM_ROS_CAMERA__CAMERA_HPP_ \ No newline at end of file diff --git a/cloisim_ros_camera/include/cloisim_ros_camera/camera_base.hpp b/cloisim_ros_camera/include/cloisim_ros_camera/camera_base.hpp new file mode 100644 index 00000000..1d7575c3 --- /dev/null +++ b/cloisim_ros_camera/include/cloisim_ros_camera/camera_base.hpp @@ -0,0 +1,66 @@ +/** + * @file camera_base.hpp + * @date 2024-03-08 + * @author Hyunseok Yang + * @brief + * ROS2 CameraBase class + * @remark + * @warning + * LGE Advanced Robotics Laboratory + * Copyright(C) 2024 LG Electronics Co., LTD., Seoul, Korea + * All Rights are Reserved. + */ +#ifndef CLOISIM_ROS_CAMERA__CAMERA_BASE_HPP_ +#define CLOISIM_ROS_CAMERA__CAMERA_BASE_HPP_ + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace cloisim_ros +{ +class CameraBase : public Base +{ + public: + explicit CameraBase(const rclcpp::NodeOptions &options_, const std::string node_name, const std::string namespace_ = ""); + explicit CameraBase(const std::string node_name, const std::string namespace_ = ""); + virtual ~CameraBase(); + + protected: + void Initialize() override; + void Deinitialize() override; + + void InitializeCameraInfo(); + + void InitializeCameraPublish(); + virtual void InitializeCameraData(); + + protected: + void PublishData(const std::string &buffer); + void PublishData(const cloisim::msgs::ImageStamped& pb_msg); + + protected: + std::string frame_id_; + + // image buffer from simulator + cloisim::msgs::ImageStamped pb_img_; + + // message for ROS2 communictaion + sensor_msgs::msg::Image msg_img_; + + // Image publisher + image_transport::CameraPublisher pub_; + + // Camera info manager + std::shared_ptr camera_info_manager_; +}; +} // namespace cloisim_ros +#endif // CLOISIM_ROS_CAMERA__CAMERA_BASE_HPP_ diff --git a/cloisim_ros_depthcamera/include/cloisim_ros_depthcamera/depthcamera.hpp b/cloisim_ros_camera/include/cloisim_ros_camera/depth_camera.hpp similarity index 57% rename from cloisim_ros_depthcamera/include/cloisim_ros_depthcamera/depthcamera.hpp rename to cloisim_ros_camera/include/cloisim_ros_camera/depth_camera.hpp index af45c7b4..fa6d9037 100644 --- a/cloisim_ros_depthcamera/include/cloisim_ros_depthcamera/depthcamera.hpp +++ b/cloisim_ros_camera/include/cloisim_ros_camera/depth_camera.hpp @@ -12,17 +12,17 @@ * All Rights are Reserved. */ -#ifndef CLOISIM_ROS_DEPTHCAMERA__DEPTHCAMERA_HPP_ -#define CLOISIM_ROS_DEPTHCAMERA__DEPTHCAMERA_HPP_ +#ifndef CLOISIM_ROS_CAMERA__DEPTHCAMERA_HPP_ +#define CLOISIM_ROS_CAMERA__DEPTHCAMERA_HPP_ #include -#include +#include #include namespace cloisim_ros { -class DepthCamera : public Camera +class DepthCamera : public CameraBase { public: explicit DepthCamera(const rclcpp::NodeOptions &options_, const std::string node_name, const std::string namespace_ = ""); @@ -30,21 +30,11 @@ class DepthCamera : public Camera virtual ~DepthCamera(); private: - void Initialize() override; - void Deinitialize() override; - - private: - void PublishData(const std::string &buffer); - - private: - // Camera info publisher - // rclcpp::Publisher::SharedPtr pubDepthCameraInfo{nullptr}; - // Store current point cloud. - // sensor_msgs::msg::PointCloud2 msg_pc2; + sensor_msgs::msg::PointCloud2 msg_pc2; // Point cloud publisher. - // rclcpp::Publisher::SharedPtr pubPointCloud; + rclcpp::Publisher::SharedPtr pubPointCloud; }; } // namespace cloisim_ros -#endif // CLOISIM_ROS_DEPTHCAMERA__DEPTHCAMERA_HPP_ +#endif // CLOISIM_ROS_CAMERA__DEPTHCAMERA_HPP_ diff --git a/cloisim_ros_camera/include/cloisim_ros_camera/segmentation_camera.hpp b/cloisim_ros_camera/include/cloisim_ros_camera/segmentation_camera.hpp new file mode 100644 index 00000000..b8d81a54 --- /dev/null +++ b/cloisim_ros_camera/include/cloisim_ros_camera/segmentation_camera.hpp @@ -0,0 +1,60 @@ +/** + * @file segmentation_camera.hpp + * @date 2024-03-01 + * @author hyunseok Yang + * @brief + * ROS2 Segmentation Camera class for simulator + * @remark + * @warning + * LGE Advanced Robotics Laboratory + * Copyright(C) 2024 LG Electronics Co., LTD., Seoul, Korea + * All Rights are Reserved. + * SPDX-License-Identifier: MIT + */ + +#ifndef CLOISIM_ROS_CAMERA__SEGMENTATION_CAMERA_HPP_ +#define CLOISIM_ROS_CAMERA__SEGMENTATION_CAMERA_HPP_ + +#include +#include +#include +#include + +namespace cloisim_ros +{ +class SegmentationCamera : public CameraBase +{ + public: + explicit SegmentationCamera(const rclcpp::NodeOptions &options_, const std::string node_name, const std::string namespace_ = ""); + explicit SegmentationCamera(const std::string node_name = "cloisim_ros_segmentationcamera", const std::string namespace_ = ""); + virtual ~SegmentationCamera(); + + private: + void InitializeCameraData() override; + + private: + void PublishData(const std::string &buffer); + + private: + cloisim::msgs::Segmentation pb_seg_; + + // + // https://github.com/ros-perception/vision_msgs/tree/ros2?tab=readme-ov-file + // + + // To transmit the metadata associated with the vision pipeline, + // you should use the /vision_msgs/LabelInfo message. + // This message works the same as /sensor_msgs/CameraInfo or /vision_msgs/VisionInfo: + // Publish LabelInfo to a topic. + + // The topic should be at same namespace level as the associated image. + // That is, if your image is published at /my_segmentation_node/image, + // the LabelInfo should be published at /my_segmentation_node/label_info. + // Use a latched publisher for LabelInfo, so that new nodes joining + + // Label info publisher + rclcpp::Publisher::SharedPtr pub_labelinfo_{nullptr}; +}; +} // namespace cloisim_ros + +#endif // CLOISIM_ROS_CAMERA__SEGMENTATION_CAMERA_HPP_ \ No newline at end of file diff --git a/cloisim_ros_camera/package.xml b/cloisim_ros_camera/package.xml index d81b555f..99683cff 100644 --- a/cloisim_ros_camera/package.xml +++ b/cloisim_ros_camera/package.xml @@ -3,9 +3,9 @@ cloisim_ros_camera 3.4.3 - virtual camera for cloisim - Sungkyu Kang + Virtual camera/depth camera/segmentation camera for CLOiSim Hyunseok Yang + Sungkyu Kang Sungkyu Kang MIT @@ -17,6 +17,7 @@ camera_info_manager image_transport image_transport_plugins + vision_msgs ament_cmake diff --git a/cloisim_ros_camera/src/camera.cpp b/cloisim_ros_camera/src/camera.cpp index 736f86c7..67f9ad38 100644 --- a/cloisim_ros_camera/src/camera.cpp +++ b/cloisim_ros_camera/src/camera.cpp @@ -1,16 +1,16 @@ /** * @file camera.cpp - * @date 2021-01-14 - * @author hyunseok Yang - * @author Sungkyu Kang + * @date 2024-03-08 + * @author Hyunseok Yang * @brief - * ROS2 Camera class for cloisim + * ROS2 Camera class * @remark * @warning * LGE Advanced Robotics Laboratory - * Copyright(C) 2019 LG Electronics Co., LTD., Seoul, Korea + * Copyright(C) 2024 LG Electronics Co., LTD., Seoul, Korea * All Rights are Reserved. */ + #include #include #include @@ -24,97 +24,27 @@ using namespace std::chrono_literals; namespace cloisim_ros { -Camera::Camera(const rclcpp::NodeOptions &options_, const string node_name, const string namespace_) - : Base(node_name, namespace_, options_) +Camera::Camera( + const rclcpp::NodeOptions &options_, + const string node_name, + const string namespace_) + : CameraBase(options_, node_name, namespace_) { - topic_name_ = "camera"; + DBG_SIM_INFO("Camera"); Start(); } -Camera::Camera(const string node_name, const string namespace_) +Camera::Camera( + const string node_name, + const string namespace_) : Camera(rclcpp::NodeOptions(), node_name, namespace_) { + // DBG_SIM_INFO("Camera"); } Camera::~Camera() { - // DBG_SIM_INFO("Delete"); + // DBG_SIM_INFO("Delete Camera"); Stop(); } - -void Camera::Initialize() -{ - uint16_t portInfo, portData; - get_parameter_or("bridge.Data", portData, uint16_t(0)); - get_parameter_or("bridge.Info", portInfo, uint16_t(0)); - - const auto hashKeyData = GetTargetHashKey("Data"); - const auto hashKeyInfo = GetTargetHashKey("Info"); - DBG_SIM_INFO("hashKey: data(%s), info(%s)", hashKeyData.c_str(), hashKeyInfo.c_str()); - - auto data_bridge_ptr = CreateBridge(); - auto info_bridge_ptr = CreateBridge(); - - string frame_id = "camera_link"; - if (info_bridge_ptr != nullptr) - { - info_bridge_ptr->Connect(zmq::Bridge::Mode::CLIENT, portInfo, hashKeyInfo); - - GetRos2Parameter(info_bridge_ptr); - - frame_id = GetPartsName() + "_" + GetFrameId("camera_link"); - auto transform_pose = GetObjectTransform(info_bridge_ptr); - transform_pose.set_name(frame_id); - SetStaticTf2(transform_pose); - - camera_info_manager_ = std::make_shared(GetNode().get()); - const auto camSensorMsg = GetCameraSensorMessage(info_bridge_ptr); - SetCameraInfoInManager(camera_info_manager_, camSensorMsg, frame_id); - } - - msg_img_.header.frame_id = frame_id; - const auto topic_base_name_ = GetPartsName() + "/" + topic_name_; - - image_transport::ImageTransport it(GetNode()); - pub_ = it.advertiseCamera(topic_base_name_ + "/image_raw", 1); - - if (data_bridge_ptr != nullptr) - { - data_bridge_ptr->Connect(zmq::Bridge::Mode::SUB, portData, hashKeyData); - AddPublisherThread(data_bridge_ptr, bind(&Camera::PublishData, this, std::placeholders::_1)); - } -} - -void Camera::Deinitialize() -{ - pub_.shutdown(); -} - -void Camera::PublishData(const string &buffer) -{ - if (!pb_img_.ParseFromString(buffer)) - { - DBG_SIM_ERR("Parsing error, size(%d)", buffer.length()); - return; - } - - SetTime(pb_img_.time()); - - msg_img_.header.stamp = GetTime(); - - const auto encoding_arg = GetImageEncondingType(pb_img_.image().pixel_format()); - const uint32_t cols_arg = pb_img_.image().width(); - const uint32_t rows_arg = pb_img_.image().height(); - const uint32_t step_arg = pb_img_.image().step(); - - // Copy from src to image_msg - sensor_msgs::fillImage(msg_img_, encoding_arg, rows_arg, cols_arg, step_arg, - reinterpret_cast(pb_img_.image().data().data())); - - // Publish camera info - auto camera_info_msg = camera_info_manager_->getCameraInfo(); - camera_info_msg.header.stamp = msg_img_.header.stamp; - - pub_.publish(msg_img_, camera_info_msg); -} } // namespace cloisim_ros diff --git a/cloisim_ros_camera/src/camera_base.cpp b/cloisim_ros_camera/src/camera_base.cpp new file mode 100644 index 00000000..869ebf71 --- /dev/null +++ b/cloisim_ros_camera/src/camera_base.cpp @@ -0,0 +1,157 @@ +/** + * @file camera_base.cpp + * @date 2024-03-08 + * @author Hyunseok Yang + * @brief + * ROS2 CameraBase class + * @remark + * @warning + * LGE Advanced Robotics Laboratory + * Copyright(C) 2024 LG Electronics Co., LTD., Seoul, Korea + * All Rights are Reserved. + */ + +#include +#include +#include + +#include +#include +#include + +using namespace std; +using namespace std::chrono_literals; + +namespace cloisim_ros +{ +CameraBase::CameraBase( + const rclcpp::NodeOptions &options_, + const string node_name, + const string namespace_) + : Base(node_name, namespace_, options_) + , frame_id_("camera_link") +{ + // DBG_SIM_INFO("CameraBase"); +} + +CameraBase::CameraBase( + const string node_name, + const string namespace_) + : CameraBase(rclcpp::NodeOptions(), node_name, namespace_) +{ + // DBG_SIM_INFO("CameraBase"); +} + +CameraBase::~CameraBase() +{ +// DBG_SIM_INFO("Delete CameraBase"); +} + +void CameraBase::Initialize() +{ +// DBG_SIM_INFO("CameraBase Initialization"); + InitializeCameraInfo(); + InitializeCameraPublish(); + InitializeCameraData(); +} + +void CameraBase::InitializeCameraInfo() +{ + uint16_t portInfo; + get_parameter_or("bridge.Info", portInfo, uint16_t(0)); + + const auto hashKeyInfo = GetTargetHashKey("Info"); + + auto info_bridge_ptr = CreateBridge(); + + if (info_bridge_ptr != nullptr) + { + info_bridge_ptr->Connect(zmq::Bridge::Mode::CLIENT, portInfo, hashKeyInfo); + + GetRos2Parameter(info_bridge_ptr); + + frame_id_ = GetPartsName() + "_" + GetFrameId("camera_link"); + auto transform_pose = GetObjectTransform(info_bridge_ptr); + transform_pose.set_name(frame_id_); + SetStaticTf2(transform_pose); + + camera_info_manager_ = std::make_shared(GetNode().get()); + const auto camSensorMsg = GetCameraSensorMessage(info_bridge_ptr); + SetCameraInfoInManager(camera_info_manager_, camSensorMsg, frame_id_); + } + + DBG_SIM_INFO("hashKey: info(%s)", hashKeyInfo.c_str()); +} + +void CameraBase::InitializeCameraPublish() +{ + topic_name_ = "camera"; + + msg_img_.header.frame_id = frame_id_; + + const auto topic_base_name_ = GetPartsName() + "/" + topic_name_; + + image_transport::ImageTransport it(GetNode()); + pub_ = it.advertiseCamera(topic_base_name_ + "/image_raw", 1); +} + +void CameraBase::InitializeCameraData() +{ + uint16_t portData; + get_parameter_or("bridge.Data", portData, uint16_t(0)); + + const auto hashKeyData = GetTargetHashKey("Data"); + + auto data_bridge_ptr = CreateBridge(); + + if (data_bridge_ptr != nullptr) + { + data_bridge_ptr->Connect(zmq::Bridge::Mode::SUB, portData, hashKeyData); + AddPublisherThread(data_bridge_ptr, + bind(static_cast(&CameraBase::PublishData), + this, + std::placeholders::_1)); + } + + DBG_SIM_INFO("hashKey: data(%s)", hashKeyData.c_str()); +} + +void CameraBase::Deinitialize() +{ + pub_.shutdown(); +} + + +void CameraBase::PublishData(const string &buffer) +{ + if (!pb_img_.ParseFromString(buffer)) + { + DBG_SIM_ERR("##Parsing error, size(%d)", buffer.length()); + return; + } + + PublishData(pb_img_); +} + +void CameraBase::PublishData(const cloisim::msgs::ImageStamped& pb_msg) +{ + SetTime(pb_msg.time()); + + msg_img_.header.stamp = GetTime(); + + const auto encoding_arg = GetImageEncondingType(pb_msg.image().pixel_format()); + const uint32_t cols_arg = pb_msg.image().width(); + const uint32_t rows_arg = pb_msg.image().height(); + const uint32_t step_arg = pb_msg.image().step(); + + // Copy from src to image_msg + sensor_msgs::fillImage(msg_img_, encoding_arg, rows_arg, cols_arg, step_arg, + reinterpret_cast(pb_msg.image().data().data())); + + // Publish camera info + auto camera_info_msg = camera_info_manager_->getCameraInfo(); + camera_info_msg.header.stamp = msg_img_.header.stamp; + + pub_.publish(msg_img_, camera_info_msg); +} +} // namespace cloisim_ros diff --git a/cloisim_ros_camera/src/depth_camera.cpp b/cloisim_ros_camera/src/depth_camera.cpp new file mode 100644 index 00000000..440f5dd4 --- /dev/null +++ b/cloisim_ros_camera/src/depth_camera.cpp @@ -0,0 +1,44 @@ +/** + * @file depth_camera.cpp + * @date 2021-01-14 + * @author hyunseok Yang + * @author Sungkyu Kang + * @brief + * ROS2 Depth Camera class for simulator + * @remark + * @warning + * LGE Advanced Robotics Laboratory + * Copyright(C) 2019 LG Electronics Co., LTD., Seoul, Korea + * All Rights are Reserved. + */ + +#include "cloisim_ros_camera/depth_camera.hpp" + +using namespace std; + +namespace cloisim_ros +{ + +DepthCamera::DepthCamera( + const rclcpp::NodeOptions &options_, + const string node_name, + const string namespace_) + : CameraBase(options_, node_name, namespace_) +{ + Start(); +} + +DepthCamera::DepthCamera( + const string node_name, + const string namespace_) + : DepthCamera(rclcpp::NodeOptions(), node_name, namespace_) +{ +} + +DepthCamera::~DepthCamera() +{ + // DBG_SIM_INFO("Delete DepthCamera"); + Stop(); +} + +} // namespace cloisim_ros diff --git a/cloisim_ros_camera/src/main.cpp b/cloisim_ros_camera/src/main_cam.cpp similarity index 100% rename from cloisim_ros_camera/src/main.cpp rename to cloisim_ros_camera/src/main_cam.cpp diff --git a/cloisim_ros_depthcamera/src/main.cpp b/cloisim_ros_camera/src/main_depthcam.cpp similarity index 96% rename from cloisim_ros_depthcamera/src/main.cpp rename to cloisim_ros_camera/src/main_depthcam.cpp index beb5736f..a47895c5 100644 --- a/cloisim_ros_depthcamera/src/main.cpp +++ b/cloisim_ros_camera/src/main_depthcam.cpp @@ -13,7 +13,7 @@ * SPDX-License-Identifier: MIT */ -#include "cloisim_ros_depthcamera/depthcamera.hpp" +#include "cloisim_ros_camera/depth_camera.hpp" #include int main(int argc, char** argv) diff --git a/cloisim_ros_segmentationcamera/src/main.cpp b/cloisim_ros_camera/src/main_segcam.cpp similarity index 94% rename from cloisim_ros_segmentationcamera/src/main.cpp rename to cloisim_ros_camera/src/main_segcam.cpp index 6eeb776d..8ed54b22 100644 --- a/cloisim_ros_segmentationcamera/src/main.cpp +++ b/cloisim_ros_camera/src/main_segcam.cpp @@ -1,5 +1,5 @@ /** - * @file main.cpp + * @file main_segcam.cpp * @date 2024-03-01 * @author hyunseok Yang * @brief @@ -12,7 +12,7 @@ * SPDX-License-Identifier: MIT */ -#include "cloisim_ros_segmentationcamera/segmentation_camera.hpp" +#include "cloisim_ros_camera/segmentation_camera.hpp" #include int main(int argc, char** argv) diff --git a/cloisim_ros_camera/src/segmentation_camera.cpp b/cloisim_ros_camera/src/segmentation_camera.cpp new file mode 100644 index 00000000..6be9874a --- /dev/null +++ b/cloisim_ros_camera/src/segmentation_camera.cpp @@ -0,0 +1,81 @@ +/** + * @file segmentation_camera.cpp + * @date 2024-03-01 + * @author hyunseok Yang + * @brief + * ROS2 Segmentation Camera class for simulator + * @remark + * @warning + * LGE Advanced Robotics Laboratory + * Copyright(C) 2024 LG Electronics Co., LTD., Seoul, Korea + * All Rights are Reserved. + * SPDX-License-Identifier: MIT + */ + +#include "cloisim_ros_camera/segmentation_camera.hpp" + +using namespace std; + +namespace cloisim_ros +{ +SegmentationCamera::SegmentationCamera( + const rclcpp::NodeOptions &options_, + const string node_name, + const string namespace_) + : CameraBase(options_, node_name, namespace_) +{ + Start(); +} + +SegmentationCamera::SegmentationCamera( + const string node_name, + const string namespace_) + : SegmentationCamera(rclcpp::NodeOptions(), node_name, namespace_) +{ +} + +SegmentationCamera::~SegmentationCamera() +{ + // DBG_SIM_INFO("Delete SegmentationCamera"); + Stop(); +} + +void SegmentationCamera::InitializeCameraData() +{ + uint16_t portData; + get_parameter_or("bridge.Data", portData, uint16_t(0)); + + const auto hashKeyData = GetTargetHashKey("Data"); + + auto data_bridge_ptr = CreateBridge(); + if (data_bridge_ptr != nullptr) + { + data_bridge_ptr->Connect(zmq::Bridge::Mode::SUB, portData, hashKeyData); + AddPublisherThread(data_bridge_ptr, bind(&SegmentationCamera::PublishData, this, std::placeholders::_1)); + } + + pub_labelinfo_ = create_publisher("label_info", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()); + + DBG_SIM_INFO("SegmentationCamera hashKey: data(%s)", hashKeyData.c_str()); +} + +void SegmentationCamera::PublishData(const string &buffer) +{ + if (!pb_seg_.ParseFromString(buffer)) + { + DBG_SIM_ERR("!!!Parsing error, size(%d)", buffer.length()); + return; + } + + SetTime(pb_seg_.image_stamped().time()); + + vision_msgs::msg::LabelInfo msg_label_info; + + // pb_seg_.clas + + pub_labelinfo_->publish(msg_label_info); + + // send camera image + CameraBase::PublishData(pb_seg_.image_stamped()); +} +} // namespace cloisim_ros diff --git a/cloisim_ros_depthcamera/CMakeLists.txt b/cloisim_ros_depthcamera/CMakeLists.txt deleted file mode 100644 index 7dbf5852..00000000 --- a/cloisim_ros_depthcamera/CMakeLists.txt +++ /dev/null @@ -1,74 +0,0 @@ -############################################################################### -# Set minimum required version of cmake, project name and compile options -################################################################################ -cmake_minimum_required(VERSION 3.5) -project(cloisim_ros_depthcamera) -set(EXEC_NAME "standalone") - -include("../cloisim_ros_base/cmake/cloisim_ros_package.cmake") -cloisim_ros_package() - -################################################################################ -# Find colcon packages and libraries for colcon and system dependencies -################################################################################ -find_package(ament_cmake REQUIRED) -find_package(cloisim_ros_camera REQUIRED) - -set(dependencies - cloisim_ros_camera -) - -################################################################################ -# Build -################################################################################ -add_library( - ${PROJECT_NAME}_core SHARED - src/depthcamera.cpp -) - -target_include_directories( - ${PROJECT_NAME}_core PUBLIC - $ - $ -) - -ament_target_dependencies( - ${PROJECT_NAME}_core - ${dependencies} -) - -add_executable( - ${STANDALONE_EXEC_NAME} - src/main.cpp -) - -target_link_libraries( - ${STANDALONE_EXEC_NAME} PRIVATE - ${PROJECT_NAME}_core -) - -################################################################################ -# Install -################################################################################ -install( - DIRECTORY include/ - DESTINATION include -) - -install( - TARGETS ${PROJECT_NAME}_core - EXPORT export_${PROJECT_NAME} - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - INCLUDES DESTINATION include -) - -install( - TARGETS ${STANDALONE_EXEC_NAME} - RUNTIME DESTINATION lib/${PROJECT_NAME} -) - -ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) -ament_export_dependencies(${dependencies}) - -ament_package() \ No newline at end of file diff --git a/cloisim_ros_depthcamera/README.md b/cloisim_ros_depthcamera/README.md deleted file mode 100644 index df9425ce..00000000 --- a/cloisim_ros_depthcamera/README.md +++ /dev/null @@ -1,19 +0,0 @@ -# CLOiSim-ROS Depth Camera - -support ros remapping, --ros-args -r /test:=test - -```shell -ros2 run cloisim_ros_depthcamera standalone -``` - -or - -```shell -ros2 run cloisim_ros_depthcamera standalone --ros-args -p single_mode:=True -p target_model:=cloi1 -p target_parts_name:=depthcamera -``` - -or - -```shell -ros2 run cloisim_ros_depthcamera standalone --ros-args -p target_model:=cloi1 -p target_parts_name:=depthcamera -``` diff --git a/cloisim_ros_depthcamera/package.xml b/cloisim_ros_depthcamera/package.xml deleted file mode 100644 index cc6220a5..00000000 --- a/cloisim_ros_depthcamera/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - cloisim_ros_depthcamera - 3.4.3 - virtual depth camera for simulator - Sungkyu Kang - Hyunseok Yang - Sungkyu Kang - MIT - - ament_cmake - - rclcpp - sensor_msgs - cloisim_ros_camera - - - ament_cmake - - \ No newline at end of file diff --git a/cloisim_ros_depthcamera/src/depthcamera.cpp b/cloisim_ros_depthcamera/src/depthcamera.cpp deleted file mode 100644 index 7a397d7e..00000000 --- a/cloisim_ros_depthcamera/src/depthcamera.cpp +++ /dev/null @@ -1,57 +0,0 @@ -/** - * @file depthcamera.cpp - * @date 2021-01-14 - * @author hyunseok Yang - * @author Sungkyu Kang - * @brief - * ROS2 Depth Camera class for simulator - * @remark - * @warning - * LGE Advanced Robotics Laboratory - * Copyright(C) 2019 LG Electronics Co., LTD., Seoul, Korea - * All Rights are Reserved. - */ - -#include "cloisim_ros_depthcamera/depthcamera.hpp" - -using namespace std; - -namespace cloisim_ros -{ - -DepthCamera::DepthCamera(const rclcpp::NodeOptions &options_, const string node_name, const string namespace_) - : Camera(options_, node_name, namespace_) -{ -} - -DepthCamera::DepthCamera(const string node_name, const string namespace_) - : DepthCamera(rclcpp::NodeOptions(), node_name, namespace_) -{ -} - -DepthCamera::~DepthCamera() -{ - // DBG_SIM_INFO("Delete"); -} - -void DepthCamera::Initialize() -{ - Camera::Initialize(); - - DBG_SIM_INFO("DepthCamera Initlaization"); - // pubPointCloud = create_publisher(topic_base_name_ + "/points", rclcpp::QoS(rclcpp::KeepLast(1))); -} - -void DepthCamera::Deinitialize() -{ - Camera::Deinitialize(); - - DBG_SIM_INFO("DepthCamera Deinitialization"); -} - -void DepthCamera::PublishData(const string &buffer) -{ - Camera::PublishData(buffer); -} - -} // namespace cloisim_ros diff --git a/cloisim_ros_segmentationcamera/CMakeLists.txt b/cloisim_ros_segmentationcamera/CMakeLists.txt deleted file mode 100644 index 5d998a5a..00000000 --- a/cloisim_ros_segmentationcamera/CMakeLists.txt +++ /dev/null @@ -1,74 +0,0 @@ -############################################################################### -# Set minimum required version of cmake, project name and compile options -################################################################################ -cmake_minimum_required(VERSION 3.5) -project(cloisim_ros_segmentationcamera) -set(EXEC_NAME "standalone") - -include("../cloisim_ros_base/cmake/cloisim_ros_package.cmake") -cloisim_ros_package() - -################################################################################ -# Find colcon packages and libraries for colcon and system dependencies -################################################################################ -find_package(ament_cmake REQUIRED) -find_package(cloisim_ros_camera REQUIRED) - -set(dependencies -cloisim_ros_camera -) - -################################################################################ -# Build -################################################################################ -add_library( - ${PROJECT_NAME}_core SHARED - src/segmentation_camera.cpp -) - -target_include_directories( - ${PROJECT_NAME}_core PUBLIC - $ - $ -) - -ament_target_dependencies( - ${PROJECT_NAME}_core - ${dependencies} -) - -add_executable( - ${STANDALONE_EXEC_NAME} - src/main.cpp -) - -target_link_libraries( - ${STANDALONE_EXEC_NAME} PRIVATE - ${PROJECT_NAME}_core -) - -################################################################################ -# Install -################################################################################ -install( - DIRECTORY include/ - DESTINATION include -) - -install( - TARGETS ${PROJECT_NAME}_core - EXPORT export_${PROJECT_NAME} - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - INCLUDES DESTINATION include -) - -install( - TARGETS ${STANDALONE_EXEC_NAME} - RUNTIME DESTINATION lib/${PROJECT_NAME} -) - -ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) -ament_export_dependencies(${dependencies}) - -ament_package() \ No newline at end of file diff --git a/cloisim_ros_segmentationcamera/README.md b/cloisim_ros_segmentationcamera/README.md deleted file mode 100644 index 8cbb74d7..00000000 --- a/cloisim_ros_segmentationcamera/README.md +++ /dev/null @@ -1,19 +0,0 @@ -# CLOiSim-ROS Segmentation Camera - -support ros remapping, --ros-args -r /test:=test - -```shell -ros2 run cloisim_ros_segmentationcamera standalone -``` - -or - -```shell -ros2 run cloisim_ros_segmentationcamera standalone --ros-args -p single_mode:=True -p target_model:=cloi -p target_parts_name:=segmentationcamera -``` - -or - -```shell -ros2 run cloisim_ros_segmentationcamera standalone --ros-args -p target_model:=cloi -p target_parts_name:=segmentationcamera -``` diff --git a/cloisim_ros_segmentationcamera/include/cloisim_ros_segmentationcamera/segmentation_camera.hpp b/cloisim_ros_segmentationcamera/include/cloisim_ros_segmentationcamera/segmentation_camera.hpp deleted file mode 100644 index 003ddca9..00000000 --- a/cloisim_ros_segmentationcamera/include/cloisim_ros_segmentationcamera/segmentation_camera.hpp +++ /dev/null @@ -1,51 +0,0 @@ -/** - * @file segmentation_camera.hpp - * @date 2024-03-01 - * @author hyunseok Yang - * @brief - * ROS2 Segmentation Camera class for simulator - * @remark - * @warning - * LGE Advanced Robotics Laboratory - * Copyright(C) 2024 LG Electronics Co., LTD., Seoul, Korea - * All Rights are Reserved. - * SPDX-License-Identifier: MIT - */ - -#ifndef CLOISIM_ROS_SEGMENTATIONCAMERA__SEGMENTATION_CAMERA_HPP_ -#define CLOISIM_ROS_SEGMENTATIONCAMERA__SEGMENTATION_CAMERA_HPP_ - -#include - -#include -#include - -namespace cloisim_ros -{ -class SegmentationCamera : public Camera -{ - public: - explicit SegmentationCamera(const rclcpp::NodeOptions &options_, const std::string node_name, const std::string namespace_ = ""); - explicit SegmentationCamera(const std::string node_name = "cloisim_ros_segmentationcamera", const std::string namespace_ = ""); - virtual ~SegmentationCamera(); - - private: - void Initialize() override; - void Deinitialize() override; - - private: - void PublishData(const std::string &buffer); - - private: - // Camera info publisher - // rclcpp::Publisher::SharedPtr pubDepthCameraInfo{nullptr}; - - // Store current point cloud. - // sensor_msgs::msg::PointCloud2 msg_pc2; - - // Point cloud publisher. - // rclcpp::Publisher::SharedPtr pubPointCloud; -}; -} // namespace cloisim_ros - -#endif // CLOISIM_ROS_SEGMENTATIONCAMERA__SEGMENTATION_CAMERA_HPP_ \ No newline at end of file diff --git a/cloisim_ros_segmentationcamera/package.xml b/cloisim_ros_segmentationcamera/package.xml deleted file mode 100644 index 37dd4ee8..00000000 --- a/cloisim_ros_segmentationcamera/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - cloisim_ros_segmentationcamera - 3.4.3 - virtual segmentation camera for simulator - Sungkyu Kang - Hyunseok Yang - Sungkyu Kang - MIT - - ament_cmake - - rclcpp - sensor_msgs - cloisim_ros_camera - - - ament_cmake - - \ No newline at end of file diff --git a/cloisim_ros_segmentationcamera/src/segmentation_camera.cpp b/cloisim_ros_segmentationcamera/src/segmentation_camera.cpp deleted file mode 100644 index 7c98419d..00000000 --- a/cloisim_ros_segmentationcamera/src/segmentation_camera.cpp +++ /dev/null @@ -1,59 +0,0 @@ -/** - * @file segmentation_camera.cpp - * @date 2024-03-01 - * @author hyunseok Yang - * @brief - * ROS2 Segmentation Camera class for simulator - * @remark - * @warning - * LGE Advanced Robotics Laboratory - * Copyright(C) 2024 LG Electronics Co., LTD., Seoul, Korea - * All Rights are Reserved. - * SPDX-License-Identifier: MIT - */ - -#include "cloisim_ros_segmentationcamera/segmentation_camera.hpp" - -using namespace std; - -namespace cloisim_ros -{ - -SegmentationCamera::SegmentationCamera( - const rclcpp::NodeOptions &options_, const string node_name, const string namespace_) - : Camera(options_, node_name, namespace_) -{ -} - -SegmentationCamera::SegmentationCamera( - const string node_name, const string namespace_) - : SegmentationCamera(rclcpp::NodeOptions(), node_name, namespace_) -{ -} - -SegmentationCamera::~SegmentationCamera() -{ - // DBG_SIM_INFO("Delete"); -} - -void SegmentationCamera::Initialize() -{ - Camera::Initialize(); - - DBG_SIM_INFO("SegmentationCamera Initlaization"); - // pubPointCloud = create_publisher(topic_base_name_ + "/points", rclcpp::QoS(rclcpp::KeepLast(1))); -} - -void SegmentationCamera::Deinitialize() -{ - Camera::Deinitialize(); - - DBG_SIM_INFO("SegmentationCamera Deinitialization"); -} - -void SegmentationCamera::PublishData(const string &buffer) -{ - Camera::PublishData(buffer); -} - -} // namespace cloisim_ros From ebdb3f722c0892fd559940cda3d40bd2568a7f81 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Fri, 8 Mar 2024 12:36:48 +0900 Subject: [PATCH 07/12] Create new static function for standalone execution in cloisim_ros_camera --- .../src/bringup_param.cpp | 7 ++- .../include/cloisim_ros_camera/standalone.hpp | 48 +++++++++++++++++++ cloisim_ros_camera/src/main_cam.cpp | 35 ++------------ cloisim_ros_camera/src/main_depthcam.cpp | 35 ++------------ cloisim_ros_camera/src/main_segcam.cpp | 35 ++------------ 5 files changed, 62 insertions(+), 98 deletions(-) create mode 100644 cloisim_ros_camera/include/cloisim_ros_camera/standalone.hpp diff --git a/cloisim_ros_bringup_param/src/bringup_param.cpp b/cloisim_ros_bringup_param/src/bringup_param.cpp index 4ab1e83e..1348d3df 100644 --- a/cloisim_ros_bringup_param/src/bringup_param.cpp +++ b/cloisim_ros_bringup_param/src/bringup_param.cpp @@ -73,10 +73,12 @@ Json::Value BringUpParam::RequestBringUpList() if (ws_service_ptr_ == nullptr) ws_service_ptr_ = new WebSocketService(); + // cout << "ws Run" << endl; ws_service_ptr_->Run(); while (true) { + // cout << "ws request" << endl; ws_service_ptr_->Request(); // cout << "start to load payload" << endl; @@ -107,8 +109,9 @@ Json::Value BringUpParam::RequestBringUpList() if (root["result"].size() > 0) { - // cout << "There is node map list: " << result_map_.size() << endl; result = root["result"]; + // cout << "There is node map list: " << result.size() << endl; + // cout << result << endl; break; } else @@ -125,7 +128,7 @@ Json::Value BringUpParam::RequestBringUpList() Json::Value BringUpParam::GetFilteredListByParameters(const Json::Value result) { Json::Value root; - // cout << result << endl; + cout << result << endl; for (auto it = result.begin(); it != result.end(); it++) { const auto node_namespace = it.key().asString(); diff --git a/cloisim_ros_camera/include/cloisim_ros_camera/standalone.hpp b/cloisim_ros_camera/include/cloisim_ros_camera/standalone.hpp new file mode 100644 index 00000000..99d1b953 --- /dev/null +++ b/cloisim_ros_camera/include/cloisim_ros_camera/standalone.hpp @@ -0,0 +1,48 @@ +/** + * @file standalone.hpp + * @date 2024-03-08 + * @author Hyunseok Yang + * @brief + * rclcpp init for standalone + * @remark + * @warning + * LGE Advanced Robotics Laboratory + * Copyright (c) 2024 LG Electronics Inc., LTD., Seoul, Korea + * All Rights are Reserved. + * + * SPDX-License-Identifier: MIT + */ + +#include + +template +static void run_standalone_single_executor( + const std::string target_node_name, + const std::string target_parts_name) +{ + rclcpp::executors::SingleThreadedExecutor executor; + + const auto bringup_param_node = std::make_shared(target_node_name); + bringup_param_node->TargetPartsType(target_parts_name); + + executor.add_node(bringup_param_node); + + const auto filtered_result = bringup_param_node->GetBringUpList(true); + if (filtered_result.empty()) + { + return; + } + + rclcpp::NodeOptions node_options; + bringup_param_node->StoreFilteredInfoAsParameters(filtered_result, node_options); + + const auto is_single_mode = bringup_param_node->IsSingleMode(); + const auto model_name = bringup_param_node->TargetModel(); + const auto node_name = bringup_param_node->TargetPartsName(); + + auto node = (is_single_mode) ? std::make_shared(node_options, node_name) + : std::make_shared(node_options, node_name, model_name); + + executor.add_node(node); + executor.spin(); +} diff --git a/cloisim_ros_camera/src/main_cam.cpp b/cloisim_ros_camera/src/main_cam.cpp index ff1245f8..d2561d40 100644 --- a/cloisim_ros_camera/src/main_cam.cpp +++ b/cloisim_ros_camera/src/main_cam.cpp @@ -13,42 +13,13 @@ * SPDX-License-Identifier: MIT */ +#include "cloisim_ros_camera/standalone.hpp" #include "cloisim_ros_camera/camera.hpp" -#include int main(int argc, char** argv) { rclcpp::init(argc, argv); - rclcpp::executors::SingleThreadedExecutor executor; - - const auto bringup_param_node = std::make_shared("cloisim_ros_camera"); - bringup_param_node->TargetPartsType("CAMERA"); - executor.add_node(bringup_param_node); - - const auto filtered_result = bringup_param_node->GetBringUpList(true); - - std::shared_ptr node = nullptr; - if (!filtered_result.empty()) - { - rclcpp::NodeOptions node_options; - bringup_param_node->StoreFilteredInfoAsParameters(filtered_result, node_options); - - const auto is_single_mode = bringup_param_node->IsSingleMode(); - const auto model_name = bringup_param_node->TargetModel(); - const auto node_name = bringup_param_node->TargetPartsName(); - - if (is_single_mode) - node = std::make_shared(node_options, node_name); - else - node = std::make_shared(node_options, node_name, model_name); - } - - if (node != nullptr) - { - executor.add_node(node); - } - - executor.spin(); + run_standalone_single_executor("cloisim_ros_camera", "CAMERA"); rclcpp::shutdown(); return 0; -} +} \ No newline at end of file diff --git a/cloisim_ros_camera/src/main_depthcam.cpp b/cloisim_ros_camera/src/main_depthcam.cpp index a47895c5..9f6b416a 100644 --- a/cloisim_ros_camera/src/main_depthcam.cpp +++ b/cloisim_ros_camera/src/main_depthcam.cpp @@ -14,41 +14,12 @@ */ #include "cloisim_ros_camera/depth_camera.hpp" -#include +#include "cloisim_ros_camera/standalone.hpp" int main(int argc, char** argv) { rclcpp::init(argc, argv); - rclcpp::executors::SingleThreadedExecutor executor; - - const auto bringup_param_node = std::make_shared("cloisim_ros_depthcamera"); - bringup_param_node->TargetPartsType("DEPTHCAMERA"); - executor.add_node(bringup_param_node); - - const auto filtered_result = bringup_param_node->GetBringUpList(true); - - std::shared_ptr node = nullptr; - if (!filtered_result.empty()) - { - rclcpp::NodeOptions node_options; - bringup_param_node->StoreFilteredInfoAsParameters(filtered_result, node_options); - - const auto is_single_mode = bringup_param_node->IsSingleMode(); - const auto model_name = bringup_param_node->TargetModel(); - const auto node_name = bringup_param_node->TargetPartsName(); - - if (is_single_mode) - node = std::make_shared(node_options, node_name); - else - node = std::make_shared(node_options, node_name, model_name); - } - - if (node != nullptr) - { - executor.add_node(node); - } - - executor.spin(); + run_standalone_single_executor("cloisim_ros_depthcamera", "DEPTHCAMERA"); rclcpp::shutdown(); return 0; -} +} \ No newline at end of file diff --git a/cloisim_ros_camera/src/main_segcam.cpp b/cloisim_ros_camera/src/main_segcam.cpp index 8ed54b22..ed4e0196 100644 --- a/cloisim_ros_camera/src/main_segcam.cpp +++ b/cloisim_ros_camera/src/main_segcam.cpp @@ -13,41 +13,12 @@ */ #include "cloisim_ros_camera/segmentation_camera.hpp" -#include +#include "cloisim_ros_camera/standalone.hpp" int main(int argc, char** argv) { rclcpp::init(argc, argv); - rclcpp::executors::SingleThreadedExecutor executor; - - const auto bringup_param_node = std::make_shared("cloisim_ros_segmentationcamera"); - bringup_param_node->TargetPartsType("SEGMENTCAMERA"); - executor.add_node(bringup_param_node); - - const auto filtered_result = bringup_param_node->GetBringUpList(true); - - std::shared_ptr node = nullptr; - if (!filtered_result.empty()) - { - rclcpp::NodeOptions node_options; - bringup_param_node->StoreFilteredInfoAsParameters(filtered_result, node_options); - - const auto is_single_mode = bringup_param_node->IsSingleMode(); - const auto model_name = bringup_param_node->TargetModel(); - const auto node_name = bringup_param_node->TargetPartsName(); - - if (is_single_mode) - node = std::make_shared(node_options, node_name); - else - node = std::make_shared(node_options, node_name, model_name); - } - - if (node != nullptr) - { - executor.add_node(node); - } - - executor.spin(); + run_standalone_single_executor("cloisim_ros_segmentationcamera", "SEGMENTCAMERA"); rclcpp::shutdown(); return 0; -} +} \ No newline at end of file From 392fa5d2a3ba33356fdcdb12dde106c578e2cfe0 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Fri, 8 Mar 2024 13:04:53 +0900 Subject: [PATCH 08/12] fill label_info for segmentation_camera --- cloisim_ros_camera/src/camera.cpp | 2 +- cloisim_ros_camera/src/segmentation_camera.cpp | 13 +++++++++++-- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/cloisim_ros_camera/src/camera.cpp b/cloisim_ros_camera/src/camera.cpp index 67f9ad38..412d6af9 100644 --- a/cloisim_ros_camera/src/camera.cpp +++ b/cloisim_ros_camera/src/camera.cpp @@ -30,7 +30,7 @@ Camera::Camera( const string namespace_) : CameraBase(options_, node_name, namespace_) { - DBG_SIM_INFO("Camera"); + // DBG_SIM_INFO("Camera"); Start(); } diff --git a/cloisim_ros_camera/src/segmentation_camera.cpp b/cloisim_ros_camera/src/segmentation_camera.cpp index 6be9874a..2be0c40b 100644 --- a/cloisim_ros_camera/src/segmentation_camera.cpp +++ b/cloisim_ros_camera/src/segmentation_camera.cpp @@ -56,7 +56,7 @@ void SegmentationCamera::InitializeCameraData() pub_labelinfo_ = create_publisher("label_info", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()); - DBG_SIM_INFO("SegmentationCamera hashKey: data(%s)", hashKeyData.c_str()); + DBG_SIM_INFO("%s hashKey: data(%s)", typeid(this).name(), hashKeyData.c_str()); } void SegmentationCamera::PublishData(const string &buffer) @@ -70,8 +70,17 @@ void SegmentationCamera::PublishData(const string &buffer) SetTime(pb_seg_.image_stamped().time()); vision_msgs::msg::LabelInfo msg_label_info; + msg_label_info.header.stamp = GetTime(); + for (auto i = 0; i < pb_seg_.class_map_size(); i++) + { + vision_msgs::msg::VisionClass msg_vision_class; + + auto &class_map = pb_seg_.class_map(i); + msg_vision_class.class_id = class_map.class_id(); + msg_vision_class.class_name = class_map.class_name(); - // pb_seg_.clas + msg_label_info.class_map.push_back(msg_vision_class); + } pub_labelinfo_->publish(msg_label_info); From 93cfe640b7b7f246a69121030da9c85dc7351a38 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Fri, 8 Mar 2024 13:15:40 +0900 Subject: [PATCH 09/12] set inference threshold value to 1 for LabelInfo --- cloisim_ros_camera/src/segmentation_camera.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/cloisim_ros_camera/src/segmentation_camera.cpp b/cloisim_ros_camera/src/segmentation_camera.cpp index 2be0c40b..24a2560c 100644 --- a/cloisim_ros_camera/src/segmentation_camera.cpp +++ b/cloisim_ros_camera/src/segmentation_camera.cpp @@ -82,6 +82,8 @@ void SegmentationCamera::PublishData(const string &buffer) msg_label_info.class_map.push_back(msg_vision_class); } + msg_label_info.threshold = 1; + pub_labelinfo_->publish(msg_label_info); // send camera image From e186e5db0e0a674d949ec133f6e5e3109c752eee Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Fri, 8 Mar 2024 13:16:56 +0900 Subject: [PATCH 10/12] Fix PublishData in segmentation_camera --- cloisim_ros_camera/src/segmentation_camera.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/cloisim_ros_camera/src/segmentation_camera.cpp b/cloisim_ros_camera/src/segmentation_camera.cpp index 24a2560c..b4c97d95 100644 --- a/cloisim_ros_camera/src/segmentation_camera.cpp +++ b/cloisim_ros_camera/src/segmentation_camera.cpp @@ -36,7 +36,7 @@ SegmentationCamera::SegmentationCamera( SegmentationCamera::~SegmentationCamera() { - // DBG_SIM_INFO("Delete SegmentationCamera"); + // DBG_SIM_INFO("Delete %s", typeid(this).name()); Stop(); } @@ -63,18 +63,18 @@ void SegmentationCamera::PublishData(const string &buffer) { if (!pb_seg_.ParseFromString(buffer)) { - DBG_SIM_ERR("!!!Parsing error, size(%d)", buffer.length()); + DBG_SIM_ERR("Parsing error, size(%d)", buffer.length()); return; } SetTime(pb_seg_.image_stamped().time()); vision_msgs::msg::LabelInfo msg_label_info; + vision_msgs::msg::VisionClass msg_vision_class; msg_label_info.header.stamp = GetTime(); + for (auto i = 0; i < pb_seg_.class_map_size(); i++) { - vision_msgs::msg::VisionClass msg_vision_class; - auto &class_map = pb_seg_.class_map(i); msg_vision_class.class_id = class_map.class_id(); msg_vision_class.class_name = class_map.class_name(); From 16f7080583995dfaad49a75976f22fe640d24423 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Fri, 8 Mar 2024 15:44:47 +0900 Subject: [PATCH 11/12] Modify topic for label_info in segmentation_camera --- .../include/cloisim_ros_camera/camera_base.hpp | 1 + cloisim_ros_camera/src/camera_base.cpp | 6 +++--- cloisim_ros_camera/src/segmentation_camera.cpp | 3 ++- 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/cloisim_ros_camera/include/cloisim_ros_camera/camera_base.hpp b/cloisim_ros_camera/include/cloisim_ros_camera/camera_base.hpp index 1d7575c3..38f3eba0 100644 --- a/cloisim_ros_camera/include/cloisim_ros_camera/camera_base.hpp +++ b/cloisim_ros_camera/include/cloisim_ros_camera/camera_base.hpp @@ -49,6 +49,7 @@ class CameraBase : public Base protected: std::string frame_id_; + std::string topic_base_name_; // image buffer from simulator cloisim::msgs::ImageStamped pb_img_; diff --git a/cloisim_ros_camera/src/camera_base.cpp b/cloisim_ros_camera/src/camera_base.cpp index 869ebf71..05032850 100644 --- a/cloisim_ros_camera/src/camera_base.cpp +++ b/cloisim_ros_camera/src/camera_base.cpp @@ -30,7 +30,9 @@ CameraBase::CameraBase( const string namespace_) : Base(node_name, namespace_, options_) , frame_id_("camera_link") + , topic_base_name_("") { + topic_name_ = "camera"; // DBG_SIM_INFO("CameraBase"); } @@ -85,11 +87,9 @@ void CameraBase::InitializeCameraInfo() void CameraBase::InitializeCameraPublish() { - topic_name_ = "camera"; - msg_img_.header.frame_id = frame_id_; - const auto topic_base_name_ = GetPartsName() + "/" + topic_name_; + topic_base_name_ = GetPartsName() + "/" + topic_name_; image_transport::ImageTransport it(GetNode()); pub_ = it.advertiseCamera(topic_base_name_ + "/image_raw", 1); diff --git a/cloisim_ros_camera/src/segmentation_camera.cpp b/cloisim_ros_camera/src/segmentation_camera.cpp index b4c97d95..c331febc 100644 --- a/cloisim_ros_camera/src/segmentation_camera.cpp +++ b/cloisim_ros_camera/src/segmentation_camera.cpp @@ -54,7 +54,8 @@ void SegmentationCamera::InitializeCameraData() AddPublisherThread(data_bridge_ptr, bind(&SegmentationCamera::PublishData, this, std::placeholders::_1)); } - pub_labelinfo_ = create_publisher("label_info", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()); + pub_labelinfo_ = create_publisher( + topic_base_name_ + "/label_info", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()); DBG_SIM_INFO("%s hashKey: data(%s)", typeid(this).name(), hashKeyData.c_str()); } From 1c535e9fdf4ef7b35845979015301e947cbdafde Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Fri, 8 Mar 2024 17:20:47 +0900 Subject: [PATCH 12/12] Update version info in package.xml --- cloisim_ros_actor/package.xml | 2 +- cloisim_ros_base/package.xml | 2 +- cloisim_ros_bridge_zmq/package.xml | 2 +- cloisim_ros_bringup/package.xml | 2 +- cloisim_ros_bringup_param/package.xml | 2 +- cloisim_ros_camera/package.xml | 2 +- cloisim_ros_elevator_system/package.xml | 2 +- cloisim_ros_gps/package.xml | 2 +- cloisim_ros_ground_truth/package.xml | 2 +- cloisim_ros_imu/package.xml | 2 +- cloisim_ros_joint_control/package.xml | 2 +- cloisim_ros_lidar/package.xml | 2 +- cloisim_ros_micom/package.xml | 2 +- cloisim_ros_msgs/package.xml | 2 +- cloisim_ros_multicamera/package.xml | 2 +- cloisim_ros_protobuf_msgs/package.xml | 2 +- cloisim_ros_realsense/package.xml | 2 +- cloisim_ros_sonar/package.xml | 2 +- cloisim_ros_websocket_service/package.xml | 2 +- cloisim_ros_world/package.xml | 2 +- 20 files changed, 20 insertions(+), 20 deletions(-) diff --git a/cloisim_ros_actor/package.xml b/cloisim_ros_actor/package.xml index dc9fa832..d09af091 100644 --- a/cloisim_ros_actor/package.xml +++ b/cloisim_ros_actor/package.xml @@ -2,7 +2,7 @@ cloisim_ros_actor - 3.4.3 + 3.5.0 node for actor plugin Hyunseok Yang diff --git a/cloisim_ros_base/package.xml b/cloisim_ros_base/package.xml index 54163f7f..337bd35c 100644 --- a/cloisim_ros_base/package.xml +++ b/cloisim_ros_base/package.xml @@ -2,7 +2,7 @@ cloisim_ros_base - 3.4.3 + 3.5.0 CLOiSim-ROS base class for other CLOiSim-ROS Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_bridge_zmq/package.xml b/cloisim_ros_bridge_zmq/package.xml index 07718120..cd82c921 100644 --- a/cloisim_ros_bridge_zmq/package.xml +++ b/cloisim_ros_bridge_zmq/package.xml @@ -2,7 +2,7 @@ cloisim_ros_bridge_zmq - 3.4.3 + 3.5.0 bridge for cloisim(simulator) connection through ZMQ Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_bringup/package.xml b/cloisim_ros_bringup/package.xml index 749f69bd..b724be12 100644 --- a/cloisim_ros_bringup/package.xml +++ b/cloisim_ros_bringup/package.xml @@ -2,7 +2,7 @@ cloisim_ros_bringup - 3.4.3 + 3.5.0 Bringup scripts and configurations for cloisim_ros Hyunseok Yang diff --git a/cloisim_ros_bringup_param/package.xml b/cloisim_ros_bringup_param/package.xml index 5732d548..6ea0240e 100644 --- a/cloisim_ros_bringup_param/package.xml +++ b/cloisim_ros_bringup_param/package.xml @@ -2,7 +2,7 @@ cloisim_ros_bringup_param - 3.4.3 + 3.5.0 Bringup scripts and configurations for cloisim_ros Hyunseok Yang diff --git a/cloisim_ros_camera/package.xml b/cloisim_ros_camera/package.xml index 99683cff..25ec0948 100644 --- a/cloisim_ros_camera/package.xml +++ b/cloisim_ros_camera/package.xml @@ -2,7 +2,7 @@ cloisim_ros_camera - 3.4.3 + 3.5.0 Virtual camera/depth camera/segmentation camera for CLOiSim Hyunseok Yang Sungkyu Kang diff --git a/cloisim_ros_elevator_system/package.xml b/cloisim_ros_elevator_system/package.xml index 5993fbaa..2679e49b 100644 --- a/cloisim_ros_elevator_system/package.xml +++ b/cloisim_ros_elevator_system/package.xml @@ -2,7 +2,7 @@ cloisim_ros_elevator_system - 3.4.3 + 3.5.0 elevator system for simulation Sungkyu Kang diff --git a/cloisim_ros_gps/package.xml b/cloisim_ros_gps/package.xml index 3bc454e4..1b514088 100644 --- a/cloisim_ros_gps/package.xml +++ b/cloisim_ros_gps/package.xml @@ -2,7 +2,7 @@ cloisim_ros_gps - 3.4.3 + 3.5.0 virtual gps for simulation Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_ground_truth/package.xml b/cloisim_ros_ground_truth/package.xml index b93d1ba9..8ad8ed19 100644 --- a/cloisim_ros_ground_truth/package.xml +++ b/cloisim_ros_ground_truth/package.xml @@ -2,7 +2,7 @@ cloisim_ros_ground_truth - 3.4.3 + 3.5.0 world plugin to retrieve ground truth Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_imu/package.xml b/cloisim_ros_imu/package.xml index c54b58bd..808edfdd 100644 --- a/cloisim_ros_imu/package.xml +++ b/cloisim_ros_imu/package.xml @@ -2,7 +2,7 @@ cloisim_ros_imu - 3.4.3 + 3.5.0 virtual imu for simulation Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_joint_control/package.xml b/cloisim_ros_joint_control/package.xml index 55b62083..5d97e3f3 100644 --- a/cloisim_ros_joint_control/package.xml +++ b/cloisim_ros_joint_control/package.xml @@ -2,7 +2,7 @@ cloisim_ros_joint_control - 3.4.3 + 3.5.0 joint_control package for simulator Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_lidar/package.xml b/cloisim_ros_lidar/package.xml index cbec5cb9..b8bb88e0 100644 --- a/cloisim_ros_lidar/package.xml +++ b/cloisim_ros_lidar/package.xml @@ -2,7 +2,7 @@ cloisim_ros_lidar - 3.4.3 + 3.5.0 virtual lidar for simulation Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_micom/package.xml b/cloisim_ros_micom/package.xml index cfc56ef6..35f9c7dc 100644 --- a/cloisim_ros_micom/package.xml +++ b/cloisim_ros_micom/package.xml @@ -2,7 +2,7 @@ cloisim_ros_micom - 3.4.3 + 3.5.0 micom package for simulator Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_msgs/package.xml b/cloisim_ros_msgs/package.xml index 8426136f..444b86c2 100644 --- a/cloisim_ros_msgs/package.xml +++ b/cloisim_ros_msgs/package.xml @@ -2,7 +2,7 @@ cloisim_ros_msgs - 3.4.3 + 3.5.0 interfaces package for cloisim_ros Hyunseok Yang diff --git a/cloisim_ros_multicamera/package.xml b/cloisim_ros_multicamera/package.xml index 8007b76b..61820c84 100644 --- a/cloisim_ros_multicamera/package.xml +++ b/cloisim_ros_multicamera/package.xml @@ -2,7 +2,7 @@ cloisim_ros_multicamera - 3.4.3 + 3.5.0 virtual multi-camera for simulator Sungkyu Kang Hyunseok Yang diff --git a/cloisim_ros_protobuf_msgs/package.xml b/cloisim_ros_protobuf_msgs/package.xml index 4cd57272..50e1193c 100644 --- a/cloisim_ros_protobuf_msgs/package.xml +++ b/cloisim_ros_protobuf_msgs/package.xml @@ -2,7 +2,7 @@ cloisim_ros_protobuf_msgs - 3.4.3 + 3.5.0 CLOiSim-ROS interafces for communication between simulator and CLOiSim-ROS Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_realsense/package.xml b/cloisim_ros_realsense/package.xml index 748e43a5..6d8d492a 100644 --- a/cloisim_ros_realsense/package.xml +++ b/cloisim_ros_realsense/package.xml @@ -2,7 +2,7 @@ cloisim_ros_realsense - 3.4.3 + 3.5.0 virtual realsense for simulator Sungkyu Kang Hyunseok Yang diff --git a/cloisim_ros_sonar/package.xml b/cloisim_ros_sonar/package.xml index 3528c299..6e8d816d 100644 --- a/cloisim_ros_sonar/package.xml +++ b/cloisim_ros_sonar/package.xml @@ -2,7 +2,7 @@ cloisim_ros_sonar - 3.4.3 + 3.5.0 virtual sonar for simulation Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_websocket_service/package.xml b/cloisim_ros_websocket_service/package.xml index 8b612c50..5f695f6f 100644 --- a/cloisim_ros_websocket_service/package.xml +++ b/cloisim_ros_websocket_service/package.xml @@ -2,7 +2,7 @@ cloisim_ros_websocket_service - 3.4.3 + 3.5.0 websocket service for cloisim(simulator) connection port control Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_world/package.xml b/cloisim_ros_world/package.xml index 011407fd..3049f12b 100644 --- a/cloisim_ros_world/package.xml +++ b/cloisim_ros_world/package.xml @@ -2,7 +2,7 @@ cloisim_ros_world - 3.4.3 + 3.5.0 Utilities to interface with Unity through ROS. Hyunseok Yang Hyunseok Yang