diff --git a/.ros1_unported/gazebo_ros/scripts/spawn_model b/.ros1_unported/gazebo_ros/scripts/spawn_model
deleted file mode 100755
index 226988e66..000000000
--- a/.ros1_unported/gazebo_ros/scripts/spawn_model
+++ /dev/null
@@ -1,233 +0,0 @@
-#!/usr/bin/env python
-#
-# Copyright 2018 Open Source Robotics Foundation
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-#
-# Desc: helper script for spawning models in gazebo
-# Author: John Hsu, Dave Coleman
-#
-import rospy
-import sys
-import os
-import argparse
-import xml
-from urlparse import urlsplit, SplitResult
-from gazebo_ros import gazebo_interface
-from gazebo_msgs.msg import ModelStates
-from gazebo_msgs.srv import DeleteModel
-from std_srvs.srv import Empty
-from geometry_msgs.msg import Pose, Quaternion
-from tf.transformations import quaternion_from_euler
-
-
-class SpawnModelNode():
- '''
- Node to spawn a model in Gazebo using the ROS API
- '''
- MODEL_DATABASE_TEMPLATE = """\
-
-
-
- model://{}
-
-
-"""
-
- def __init__(self):
- parser = argparse.ArgumentParser(description='Spawn a model in gazebo using the ROS API')
- xmlformat = parser.add_mutually_exclusive_group(required=True)
- xmlformat.add_argument('-urdf', action='store_true', help='Incoming xml is in urdf format')
- xmlformat.add_argument('-sdf', action='store_true', help='Incoming xml is in sdf format')
- source = parser.add_mutually_exclusive_group(required=True)
- source.add_argument('-file', type=str, metavar='FILE_NAME', help='Load model xml from file')
- source.add_argument('-param', type=str, metavar='PARAM_NAME', help='Load model xml from ROS parameter')
- source.add_argument('-database', type=str, metavar='MODEL_NAME',
- help='Load model XML from specified model in Gazebo Model Database')
- source.add_argument('-stdin', action='store_true', help='Load model from stdin')
- parser.add_argument('-model', required=True, type=str, metavar='MODEL_NAME', help='Name of model to spawn')
- parser.add_argument('-reference_frame', type=str, default='',
- help='Name of the model/body where initial pose is defined.\
- If left empty or specified as "world", gazebo world frame is used')
- parser.add_argument('-gazebo_namespace', type=str, default='/gazebo',
- help='ROS namespace of gazebo offered ROS interfaces. Defaults to /gazebo/')
- parser.add_argument('-robot_namespace', type=str, default=rospy.get_namespace(),
- help='change ROS namespace of gazebo-plugins')
- parser.add_argument('-unpause', action='store_true',
- help='!!!Experimental!!! unpause physics after spawning model')
- parser.add_argument('-wait', type=str, metavar='MODEL_NAME', help='!!!Experimental!!! wait for model to exist')
- parser.add_argument('-x', type=float, default=0, help='x component of initial position, meters')
- parser.add_argument('-y', type=float, default=0, help='y component of initial position, meters')
- parser.add_argument('-z', type=float, default=0, help='z component of initial position, meters')
- parser.add_argument('-R', type=float, default=0, help='roll angle of initial orientation, radians')
- parser.add_argument('-P', type=float, default=0, help='pitch angle of initial orientation, radians')
- parser.add_argument('-Y', type=float, default=0, help='yaw angle of initial orientation, radians')
- parser.add_argument('-J', dest='joints', default=[], action='append', metavar=('JOINT_NAME', 'JOINT_POSITION'),
- type=str, nargs=2, help='initialize the specified joint at the specified position')
- parser.add_argument('-package_to_model', action='store_true',
- help='convert urdf gazebo_dev
gazebo_msgs
rclcpp
+ rclpy
std_srvs
tinyxml_vendor
diff --git a/gazebo_ros/scripts/spawn_entity.py b/gazebo_ros/scripts/spawn_entity.py
new file mode 100755
index 000000000..112752a80
--- /dev/null
+++ b/gazebo_ros/scripts/spawn_entity.py
@@ -0,0 +1,347 @@
+#!/usr/bin/env python3
+#
+# Copyright 2019 Open Source Robotics Foundation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+# Desc: helper script for spawning entities in gazebo
+# Author: John Hsu, Dave Coleman
+#
+import argparse
+import math
+import os
+import sys
+from urllib.parse import SplitResult, urlsplit
+from xml.etree import ElementTree
+
+from gazebo_msgs.msg import ModelStates
+# from gazebo_msgs.srv import DeleteEntity
+# from gazebo_msgs.srv import SetModelConfiguration
+from gazebo_msgs.srv import SpawnEntity
+from geometry_msgs.msg import Pose
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSDurabilityPolicy
+from std_msgs.msg import String
+from std_srvs.srv import Empty
+
+
+class SpawnEntityNode(Node):
+ # Node to spawn an entity in Gazebo.
+ MODEL_DATABASE_TEMPLATE = """\
+
+
+
+ model://{}
+
+
+ """
+
+ def __init__(self, args):
+ super().__init__('spawn_entity')
+ parser = argparse.ArgumentParser(
+ description='Spawn an entity in gazebo. Gazebo must be started with gazebo_ros_init,\
+ gazebo_ros_factory and gazebo_ros_state for all functionalities to work')
+ source = parser.add_mutually_exclusive_group(required=True)
+ source.add_argument('-file', type=str, metavar='FILE_NAME',
+ help='Load entity xml from file')
+ source.add_argument('-topic', type=str, metavar='TOPIC_NAME',
+ help='Load entity xml published on topic')
+ source.add_argument('-database', type=str, metavar='ENTITY_NAME',
+ help='Load entity XML from specified entity in GAZEBO_MODEL_PATH \
+ or Gazebo Model Database')
+ source.add_argument('-stdin', action='store_true', help='Load entity from stdin')
+ parser.add_argument('-entity', required=True, type=str, metavar='ENTITY_NAME',
+ help='Name of entity to spawn')
+ parser.add_argument('-reference_frame', type=str, default='',
+ help='Name of the model/body where initial pose is defined.\
+ If left empty or specified as "world", gazebo world frame is used')
+ parser.add_argument('-gazebo_namespace', type=str, default='',
+ help='ROS namespace of gazebo offered ROS interfaces. \
+ Default is without any namespace')
+ parser.add_argument('-robot_namespace', type=str, default=self.get_namespace(),
+ help='change ROS namespace of gazebo-plugins')
+ parser.add_argument('-unpause', action='store_true',
+ help='unpause physics after spawning entity')
+ parser.add_argument('-wait', type=str, metavar='ENTITY_NAME',
+ help='Wait for entity to exist')
+ parser.add_argument('-x', type=float, default=0,
+ help='x component of initial position, meters')
+ parser.add_argument('-y', type=float, default=0,
+ help='y component of initial position, meters')
+ parser.add_argument('-z', type=float, default=0,
+ help='z component of initial position, meters')
+ parser.add_argument('-R', type=float, default=0,
+ help='roll angle of initial orientation, radians')
+ parser.add_argument('-P', type=float, default=0,
+ help='pitch angle of initial orientation, radians')
+ parser.add_argument('-Y', type=float, default=0,
+ help='yaw angle of initial orientation, radians')
+
+ # TODO(shivesh): Wait for /set_model_configuration
+ # (https://github.com/ros-simulation/gazebo_ros_pkgs/issues/779)
+ # parser.add_argument('-J', dest='joints', default=[], action='append',
+ # metavar=('JOINT_NAME', 'JOINT_POSITION'), type=str, nargs=2,
+ # help='initialize the specified joint at the specified position')
+
+ parser.add_argument('-package_to_model', action='store_true', help='convert urdf \
+