From c849b7e3acb711a2bfd487ef95fef9225a31bfed Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Sun, 30 Jun 2024 04:07:18 -0400 Subject: [PATCH] cv --- ...ter_vision_instance_segmentation.launch.py | 78 ++++++++++++ ...mputer_vision_instance_segmentation.launch | 30 +++++ ...d_computer_vision_instance_segmentation.py | 111 ++++++++++++++++++ ...nde_3d_computer_vision_object_detection.py | 2 +- 4 files changed, 220 insertions(+), 1 deletion(-) create mode 100644 ros_web_api_bellande_3d_computer_vision/launch/bellande_3d_computer_vision_instance_segmentation.launch.py create mode 100644 ros_web_api_bellande_3d_computer_vision/launch/ros1/bellande_3d_computer_vision_instance_segmentation.launch create mode 100755 ros_web_api_bellande_3d_computer_vision/src/bellande_3d_computer_vision_instance_segmentation.py diff --git a/ros_web_api_bellande_3d_computer_vision/launch/bellande_3d_computer_vision_instance_segmentation.launch.py b/ros_web_api_bellande_3d_computer_vision/launch/bellande_3d_computer_vision_instance_segmentation.launch.py new file mode 100644 index 0000000..3c63927 --- /dev/null +++ b/ros_web_api_bellande_3d_computer_vision/launch/bellande_3d_computer_vision_instance_segmentation.launch.py @@ -0,0 +1,78 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import os +import sys +import subprocess +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + + + +def ros1_launch_description(): + args = sys.argv[1:] + + roslaunch_command = ["roslaunch", "ros_web_api_bellande_3d_computer_vision", "bellande_3d_computer_vision_prediction.launch"] + args + + roslaunch_command.extend([ + "param", "config_file", + "value:=$(find ros_web_api_bellande_3d_computer_vision)/config/configs.json" + ]) + + roslaunch_command.extend([ + "ros_web_api_bellande_3d_computer_vision", "bellande_3d_computer_vision_prediction.py", "name:=pointcloud_prediction_node" + ]) + + roslaunch_command.extend([ + "rviz", "rviz", "name:=rviz", + "args:=-d $(find ros_web_api_bellande_3d_computer_vision)/rviz/pointcloud_visualization.rviz" + ]) + + subprocess.call(roslaunch_command) + + +def ros2_launch_description(): + nodes_to_launch = [] + + nodes_to_launch.append(Node( + package='ros_web_api_bellande_3d_computer_vision', + executable='bellande_3d_computer_vision_prediction.py', + name='pointcloud_prediction_node', + output='screen', + remappings=[('input_pointcloud', '/pointcloud_topic')], + parameters=[{'config_file': LaunchConfiguration('config_file')}] + )) + + nodes_to_launch.append(Node( + package='rviz2', + executable='rviz2', + name='rviz', + arguments=['-d', '$(find ros_web_api_bellande_3d_computer_vision)/rviz/pointcloud_visualization.rviz'] + )) + + return LaunchDescription(nodes_to_launch) + + +if __name__ == "__main__": + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + ros1_launch_description() + elif ros_version == "2": + ros2_launch_description() + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/ros_web_api_bellande_3d_computer_vision/launch/ros1/bellande_3d_computer_vision_instance_segmentation.launch b/ros_web_api_bellande_3d_computer_vision/launch/ros1/bellande_3d_computer_vision_instance_segmentation.launch new file mode 100644 index 0000000..ac6e1ab --- /dev/null +++ b/ros_web_api_bellande_3d_computer_vision/launch/ros1/bellande_3d_computer_vision_instance_segmentation.launch @@ -0,0 +1,30 @@ + + + + + + + + + + + + diff --git a/ros_web_api_bellande_3d_computer_vision/src/bellande_3d_computer_vision_instance_segmentation.py b/ros_web_api_bellande_3d_computer_vision/src/bellande_3d_computer_vision_instance_segmentation.py new file mode 100755 index 0000000..a56acb4 --- /dev/null +++ b/ros_web_api_bellande_3d_computer_vision/src/bellande_3d_computer_vision_instance_segmentation.py @@ -0,0 +1,111 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import json +import os +import requests +import numpy as np +import base64 +from sensor_msgs.msg import PointCloud2 +import sensor_msgs.point_cloud2 as pc2 + + +def pointcloud_instance_segmentation(cloud_msg): + points = np.array(list(pc2.read_points(cloud_msg, skip_nans=True, field_names=("x", "y", "z")))) + + points_base64 = base64.b64encode(points.tobytes()).decode('utf-8') + + payload = { + "pointcloud": points_base64 + } + + headers = { + "Authorization": f"Bearer {api_access_key}" + } + + response = requests.post(api_url, json=payload, headers=headers) + + if response.status_code == 200: + result = response.json() + instance_labels = np.array(result['instance_labels']) + + fields = [ + pc2.PointField(name="x", offset=0, datatype=pc2.PointField.FLOAT32, count=1), + pc2.PointField(name="y", offset=4, datatype=pc2.PointField.FLOAT32, count=1), + pc2.PointField(name="z", offset=8, datatype=pc2.PointField.FLOAT32, count=1), + pc2.PointField(name="instance", offset=12, datatype=pc2.PointField.UINT32, count=1), + ] + + segmented_cloud = pc2.create_cloud(cloud_msg.header, fields, np.hstack((points, instance_labels.reshape(-1, 1)))) + return segmented_cloud + else: + print(f"Error: {response.status_code} - {response.text}") + return None + + +def pointcloud_callback(msg): + segmented_cloud = pointcloud_instance_segmentation(msg) + if segmented_cloud: + pub.publish(segmented_cloud) + + +def main(): + global api_url, api_access_key, pub + + config_file_path = os.path.join(os.path.dirname(__file__), '../config/configs.json') + if not os.path.exists(config_file_path): + print("Config file not found:", config_file_path) + return + with open(config_file_path, 'r') as config_file: + config = json.load(config_file) + url = config['url'] + endpoint_path = config['endpoint_path']["instance_segmentation"] + api_access_key = config["Bellande_Framework_Access_Key"] + + if ros_version == "1": + rospy.init_node('pointcloud_instance_segmentation_node', anonymous=True) + pub = rospy.Publisher('pointcloud_instance_segmentation_result', PointCloud2, queue_size=10) + sub = rospy.Subscriber('input_pointcloud', PointCloud2, pointcloud_callback) + elif ros_version == "2": + rclpy.init() + node = rclpy.create_node('pointcloud_instance_segmentation_node') + pub = node.create_publisher(PointCloud2, 'pointcloud_instance_segmentation_result', 10) + sub = node.create_subscription(PointCloud2, 'input_pointcloud', pointcloud_callback, 10) + + api_url = f"{url}{endpoint_path}" + + try: + print("Pointcloud instance segmentation node is running. Ctrl+C to exit.") + if ros_version == "1": + rospy.spin() + elif ros_version == "2": + rclpy.spin(node) + except KeyboardInterrupt: + print("Shutting down pointcloud instance segmentation node.") + except Exception as e: + print(f"An error occurred: {str(e)}") + finally: + if ros_version == "2": + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + import rospy + elif ros_version == "2": + import rclpy + main() diff --git a/ros_web_api_bellande_3d_computer_vision/src/bellande_3d_computer_vision_object_detection.py b/ros_web_api_bellande_3d_computer_vision/src/bellande_3d_computer_vision_object_detection.py index 491c96f..6af125c 100755 --- a/ros_web_api_bellande_3d_computer_vision/src/bellande_3d_computer_vision_object_detection.py +++ b/ros_web_api_bellande_3d_computer_vision/src/bellande_3d_computer_vision_object_detection.py @@ -81,7 +81,7 @@ def main(): with open(config_file_path, 'r') as config_file: config = json.load(config_file) url = config['url'] - endpoint_path = config['endpoint_path']["pointcloud_api"] + endpoint_path = config['endpoint_path']["object_detection"] api_access_key = config["Bellande_Framework_Access_Key"] if ros_version == "1":