diff --git a/simple_launch/__init__.py b/simple_launch/__init__.py index 776029a..edcd283 100644 --- a/simple_launch/__init__.py +++ b/simple_launch/__init__.py @@ -460,8 +460,6 @@ def robot_state_publisher(self, package=None, description_file=None, description ''' Add a robot state publisher node to the launch tree using the given description (urdf / xacro) file. - If the file ends with 'xacro', or any path element is defined from an Argument, or xacro_args are passed, runs xacro on this file. - * package -- is the name of the package that contains the description file (if None then assume an absolute description file) * description_file -- is the name of the urdf/xacro file * description_dir -- the name of the directory containing the file (None to have it found) @@ -519,7 +517,7 @@ def joint_state_publisher(self, use_gui = True, **node_args): def gz_prefix(): return 'ign' if SimpleLauncher.ros_version() < 'humble' else 'gz' - def create_gz_bridge(self, bridges: list[GazeboBridge], name = 'gz_bridge'): + def create_gz_bridge(self, bridges: [], name = 'gz_bridge'): ''' Create a ros_gz_bridge::parameter_bridge with the passed GazeboBridge instances The bridge has a default name if not specified @@ -530,8 +528,7 @@ def create_gz_bridge(self, bridges: list[GazeboBridge], name = 'gz_bridge'): if len(bridges) == 0: return - gz = self.gz_prefix() - ros_gz = f'ros_{gz}' + ros_gz = 'ros_' + self.gz_prefix() # add camera_info for image bridges im_bridges = [bridge for bridge in bridges if bridge.is_image] @@ -550,7 +547,7 @@ def create_gz_bridge(self, bridges: list[GazeboBridge], name = 'gz_bridge'): bridges.append(GazeboBridge(gz_head + [cam[0]], ros_head + [cam[1]], 'sensor_msgs/CameraInfo', GazeboBridge.gz2ros)) - std_config = sum([bridge.yaml(gz) for bridge in bridges if not bridge.is_image], []) + std_config = sum([bridge.yaml() for bridge in bridges if not bridge.is_image], []) if std_config.has_elems(): # use YAML-based configuration, handles Gazebo topics that are invalid to ROS @@ -600,6 +597,7 @@ def spawn_gz_model(self, name, topic = 'robot_description', model_file = None, s Spawns a model into Gazebo under the given name, from the given topic or file Additional spawn_args can be given to specify e.g. the initial pose ''' + if model_file is not None: spawn_args = flatten(spawn_args + ['-file',model_file,'-name', name]) else: diff --git a/simple_launch/gazebo.py b/simple_launch/gazebo.py index 3f9a8d2..6580416 100644 --- a/simple_launch/gazebo.py +++ b/simple_launch/gazebo.py @@ -31,36 +31,38 @@ class GazeboBridge: models = None world_name = None + gz_exec = 'gz' + # ros <-> gz mapping # from https://github.com/ignitionrobotics/ros_ign/blob/foxy/ros_ign_bridge/README.md - msg_map = {'std_msgs/msg/Bool': 'ignition.msgs.Boolean', - 'std_msgs/msg/Empty': 'ignition.msgs.Empty', - 'std_msgs/msg/Float32': 'ignition.msgs.Float', - 'std_msgs/msg/Float64': 'ignition.msgs.Double', - 'std_msgs/msg/Header': 'ignition.msgs.Header', - 'std_msgs/msg/Int32': 'ignition.msgs.Int32', - 'std_msgs/msg/String': 'ignition.msgs.StringMsg', - 'geometry_msgs/msg/Quaternion': 'ignition.msgs.Quaternion', - 'geometry_msgs/msg/Vector3': 'ignition.msgs.Vector3d', - 'geometry_msgs/msg/Point': 'ignition.msgs.Vector3d', - 'geometry_msgs/msg/Pose': 'ignition.msgs.Pose', - 'geometry_msgs/msg/PoseStamped': 'ignition.msgs.Pose', - 'geometry_msgs/msg/Transform': 'ignition.msgs.Pose', - 'geometry_msgs/msg/TransformStamped': 'ignition.msgs.Pose', - 'geometry_msgs/msg/Twist': 'ignition.msgs.Twist', - 'nav_msgs/msg/Odometry': 'ignition.msgs.Odometry', - 'rosgraph_msgs/msg/Clock': 'ignition.msgs.Clock', - 'sensor_msgs/msg/BatteryState': 'ignition.msgs.BatteryState', - 'sensor_msgs/msg/CameraInfo': 'ignition.msgs.CameraInfo', - 'sensor_msgs/msg/FluidPressure': 'ignition.msgs.FluidPressure', - 'sensor_msgs/msg/Imu': 'ignition.msgs.IMU', - 'sensor_msgs/msg/Image': 'ignition.msgs.Image', - 'sensor_msgs/msg/JointState': 'ignition.msgs.Model', - 'sensor_msgs/msg/LaserScan': 'ignition.msgs.LaserScan', - 'sensor_msgs/msg/MagneticField': 'ignition.msgs.Magnetometer', - 'sensor_msgs/msg/PointCloud2': 'ignition.msgs.PointCloudPacked', - 'tf2_msgs/msg/TFMessage': 'ignition.msgs.Pose_V', - 'trajectory_msgs/msg/JointTrajectory': 'ignition.msgs.JointTrajectory'} + msg_map = {'std_msgs/msg/Bool': 'gz.msgs.Boolean', + 'std_msgs/msg/Empty': 'gz.msgs.Empty', + 'std_msgs/msg/Float32': 'gz.msgs.Float', + 'std_msgs/msg/Float64': 'gz.msgs.Double', + 'std_msgs/msg/Header': 'gz.msgs.Header', + 'std_msgs/msg/Int32': 'gz.msgs.Int32', + 'std_msgs/msg/String': 'gz.msgs.StringMsg', + 'geometry_msgs/msg/Quaternion': 'gz.msgs.Quaternion', + 'geometry_msgs/msg/Vector3': 'gz.msgs.Vector3d', + 'geometry_msgs/msg/Point': 'gz.msgs.Vector3d', + 'geometry_msgs/msg/Pose': 'gz.msgs.Pose', + 'geometry_msgs/msg/PoseStamped': 'gz.msgs.Pose', + 'geometry_msgs/msg/Transform': 'gz.msgs.Pose', + 'geometry_msgs/msg/TransformStamped': 'gz.msgs.Pose', + 'geometry_msgs/msg/Twist': 'gz.msgs.Twist', + 'nav_msgs/msg/Odometry': 'gz.msgs.Odometry', + 'rosgraph_msgs/msg/Clock': 'gz.msgs.Clock', + 'sensor_msgs/msg/BatteryState': 'gz.msgs.BatteryState', + 'sensor_msgs/msg/CameraInfo': 'gz.msgs.CameraInfo', + 'sensor_msgs/msg/FluidPressure': 'gz.msgs.FluidPressure', + 'sensor_msgs/msg/Imu': 'gz.msgs.IMU', + 'sensor_msgs/msg/Image': 'gz.msgs.Image', + 'sensor_msgs/msg/JointState': 'gz.msgs.Model', + 'sensor_msgs/msg/LaserScan': 'gz.msgs.LaserScan', + 'sensor_msgs/msg/MagneticField': 'gz.msgs.Magnetometer', + 'sensor_msgs/msg/PointCloud2': 'gz.msgs.PointCloudPacked', + 'tf2_msgs/msg/TFMessage': 'gz.msgs.Pose_V', + 'trajectory_msgs/msg/JointTrajectory': 'gz.msgs.JointTrajectory'} @staticmethod def read_models(): @@ -78,12 +80,11 @@ def read_models(): for key in ('GZ_VERSION', 'IGNITION_VERSION'): if key in environ: if environ[key] == 'fortress': - models = silent_exec('ign model --list') + GazeboBridge.gz_exec = 'ign' else: - models = silent_exec('gz model --list') + GazeboBridge.gz_exec = 'gz' break - else: - models = silent_exec('ign model --list') + silent_exec('gz model --list') + models = silent_exec(GazeboBridge.gz_exec + ' model --list') for line in models: if line.startswith('Requesting'): @@ -96,6 +97,13 @@ def read_models(): GazeboBridge.models = [line.strip('- ') for line in models if line.startswith('- ')] print('\033[92mGazeboBridge: connected to a running Gazebo instance\033[0m') + @staticmethod + def use_prefix(prefix): + ''' + Tell GazeboBridge to use either ign or gz prefix (default) + ''' + GazeboBridge.gz_exec = prefix + def __init__(self, gz_topic, ros_topic, msg, direction): ''' Create a bridge instance to be passed to SimpleLauncher.create_gz_bridge @@ -124,7 +132,7 @@ def __init__(self, gz_topic, ros_topic, msg, direction): self.direction = direction self.ros_msg = msg - def yaml(self, gz_ign): + def yaml(self): ''' use YAML-based config for other bridges - topic_name: "chatter" @@ -134,18 +142,20 @@ def yaml(self, gz_ign): direction: BIDIRECTIONAL # Default "BIDIRECTIONAL" - Bridge both directions # "IGN_TO_ROS" - Bridge Ignition topic to ROS # "ROS_TO_IGN" - Bridge ROS topic - - takes gz prefix that is either gz or ign depending on current ROS 2 version ''' direction = 'BIDIRECTIONAL' if self.direction == self.gz2ros: - direction = f'{gz_ign.upper()}_TO_ROS' + direction = f'{self.gz_exec.upper()}_TO_ROS' elif self.direction == self.ros2gz: - direction = f'ROS_TO_{gz_ign.upper()}' + direction = f'ROS_TO_{self.gz_exec.upper()}' + + gz_msg = self.msg_map[self.ros_msg] + if self.gz_exec == 'ign': + gz_msg = gz_msg.replace('gz.','ignition.') - return SimpleSubstitution(f'- {gz_ign}_topic_name: ', self.gz_topic, '\n', - f' {gz_ign}_type_name: ', self.msg_map[self.ros_msg], '\n', + return SimpleSubstitution(f'- {self.gz_exec}_topic_name: ', self.gz_topic, '\n', + f' {self.gz_exec}_type_name: ', gz_msg, '\n', ' ros_topic_name: ',self.ros_topic, '\n', ' ros_type_name: ', self.ros_msg, '\n', ' direction: ', direction, '\n')