How to add real ZED camera to my robot urdf?

asked 2020-11-09 22:41:35 -0500

Astronaut gravatar image

Hello,

I am trying to add a real ZED camera to the robot link using xacros. So the ZED camera should be added to the robot parent="crane_block" . The robot urdf.xacro is this one

    <?xml version='1.0' encoding='utf-8'?>
    <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="crane">
      <xacro:property name="base_line" value="0.2" />
      <xacro:include filename="$(find crane_description)/robots/crane_3.xacro" />
    <xacro:include filename="$(find hector_sensors_description)/urdf/zed_camera.urdf.xacro"/>

<zed_camera parent="crane_block" name="zed" ros_topic="/zed/zed_node/point_cloud/cloud_registered" cam_info_topic="zed/zed_node/left/camera_info" update_rate="30" res_x="640" res_y="480" image_format="R8G8B8" hfov="1.5708">
        <origin xyz="1 0.4" rpy="0 1.2211 1.5708" />
      </zed_camera>

      <xacro:crane_model />
    </robot>

The camera link URDF is:

<?xml version="1.0"?>

<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

<!-- Includes -->
    <xacro:include filename="$(find hector_sensors_description)/meshes/zed_camera/include/materials.urdf.xacro" />

    <!-- Arguments -->
    <xacro:arg name="camera_name"   default="zed" />
    <xacro:arg name="camera_model"  default="zed" />
    <xacro:arg name="cam_pos_x"     default="0.0" />
    <xacro:arg name="cam_pos_y"     default="0.0" />
    <xacro:arg name="cam_pos_z"     default="0.0" />
    <xacro:arg name="cam_roll"      default="0.0" />
    <xacro:arg name="cam_pitch"     default="0.0" />
    <xacro:arg name="cam_yaw"       default="0.0" />

    <!-- Properties -->
    <xacro:property name="M_PI"     value="3.1415926535897931" />
    <xacro:property name="model"    value="$(arg camera_model)" />

    <xacro:if value="${model == 'zed'}">
        <xacro:property name="baseline" value="0.12" />
    </xacro:if>
    <xacro:if value="${model == 'zedm'}">
        <xacro:property name="baseline" value="0.06" />
    </xacro:if>
    <xacro:if value="${model == 'zed2'}">
        <xacro:property name="baseline" value="0.12" />
    </xacro:if>

     <xacro:macro name="zed_camera" params="name parent *origin ros_topic cam_info_topic update_rate res_x res_y image_format hfov">

<!-- base_link -->
    <link name="zed_link" />

<!-- Camera Center -->
    <joint name="$(arg camera_name)_camera_center_joint" type="fixed">
        <parent link="${parent}"/>
        <child link="$(arg camera_name)_camera_center"/>
        <origin xyz="$(arg cam_pos_x) $(arg cam_pos_y) $(arg cam_pos_z)" rpy="$(arg cam_roll) $(arg cam_pitch) $(arg cam_yaw)" />
    </joint>

    <link name="$(arg camera_name)_camera_center">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <mesh filename="package://hector_sensors_description/meshes/zed_camera/models/${model}.stl" />
            </geometry>
            <material name="${model}_mat" />
        </visual>
    </link>

<!-- Left Camera -->
    <joint name="$(arg camera_name)_left_camera_joint" type="fixed">
        <parent link="$(arg camera_name)_camera_center"/>
        <child link="$(arg camera_name)_left_camera_frame"/>
        <origin xyz="0 ${baseline/2} 0" rpy="0 0 0" />
    </joint>

    <link name="$(arg camera_name)_left_camera_frame" />

    <joint name="$(arg camera_name)_left_camera_optical_joint" type="fixed">
        <origin xyz="0 0 0" rpy="-${M_PI/2} 0.0 -${M_PI/2}"/>
        <parent link="$(arg camera_name)_left_camera_frame"/>
        <child link="$(arg camera_name)_left_camera_optical_frame"/>
    </joint>

    <link name="$(arg camera_name)_left_camera_optical_frame"/>

<!-- Right Camera -->
    <joint name="$(arg camera_name)_right_camera_joint" type="fixed">
        <parent link="$(arg ...
(more)
edit retag flag offensive close merge delete