diff --git a/urdf/include/sensors.xacro b/urdf/include/sensors.xacro index a661421172117fa3cdcbfa23307ace12b68d88e6..2a00506234170e0cbfcd011e234116e1248700e7 100644 --- a/urdf/include/sensors.xacro +++ b/urdf/include/sensors.xacro @@ -59,14 +59,28 @@ <origin xyz="0.26374 0 -0.0485" rpy="0 0 0"/> </joint> - <xacro:realsense_d435_depth parent="${name}_realsense_support" sim_config="$(find iri_ana_gazebo)/config/ana_realsense_config.yaml"> + <xacro:realsense_d435_depth parent="${name}_realsense_support" name="nav_cam" sim_config="$(find iri_ana_gazebo)/config/ana_realsense_config.yaml"> <origin xyz="0.0 0.0 0.0265" rpy="0 0.26 0" /> </xacro:realsense_d435_depth> + <link name="${name}_realsense_support_top"> + </link> + + <joint name="joint_${parent}_to_${name}_realsense_support_top" type="fixed" > + <parent link="${parent}"/> + <child link="${name}_realsense_support_top"/> + <origin xyz="0.36 0 0.44" rpy="0 0 0"/> + </joint> + + <xacro:realsense_d435_depth parent="${name}_realsense_support_top" name="nav_cam_top" sim_config="$(find iri_ana_gazebo)/config/ana_realsense_top_config.yaml"> + <origin xyz="0.0 0.0 0.0265" rpy="0 1.0 0" /> + </xacro:realsense_d435_depth> + <xacro:bno055_imu name="imu" parent="base_footprint" resolution="low_res" sim_config="$(find iri_ana_gazebo)/config/ana_bno055_config.yaml"> <origin xyz="0.11928 -0.08805 0.23061" rpy="0 -1.5707 0" /> </xacro:bno055_imu> + <!-- <link name="${pan_tilt_xacro_properties['load_name']}"> </link> @@ -77,7 +91,7 @@ <xacro:pan_tilt parent="${name}_box" pan_joint_name="${pan_tilt_xacro_properties['pan_joint_name']}" tilt_joint_name="${pan_tilt_xacro_properties['tilt_joint_name']}" load_name="${pan_tilt_xacro_properties['load_name']}" joint_interface="${pan_tilt_xacro_properties['joint_interface']}" sim_config="$(find iri_ana_gazebo)/config/ana_pan_tilt_gazebo.yaml"> <origin xyz="0.15 0.0 0.2" rpy="0 0 0" /> </xacro:pan_tilt> - +--> </xacro:macro> </root>