From eab32660e5698ea7fd7df4137f457707fd169e93 Mon Sep 17 00:00:00 2001 From: Paul Date: Thu, 12 Sep 2024 18:40:31 -0400 Subject: [PATCH 1/4] Add realsense camera support (technology from the depths of hell) --- .gitmodules | 3 + nk_vision_pkg/CMakeLists.txt | 2 +- nk_vision_pkg/launch/nk_vision.launch.py | 139 ++++++++++++++------- nk_vision_pkg/nk_vision/pose_estimation.py | 1 + nk_vision_pkg/scripts/startup.sh | 1 + 5 files changed, 99 insertions(+), 47 deletions(-) diff --git a/.gitmodules b/.gitmodules index 65e624c..d0770e1 100644 --- a/.gitmodules +++ b/.gitmodules @@ -6,3 +6,6 @@ path = ros_aruco_opencv url = https://github.com/NASAKnights/ros_aruco_opencv branch = humble +[submodule "realsense-ros"] + path = realsense-ros + url = https://github.com/IntelRealSense/realsense-ros.git diff --git a/nk_vision_pkg/CMakeLists.txt b/nk_vision_pkg/CMakeLists.txt index 21fd300..43990a1 100644 --- a/nk_vision_pkg/CMakeLists.txt +++ b/nk_vision_pkg/CMakeLists.txt @@ -34,4 +34,4 @@ install( DESTINATION lib/${PROJECT_NAME} ) -ament_auto_package(INSTALL_TO_SHARE launch config) +ament_auto_package(INSTALL_TO_SHARE launch config scripts) diff --git a/nk_vision_pkg/launch/nk_vision.launch.py b/nk_vision_pkg/launch/nk_vision.launch.py index e55ce82..ea3f24a 100644 --- a/nk_vision_pkg/launch/nk_vision.launch.py +++ b/nk_vision_pkg/launch/nk_vision.launch.py @@ -31,9 +31,9 @@ # POSSIBILITY OF SUCH DAMAGE. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, ExecuteProcess from launch.substitutions import LaunchConfiguration -from launch.launch_description_sources import AnyLaunchDescriptionSource +from launch.launch_description_sources import AnyLaunchDescriptionSource, PythonLaunchDescriptionSource import launch_ros.actions from ament_index_python.packages import get_package_share_directory @@ -44,58 +44,105 @@ def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time', default='false') return LaunchDescription([ - DeclareLaunchArgument( - 'use_sim_time', - default_value='false', - description='Use simulation clock if true'), + # DeclareLaunchArgument( + # 'use_sim_time', + # default_value='false', + # description='Use simulation clock if true'), - launch_ros.actions.Node( - package='image_publisher', executable='image_publisher_node', output='screen', - arguments=[device_0], - parameters=[{'use_sim_time': use_sim_time, - 'filename': device_0, - 'publish_rate': 50.0, - 'camera_info_url': 'package://nk_vision/config/arducam.yaml', - 'frame_id': 'camera_2'}], - remappings=[('image_raw', '/camera_2/image_raw'), - ('camera_info', '/camera_2/camera_info')]), + # launch_ros.actions.Node( + # package='image_publisher', executable='image_publisher_node', output='screen', + # arguments=[device_0], + # parameters=[{'use_sim_time': use_sim_time, + # 'filename': device_0, + # 'publish_rate': 50.0, + # 'camera_info_url': 'package://nk_vision/config/arducam.yaml', + # 'frame_id': 'camera_2'}], + # remappings=[('image_raw', '/camera_2/image_raw'), + # ('camera_info', '/camera_2/camera_info')]),g - launch_ros.actions.Node( - package='image_publisher', executable='image_publisher_node', output='screen', - arguments=[device_1], - parameters=[{'use_sim_time': use_sim_time, - 'publish_rate': 50.0, - 'filename': device_1, - 'camera_info_url': 'package://nk_vision/config/arducam.yaml', - 'frame_id': 'camera_1'}], - remappings=[('image_raw', '/camera_1/image_raw'), - ('camera_info', '/camera_1/camera_info')]), + # launch_ros.actions.Node( + # package='image_publisher', executable='image_publisher_node', output='screen', + # arguments=[device_1], + # parameters=[{'use_sim_time': use_sim_time, + # 'publish_rate': 50.0, + # 'filename': device_1, + # 'camera_info_url': 'package://nk_vision/config/arducam.yaml', + # 'frame_id': 'camera_1'}], + # remappings=[('image_raw', '/camera_1/image_raw'), + # ('camera_info', '/camera_1/camera_info')]), - launch_ros.actions.Node( - package='rviz2', executable='rviz2', output='screen', - arguments=['-d', get_package_share_directory('nk_vision') + '/config/config_file.rviz'] - ), + # launch_ros.actions.Node( + # package='rviz2', executable='rviz2', output='screen', + # arguments=['-d', get_package_share_directory('nk_vision') + '/config/config_file.rviz'] + # ), - IncludeLaunchDescription(AnyLaunchDescriptionSource( - get_package_share_directory('nk_vision') + '/launch/aruco_tracker_1.launch.xml')), + # IncludeLaunchDescription(AnyLaunchDescriptionSource( + # get_package_share_directory('nk_vision') + '/launch/aruco_tracker_1.launch.xml')), - IncludeLaunchDescription(AnyLaunchDescriptionSource( - get_package_share_directory('nk_vision') + '/launch/aruco_tracker_2.launch.xml')), + # IncludeLaunchDescription(AnyLaunchDescriptionSource( + # get_package_share_directory('nk_vision') + '/launch/aruco_tracker_2.launch.xml')), - IncludeLaunchDescription(AnyLaunchDescriptionSource( - get_package_share_directory('frc_2024_field_description') + '/launch/main.launch.py')), + # IncludeLaunchDescription(AnyLaunchDescriptionSource( + # get_package_share_directory('frc_2024_field_description') + '/launch/main.launch.py')), - IncludeLaunchDescription(AnyLaunchDescriptionSource( - get_package_share_directory('robot_2024_description') + '/launch/main.launch.py')), + # IncludeLaunchDescription(AnyLaunchDescriptionSource( + # get_package_share_directory('robot_2024_description') + '/launch/main.launch.py')), - # launch_ros.actions.Node( - # package='nk_vision', executable='pose_estimation.py', output='screen'), + # # launch_ros.actions.Node( + # # package='nk_vision', executable='pose_estimation.py', output='screen'), - launch_ros.actions.Node( - package='nk_vision', executable='tf2network_table.py', output='screen', - parameters=[{'transfer_topics': ["base_link_1", "base_link_2", "base_link_3"]}]), + # launch_ros.actions.Node( + # package='nk_vision', executable='tf2network_table.py', output='screen', + # parameters=[{'transfer_topics': ["base_link_1", "base_link_2", "base_link_3"]}]), - launch_ros.actions.Node( - package='nk_vision', executable='network_table2tf.py', output='screen', - parameters=[{'transfer_topics': ["base_link"]}]), + # launch_ros.actions.Node( + # package='nk_vision', executable='network_table2tf.py', output='screen', + # parameters=[{'transfer_topics': ["base_link"]}]), + + # ExecuteProcess( + # cmd=['ros2', 'bag', 'record', + # '/tf', + # '/camera_1/robot_description', + # '/camera_2/robot_description', + # '/robot_description', + # '/joint_states', + # '/tf', + # '/field/robot_description', + # '-o', + # '/home/nasa-knights/vision_logs/data'], + # output='screen' + # ), + + IncludeLaunchDescription(PythonLaunchDescriptionSource( + get_package_share_directory('realsense2_camera') + '/launch/rs_launch.py'), + launch_arguments=[('camera_name', 'rs_cam_1'), + # ("serial_no", "'036222071448'"), + ('enable_color', 'true'), + ('rgb_camera.color_format', 'RGB8'), + ('rgb_camera.enable_auto_exposure', 'true'), + ('enable_depth', 'true'), + ('enable_infra', 'false'), + ('enable_infra1', 'false'), + ('enable_infra2', 'false'), + ('depth_module.depth_profile', '640,480,15'), + ('depth_module.depth_format', 'Z16'), + ('depth_module.infra_format', 'RGB8'), + ('depth_module.hdr_enabled', 'false'), + ('depth_module.enable_auto_exposure', 'true'), + ('enable_sync', 'true'), + ('enable_rgbd', 'true'), + # ('enable_gyro'), + # ('enable_accel'), + # ('clip_distance'), + ('publish_tf', 'true'), + ('tf_publish_rate', '10.0'), + ('pointcloud.enable', 'true'), + ('align_depth.enable', 'true'), + ('decimation_filter.enable', 'false'), + ('spatial_filter.enable', 'true'), + ('temporal_filter.enable', 'true'), + ('disparity_filter.enable', 'false'), + ('hole_filling_filter.enable', 'false'), + ('reconnect_timeout', '30.0' )] + ) ]) diff --git a/nk_vision_pkg/nk_vision/pose_estimation.py b/nk_vision_pkg/nk_vision/pose_estimation.py index e90789a..3164b71 100755 --- a/nk_vision_pkg/nk_vision/pose_estimation.py +++ b/nk_vision_pkg/nk_vision/pose_estimation.py @@ -80,6 +80,7 @@ def find_avg_pose(self): if (len(self.pose_estimates) != 0 ): self.get_logger().info('Recent Markers Found, fusing', throttle_duration_sec = 1.0) self.find_avg_position() + self.pose_sources self.find_avg_orientation() self.pose.position.x = self.avg_position[0] self.pose.position.y = self.avg_position[1] diff --git a/nk_vision_pkg/scripts/startup.sh b/nk_vision_pkg/scripts/startup.sh index d0f6f98..cd4a46b 100644 --- a/nk_vision_pkg/scripts/startup.sh +++ b/nk_vision_pkg/scripts/startup.sh @@ -1,3 +1,4 @@ #!/bin/bash +cd /home//home/nasa-knights/vision_logs source /vision_ws/install/setup.bash ros2 launch nk_vision nk_vision.launch.py > /vision_ws/log/recent.log \ No newline at end of file From 6df07570895565a39a32e4ad95694cf6755cc93e Mon Sep 17 00:00:00 2001 From: Thanco002 Date: Thu, 12 Sep 2024 19:05:11 -0400 Subject: [PATCH 2/4] Fixed git submodules --- .gitmodules | 2 +- realsense-ros | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) create mode 160000 realsense-ros diff --git a/.gitmodules b/.gitmodules index d0770e1..b40649d 100644 --- a/.gitmodules +++ b/.gitmodules @@ -8,4 +8,4 @@ branch = humble [submodule "realsense-ros"] path = realsense-ros - url = https://github.com/IntelRealSense/realsense-ros.git + url = https://github.com/IntelRealSense/realsense-ros diff --git a/realsense-ros b/realsense-ros new file mode 160000 index 0000000..1cbd81b --- /dev/null +++ b/realsense-ros @@ -0,0 +1 @@ +Subproject commit 1cbd81be81e807eefb46f098e76381888ffc7001 From 9f2f319694bf21c7c867825cca583b8ffe53bc19 Mon Sep 17 00:00:00 2001 From: Thanco002 Date: Mon, 23 Sep 2024 19:51:10 -0400 Subject: [PATCH 3/4] Added Node with visualization, Needs Optimization --- .gitignore | 2 + .vscode/build.sh | 10 + .vscode/c_cpp_properties.json | 18 ++ .vscode/launch.json | 90 +++++++ .vscode/tasks.json | 254 ++++++++++++++++++ icp_obj_pose/CMakeLists.txt | 39 +++ .../include/icp_obj_pose/icp_obj_pose.hpp | 146 ++++++++++ icp_obj_pose/launch/obj_detect.launch.py | 69 +++++ icp_obj_pose/package.xml | 29 ++ icp_obj_pose/src/icp_obj_pose.cpp | 1 + install-ros.bash | 2 +- 11 files changed, 659 insertions(+), 1 deletion(-) create mode 100755 .vscode/build.sh create mode 100644 .vscode/c_cpp_properties.json create mode 100644 .vscode/launch.json create mode 100644 .vscode/tasks.json create mode 100644 icp_obj_pose/CMakeLists.txt create mode 100644 icp_obj_pose/include/icp_obj_pose/icp_obj_pose.hpp create mode 100755 icp_obj_pose/launch/obj_detect.launch.py create mode 100644 icp_obj_pose/package.xml create mode 100644 icp_obj_pose/src/icp_obj_pose.cpp diff --git a/.gitignore b/.gitignore index 46952c4..439333b 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,5 @@ **/build +**/install +**/log .vscode/settings.json diff --git a/.vscode/build.sh b/.vscode/build.sh new file mode 100755 index 0000000..071b3f5 --- /dev/null +++ b/.vscode/build.sh @@ -0,0 +1,10 @@ +#!/bin/bash +set -e +source /opt/ros/humble/setup.bash +# Set the default build type +BUILD_TYPE=RelWithDebInfo +colcon build \ + --merge-install \ + --symlink-install \ + --cmake-args "-DCMAKE_BUILD_TYPE=$BUILD_TYPE" "-DCMAKE_EXPORT_COMPILE_COMMANDS=On" \ + -Wall -Wextra -Wpedantic \ No newline at end of file diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..ad4d7af --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,18 @@ +{ + "configurations": [ + { + "name": "Linux", + "includePath": [ + "${workspaceFolder}/**", + "/opt/ros/humble/include/**" + ], + "defines": [], + "compilerPath": "/usr/bin/gcc", + "compileCommands": "${workspaceFolder}/build/compile_commands.json", + "cStandard": "c99", + "cppStandard": "c++17", + "intelliSenseMode": "linux-gcc-x64" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..5ef2724 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,90 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + // Example launch of a python file + { + "name": "Python: Current File", + "type": "debugpy", + "request": "launch", + "program": "${file}", + "console": "integratedTerminal", + }, + // Example gdb launch of a ros executable + { + "name": "(gdb) Launch (merge-install)", + "type": "cppdbg", + "request": "launch", + "program": "${workspaceFolder}/install/lib/${input:package}/${input:program}", + "args": [], + "preLaunchTask": "build", + "stopAtEntry": true, + "cwd": "${workspaceFolder}", + "externalConsole": false, + "MIMode": "gdb", + "setupCommands": [ + { + "description": "Enable pretty-printing for gdb", + "text": "-enable-pretty-printing", + "ignoreFailures": true + } + ] + }, + { + "name": "(gdb) Launch (isolated-install)", + "type": "cppdbg", + "request": "launch", + "program": "${workspaceFolder}/install/${input:package}/lib/${input:package}/${input:program}", + "args": [], + "preLaunchTask": "build", + "stopAtEntry": true, + "cwd": "${workspaceFolder}", + "externalConsole": false, + "MIMode": "gdb", + "setupCommands": [ + { + "description": "Enable pretty-printing for gdb", + "text": "-enable-pretty-printing", + "ignoreFailures": true + } + ] + }, + //Example of a ROS Launch file + { + "name": "ROS: Launch File (merge-install)", + "type": "ros", + "request": "launch", + "preLaunchTask": "build", + "target": "${workspaceFolder}/install/share/${input:package}/launch/${input:ros_launch}", + }, + { + "name": "ROS: Launch File (isolated-install)", + "type": "ros", + "request": "launch", + "preLaunchTask": "build", + "target": "${workspaceFolder}/install/${input:package}/share/${input:package}/launch/${input:ros_launch}", + }, + ], + "inputs": [ + { + "id": "package", + "type": "promptString", + "description": "Package name", + "default": "examples_rclcpp_minimal_publisher" + }, + { + "id": "program", + "type": "promptString", + "description": "Program name", + "default": "publisher_member_function" + }, + { + "id": "ros_launch", + "type": "promptString", + "description": "ROS launch name", + "default": "file_name_launch.py" + } + ] + } \ No newline at end of file diff --git a/.vscode/tasks.json b/.vscode/tasks.json new file mode 100644 index 0000000..6644084 --- /dev/null +++ b/.vscode/tasks.json @@ -0,0 +1,254 @@ +{ + // See https://go.microsoft.com/fwlink/?LinkId=733558 + // for the documentation about the tasks.json format + "version": "2.0.0", + "tasks": [ + // Build tasks + { + "label": "build", + "detail": "Build workspace (default)", + "type": "shell", + "command": ".vscode/build.sh", + "group": { + "kind": "build", + "isDefault": true + }, + "problemMatcher": "$gcc" + }, + { + "label": "debug", + "detail": "Build workspace (debug)", + "type": "shell", + "command": ".vscode/build.sh", + "options": { + "env": { + "BUILD_TYPE": "Debug" + } + }, + "group": "build", + "problemMatcher": "$gcc" + }, + // Test tasks + { + "label": "test", + "detail": "Run all unit tests and show results.", + "type": "shell", + "command": "./test.sh", + "group": { + "kind": "test", + "isDefault": true + } + }, + // Clean + { + "label": "clean", + "detail": "Run the clean target", + "type": "shell", + "command": "colcon build --cmake-target clean", + "problemMatcher": "$gcc" + }, + { + "label": "purge", + "detail": "Purge workspace by deleting all generated files.", + "type": "shell", + "command": "sudo rm -fr build install log; sudo py3clean .", + "problemMatcher": [] + }, + { + "label": "source", + "detail": "Source workspace", + "type": "shell", + "command": "source /opt/ros/humble/setup.bash", + "problemMatcher": [] + }, + // Linting and static code analysis tasks + { + "label": "fix", + "detail": "Reformat files with uncrustify.", + "type": "shell", + "command": "ament_uncrustify --reformat nk_ros_pkgs/", + "problemMatcher": [] + }, + { + "label": "uncrustify", + "detail": "Lint files with uncrustify.", + "type": "shell", + "command": "ament_uncrustify nk_ros_pkgs/", + "presentation": { + "panel": "dedicated", + "reveal": "silent", + "clear": true + }, + "problemMatcher": [ + { + "owner": "uncrustify", + "source": "uncrustify", + "fileLocation": "relative", + "pattern": [ + // just the file name message + { + "regexp": "^(.*)'(.*)':", + "kind": "file", + "file": 2, + "message": 1 + } + ] + } + ] + }, + { + "label": "cpplint", + "detail": "Lint files with cpplint.", + "type": "ament", + "task": "cpplint", + "path": "nk_ros_pkgs/", + "problemMatcher": "$ament_cpplint", + "presentation": { + "panel": "dedicated", + "reveal": "silent", + "clear": true + }, + }, + { + "label": "cppcheck", + "detail": "Run static code checker cppcheck.", + "type": "ament", + "task": "cppcheck", + "path": "nk_ros_pkgs/", + "problemMatcher": "$ament_cppcheck", + "presentation": { + "panel": "dedicated", + "reveal": "silent", + "clear": true + }, + "options": { + "env": { + "AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS": "1" + } + } + }, + { + "label": "lint_cmake", + "detail": "Run lint on cmake files.", + "type": "ament", + "task": "lint_cmake", + "path": "nk_ros_pkgs/", + "problemMatcher": "$ament_lint_cmake", + "presentation": { + "panel": "dedicated", + "reveal": "silent", + "clear": true + } + }, + { + "label": "flake8", + "detail": "Run flake8 on python files.", + "type": "ament", + "task": "flake8", + "path": "nk_ros_pkgs/", + "problemMatcher": "$ament_flake8", + "presentation": { + "panel": "dedicated", + "reveal": "silent", + "clear": true + } + }, + { + "label": "pep257", + "detail": "Run pep257 on python files.", + "type": "ament", + "task": "pep257", + "path": "nk_ros_pkgs/", + "problemMatcher": "$ament_pep257", + "presentation": { + "panel": "dedicated", + "reveal": "silent", + "clear": true + } + }, + { + "label": "xmllint", + "detail": "Run xmllint on xml files.", + "type": "ament", + "task": "xmllint", + "path": "nk_ros_pkgs/", + "problemMatcher": "$ament_xmllint", + "presentation": { + "panel": "dedicated", + "reveal": "silent", + "clear": true + } + }, + { + "label": "lint all", + "detail": "Run all linters.", + "dependsOn": [ + "cppcheck", + "cpplint", + "flake8", + "lint_cmake", + "pep257", + "xmllint", + "uncrustify" + ], + "problemMatcher": [] + }, + // Workspace editing tasks + { + "label": "new ament_cmake package", + "detail": "Create a new ROS cpp package from a template.", + "type": "shell", + "command": "ros2 pkg create --destination-directory nk_ros_pkgs --build-type ament_cmake ${input:package}", + "problemMatcher": [] + }, + { + "label": "new ament_python package", + "detail": "Create a new ROS python package from a template.", + "type": "shell", + "command": "ros2 pkg create --destination-directory nk_ros_pkgs --build-type ament_python ${input:package}", + "problemMatcher": [] + }, + { + "label": "import from workspace file", + "detail": "Use vcs to import modules specified by a workspace/rosinstall file.", + "type": "shell", + "command": "vcs import < nk_ros_pkgs/ros2.repos nk_ros_pkgs", + "problemMatcher": [] + }, + { + "label": "update workspace file", + "detail": "Use vcs to update repositories in nk_ros_pkgs to workspace file.", + "type": "shell", + "command": "vcs export nk_ros_pkgs > nk_ros_pkgs/ros2.repos", + "problemMatcher": [] + }, + { + "label": "install dependencies", + "detail": "Install all dependencies specified in the workspaces package.xml files.", + "type": "shell", + "command": "sudo apt-get update && rosdep update && rosdep install --from-paths nk_ros_pkgs --ignore-nk_ros_pkgs -y", + "problemMatcher": [] + }, + { + "label": "setup", + "detail": "Set up the workspace", + "type": "shell", + "command": "./setup.sh", + "problemMatcher": [] + }, + { + "label": "add submodules from .repos", + "detail": "Create a git submodule for all repositories in your .repos file", + "type": "shell", + "command": "python3 .devcontainer/repos_to_submodules.py", + "problemMatcher": [] + } + ], + "inputs": [ + { + "id": "package", + "type": "promptString", + "description": "Package name" + } + ] +} \ No newline at end of file diff --git a/icp_obj_pose/CMakeLists.txt b/icp_obj_pose/CMakeLists.txt new file mode 100644 index 0000000..a2f687a --- /dev/null +++ b/icp_obj_pose/CMakeLists.txt @@ -0,0 +1,39 @@ +cmake_minimum_required(VERSION 3.5) +project(icp_obj_pose) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +find_package(rclcpp REQUIRED) +# find_package(rclpy REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(PCL REQUIRED) +find_package(pcl_conversions REQUIRED) + + +add_executable(icp_obj_pose src/icp_obj_pose.cpp) +target_include_directories(icp_obj_pose + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/include +) + +target_link_libraries(icp_obj_pose + rclcpp::rclcpp + ${sensor_msgs_TARGETS} + ${PCL_LIBRARIES} +) + +install(TARGETS icp_obj_pose + DESTINATION lib/${PROJECT_NAME}) + +ament_target_dependencies(icp_obj_pose rclcpp sensor_msgs PCL pcl_conversions) + +ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/icp_obj_pose/include/icp_obj_pose/icp_obj_pose.hpp b/icp_obj_pose/include/icp_obj_pose/icp_obj_pose.hpp new file mode 100644 index 0000000..889bd10 --- /dev/null +++ b/icp_obj_pose/include/icp_obj_pose/icp_obj_pose.hpp @@ -0,0 +1,146 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class ICPNode : public rclcpp::Node { +public: + ICPNode() : Node("icp_node") { + // Declare parameters + this->declare_parameter("input_topic", "/input_pointcloud"); + this->declare_parameter("reference_ply_filepath", "/path/to/your/file.ply"); + + // Retrieve parameters + std::string input_topic, file_path; + this->get_parameter("input_topic", input_topic); + this->get_parameter("reference_ply_filepath", file_path); + + // Publishers + // auto qos = rclcpp::QoS::transient_local; + pub_target_ = this->create_publisher("target_pointcloud", 10); + pub_source_ = this->create_publisher("source_pointcloud", 10); + pub_aligned_ = this->create_publisher("aligned_pointcloud", 10); + pub_transform_ = this->create_publisher("icp_transform", 10); + + // Subscriber to input point cloud + sub_ = this->create_subscription( + input_topic, 10, std::bind(&ICPNode::pointCloudCallback, this, std::placeholders::_1) + ); + + // Load the target point cloud from the .ply file + target_cloud_.reset(new pcl::PointCloud()); + if (pcl::io::loadPLYFile(file_path, *target_cloud_) == -1) { + RCLCPP_ERROR(this->get_logger(), "Failed to load .ply file: %s", file_path.c_str()); + return; + } + RCLCPP_INFO(this->get_logger(), "Initialized ICPNode, Subscribed to topic %s", input_topic.c_str()); + } + +private: + // Subscriber + rclcpp::Subscription::SharedPtr sub_; + + // Publishers for visualizing point clouds and transformations + rclcpp::Publisher::SharedPtr pub_source_; + rclcpp::Publisher::SharedPtr pub_target_; + rclcpp::Publisher::SharedPtr pub_aligned_; + rclcpp::Publisher::SharedPtr pub_transform_; + + // Point clouds + pcl::PointCloud::Ptr target_cloud_; + pcl::PointCloud::Ptr source_cloud_; + pcl::IterativeClosestPoint icp_; + + // Callback function for receiving the source point cloud + void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "Started CALL"); + // Convert ROS PointCloud2 message to PCL point cloud + source_cloud_.reset(new pcl::PointCloud()); + pcl::fromROSMsg(*msg, *source_cloud_); + pcl::VoxelGrid voxel_grid; + pcl::PointCloud::Ptr filtered_cloud(new pcl::PointCloud); + voxel_grid.setInputCloud(source_cloud_); + voxel_grid.setLeafSize(0.05f, 0.05f, 0.05f); // Adjust voxel size if necessary + voxel_grid.filter(*filtered_cloud); + + + // Publish the source point cloud for visualization + pub_source_->publish(*msg); + RCLCPP_INFO(this->get_logger(), "MID CALL"); + + // Perform ICP + pcl::PointCloud::Ptr aligned_cloud(new pcl::PointCloud()); + icp_.setInputSource(filtered_cloud); + icp_.setMaximumIterations(1); + icp_.setInputTarget(target_cloud_); + icp_.align(*aligned_cloud); + RCLCPP_INFO(this->get_logger(), "MId-Late CALL"); + + // if (icp_.hasConverged()) { + // // Publish the aligned point cloud + // sensor_msgs::msg::PointCloud2 aligned_msg; + // pcl::toROSMsg(*aligned_cloud, aligned_msg); + // aligned_msg.header.frame_id = "map"; + // pub_aligned_->publish(aligned_msg); + + // // Extract and publish the transformation matrix as a PoseStamped + // Eigen::Matrix4f transformation = icp_.getFinalTransformation(); + // publishTransform(transformation); + // } else { + // RCLCPP_WARN(this->get_logger(), "ICP did not converge."); + // } + + // Publish the target point cloud for visualization + sensor_msgs::msg::PointCloud2::SharedPtr target_msg, aligned_msg, filtered_msg; + target_msg.reset(new sensor_msgs::msg::PointCloud2()); + aligned_msg.reset(new sensor_msgs::msg::PointCloud2()); + filtered_msg.reset(new sensor_msgs::msg::PointCloud2()); + + pcl::toROSMsg(*aligned_cloud, *aligned_msg); + aligned_msg->header.frame_id = "world"; + pub_target_->publish(*aligned_msg); + target_msg->header.frame_id = "world"; + pcl::toROSMsg(*target_cloud_, *target_msg); + pub_target_->publish(*target_msg); + + filtered_msg->header.frame_id = "world"; + pcl::toROSMsg(*filtered_cloud, *filtered_msg); + pub_target_->publish(*filtered_msg); + + RCLCPP_INFO(this->get_logger(), "Ran ICP CALL"); + } + + // Helper function to publish the transformation matrix as PoseStamped + void publishTransform(const Eigen::Matrix4f& transformation) { + geometry_msgs::msg::PoseStamped pose_msg; + pose_msg.header.frame_id = "map"; + pose_msg.header.stamp = this->get_clock()->now(); + + // Extract translation and rotation from the transformation matrix + pose_msg.pose.position.x = transformation(0, 3); + pose_msg.pose.position.y = transformation(1, 3); + pose_msg.pose.position.z = transformation(2, 3); + + Eigen::Matrix3f rotation_matrix = transformation.block<3, 3>(0, 0); + Eigen::Quaternionf quaternion(rotation_matrix); + pose_msg.pose.orientation.x = quaternion.x(); + pose_msg.pose.orientation.y = quaternion.y(); + pose_msg.pose.orientation.z = quaternion.z(); + pose_msg.pose.orientation.w = quaternion.w(); + + pub_transform_->publish(pose_msg); + } +}; + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/icp_obj_pose/launch/obj_detect.launch.py b/icp_obj_pose/launch/obj_detect.launch.py new file mode 100755 index 0000000..ccb2923 --- /dev/null +++ b/icp_obj_pose/launch/obj_detect.launch.py @@ -0,0 +1,69 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import AnyLaunchDescriptionSource, PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + return LaunchDescription([ + # Declare launch arguments + DeclareLaunchArgument( + 'input_topic', + default_value='/camera/rs_cam_1/depth/color/points', + description='Input PointCloud2 topic' + ), + DeclareLaunchArgument( + 'reference_ply_filepath', + default_value='/path/to/your/file.ply', + description='Path to the .ply file' + ), + + # Node action for icp_node + Node( + package='icp_obj_pose', + executable='icp_obj_pose', + name='icp_node', + output='screen', + parameters=[{ + 'input_topic': LaunchConfiguration('input_topic'), + 'reference_ply_filepath': LaunchConfiguration('reference_ply_filepath') + }], + remappings=[('/input_pointcloud', LaunchConfiguration('input_topic'))] + ), + IncludeLaunchDescription(PythonLaunchDescriptionSource( + get_package_share_directory('realsense2_camera') + '/launch/rs_launch.py'), + launch_arguments=[ + ('camera_name', 'rs_cam_1'), + # ("serial_no", "\'036222071448\'"), + ('enable_color', 'true'), + ('rgb_camera.color_format', 'RGB8'), + ('rgb_camera.enable_auto_exposure', 'true'), + ('enable_depth', 'true'), + ('enable_infra', 'false'), + ('enable_infra1', 'false'), + ('enable_infra2', 'false'), + ('depth_module.depth_profile', '640,480,15'), + ('depth_module.depth_format', 'Z16'), + ('depth_module.infra_format', 'RGB8'), + ('depth_module.hdr_enabled', 'false'), + ('depth_module.enable_auto_exposure', 'true'), + ('enable_sync', 'true'), + ('enable_rgbd', 'true'), + # ('enable_gyro'), + # ('enable_accel'), + # ('clip_distance'), + ('publish_tf', 'true'), + ('tf_publish_rate', '10.0'), + ('pointcloud.enable', 'true'), + ('align_depth.enable', 'true'), + ('decimation_filter.enable', 'false'), + ('spatial_filter.enable', 'true'), + ('temporal_filter.enable', 'true'), + ('disparity_filter.enable', 'false'), + ('hole_filling_filter.enable', 'false'), + ('reconnect_timeout', '30.0' ) + ] + ) + ]) \ No newline at end of file diff --git a/icp_obj_pose/package.xml b/icp_obj_pose/package.xml new file mode 100644 index 0000000..01d055e --- /dev/null +++ b/icp_obj_pose/package.xml @@ -0,0 +1,29 @@ + + + + icp_obj_pose + 0.1.0 + + FRC Team 122 Vision setup + + + Tristan Hancock + + BSD + Tristan Hancock + + ament_cmake_auto + + rclcpp + rclcpp_components + sensor_msgs + PCL + pcl_conversions + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/icp_obj_pose/src/icp_obj_pose.cpp b/icp_obj_pose/src/icp_obj_pose.cpp new file mode 100644 index 0000000..958cdc8 --- /dev/null +++ b/icp_obj_pose/src/icp_obj_pose.cpp @@ -0,0 +1 @@ +#include "icp_obj_pose/icp_obj_pose.hpp" \ No newline at end of file diff --git a/install-ros.bash b/install-ros.bash index 1f16378..e6dda04 100755 --- a/install-ros.bash +++ b/install-ros.bash @@ -24,7 +24,7 @@ sudo apt upgrade -y sudo apt install ros-humble-desktop -y -sudo apt install ros-humble-aruco ros-humble-usb-cam ros-humble-image-pipeline python3-pip -y +sudo apt install ros-humble-aruco ros-humble-usb-cam ros-humble-image-pipeline ros-humble-diagnostic-updater python3-pip -y pip install numpy==1.23.0 # Default version breaks with dependent packages From cb6f8d7ed483c47e042bcab80da5c27fd9fcca1a Mon Sep 17 00:00:00 2001 From: Thanco002 Date: Mon, 30 Sep 2024 19:40:13 -0400 Subject: [PATCH 4/4] Fixed the depth camera outputs (now outputs data to topics) --- .../include/icp_obj_pose/icp_obj_pose.hpp | 59 ++++++++++--------- 1 file changed, 30 insertions(+), 29 deletions(-) diff --git a/icp_obj_pose/include/icp_obj_pose/icp_obj_pose.hpp b/icp_obj_pose/include/icp_obj_pose/icp_obj_pose.hpp index 889bd10..c949653 100644 --- a/icp_obj_pose/include/icp_obj_pose/icp_obj_pose.hpp +++ b/icp_obj_pose/include/icp_obj_pose/icp_obj_pose.hpp @@ -23,7 +23,7 @@ class ICPNode : public rclcpp::Node { // Publishers // auto qos = rclcpp::QoS::transient_local; - pub_target_ = this->create_publisher("target_pointcloud", 10); + pub_object_ = this->create_publisher("object_pointcloud", 10); pub_source_ = this->create_publisher("source_pointcloud", 10); pub_aligned_ = this->create_publisher("aligned_pointcloud", 10); pub_transform_ = this->create_publisher("icp_transform", 10); @@ -34,12 +34,22 @@ class ICPNode : public rclcpp::Node { ); // Load the target point cloud from the .ply file - target_cloud_.reset(new pcl::PointCloud()); - if (pcl::io::loadPLYFile(file_path, *target_cloud_) == -1) { + object_cloud_.reset(new pcl::PointCloud()); + if (pcl::io::loadPLYFile(file_path, *object_cloud_) == -1) { RCLCPP_ERROR(this->get_logger(), "Failed to load .ply file: %s", file_path.c_str()); + throw std::runtime_error("PCL FAILED TO LOAD"); return; } + RCLCPP_INFO(this->get_logger(), "Initialized ICPNode, Subscribed to topic %s", input_topic.c_str()); + pcl::VoxelGrid voxel_grid; + pcl::PointCloud::Ptr filtered_cloud(new pcl::PointCloud); + voxel_grid.setInputCloud(object_cloud_); + voxel_grid.setLeafSize(0.025f, 0.025f, 0.025f); // Adjust voxel size if necessary + voxel_grid.filter(*filtered_cloud); + RCLCPP_INFO(this->get_logger(), "Object has points: %d", object_cloud_->size()); + RCLCPP_INFO(this->get_logger(), "Voxelized filter object has points: %d", filtered_cloud->size()); + object_cloud_ = filtered_cloud; } private: @@ -48,39 +58,32 @@ class ICPNode : public rclcpp::Node { // Publishers for visualizing point clouds and transformations rclcpp::Publisher::SharedPtr pub_source_; - rclcpp::Publisher::SharedPtr pub_target_; + rclcpp::Publisher::SharedPtr pub_object_; rclcpp::Publisher::SharedPtr pub_aligned_; rclcpp::Publisher::SharedPtr pub_transform_; // Point clouds - pcl::PointCloud::Ptr target_cloud_; + pcl::PointCloud::Ptr object_cloud_; pcl::PointCloud::Ptr source_cloud_; pcl::IterativeClosestPoint icp_; // Callback function for receiving the source point cloud void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { - RCLCPP_INFO(this->get_logger(), "Started CALL"); // Convert ROS PointCloud2 message to PCL point cloud source_cloud_.reset(new pcl::PointCloud()); pcl::fromROSMsg(*msg, *source_cloud_); pcl::VoxelGrid voxel_grid; pcl::PointCloud::Ptr filtered_cloud(new pcl::PointCloud); voxel_grid.setInputCloud(source_cloud_); - voxel_grid.setLeafSize(0.05f, 0.05f, 0.05f); // Adjust voxel size if necessary + voxel_grid.setLeafSize(0.02f, 0.02f, 0.02f); // Adjust voxel size if necessary voxel_grid.filter(*filtered_cloud); - - // Publish the source point cloud for visualization - pub_source_->publish(*msg); - RCLCPP_INFO(this->get_logger(), "MID CALL"); - // Perform ICP pcl::PointCloud::Ptr aligned_cloud(new pcl::PointCloud()); - icp_.setInputSource(filtered_cloud); - icp_.setMaximumIterations(1); - icp_.setInputTarget(target_cloud_); + icp_.setInputSource(object_cloud_); + icp_.setMaximumIterations(10); + icp_.setInputTarget(filtered_cloud); icp_.align(*aligned_cloud); - RCLCPP_INFO(this->get_logger(), "MId-Late CALL"); // if (icp_.hasConverged()) { // // Publish the aligned point cloud @@ -97,23 +100,21 @@ class ICPNode : public rclcpp::Node { // } // Publish the target point cloud for visualization - sensor_msgs::msg::PointCloud2::SharedPtr target_msg, aligned_msg, filtered_msg; - target_msg.reset(new sensor_msgs::msg::PointCloud2()); + sensor_msgs::msg::PointCloud2::SharedPtr object_msg, aligned_msg; + object_msg.reset(new sensor_msgs::msg::PointCloud2()); aligned_msg.reset(new sensor_msgs::msg::PointCloud2()); - filtered_msg.reset(new sensor_msgs::msg::PointCloud2()); + + pcl::toROSMsg(*filtered_cloud, *object_msg); + object_msg->header.frame_id = "world"; + pub_object_->publish(*object_msg); pcl::toROSMsg(*aligned_cloud, *aligned_msg); aligned_msg->header.frame_id = "world"; - pub_target_->publish(*aligned_msg); - target_msg->header.frame_id = "world"; - pcl::toROSMsg(*target_cloud_, *target_msg); - pub_target_->publish(*target_msg); - - filtered_msg->header.frame_id = "world"; - pcl::toROSMsg(*filtered_cloud, *filtered_msg); - pub_target_->publish(*filtered_msg); - - RCLCPP_INFO(this->get_logger(), "Ran ICP CALL"); + pub_aligned_->publish(*aligned_msg); + + + publishTransform(icp_.transformation_); + } // Helper function to publish the transformation matrix as PoseStamped