How to add a ROS camera to IRIS for gazebo simulation?

kiranarcot · December 12, 2017, 10:14pm

I would like to add a ROS camera sensor to the IRIS quad copter in gazebo simulation. I was able to add the camera at the bottom of the xacro file

https://github.com/PX4/sitl_gazebo/blob/b052c97f7c3a2c39ab8ec06ae79c66431f16c659/models/rotors_description/urdf/iris.xacro

like so

<joint name="camera_joint" type="fixed">
    <axis xyz="1 0 0" />
    <origin xyz="0 0 0.07" rpy="0 0 0" />
    <parent link="base_link"/>
    <child link="camera_link"/>
  </joint>

  <!-- Camera -->
  <link name="camera_link">
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
	<box size="${camera_link_box} ${camera_link_box} ${camera_link_box}"/>
      </geometry>
    </collision>

    <visual>
      <geometry>
	<box size="${camera_link_box} ${camera_link_box} ${camera_link_box}"/>
      </geometry>
    </visual>

    <inertial>
      <mass value="1e-6" />
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <inertia ixx="1e-7" ixy="0" ixz="0" iyy="1e-7" iyz="0" izz="1e-7" />
    </inertial>
  </link>

  <!-- generate an optical frame http://www.ros.org/reps/rep-0103.html#suffix-frames
      so that ros and opencv can operate on the camera frame correctly -->

  <joint name="camera_optical_joint" type="fixed">
  <!--   these values have to be these values otherwise the gazebo camera image
        won't be aligned properly with the frame it is supposedly originating from -->
    <origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>
    <parent link="camera_link"/>
    <child link="camera_link_optical"/>
  </joint>

  <link name="camera_link_optical">
  </link>

And the result in gazebo is this

The camera starts transmitting images to rviz too. However, when I want to fly a mission via GPS way points I get

PREFLIGHT FAIL: ACCELS INCONSISTENT - CHECK CAL"

message and the simulated IRIS cannot take off.

So my question is how/what do I update whatever needs updating to make the ACCELS consistent with the added camera to the simulated IRIS.

kiranarcot · January 19, 2018, 6:48pm

I figured it out. The correct way to use a ROS camera on the Iris is to make use of the

https://github.com/PX4/sitl_gazebo/blob/master/models/rotors_description/urdf/component_snippets.xacro

macros and place the camera in

https://github.com/PX4/sitl_gazebo/blob/master/models/rotors_description/urdf/iris_base.xacro

and place it above the ADIS16448 IMU

Then you can see multiple UAVs in gazebo

Then you can view them in rviz like below

Hope this helps, if anyone is looking for this.

James_Mchugh · March 20, 2018, 6:15pm

Can you show your launch file? I have been trying to get this to work, but Gazebo is having trouble parsing the urdf generated from the xacro file. Thank you.

James_Mchugh · March 23, 2018, 4:47pm

I found out what I was doing wrong. There was a mistake in my edited xacro file. Sorry. All seems to be working now.

gajena · March 27, 2018, 7:59pm

Hi James

I am having the same problem so please can you share your code of models file

thank you

jmorley1 · March 28, 2018, 5:53pm

I am also having difficulty showing the iris model in rviz. Anyone know where I can start? What modifications to files need to be made?

Is there anything in particular that needs to be done with transforms to display in rviz?

Any help is greatly appreciated!

gajena · April 12, 2018, 7:30pm

In this, you learn how to install a simple camera facing downwards
at bottom of IRIS quadrotor model in Gazebo and publish its frames on ROS(Kinetic).

This tutorial assumes your have cloned Firmware folder in ~/src/Firmware and
have successfully all its dependencies.

I) To install a camera in IRIS proceed as follows:-

Open iris_base.xacro present in ~/src/Firmware/Tools/sitl_gazebo/models/rotors_description/urdf/
Search in iris_base.xacro and add the
following code just above it and save it.

<xacro:camera_macro
    namespace="${namespace}"
    parent_link="base_link"
    camera_suffix="red_iris"
    frame_rate="30.0"
    horizontal_fov="1.3962634"
    image_width="800"
    image_height="800"
    image_format="R8G8B8"
    min_distance="0.02"
    max_distance="300"
    noise_mean="0.0"
    noise_stddev="0.007"
    enable_visual="1"
    >
    <box size="0.05 0.05 0.05" />
    <origin xyz="0 0 -0.07" rpy="0 1.57079 0"/>
    </xacro:camera_macro>

Now, the camera is installed. You can take assistance of component_snippets.xacro to
understand the values.

