pcg-spawn-sdf-model

#!/usr/bin/env python
# Copyright (c) 2019 - The Procedural Generation for Gazebo authors
# For information on the respective copyright owner see the NOTICE file
#
# 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.
import argparse
import rospy
from std_srvs.srv import Trigger, TriggerResponse
from pcg_gazebo.task_manager import Server


def spawn_sdf_model(req, robot_name, param_name, x, y, z, roll, pitch, yaw):
    rospy.loginfo('Spawning SDF model from parameter {}'.format(param_name))

    rospy.loginfo('Spawning robot <{}> from parameter <{}>'.format(
        robot_name, param_name))

    name = 'spawn_sdf_model'

    server = Server()
    # TODO: Use user input to set host and port
    server.create_simulation(
        name,
        ros_host='localhost',
        ros_port=11311,
        gazebo_host='localhost',
        gazebo_port=11345,
        anonymous=True,
        output_log_dir=None)

    simulation = server.get_simulation(name)
    simulation.init_task(
        name='spawn_sdf_model',
        command='rosrun gazebo_ros spawn_model -x {x} -y {y} -z {z} -R {roll} -P {pitch} -Y {yaw} -sdf -param {param} -model {robot_name}',
        has_gazebo=False,
        params=dict(
            x=x,
            y=y,
            z=z,
            roll=roll,
            pitch=pitch,
            yaw=yaw,
            param=param_name,
            robot_name=robot_name))
    simulation.run_all_tasks()

    # TODO: Unpause simulation if unpause-simulation is True

    return TriggerResponse(True, 'OK')


if __name__ == '__main__':
    parser = argparse.ArgumentParser(
        description='Spawn SDF model after trigger')
    parser.add_argument(
        '--param_name',
        type=str,
        help='Input parameter name where the SDF content is stored')
    parser.add_argument(
        '--robot_name',
        type=str,
        help='Name of the robot model')
    parser.add_argument(
        '--x',
        type=float,
        default=0.0,
        help='X coordinate of the spawning position in meters')
    parser.add_argument(
        '--y',
        type=float,
        default=0.0,
        help='Y coordinate of the spawning position in meters')
    parser.add_argument(
        '--z',
        type=float,
        default=0.0,
        help='Z coordinate of the spawning position in meters')
    parser.add_argument(
        '--roll',
        type=float,
        default=0.0,
        help='Roll angle of the spawning position in radians')
    parser.add_argument(
        '--pitch',
        type=float,
        default=0.0,
        help='Pitch angle of the spawning position in radians')
    parser.add_argument(
        '--yaw',
        type=float,
        default=0.0,
        help='Yaw angle of the spawning position in radians')
    parser.add_argument(
        '--unpause-simulation',
        action='store_true',
        help='Unpause simulation once the model has been spawned')

    args = parser.parse_args()

    rospy.init_node('spawn_sdf_model', anonymous=True)

    rospy.loginfo('Waiting for trigger to service to spawn a SDF model')

    srv = rospy.Service(
        'spawn_sdf_model',
        Trigger,
        lambda req: spawn_sdf_model(
            req,
            args.robot_name,
            args.param_name,
            args.x,
            args.y,
            args.z,
            args.roll,
            args.pitch,
            args.yaw))

    rospy.spin()