robossembler/runtime
7
0
Fork 0
Code Issues 4 Pull requests Projects Releases Packages Wiki Activity Actions

refactor(launch): update launch configuration for real_robot and cleanup

Browse source 
- Add controller manager node when `real_robot` flag is set
- Remove unused Open3D dependency
This commit is contained in:
Ilya Uraev 2024-12-01 17:33:22 +03:00
parent fd0bb140cf
commit e8e6c052b0
3 changed files with 90 additions and 81 deletions
Download patch file Download diff file Expand all files Collapse all files

154
env_manager/env_manager/env_manager/utils/conversions.py
View file

@ -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 "<f4"
xyz = numpy.ndarray(
shape=(size, 3),
dtype=xyz_dtype,
buffer=ros_point_cloud2.data,
offset=0,
strides=(ros_point_cloud2.point_step, 4),
)
valid_points = numpy.isfinite(xyz).any(axis=1)
open3d_pc.points = open3d.utility.Vector3dVector(
xyz[valid_points].astype(numpy.float64)
)
if include_color or include_intensity:
if len(ros_point_cloud2.fields) > 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 "<f4"
# xyz = numpy.ndarray(
# shape=(size, 3),
# dtype=xyz_dtype,
# buffer=ros_point_cloud2.data,
# offset=0,
# strides=(ros_point_cloud2.point_step, 4),
# )
#
# valid_points = numpy.isfinite(xyz).any(axis=1)
# open3d_pc.points = open3d.utility.Vector3dVector(
# xyz[valid_points].astype(numpy.float64)
# )
#
# if include_color or include_intensity:
# if len(ros_point_cloud2.fields) > 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:

2
env_manager/rbs_gym/rbs_gym/envs/__init__.py
View file

@ -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`

15
env_manager/rbs_runtime/launch/runtime.launch.py
View file

@ -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",