diff --git a/ros_web_api_bellande_3d_computer_vision/launch/bellande_3d_computer_vision_object_detection.launch.py b/ros_web_api_bellande_3d_computer_vision/launch/bellande_3d_computer_vision_object_detection.launch.py
new file mode 100644
index 0000000..9c7ed40
--- /dev/null
+++ b/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 .
+
+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)
diff --git a/ros_web_api_bellande_3d_computer_vision/launch/bellande_3d_computer_vision_prediction.launch.py b/ros_web_api_bellande_3d_computer_vision/launch/bellande_3d_computer_vision_prediction.launch.py
index 922711f..3c63927 100644
--- a/ros_web_api_bellande_3d_computer_vision/launch/bellande_3d_computer_vision_prediction.launch.py
+++ b/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)
diff --git a/ros_web_api_bellande_3d_computer_vision/launch/ros1/bellande_3d_computer_vision_object_detection.launch b/ros_web_api_bellande_3d_computer_vision/launch/ros1/bellande_3d_computer_vision_object_detection.launch
new file mode 100644
index 0000000..3203fe5
--- /dev/null
+++ b/ros_web_api_bellande_3d_computer_vision/launch/ros1/bellande_3d_computer_vision_object_detection.launch
@@ -0,0 +1,30 @@
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/ros_web_api_bellande_3d_computer_vision/launch/ros1/bellande_3d_computer_vision_prediction.launch b/ros_web_api_bellande_3d_computer_vision/launch/ros1/bellande_3d_computer_vision_prediction.launch
index 5e03f83..ac6e1ab 100644
--- a/ros_web_api_bellande_3d_computer_vision/launch/ros1/bellande_3d_computer_vision_prediction.launch
+++ b/ros_web_api_bellande_3d_computer_vision/launch/ros1/bellande_3d_computer_vision_prediction.launch
@@ -2,33 +2,28 @@
-
-
-
-
+
-
+ type="bellande_3d_computer_vision_prediction.py" output="screen">
-
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
new file mode 100755
index 0000000..491c96f
--- /dev/null
+++ b/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 .
+
+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()
diff --git a/ros_web_api_bellande_3d_computer_vision/src/bellande_3d_computer_vision_prediction.py b/ros_web_api_bellande_3d_computer_vision/src/bellande_3d_computer_vision_prediction.py
index 57b08b8..ab007d2 100755
--- a/ros_web_api_bellande_3d_computer_vision/src/bellande_3d_computer_vision_prediction.py
+++ b/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 = {