Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
An example on performing a 3D projection using Ignitionrobotics, gym-ignition and ropy.
from scenario import gazebo as scenario_gazebo
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Circle
from scipy.spatial.transform import Rotation as R
import ropy.transform as tf
import ropy.ignition as ign
def camera_parser(msg):
image_msg = ign.messages.Image()
image_msg.parse(msg[2])
image = np.frombuffer(image_msg.data, dtype=np.uint8)
image = image.reshape((image_msg.height, image_msg.width, 3))
return image
def camera_info_parser(msg):
return ign.messages.CameraInfo().parse(msg[2])
gazebo = scenario_gazebo.GazeboSimulator(step_size=0.001, rtf=1.0, steps_per_run=1)
assert gazebo.insert_world_from_sdf("./world1.sdf")
gazebo.initialize()
# Fix: available topics seem to only be updated at the end
# of a step. This allows the subscribers to find the topic's
# address
gazebo.run(paused=True)
# get extrinsic matrix
camera = gazebo.get_world("camera_sensor").get_model("camera").get_link("link")
cam_pos_world = np.array(camera.position())
cam_ori_world_quat = np.array(camera.orientation())[[1, 2, 3, 0]]
cam_ori_world = R.from_quat(cam_ori_world_quat).as_euler("xyz")
camera_frame_world = np.stack((cam_pos_world, cam_ori_world)).ravel()
extrinsic_transform = tf.coordinates.transform(camera_frame_world)
# scene objects
box = gazebo.get_world("camera_sensor").get_model("box")
with ign.Subscriber("/camera", parser=camera_parser) as camera_topic, ign.Subscriber(
"/camera_info", parser=camera_info_parser
) as cam_info_topic:
# Fix: the first step doesn't generate messages.
# I don't exactly know why; I assume it has
# to do with subscriptions being updated at the end
# of the sim loop instead of the beginning?
gazebo.run(paused=True)
img = camera_topic.recv()
# get intrinsic matrix
cam_info = cam_info_topic.recv()
rot = tf.coordinates.transform(np.array((0, 0, 0, -np.pi / 2, -np.pi / 2, 0)))
projection = np.array(cam_info.projection.p).reshape((3, 4))
intrinsic_transform = np.matmul(projection, rot)
# project cone
box_corner = np.array(box.base_position()) + np.array((0.5, -0.5, 0.5))
pos_world = tf.homogenize(box_corner)
pos_cam = np.matmul(extrinsic_transform, pos_world)
pos_px_hom = np.matmul(intrinsic_transform, pos_cam)
cube_pos_px = tf.cartesianize(pos_px_hom)
# visualize
fig, ax = plt.subplots(1)
ax.imshow(img)
ax.add_patch(Circle(cube_pos_px, radius=6))
plt.show()
gazebo.close()
<sdf version='1.7'>
<world name='camera_sensor'>
<physics name='1ms' type='ignored'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<plugin name='ignition::gazebo::systems::Physics' filename='ignition-gazebo-physics-system'/>
<plugin name='ignition::gazebo::systems::Sensors' filename='ignition-gazebo-sensors-system'>
<render_engine>ogre</render_engine>
</plugin>
<plugin name='ignition::gazebo::systems::UserCommands' filename='ignition-gazebo-user-commands-system'/>
<plugin name='ignition::gazebo::systems::SceneBroadcaster' filename='ignition-gazebo-scene-broadcaster-system'/>
<scene>
<ambient>1 1 1 1</ambient>
<background>0.8 0.8 0.8 1</background>
<grid>1</grid>
<shadows>1</shadows>
</scene>
<gui fullscreen='0'>
<plugin name='3D View' filename='GzScene3D'>
<ignition-gui>
<title>3D View</title>
<property type='bool' key='showTitleBar'>0</property>
<property type='string' key='state'>docked</property>
</ignition-gui>
<engine>ogre</engine>
<scene>scene</scene>
<ambient_light>1.0 1.0 1.0</ambient_light>
<background_color>0.8 0.8 0.8</background_color>
<camera_pose>-6 0 6 0 0.5 0</camera_pose>
</plugin>
<plugin name='World control' filename='WorldControl'>
<ignition-gui>
<title>World control</title>
<property type='bool' key='showTitleBar'>0</property>
<property type='bool' key='resizable'>0</property>
<property type='double' key='height'>72</property>
<property type='double' key='width'>121</property>
<property type='double' key='z'>1</property>
<property type='string' key='state'>floating</property>
<anchors target='3D View'>
<line own='left' target='left'/>
<line own='bottom' target='bottom'/>
</anchors>
</ignition-gui>
<play_pause>1</play_pause>
<step>1</step>
<start_paused>1</start_paused>
</plugin>
<plugin name='World stats' filename='WorldStats'>
<ignition-gui>
<title>World stats</title>
<property type='bool' key='showTitleBar'>0</property>
<property type='bool' key='resizable'>0</property>
<property type='double' key='height'>110</property>
<property type='double' key='width'>290</property>
<property type='double' key='z'>1</property>
<property type='string' key='state'>floating</property>
<anchors target='3D View'>
<line own='right' target='right'/>
<line own='bottom' target='bottom'/>
</anchors>
</ignition-gui>
<sim_time>1</sim_time>
<real_time>1</real_time>
<real_time_factor>1</real_time_factor>
<iterations>1</iterations>
</plugin>
<plugin name='Image Display' filename='ImageDisplay'>
<ignition-gui>
<property key='state' type='string'>docked</property>
</ignition-gui>
</plugin>
</gui>
<light name='sun' type='directional'>
<cast_shadows>1</cast_shadows>
<pose>0 0 10 0 -0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
<spot>
<inner_angle>0</inner_angle>
<outer_angle>0</outer_angle>
<falloff>0</falloff>
</spot>
</light>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>
<model name='ground_plane'>
<static>1</static>
<link name='link'>
<collision name='collision'>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<surface>
<friction>
<ode/>
</friction>
<contact/>
</surface>
</collision>
<visual name='visual'>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
<plugin name='__default__' filename='__default__'/>
</visual>
</link>
<plugin name='__default__' filename='__default__'/>
<pose>0 0 0 0 -0 0</pose>
</model>
<model name='box'>
<pose>0 -1 0.5 0 -0 0</pose>
<link name='box_link'>
<inertial>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
<mass>1</mass>
</inertial>
<collision name='box_collision'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<surface>
<friction>
<ode/>
</friction>
<contact/>
</surface>
</collision>
<visual name='box_visual'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>1 0 0 1</diffuse>
<specular>1 0 0 1</specular>
</material>
<plugin name='__default__' filename='__default__'/>
</visual>
</link>
<plugin name='__default__' filename='__default__'/>
</model>
<model name='sphere'>
<static>1</static>
<pose>3 0 0.5 0 -0 0</pose>
<link name='sphere_link'>
<collision name='sphere_collision'>
<geometry>
<sphere>
<radius>0.125</radius>
</sphere>
</geometry>
<surface>
<friction>
<ode/>
</friction>
<contact/>
</surface>
</collision>
<visual name='sphere_visual'>
<geometry>
<sphere>
<radius>0.125</radius>
</sphere>
</geometry>
<material>
<ambient>0 1 0 1</ambient>
<diffuse>0 1 0 1</diffuse>
<specular>0 1 0 1</specular>
</material>
<cast_shadows>0</cast_shadows>
<plugin name='__default__' filename='__default__'/>
</visual>
</link>
<plugin name='__default__' filename='__default__'/>
</model>
<model name='camera'>
<static>1</static>
<pose>4 -0 1 0 -0 3.14</pose>
<link name='link'>
<pose>0.05 0.05 0.05 0 -0 0</pose>
<inertial>
<mass>0.1</mass>
<inertia>
<ixx>0.000166667</ixx>
<iyy>0.000166667</iyy>
<izz>0.000166667</izz>
<ixy>0</ixy>
<ixz>0</ixz>
<iyz>0</iyz>
</inertia>
</inertial>
<collision name='collision'>
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
<surface>
<friction>
<ode/>
</friction>
<contact/>
</surface>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
<plugin name='__default__' filename='__default__'/>
</visual>
<sensor name='camera' type='camera'>
<camera>
<horizontal_fov>1.047</horizontal_fov>
<image>
<width>320</width>
<height>240</height>
</image>
<clip>
<near>0.1</near>
<far>100</far>
</clip>
</camera>
<always_on>1</always_on>
<update_rate>30</update_rate>
<visualize>1</visualize>
<topic>camera</topic>
<plugin name='__default__' filename='__default__'/>
</sensor>
</link>
<plugin name='__default__' filename='__default__'/>
</model>
</world>
</sdf>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment