refactor(launch): update launch configuration for real_robot and cleanup
- Add controller manager node when `real_robot` flag is set - Remove unused Open3D dependency
This commit is contained in:
parent
fd0bb140cf
commit
e8e6c052b0
3 changed files with 90 additions and 81 deletions
|
@ -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:
|
||||||
|
|
|
@ -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`
|
||||||
|
|
|
@ -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",
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue