diff --git a/urdf/include/head.xacro b/urdf/include/head.xacro index 9cbbe8b433385aacc2d37cb6ecf5bc5bf45fa34d..3962ab9d8b947dc99e2a237cd13b403d4edbac5a 100644 --- a/urdf/include/head.xacro +++ b/urdf/include/head.xacro @@ -92,6 +92,15 @@ </inertial> </link> + <link name="${load_name}_base"> + </link> + + <joint name="${load_name}2${load_name}_base_joint" type="fixed"> + <parent link="${load_name}"/> + <child link="${load_name}_base"/> + <origin xyz="0 0 0" rpy="-${PI/2} 0 -${PI/2}"/> + </joint> + <xacro:servo_joint name="${joint1_name}" parent="${servo1_name}" child="${servo2_name}"> diff --git a/urdf/include/sensors.xacro b/urdf/include/sensors.xacro index 709ac786827393c19b0e74b8a3374f71890932d3..6befd7596257a08d822990b996fb2f77ebe4fe15 100644 --- a/urdf/include/sensors.xacro +++ b/urdf/include/sensors.xacro @@ -4,9 +4,10 @@ <xacro:include filename="$(find iri_hokuyo_laser2d_description)/urdf/hokuyo_laser2d.xacro" /> - <xacro:macro name="sensors" params="name parent"> + <xacro:property name="PI" value="3.1415926535897931" /> + <xacro:property name="front_laser_config" value="$(find iri_dabo_gazebo)/config/front_hokuyo_utm30lx.yaml"/> <xacro:property name="rear_laser_config" value="$(find iri_dabo_gazebo)/config/rear_hokuyo_utm30lx.yaml"/> @@ -17,6 +18,27 @@ <xacro:hokuyo_laser2d name="rear" prefix="${name}" parent="${parent}" mesh_resolution="low_res" model="utm30lx" config_file="${rear_laser_config}"> <origin xyz="-0.28 0.0 0.15" rpy="0 0 3.14159" /> </xacro:hokuyo_laser2d> + + <!--TODO add Bumblebee 2 --> + <xacro:include filename="$(find iri_bumblebee2_camera_description)/urdf/bumblebee2_camera.xacro" /> + + <xacro:bumblebee2_camera name="bumblebee2" parent="head_base" resolution="low_res" sim_config="$(find iri_stereo_camera_gazebo)/config/stereo_camera_config.yaml"> + <origin xyz="0.03 0.0 0.0" rpy="0 0 0"/> + </xacro:bumblebee2_camera> + + <!-- example realsense + <xacro:include filename="$(find iri_realsense_depth_description)/urdf/realsense.xacro" /> + <xacro:realsense_depth parent="head_base" model="d435" sim_config="$(find iri_depth_camera_gazebo)/config/realsense_d435_depth_sim_config.yaml"> + <origin xyz="0.0 0.0 0.0" rpy="0 0 0" /> + </xacro:realsense_depth> + --> + + <!-- example uvc_camera + <xacro:include filename="$(find iri_uvc_camera_description)/urdf/uvc_camera.xacro" /> + <xacro:uvc_camera name="camera" parent="head_base" resolution="low_res" model="default" sim_config="$(find iri_camera_gazebo)/config/camera_sim_config.yaml"> + <origin xyz="0.0 0.0 0.0" rpy="0 0 0" /> + </xacro:uvc_camera> + --> </xacro:macro>