Skip to content

Commit

Permalink
list[GazeboBridge] crashes in 20.04
Browse files Browse the repository at this point in the history
  • Loading branch information
@oKermorgant
oKermorgant committed May 16, 2023
1 parent 46ea11f commit 53f628f
Show file tree
Hide file tree
Showing 2 changed files with 53 additions and 45 deletions.
10 changes: 4 additions & 6 deletions simple_launch/__init__.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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
Expand All @@ -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]
Expand All @@ -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
Expand Down Expand Up @@ -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:
Expand Down
88 changes: 49 additions & 39 deletions simple_launch/gazebo.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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():
Expand All @@ -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'):
Expand All @@ -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
Expand Down Expand Up @@ -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"
Expand All @@ -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')
Expand Down

0 comments on commit 53f628f

Please sign in to comment.