robossembler/runtime
7
0
Fork 0
Code Issues 5 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 from typing import Tuple, Union
import numpy import numpy
import open3d # import open3d
# import pyoctree # import pyoctree
import sensor_msgs import sensor_msgs
from scipy.spatial.transform import Rotation from scipy.spatial.transform import Rotation
from sensor_msgs.msg import PointCloud2 # from sensor_msgs.msg import PointCloud2
from open3d.geometry import PointCloud # from open3d.geometry import PointCloud
from geometry_msgs.msg import Transform from geometry_msgs.msg import Transform
def pointcloud2_to_open3d( # def pointcloud2_to_open3d(
ros_point_cloud2: PointCloud2, # ros_point_cloud2: PointCloud2,
include_color: bool = False, # include_color: bool = False,
include_intensity: bool = False, # include_intensity: bool = False,
# Note: Order does not matter for DL, that's why channel swapping is disabled by default # # Note: Order does not matter for DL, that's why channel swapping is disabled by default
fix_rgb_channel_order: bool = False, # fix_rgb_channel_order: bool = False,
) -> PointCloud: # ) -> PointCloud:
#
# Create output Open3D PointCloud # # Create output Open3D PointCloud
open3d_pc = PointCloud() # open3d_pc = PointCloud()
#
size = ros_point_cloud2.width * ros_point_cloud2.height # size = ros_point_cloud2.width * ros_point_cloud2.height
xyz_dtype = ">f4" if ros_point_cloud2.is_bigendian else "<f4" # xyz_dtype = ">f4" if ros_point_cloud2.is_bigendian else "<f4"
xyz = numpy.ndarray( # xyz = numpy.ndarray(
shape=(size, 3), # shape=(size, 3),
dtype=xyz_dtype, # dtype=xyz_dtype,
buffer=ros_point_cloud2.data, # buffer=ros_point_cloud2.data,
offset=0, # offset=0,
strides=(ros_point_cloud2.point_step, 4), # strides=(ros_point_cloud2.point_step, 4),
) # )
#
valid_points = numpy.isfinite(xyz).any(axis=1) # valid_points = numpy.isfinite(xyz).any(axis=1)
open3d_pc.points = open3d.utility.Vector3dVector( # open3d_pc.points = open3d.utility.Vector3dVector(
xyz[valid_points].astype(numpy.float64) # xyz[valid_points].astype(numpy.float64)
) # )
#
if include_color or include_intensity: # if include_color or include_intensity:
if len(ros_point_cloud2.fields) > 3: # if len(ros_point_cloud2.fields) > 3:
bgr = numpy.ndarray( # bgr = numpy.ndarray(
shape=(size, 3), # shape=(size, 3),
dtype=numpy.uint8, # dtype=numpy.uint8,
buffer=ros_point_cloud2.data, # buffer=ros_point_cloud2.data,
offset=ros_point_cloud2.fields[3].offset, # offset=ros_point_cloud2.fields[3].offset,
strides=(ros_point_cloud2.point_step, 1), # strides=(ros_point_cloud2.point_step, 1),
) # )
if fix_rgb_channel_order: # if fix_rgb_channel_order:
# Swap channels to gain rgb (faster than `bgr[:, [2, 1, 0]]`) # # Swap channels to gain rgb (faster than `bgr[:, [2, 1, 0]]`)
bgr[:, 0], bgr[:, 2] = bgr[:, 2], bgr[:, 0].copy() # bgr[:, 0], bgr[:, 2] = bgr[:, 2], bgr[:, 0].copy()
open3d_pc.colors = open3d.utility.Vector3dVector( # open3d_pc.colors = open3d.utility.Vector3dVector(
(bgr[valid_points] / 255).astype(numpy.float64) # (bgr[valid_points] / 255).astype(numpy.float64)
) # )
else: # else:
open3d_pc.colors = open3d.utility.Vector3dVector( # open3d_pc.colors = open3d.utility.Vector3dVector(
numpy.zeros((len(valid_points), 3), dtype=numpy.float64) # numpy.zeros((len(valid_points), 3), dtype=numpy.float64)
) # )
# TODO: Update octree creator once L8 image format is supported in Ignition Gazebo # # TODO: Update octree creator once L8 image format is supported in Ignition Gazebo
# elif include_intensity: # # elif include_intensity:
# # Faster approach, but only the first channel gets the intensity value (rest is 0) # # # 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 = numpy.zeros((len(valid_points), 3), dtype=numpy.float64)
# intensities[:, [0]] = ( # # intensities[:, [0]] = (
# numpy.ndarray( # # numpy.ndarray(
# shape=(size, 1), # # shape=(size, 1),
# dtype=numpy.uint8, # # dtype=numpy.uint8,
# buffer=ros_point_cloud2.data, # # buffer=ros_point_cloud2.data,
# offset=ros_point_cloud2.fields[3].offset, # # offset=ros_point_cloud2.fields[3].offset,
# strides=(ros_point_cloud2.point_step, 1), # # strides=(ros_point_cloud2.point_step, 1),
# )[valid_points] # # )[valid_points]
# / 255 # # / 255
# ).astype(numpy.float64) # # ).astype(numpy.float64)
# open3d_pc.colors = open3d.utility.Vector3dVector(intensities) # # open3d_pc.colors = open3d.utility.Vector3dVector(intensities)
# # # Slower approach, but all channels get the intensity value # # # # Slower approach, but all channels get the intensity value
# # intensities = numpy.ndarray( # # # intensities = numpy.ndarray(
# # shape=(size, 1), # # # shape=(size, 1),
# # dtype=numpy.uint8, # # # dtype=numpy.uint8,
# # buffer=ros_point_cloud2.data, # # # buffer=ros_point_cloud2.data,
# # offset=ros_point_cloud2.fields[3].offset, # # # offset=ros_point_cloud2.fields[3].offset,
# # strides=(ros_point_cloud2.point_step, 1), # # # strides=(ros_point_cloud2.point_step, 1),
# # ) # # # )
# # open3d_pc.colors = open3d.utility.Vector3dVector( # # # open3d_pc.colors = open3d.utility.Vector3dVector(
# # numpy.tile(intensities[valid_points] / 255, (1, 3)).astype(numpy.float64) # # # numpy.tile(intensities[valid_points] / 255, (1, 3)).astype(numpy.float64)
# # ) # # # )
#
# Return the converted Open3D PointCloud # # Return the converted Open3D PointCloud
return open3d_pc # return open3d_pc
def transform_to_matrix(transform: Transform) -> numpy.ndarray: 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.objects import ObjectRandomizerData
from env_manager.models.configs.robot import RobotRandomizerData from env_manager.models.configs.robot import RobotRandomizerData
from env_manager.models.configs.scene import PluginsData from env_manager.models.configs.scene import PluginsData
import open3d # isort:skip # import open3d # isort:skip
import stable_baselines3 # isort:skip import stable_baselines3 # isort:skip
# Note: If installed, `tensorflow` module must be imported before `gym_gz`/`scenario` # 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) description_package.perform(context)
) )
simulation_controllers = os.path.join( controllers_file = os.path.join(
description_package_abs_path, "config", "controllers.yaml" description_package_abs_path, "config", "controllers.yaml"
) )
@ -135,7 +135,7 @@ def launch_setup(context, *args, **kwargs):
), ),
launch_arguments={ launch_arguments={
"with_gripper": with_gripper_condition, "with_gripper": with_gripper_condition,
"controllers_file": simulation_controllers, "controllers_file": controllers_file,
"robot_type": robot_type, "robot_type": robot_type,
"description_package": description_package, "description_package": description_package,
"description_file": description_file, "description_file": description_file,
@ -156,7 +156,16 @@ def launch_setup(context, *args, **kwargs):
) )
if real_robot == "true": 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( rbs_runtime = Node(
package="rbs_runtime", package="rbs_runtime",