II) If take a look a at component_snippets.xacro, you will come to know that
frames are published under /iris/camera_red_iris/image_raw , so to have it
on ros start simulator by the procedure mentioned in https://dev.px4.io/en/simulation/ros_interface.html
in What’s Happening behind the scenes section.

Vrinda · October 26, 2018, 3:06pm

@jmorley1 were you able to view the iris model in rviz ?
Thanks for your time !

michou214 · November 1, 2018, 10:22am

Hello, I’m new in this field but for a project I have to have an Iris model with a camera. So I tried to follow yours instructions (message #1 and #6) by doing:

EDIT (05/11/2018) => Found a solution to make it works:
0) Follow this tutorial, under “Build Gazebo Plugins (all operating systems)”

In /home/username/src/Firmware/Tools/sitl_gazebo/models/rotors_description/urdf/
No need to modify iris.xacro (in this location)
No need to modifiy iris.sdf (found at an other location as I saw it in other articles)
Open to edit iris_base.xacro. Before the  , I added the following bloc of code:


<xacro:camera_macro
namespace="${namespace}"
parent_link="base_link"
camera_suffix="red_iris"
frame_rate="30.0"
horizontal_fov="1.3962634"
image_width="800"
image_height="800"
image_format="R8G8B8"
min_distance="0.02"
max_distance="300"
noise_mean="0.0"
noise_stddev="0.007"
enable_visual="1"
>
<box size="0.05 0.05 0.05" />
<origin xyz="0 0 -0.07" rpy="0 1.57079 0"/>
</xacro:camera_macro>

Save it and launch your launch files or your commands as before. It should works.

I hope it will help you. When I’ll find the way to use ROS with it, I’d update the solution.

Leonardo_Garberoglio · December 16, 2018, 2:16pm

Just let me add some extra steps in case that someone (like me) can’t make it work.

modification to iris_base.xacro is ok but I was getting error with libgazebo_ros_camera.so plugin.
I’ve installed ros-kinetic-gazebo-ros-pkgs and ros-kinetic-gazebo-ros-control as suggest here:
http://gazebosim.org/tutorials?tut=ros_installing
but that does not solve my problem (maybe I have to add some GAZEBO PLUGIN PATH)
So I follow the instruction for building from source and add
export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:~/catkin_ws/build/gazebo_ros_pkgs/gazebo_plugins/
So, now I can launch the iris and it found the plugin.

The next step is how to view the camera image on rviz.
I run rosrun rviz rviz and add a camera object, but there is a message “No image received”
The problem here is with links and transformations. there is a base_link frame and a child link named iris_1/camera_red_iris_link were the image is published. So I need to add a transformation between that frames.
I run
rosrun tf static_transform_publisher 0 0 0 0 0 0 base_link /iris_1/camera_red_iris_link 50
and now I can see camera image on rviz.

Thank

saucompeng · February 28, 2019, 12:30pm

Hi @michou214, thanks for your instructions, I added camera to iris model succesfully, No error when simulation start. But then, When I want to arm model, it gives warning and error like at following lines. What is the problem ?

pxh> commander arm
WARN [commander] Preflight Fail: High Accelerometer Bias
ERROR [commander] arming failed

/diagnostic output

header: 
  seq: 135
  stamp: 
    secs: 597
    nsecs: 164000000
  frame_id: ''
