Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 6 additions & 1 deletion src/description/urdf/athena_drive.urdf.xacro
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
<xacro:include filename="imu.xacro"/>
<xacro:include filename="gps.xacro"/>
<xacro:include filename="ground_truth_odom.xacro"/>
<xacro:include filename="depth_camera.xacro"/>


<!-- Load robot's macro with parameters -->
Expand Down Expand Up @@ -49,6 +50,10 @@

<xacro:ground_truth_odometry/>

<xacro:depth_camera parent_link="base_link">
<origin xyz="0.75 0 -0.37" rpy="0 0 0"/>
</xacro:depth_camera>

<xacro:if value="$(arg use_mock_hardware)">
<xacro:athena_drive_ros2_control
name="athena_drive"
Expand All @@ -64,4 +69,4 @@
</xacro:unless>


</robot>
</robot>
68 changes: 68 additions & 0 deletions src/description/urdf/depth_camera.xacro
Original file line number Diff line number Diff line change
@@ -0,0 +1,68 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

<xacro:macro name="depth_camera" params="parent_link *origin">
<link name="depth_camera_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.025 0.09 0.025"/>
</geometry>
<material name="camera_material">
<color rgba="0.1 0.1 0.1 1.0"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.025 0.09 0.025"/>
</geometry>
</collision>
<inertial>
<mass value="0.072"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="0.00001" ixy="0.0" ixz="0.0"
iyy="0.00001" iyz="0.0"
izz="0.00001"/>
</inertial>
</link>

<link name="depth_camera_optical_frame"/>

<joint name="depth_camera_joint" type="fixed">
<xacro:insert_block name="origin"/>
<parent link="${parent_link}"/>
<child link="depth_camera_link"/>
</joint>

<joint name="depth_camera_optical_joint" type="fixed">
<origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>
<parent link="depth_camera_link"/>
<child link="depth_camera_optical_frame"/>
</joint>

<gazebo reference="depth_camera_link">
<sensor name="depth_camera" type="depth_camera">
<always_on>1</always_on>
<update_rate>30</update_rate>
<topic>depth_camera</topic>
<camera name="depth_camera">
<horizontal_fov>1.047198</horizontal_fov>
<image>
<width>640</width>
<height>480</height>
<format>R_FLOAT32</format>
</image>
<clip>
<near>0.1</near>
<far>10.0</far>
</clip>
</camera>
<plugin filename="libgz-sim-sensors-system.so" name="gz::sim::systems::Sensors">
<render_engine>ogre2</render_engine>
</plugin>
</sensor>
</gazebo>

</xacro:macro>
</robot>
11 changes: 11 additions & 0 deletions src/simulation/launch/bridge.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,17 @@ def generate_launch_description():
],
),

Node(
package='ros_gz_bridge',
executable='parameter_bridge',
name='depth_camera_bridge',
output='screen',
arguments=[
'/depth_camera@sensor_msgs/msg/Image[ignition.msgs.Image',
'/depth_camera/points@sensor_msgs/msg/PointCloud2[ignition.msgs.PointCloudPacked',
],
),

Node(
package='ros_gz_bridge',
executable='parameter_bridge',
Expand Down