From e8e6c052b0bb1532a52530d57e5d362d5d083be7 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Sun, 1 Dec 2024 17:33:22 +0300 Subject: [PATCH] refactor(launch): update launch configuration for real_robot and cleanup - Add controller manager node when `real_robot` flag is set - Remove unused Open3D dependency --- .../env_manager/utils/conversions.py | 154 +++++++++--------- env_manager/rbs_gym/rbs_gym/envs/__init__.py | 2 +- .../rbs_runtime/launch/runtime.launch.py | 15 +- 3 files changed, 90 insertions(+), 81 deletions(-) diff --git a/env_manager/env_manager/env_manager/utils/conversions.py b/env_manager/env_manager/env_manager/utils/conversions.py index 259d708..064c33d 100644 --- a/env_manager/env_manager/env_manager/utils/conversions.py +++ b/env_manager/env_manager/env_manager/utils/conversions.py @@ -1,90 +1,90 @@ from typing import Tuple, Union import numpy -import open3d +# import open3d # import pyoctree import sensor_msgs from scipy.spatial.transform import Rotation -from sensor_msgs.msg import PointCloud2 -from open3d.geometry import PointCloud +# from sensor_msgs.msg import PointCloud2 +# from open3d.geometry import PointCloud from geometry_msgs.msg import Transform -def pointcloud2_to_open3d( - ros_point_cloud2: PointCloud2, - include_color: bool = False, - include_intensity: bool = False, - # Note: Order does not matter for DL, that's why channel swapping is disabled by default - fix_rgb_channel_order: bool = False, -) -> PointCloud: - - # Create output Open3D PointCloud - open3d_pc = PointCloud() - - size = ros_point_cloud2.width * ros_point_cloud2.height - xyz_dtype = ">f4" if ros_point_cloud2.is_bigendian else " 3: - bgr = numpy.ndarray( - shape=(size, 3), - dtype=numpy.uint8, - buffer=ros_point_cloud2.data, - offset=ros_point_cloud2.fields[3].offset, - strides=(ros_point_cloud2.point_step, 1), - ) - if fix_rgb_channel_order: - # Swap channels to gain rgb (faster than `bgr[:, [2, 1, 0]]`) - bgr[:, 0], bgr[:, 2] = bgr[:, 2], bgr[:, 0].copy() - open3d_pc.colors = open3d.utility.Vector3dVector( - (bgr[valid_points] / 255).astype(numpy.float64) - ) - else: - open3d_pc.colors = open3d.utility.Vector3dVector( - numpy.zeros((len(valid_points), 3), dtype=numpy.float64) - ) - # TODO: Update octree creator once L8 image format is supported in Ignition Gazebo - # elif include_intensity: - # # Faster approach, but only the first channel gets the intensity value (rest is 0) - # intensities = numpy.zeros((len(valid_points), 3), dtype=numpy.float64) - # intensities[:, [0]] = ( - # numpy.ndarray( - # shape=(size, 1), - # dtype=numpy.uint8, - # buffer=ros_point_cloud2.data, - # offset=ros_point_cloud2.fields[3].offset, - # strides=(ros_point_cloud2.point_step, 1), - # )[valid_points] - # / 255 - # ).astype(numpy.float64) - # open3d_pc.colors = open3d.utility.Vector3dVector(intensities) - # # # Slower approach, but all channels get the intensity value - # # intensities = numpy.ndarray( - # # shape=(size, 1), - # # dtype=numpy.uint8, - # # buffer=ros_point_cloud2.data, - # # offset=ros_point_cloud2.fields[3].offset, - # # strides=(ros_point_cloud2.point_step, 1), - # # ) - # # open3d_pc.colors = open3d.utility.Vector3dVector( - # # numpy.tile(intensities[valid_points] / 255, (1, 3)).astype(numpy.float64) - # # ) - - # Return the converted Open3D PointCloud - return open3d_pc +# def pointcloud2_to_open3d( +# ros_point_cloud2: PointCloud2, +# include_color: bool = False, +# include_intensity: bool = False, +# # Note: Order does not matter for DL, that's why channel swapping is disabled by default +# fix_rgb_channel_order: bool = False, +# ) -> PointCloud: +# +# # Create output Open3D PointCloud +# open3d_pc = PointCloud() +# +# size = ros_point_cloud2.width * ros_point_cloud2.height +# xyz_dtype = ">f4" if ros_point_cloud2.is_bigendian else " 3: +# bgr = numpy.ndarray( +# shape=(size, 3), +# dtype=numpy.uint8, +# buffer=ros_point_cloud2.data, +# offset=ros_point_cloud2.fields[3].offset, +# strides=(ros_point_cloud2.point_step, 1), +# ) +# if fix_rgb_channel_order: +# # Swap channels to gain rgb (faster than `bgr[:, [2, 1, 0]]`) +# bgr[:, 0], bgr[:, 2] = bgr[:, 2], bgr[:, 0].copy() +# open3d_pc.colors = open3d.utility.Vector3dVector( +# (bgr[valid_points] / 255).astype(numpy.float64) +# ) +# else: +# open3d_pc.colors = open3d.utility.Vector3dVector( +# numpy.zeros((len(valid_points), 3), dtype=numpy.float64) +# ) +# # TODO: Update octree creator once L8 image format is supported in Ignition Gazebo +# # elif include_intensity: +# # # Faster approach, but only the first channel gets the intensity value (rest is 0) +# # intensities = numpy.zeros((len(valid_points), 3), dtype=numpy.float64) +# # intensities[:, [0]] = ( +# # numpy.ndarray( +# # shape=(size, 1), +# # dtype=numpy.uint8, +# # buffer=ros_point_cloud2.data, +# # offset=ros_point_cloud2.fields[3].offset, +# # strides=(ros_point_cloud2.point_step, 1), +# # )[valid_points] +# # / 255 +# # ).astype(numpy.float64) +# # open3d_pc.colors = open3d.utility.Vector3dVector(intensities) +# # # # Slower approach, but all channels get the intensity value +# # # intensities = numpy.ndarray( +# # # shape=(size, 1), +# # # dtype=numpy.uint8, +# # # buffer=ros_point_cloud2.data, +# # # offset=ros_point_cloud2.fields[3].offset, +# # # strides=(ros_point_cloud2.point_step, 1), +# # # ) +# # # open3d_pc.colors = open3d.utility.Vector3dVector( +# # # numpy.tile(intensities[valid_points] / 255, (1, 3)).astype(numpy.float64) +# # # ) +# +# # Return the converted Open3D PointCloud +# return open3d_pc def transform_to_matrix(transform: Transform) -> numpy.ndarray: diff --git a/env_manager/rbs_gym/rbs_gym/envs/__init__.py b/env_manager/rbs_gym/rbs_gym/envs/__init__.py index 9061b87..df4e0c0 100644 --- a/env_manager/rbs_gym/rbs_gym/envs/__init__.py +++ b/env_manager/rbs_gym/rbs_gym/envs/__init__.py @@ -8,7 +8,7 @@ from env_manager.models.configs import ( from env_manager.models.configs.objects import ObjectRandomizerData from env_manager.models.configs.robot import RobotRandomizerData from env_manager.models.configs.scene import PluginsData -import open3d # isort:skip +# import open3d # isort:skip import stable_baselines3 # isort:skip # Note: If installed, `tensorflow` module must be imported before `gym_gz`/`scenario` diff --git a/env_manager/rbs_runtime/launch/runtime.launch.py b/env_manager/rbs_runtime/launch/runtime.launch.py index d33accc..dc2d833 100644 --- a/env_manager/rbs_runtime/launch/runtime.launch.py +++ b/env_manager/rbs_runtime/launch/runtime.launch.py @@ -52,7 +52,7 @@ def launch_setup(context, *args, **kwargs): description_package.perform(context) ) - simulation_controllers = os.path.join( + controllers_file = os.path.join( description_package_abs_path, "config", "controllers.yaml" ) @@ -135,7 +135,7 @@ def launch_setup(context, *args, **kwargs): ), launch_arguments={ "with_gripper": with_gripper_condition, - "controllers_file": simulation_controllers, + "controllers_file": controllers_file, "robot_type": robot_type, "description_package": description_package, "description_file": description_file, @@ -156,7 +156,16 @@ def launch_setup(context, *args, **kwargs): ) if real_robot == "true": - return [rbs_robot_setup] + controller_manager_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + robot_description, + controllers_file, + ], + output="screen", + ) + return [rbs_robot_setup, controller_manager_node] rbs_runtime = Node( package="rbs_runtime",