status: 
  - 
    level: 0
    name: "mavros: FCU connection"
    message: "connected"
    hardware_id: "udp://localhost:14540@localhost:14557"
    values: 
      - 
        key: "Received packets:"
        value: "61099"
      - 
        key: "Dropped packets:"
        value: "0"
      - 
        key: "Buffer overruns:"
        value: "0"
      - 
        key: "Parse errors:"
        value: "0"
      - 
        key: "Rx sequence number:"
        value: "248"
      - 
        key: "Tx sequence number:"
        value: "59"
      - 
        key: "Rx total bytes:"
        value: "3401880"
      - 
        key: "Tx total bytes:"
        value: "70198"
      - 
        key: "Rx speed:"
        value: "25661.000000"
      - 
        key: "Tx speed:"
        value: "537.000000"
  - 
    level: 0
    name: "mavros: GPS"
    message: "3D fix"
    hardware_id: "udp://localhost:14540@localhost:14557"
    values: 
      - 
        key: "Satellites visible"
        value: "10"
      - 
        key: "Fix type"
        value: "3"
      - 
        key: "EPH (m)"
        value: "0.00"
      - 
        key: "EPV (m)"
        value: "0.00"
  - 
    level: 0
    name: "mavros: Heartbeat"
    message: "Normal"
    hardware_id: "udp://localhost:14540@localhost:14557"
    values: 
      - 
        key: "Heartbeats since startup"
        value: "130"
      - 
        key: "Frequency (Hz)"
        value: "1.000000"
      - 
        key: "Vehicle type"
        value: "Quadrotor"
      - 
        key: "Autopilot type"
        value: "PX4 Autopilot"
      - 
        key: "Mode"
        value: "AUTO.LOITER"
      - 
        key: "System status"
        value: "Standby"
  - 
    level: 0
    name: "mavros: System"
    message: "Normal"
    hardware_id: "udp://localhost:14540@localhost:14557"
    values: 
      - 
        key: "Sensor present"
        value: "0x00000000"
      - 
        key: "Sensor enabled"
        value: "0x00000000"
      - 
        key: "Sensor helth"
        value: "0x00000000"
      - 
        key: "CPU Load (%)"
        value: "0.0"
      - 
        key: "Drop rate (%)"
        value: "0.0"
      - 
        key: "Errors comm"
        value: "0"
      - 
        key: "Errors count #1"
        value: "0"
      - 
        key: "Errors count #2"
        value: "0"
      - 
        key: "Errors count #3"
        value: "0"
      - 
        key: "Errors count #4"
        value: "0"
  - 
    level: 0
    name: "mavros: Battery"
    message: "Normal"
    hardware_id: "udp://localhost:14540@localhost:14557"
    values: 
      - 
        key: "Voltage"
        value: "12.15"
      - 
        key: "Current"
        value: "-1.0"
      - 
        key: "Remaining"
        value: "100.0"
  - 
    level: 0
    name: "mavros: Time Sync"
    message: "Normal"
    hardware_id: "udp://localhost:14540@localhost:14557"
    values: 
      - 
        key: "Timesyncs since startup"
        value: "1306"
      - 
        key: "Frequency (Hz)"
        value: "10.000000"
      - 
        key: "Last RTT (ms)"
        value: "0.000000"
      - 
        key: "Mean RTT (ms)"
        value: "0.091883"
      - 
        key: "Last remote time (s)"
        value: "597.140000000"
      - 
        key: "Estimated time offset (s)"
        value: "0.003961661"
---

saucompeng · March 1, 2019, 6:13am

Hi All,

I found a solution, I replaced -0.07 to “-0.05”, now it is working successfully.

origin xyz="0 0 **-0.05**" rpy="0 1.57079 0"

Fernando_Medrano · July 5, 2019, 4:53pm

The procedure of adding a camera described above worked for me but I would like to add that it is necessary to

cd <Firmware_directory>
DONT_RUN=1 make px4_sitl_default gazebo

everytime you modify the iris_base.xacro file in order for the changes to have effect.

When using the RotorS simulator this is not necessary, but it is on px4/firmware

MohitSingh · May 7, 2020, 8:34am

Hey @Leonardo_Garberoglio,

I am having issue with libgazebo_ros_camera.so file. As I run the make px4_sitl gazebo_iris command, I get an error as follows,

[Err] [Plugin.hh:180] Failed to load plugin libgazebo_ros_camera.so: libCameraPlugin.so: cannot open shared object file: No such file or directory

I searched the .so file and found it here at

/opt/ros/melodic/lib/libgazebo_ros_camera.so

I am not aware of what must be done in this case. May I seek your help.

Thank You!

David.Wright · October 6, 2020, 2:59pm

Hello Every one,

I have added a camera and the aruco marker as well. I am getting my frames on " iris/camera_red_iris/image_raw topic" .

Now there are two things that i want to do further:

1 . Use the data coming on “iris/camera_red_iris/image_raw topic” and then process it.
2. Publish the position of the Aruco marker to use in ROS.

How can I process data from “iris/camera_red_iris/image_raw topic” and do the publication of Aruco marker? Can anyone help me out?

Regards

sritank · November 9, 2020, 3:52am

A small thing to note is that you need to generate the iris.sdf file again after adding the code to the xacro files above.
i.e. use command to recompile and generate the iris.sdf file:
$ DONT_RUN=1 make px4_sitl_default gazebo

REGATTE · March 23, 2022, 4:17pm

An easier method using available gazebo camera plugins would be:
You should have ROS1/2 installed for this to work.

