Gazebo | Ignition | Community
Ask Your Question
0

Add urdf to world using a python script while gazebo is running (/world/my_world/create service + EntityFactory message)

asked 2022-06-02 07:21:59 -0600

marc-marc gravatar image

Hi

I'm trying to add a cube to my gazebo scene programmatically while gazebo is running.

I found that ignition has the service /world/my_world/create service that accepts new urdf/sdf files.

However, I'm struggling to send the correct request in python, since I don't know how to install the ignition messages on my system and how the request has to be structured.

Here is a tutorial in C++: https://gazebosim.org/api/gazebo/6.9/...

Here is the terminal call which spawns a sphere in the world:

ign service -s /world/empty/create \
--reqtype ignition.msgs.EntityFactory \
--reptype ignition.msgs.Boolean \
--timeout 300 \
--req 'sdf: '\
'"<?xml version=\"1.0\" ?>'\
'<sdf version=\"1.6\">'\
'<model name=\"spawned_model\">'\
'<link name=\"link\">'\
'<visual name=\"visual\">'\
'<geometry><sphere><radius>1.0</radius></sphere></geometry>'\
'</visual>'\
'<collision name=\"visual\">'\
'<geometry><sphere><radius>1.0</radius></sphere></geometry>'\
'</collision>'\
'</link>'\
'</model>'\
'</sdf>" '\
'pose: {position: {z: 10}} '\
'name: "new_name" '\
'allow_renaming: true'

My plan:

  1. Install ignition messages (But how?)
  2. Setup ign bridge to forward message /world/my_world/create from ign to ROS2 (in launch file)

    ros_ign_bridge (clock -> ROS 2)

    Node(
        package="ros_ign_bridge",
        executable="parameter_bridge",
        output="log",
        arguments=[
            "/world/my_world/create@ignition.msgs.EntityFactory@ignition.msgs.EntityFactory",
            "--ros-args",
            "--log-level",
            log_level,
        ],
        parameters=[{"use_sim_time": use_sim_time}],
    ),
    
  3. Use python script to spawn cube.urdf in gazebo world

Importing the library:

from ignition-messages import EntityFactory # What is the correct syntax/name of the library?

Publishing the message: How do I construct a valid message?

my_sdf = """
            <?xml version="1.0" ?>
            <sdf version='1.7'>
            <model name='sphere'>
                <link name='link'>
                <pose>0 0 0.5 0 0 0</pose>
                <visual name='visual'>
                    <geometry><sphere><radius>1</radius></sphere></geometry>
                </visual>
                <collision name='collision'>
                    <geometry><sphere><radius>1</radius></sphere></geometry>
                </collision>
                </link>
            </model>
            </sdf>"""

            publisher = self.create_publisher(EntityFactory, '/world/my_world/create', 1)

            msg = EntityFactory()
            while rclpy.ok():
                msg.data = 'Hello World:  ' # How do I construct a valid message?
                msg.model = my_sdf
                publisher.publish(msg)

This is the message definition: https://github.com/gazebosim/gz-msgs/...

Thanks in advance for your help.

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2022-06-02 12:35:54 -0600

kakcalu13 gravatar image

I legit had no idea this exists! I have no idea how this works. I gotta test it ASAP! Thanks for the link!

Anyway I use python to load robot by using import os

So, I use like this

first_part = "ign service -s /world/free_world/create --reqtype ignition.msgs.EntityFactory --reptype ignition.msgs.Boolean --timeout 300 --req 'sdf_filename:'\'\""
second_part = model_name + ".sdf\" pose: {position: { x: 0.05, y"
third_part = ": " + y + ", z: " + z + "}}\' &"
add_model = first_part + second_part + third_part
os.system(add_model)

In another word, I used command line exact as this:

ign service -s /world/free_world/create --reqtype ignition.msgs.EntityFactory --reptype ignition.msgs.Boolean --timeout 300 --req 'sdf_filename:''"models/sdf/freenove_smart_car.sdf" pose: {position: { x: 0.05, y: 4.12, z: 0.325}}' &

Which would work in your command line as long as you change the free_world to your world's name and files location/name.

Basically, I use command line within python to get all data or controls from Gazebo.

Not sure how much this fits your needs.

P.S.

I never had any luck on this one, but you can try this one. This is still under heavy development, I think.

I know this is not the answer but I hope this suggestion helps.

edit flag offensive delete link more

Comments

Thanks for your code. It works like a charm.

I was struggling a lot with the single quotes and escaping characters in the --req part.

Next I want to move or set the pose of the spawned objects in the scene. I assume there is another service for this:

/world/ign_moveit2_follow_target_world/control
/world/ign_moveit2_follow_target_world/control/state
/world/ign_moveit2_follow_target_world/create

If you already have a working command, I would appreciate your insights. Best, Marc

marc-marc gravatar imagemarc-marc ( 2022-06-03 04:47:11 -0600 )edit

Looks like you got it working!

This is one of ways to run command using multiple special characters.

As for the single or double quotes and if you don't want to do this way..you can use \" so for example

ign service -s /world/free_world/create --reqtype ignition.msgs.EntityFactory --reptype ignition.msgs.Boolean --timeout 300 --req \'sdf_filename:'\'\"models/sdf/freenove_smart_car.sd\f" pose: {position: { x: 0.05, y: 4.12, z: 0.325}}' &

kakcalu13 gravatar imagekakcalu13 ( 2022-06-03 06:44:36 -0600 )edit

As for the move the pose (or maybe set of pose?), you can use this example: https://github.com/gazebosim/gz-sim/b...

kakcalu13 gravatar imagekakcalu13 ( 2022-06-03 06:45:50 -0600 )edit

The setting of the pose works with your command. However calling this command multiple times in a for-loop to set the position of items on a conveyor belt is really slow. Do you have an idea on how to solve high-frequency position updates? I opened a new question: https://answers.gazebosim.org/questio...

marc-marc gravatar imagemarc-marc ( 2022-06-22 09:04:45 -0600 )edit
0

answered 2022-06-03 05:18:09 -0600

marc-marc gravatar image

updated 2022-06-03 05:18:51 -0600

Here is the python code to set the pose of any object using a cli command:

   first_part = "ign service -s /world/my_world/set_pose --reqtype ignition.msgs.Pose --reptype ignition.msgs.Boolean --timeout 300 --req 'name: \""
    model_name = 'spawned_model'
    second_part = model_name + "\", position: {z: 5.0}'"
    system(first_part+second_part)
edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2022-06-02 06:35:25 -0600

Seen: 559 times

Last updated: Jun 03 '22