cv
This commit is contained in:
parent
d965446abf
commit
e331bb4d23
@ -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)
|
@ -18,60 +18,45 @@ import sys
|
|||||||
import subprocess
|
import subprocess
|
||||||
from launch import LaunchDescription
|
from launch import LaunchDescription
|
||||||
from launch_ros.actions import Node
|
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():
|
def ros1_launch_description():
|
||||||
# Get command-line arguments
|
|
||||||
args = sys.argv[1:]
|
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
|
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([
|
roslaunch_command.extend([
|
||||||
"ros_web_api_bellande_3d_computer_vision", "pointcloud_pradiction_node", "name:=pointcloud_publisher",
|
"param", "config_file",
|
||||||
"frame_id:=base_link"
|
"value:=$(find ros_web_api_bellande_3d_computer_vision)/config/configs.json"
|
||||||
])
|
])
|
||||||
|
|
||||||
# Add the prediction node
|
|
||||||
roslaunch_command.extend([
|
roslaunch_command.extend([
|
||||||
"ros_web_api_bellande_3d_computer_vision", "bellande_3d_computer_vision_prediction.py", "name:=pointcloud_prediction_node"
|
"ros_web_api_bellande_3d_computer_vision", "bellande_3d_computer_vision_prediction.py", "name:=pointcloud_prediction_node"
|
||||||
])
|
])
|
||||||
|
|
||||||
# Add the rviz node
|
|
||||||
roslaunch_command.extend([
|
roslaunch_command.extend([
|
||||||
"rviz", "rviz", "name:=rviz",
|
"rviz", "rviz", "name:=rviz",
|
||||||
"args:=-d $(find ros_web_api_bellande_3d_computer_vision)/rviz/pointcloud_visualization.rviz"
|
"args:=-d $(find ros_web_api_bellande_3d_computer_vision)/rviz/pointcloud_visualization.rviz"
|
||||||
])
|
])
|
||||||
|
|
||||||
# Execute the launch command
|
|
||||||
subprocess.call(roslaunch_command)
|
subprocess.call(roslaunch_command)
|
||||||
|
|
||||||
|
|
||||||
def ros2_launch_description():
|
def ros2_launch_description():
|
||||||
# Create a list to hold all nodes to be launched
|
|
||||||
nodes_to_launch = []
|
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(
|
nodes_to_launch.append(Node(
|
||||||
package='ros_web_api_bellande_3d_computer_vision',
|
package='ros_web_api_bellande_3d_computer_vision',
|
||||||
executable='bellande_3d_computer_vision_prediction.py',
|
executable='bellande_3d_computer_vision_prediction.py',
|
||||||
name='pointcloud_prediction_node',
|
name='pointcloud_prediction_node',
|
||||||
output='screen',
|
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(
|
nodes_to_launch.append(Node(
|
||||||
package='rviz2',
|
package='rviz2',
|
||||||
executable='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']
|
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)
|
return LaunchDescription(nodes_to_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>
|
@ -2,33 +2,28 @@
|
|||||||
<!--
|
<!--
|
||||||
Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
|
Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
|
||||||
|
|
||||||
Licensed under the Apache License, Version 2.0 (the "License"); you may not
|
This program is free software: you can redistribute it and/or modify
|
||||||
use this file except in compliance with the License. You may obtain a copy of
|
it under the terms of the GNU General Public License as published by
|
||||||
the License at
|
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
|
You should have received a copy of the GNU General Public License
|
||||||
distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
|
along with this program. If not, see <https://www.gnu.org/licenses/>.
|
||||||
WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
|
|
||||||
License for the specific language governing permissions and limitations under
|
|
||||||
the License.
|
|
||||||
-->
|
-->
|
||||||
<launch>
|
<launch>
|
||||||
<!-- Launch the point cloud source (adjust parameters as needed) -->
|
<param name="config_file"
|
||||||
<node name="pointcloud_publisher" pkg="ros_web_api_bellande_3d_computer_vision"
|
value="$(find ros_web_api_bellande_3d_computer_vision)/config/configs.json" />
|
||||||
type="pointcloud_pradiction_node"
|
|
||||||
output="screen">
|
|
||||||
<param name="frame_id" value="base_link" />
|
|
||||||
</node>
|
|
||||||
|
|
||||||
<!-- Launch the prediction node -->
|
|
||||||
<node name="pointcloud_prediction_node" pkg="ros_web_api_bellande_3d_computer_vision"
|
<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" />
|
<remap from="input_pointcloud" to="/pointcloud_topic" />
|
||||||
</node>
|
</node>
|
||||||
|
|
||||||
<!-- Launch RViz for visualization -->
|
|
||||||
<node name="rviz" pkg="rviz" type="rviz"
|
<node name="rviz" pkg="rviz" type="rviz"
|
||||||
args="-d $(find ros_web_api_bellande_3d_computer_vision)/rviz/pointcloud_visualization.rviz" />
|
args="-d $(find ros_web_api_bellande_3d_computer_vision)/rviz/pointcloud_visualization.rviz" />
|
||||||
|
|
||||||
|
@ -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()
|
@ -26,7 +26,6 @@ from std_msgs.msg import String
|
|||||||
def pointcloud_prediction(cloud_msg):
|
def pointcloud_prediction(cloud_msg):
|
||||||
points = np.array(list(pc2.read_points(cloud_msg, skip_nans=True, field_names=("x", "y", "z"))))
|
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')
|
points_base64 = base64.b64encode(points.tobytes()).decode('utf-8')
|
||||||
|
|
||||||
payload = {
|
payload = {
|
||||||
|
Loading…
Reference in New Issue
Block a user