<!--down cam link-->
        <link name="down_camera_link">
        <pose>0 0 -0.013633 1.5708 1.5708 0</pose>
        <inertial>
            <pose>0 0 0 0 0 0</pose>
            <mass>0.050075</mass>
            <inertia>
                <ixx>3.2061e-05</ixx>
                <ixy>-1.169e-08</ixy>
                <ixz>-4.2601e-10</ixz>
                <iyy>3.0815e-05</iyy>
                <iyz>5.2182e-10</iyz>
                <izz>5.1652e-06</izz>
            </inertia>
        </inertial>
        <collision name='down_camera_collision'>
            <pose>0 0 0 0 0 0</pose>
            <geometry>
                <box>
                    <size>0.023813 0.027686 0.092682</size>
                </box>
            </geometry>
            <surface>
                <friction>
                    <ode>
                        <mu>1</mu>
                        <mu2>1</mu2>
                    </ode>
                </friction>
                <contact>
                    <ode>
                        <kp>1e+8</kp>
                        <kd>1</kd>
                        <max_vel>0.01</max_vel>
                        <min_depth>0.001</min_depth>
                    </ode>
                </contact>
            </surface>
        </collision>
        <visual name='down_camera_visual'>
            <pose>0 0 0 0 0 0</pose>
            <geometry>
                <mesh>
                    <scale>1 1 1</scale>
                    <uri>model://iris/meshes/cam_link.STL</uri>
                </mesh>
            </geometry>
            <material>
                <script>
                    <name>Gazebo/DarkGrey</name>
                    <uri>file://media/materials/scripts/gazebo.material</uri>
                </script>
            </material>
        </visual>
        <gravity>1</gravity>
        <velocity_decay/>
        <self_collide>0</self_collide>
        <sensor name="down_camera_imu" type="imu">
            <always_on>1</always_on>
        </sensor>
        <sensor name="down_camera" type="camera">                 <!-- name of sensor topic as well -->
            <pose>0 0 0 -1.5708 0 0</pose>
            <camera>
                <horizontal_fov>2.0</horizontal_fov>
                <image>
                    <format>R8G8B8</format>
                    <width>640</width>
                    <height>480</height>
                </image>
                <clip>
                    <near>0.05</near>
                    <far>15000</far>
                </clip>
            </camera>
            <always_on>1</always_on>
            <update_rate>10</update_rate>
            <visualize>true</visualize>
            <plugin filename="libgazebo_ros_camera.so" name="camera_en_ros">
                <robotNamespace>iris</robotNamespace>
                <cameraName>down_camera_link</cameraName>
                <imageTopicName>down_raw_image</imageTopicName>
                <cameraInfoTopicName>down_info_image</cameraInfoTopicName>
                <FrameName>downCameraImage</FrameName>
                <Cy>0</Cy>
                <Cx>0</Cx>
                <updateRate>30.0</updateRate>
            </plugin>
            <plugin name="GstCameraPlugin" filename="libgazebo_gst_camera_plugin.so">
                <robotNamespace></robotNamespace>
                <udpPort>5600</udpPort>
            </plugin>
            <plugin name="GeotaggedImagesPlugin" filename="libgazebo_geotagged_images_plugin.so">
                <robotNamespace>iris</robotNamespace>
                <interval>1</interval>
                <width>3840</width>
                <height>2160</height>
                <maximum_zoom>8.0</maximum_zoom>
            </plugin>
        </sensor>
        </link>
        <joint name='down_camera_joint' type='fixed'>
            <child>down_camera_link</child>
            <parent>base_link</parent>
            <pose>0 0 0 0 0 0</pose>
            <axis>
                <xyz>-1 0 0</xyz>
            </axis>
        </joint>

copy the above code into your model.sdf.jinja under the imu_link. Then run make px4_sitl_rtps gazebo_model, in another tab run rqt,

make sure to tweak <pose>...</pose> under <link name="down_camera_link"> to suit your needs.
pose = position in the form [x,y,z, x0, y0, z0]

AlexWUrobot · January 24, 2023, 9:10pm

Thanks Regatte.

It is very helpful for me.
I add the code into iris.sdf.jinja and run

make px4_sitl gazebo

in the PX4-autopilot

AlexWUrobot · July 28, 2023, 8:17pm

Recently, I tried to add a camera in the same way on Ubuntu 20.04.
and then running DONT_RUN=1 make px4_sitl_default gazebo-classic in the PX4-autopilot.

Although the camera is successfully added, the camera does not has “frame_id”.
Since it lacks “frame_id”, the ar_track_alvar package cannot work

In ubuntu 18.04, the frame_id is iris/

ref: Adding a camera to IRIS base model