cv

ros_web_api_bellande_3d_computer_vision/launch/bellande_3d_computer_vision_object_detection.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 <https://www.gnu.org/licenses/>.
+
+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_object_detection.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_object_detection.py", "name:=pointcloud_object_detection_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='pointcloud_object_detection_node.py',
+        name='pointcloud_object_detection_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)

ros_web_api_bellande_3d_computer_vision/launch/bellande_3d_computer_vision_prediction.launch.py

@@ -18,60 +18,45 @@ import sys
 import subprocess
 from launch import LaunchDescription
 from launch_ros.actions import Node
-from launch.substitutions import FindExecutable
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+
 
 
 def ros1_launch_description():
-    # Get command-line arguments
     args = sys.argv[1:]
     
-    # Construct the ROS 1 launch command
     roslaunch_command = ["roslaunch", "ros_web_api_bellande_3d_computer_vision", "bellande_3d_computer_vision_prediction.launch"] + args
     
-    # Add the pointcloud publisher node
     roslaunch_command.extend([
-        "ros_web_api_bellande_3d_computer_vision", "pointcloud_pradiction_node", "name:=pointcloud_publisher",
-        "frame_id:=base_link"
+        "param", "config_file",
+        "value:=$(find ros_web_api_bellande_3d_computer_vision)/config/configs.json"
     ])     
     
-    # Add the prediction node
     roslaunch_command.extend([
         "ros_web_api_bellande_3d_computer_vision", "bellande_3d_computer_vision_prediction.py", "name:=pointcloud_prediction_node"
     ])
     
-    # Add the rviz node
     roslaunch_command.extend([
         "rviz", "rviz", "name:=rviz",
         "args:=-d $(find ros_web_api_bellande_3d_computer_vision)/rviz/pointcloud_visualization.rviz"
     ])
     
-    # Execute the launch command
     subprocess.call(roslaunch_command)
 
 
 def ros2_launch_description():
-    # Create a list to hold all nodes to be launched
     nodes_to_launch = []
     
-    # Add the pointcloud publisher node
-    nodes_to_launch.append(Node(
-        package='ros_web_api_bellande_3d_computer_vision',
-        executable='pointcloud_pradiction_node',
-        name='pointcloud_publisher',
-        output='screen',
-        parameters=[{'frame_id': 'base_link'}]
-    ))
-    
-    # Add the prediction node
     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')]
+        remappings=[('input_pointcloud', '/pointcloud_topic')],
+        parameters=[{'config_file': LaunchConfiguration('config_file')}]
     ))
     
-    # Add the rviz node
     nodes_to_launch.append(Node(
         package='rviz2',
         executable='rviz2',
@@ -79,7 +64,6 @@ def ros2_launch_description():
         arguments=['-d', '$(find ros_web_api_bellande_3d_computer_vision)/rviz/pointcloud_visualization.rviz']
     ))
     
-    # Return the LaunchDescription containing all nodes
     return LaunchDescription(nodes_to_launch)
 
 

ros_web_api_bellande_3d_computer_vision/launch/ros1/bellande_3d_computer_vision_object_detection.launch

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<!--
+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 <https://www.gnu.org/licenses/>.
+-->
+<launch>
+  <param name="config_file"
+    value="$(find ros_web_api_bellande_3d_computer_vision)/config/configs.json" />
+
+  <node name="pointcloud_object_detection_node" pkg="ros_web_api_bellande_3d_computer_vision"
+    type="pointcloud_object_detection_node.py" output="screen">
+    <remap from="input_pointcloud" to="/pointcloud_topic" />
+  </node>
+
+  <node name="rviz" pkg="rviz" type="rviz"
+    args="-d $(find ros_web_api_bellande_3d_computer_vision)/rviz/pointcloud_visualization.rviz" />
+
+</launch>

ros_web_api_bellande_3d_computer_vision/launch/ros1/bellande_3d_computer_vision_prediction.launch

@@ -2,33 +2,28 @@
 <!--
 Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
 
-Licensed under the Apache License, Version 2.0 (the "License"); you may not
-use this file except in compliance with the License. You may obtain a copy of
-the License at
+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.
 
-http://www.apache.org/licenses/LICENSE-2.0
+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.
 
-Unless required by applicable law or agreed to in writing, software
-distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
-WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
-License for the specific language governing permissions and limitations under
-the License.
+You should have received a copy of the GNU General Public License
+along with this program.  If not, see <https://www.gnu.org/licenses/>.
 -->
 <launch>
-  <!-- Launch the point cloud source (adjust parameters as needed) -->
-  <node name="pointcloud_publisher" pkg="ros_web_api_bellande_3d_computer_vision"
-    type="pointcloud_pradiction_node"
-    output="screen">
-    <param name="frame_id" value="base_link" />
-  </node>
+  <param name="config_file"
+    value="$(find ros_web_api_bellande_3d_computer_vision)/config/configs.json" />
 
-  <!-- Launch the prediction node -->
   <node name="pointcloud_prediction_node" pkg="ros_web_api_bellande_3d_computer_vision"
-    type="pointcloud_prediction_node.py" output="screen">
+    type="bellande_3d_computer_vision_prediction.py" output="screen">
     <remap from="input_pointcloud" to="/pointcloud_topic" />
   </node>
 
-  <!-- Launch RViz for visualization -->
   <node name="rviz" pkg="rviz" type="rviz"
     args="-d $(find ros_web_api_bellande_3d_computer_vision)/rviz/pointcloud_visualization.rviz" />
 

ros_web_api_bellande_3d_computer_vision/src/bellande_3d_computer_vision_object_detection.py

@@ -0,0 +1,121 @@
+# 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 <https://www.gnu.org/licenses/>.
+
+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
+from visualization_msgs.msg import Marker, MarkerArray
+
+
+def pointcloud_object_detection(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()
+        marker_array = MarkerArray()
+        for idx, obj in enumerate(result['objects']):
+            marker = Marker()
+            marker.header = cloud_msg.header
+            marker.ns = "object_detection"
+            marker.id = idx
+            marker.type = Marker.CUBE
+            marker.action = Marker.ADD
+            marker.pose.position.x = obj['centroid'][0]
+            marker.pose.position.y = obj['centroid'][1]
+            marker.pose.position.z = obj['centroid'][2]
+            marker.scale.x = obj['dimensions'][0]
+            marker.scale.y = obj['dimensions'][1]
+            marker.scale.z = obj['dimensions'][2]
+            marker.color.a = 0.5
+            marker.color.r = 1.0
+            marker.color.g = 0.0
+            marker.color.b = 0.0
+            marker_array.markers.append(marker)
+        return marker_array
+    else:
+        print(f"Error: {response.status_code} - {response.text}")
+        return None
+
+
+def pointcloud_callback(msg):
+    markers = pointcloud_object_detection(msg)
+    if markers:
+        pub.publish(markers)
+
+
+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']["pointcloud_api"]
+        api_access_key = config["Bellande_Framework_Access_Key"]
+    
+    if ros_version == "1":
+        rospy.init_node('pointcloud_object_detection_node', anonymous=True)
+        pub = rospy.Publisher('pointcloud_object_detection_result', MarkerArray, queue_size=10)
+        sub = rospy.Subscriber('input_pointcloud', PointCloud2, pointcloud_callback)
+    elif ros_version == "2":
+        rclpy.init()
+        node = rclpy.create_node('pointcloud_object_detection_node')
+        pub = node.create_publisher(MarkerArray, 'pointcloud_object_detection_result', 10)
+        sub = node.create_subscription(PointCloud2, 'input_pointcloud', pointcloud_callback, 10)
+
+    api_url = f"{url}{endpoint_path}"
+
+    try:
+        print("Pointcloud object detection 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 object detection 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()

ros_web_api_bellande_3d_computer_vision/src/bellande_3d_computer_vision_prediction.py

@@ -26,7 +26,6 @@ from std_msgs.msg import String
 def pointcloud_prediction(cloud_msg):
     points = np.array(list(pc2.read_points(cloud_msg, skip_nans=True, field_names=("x", "y", "z"))))
     
-    # Convert pointcloud to base64 encoding
     points_base64 = base64.b64encode(points.tobytes()).decode('utf-8')
 
     payload = {