当前位置：首页 > news >正文

木叶飞舞之【机器人ROS2】篇章_第三节、给turtlebot3安装realsense深度相机

news 来源：原创 2024/9/28 23:11:57

我们做视觉slam时会用到深度相机，但是gazebo的turtlebot3中只有rgb相机，没有深度，因此本节会修改代码，来给我们的小乌龟增加一个rgbd相机。

效果展示

在这里插入图片描述

发布topic如下图
图片大小都是640*480

1. 修改model.sdf文件

1.1 路径位置

/opt/ros/humble/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf

1.2 修改代码

最好将原来的model.sdf做一个备份。然后将下面的代码全部复制替换。

<?xml version="1.0" ?>
<sdf version="1.5"><model name="turtlebot3_waffle">  <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose><link name="base_footprint"/><link name="base_link"><inertial><pose>-0.064 0 0.048 0 0 0</pose><inertia><ixx>0.001</ixx><ixy>0.000</ixy><ixz>0.000</ixz><iyy>0.001</iyy><iyz>0.000</iyz><izz>0.001</izz></inertia><mass>1.0</mass></inertial><collision name="base_collision"><pose>-0.064 0 0.048 0 0 0</pose><geometry><box><size>0.265 0.265 0.089</size></box></geometry></collision><visual name="base_visual"><pose>-0.064 0 0 0 0 0</pose><geometry><mesh><uri>model://turtlebot3_waffle/meshes/waffle_base.dae</uri><scale>0.001 0.001 0.001</scale></mesh></geometry></visual></link><link name="imu_link"><sensor name="tb3_imu" type="imu"><always_on>true</always_on><update_rate>200</update_rate><imu><angular_velocity><x><noise type="gaussian"><mean>0.0</mean><stddev>2e-4</stddev></noise></x><y><noise type="gaussian"><mean>0.0</mean><stddev>2e-4</stddev></noise></y><z><noise type="gaussian"><mean>0.0</mean><stddev>2e-4</stddev></noise></z></angular_velocity><linear_acceleration><x><noise type="gaussian"><mean>0.0</mean><stddev>1.7e-2</stddev></noise></x><y><noise type="gaussian"><mean>0.0</mean><stddev>1.7e-2</stddev></noise></y><z><noise type="gaussian"><mean>0.0</mean><stddev>1.7e-2</stddev></noise></z></linear_acceleration></imu><plugin name="turtlebot3_imu" filename="libgazebo_ros_imu_sensor.so"><ros><!-- <namespace>/tb3</namespace> --><argument>~/out:=imu</argument></ros></plugin></sensor></link><link name="base_scan">    <inertial><pose>-0.052 0 0.111 0 0 0</pose><inertia><ixx>0.001</ixx><ixy>0.000</ixy><ixz>0.000</ixz><iyy>0.001</iyy><iyz>0.000</iyz><izz>0.001</izz></inertia><mass>0.125</mass></inertial><collision name="lidar_sensor_collision"><pose>-0.052 0 0.111 0 0 0</pose><geometry><cylinder><radius>0.0508</radius><length>0.055</length></cylinder></geometry></collision><visual name="lidar_sensor_visual"><pose>-0.064 0 0.121 0 0 0</pose><geometry><mesh><uri>model://turtlebot3_waffle/meshes/lds.dae</uri><scale>0.001 0.001 0.001</scale></mesh></geometry></visual><sensor name="hls_lfcd_lds" type="ray"><always_on>true</always_on><visualize>true</visualize><pose>-0.064 0 0.121 0 0 0</pose><update_rate>5</update_rate><ray><scan><horizontal><samples>360</samples><resolution>1.000000</resolution><min_angle>0.000000</min_angle><max_angle>6.280000</max_angle></horizontal></scan><range><min>0.120000</min><max>3.5</max><resolution>0.015000</resolution></range><noise><type>gaussian</type><mean>0.0</mean><stddev>0.01</stddev></noise></ray><plugin name="turtlebot3_laserscan" filename="libgazebo_ros_ray_sensor.so"><ros><!-- <namespace>/tb3</namespace> --><argument>~/out:=scan</argument></ros><output_type>sensor_msgs/LaserScan</output_type><frame_name>base_scan</frame_name></plugin></sensor></link><link name="wheel_left_link"><inertial><pose>0.0 0.144 0.023 -1.57 0 0</pose><inertia><ixx>0.001</ixx><ixy>0.000</ixy><ixz>0.000</ixz><iyy>0.001</iyy><iyz>0.000</iyz><izz>0.001</izz></inertia><mass>0.1</mass></inertial><collision name="wheel_left_collision"><pose>0.0 0.144 0.023 -1.57 0 0</pose><geometry><cylinder><radius>0.033</radius><length>0.018</length></cylinder></geometry><surface><!-- This friction pamareter don't contain reliable data!! --><friction><ode><mu>100000.0</mu><mu2>100000.0</mu2><fdir1>0 0 0</fdir1><slip1>0.0</slip1><slip2>0.0</slip2></ode></friction><contact><ode><soft_cfm>0</soft_cfm><soft_erp>0.2</soft_erp><kp>1e+5</kp><kd>1</kd><max_vel>0.01</max_vel><min_depth>0.001</min_depth></ode></contact></surface></collision><visual name="wheel_left_visual"><pose>0.0 0.144 0.023 0 0 0</pose><geometry><mesh><uri>model://turtlebot3_waffle/meshes/left_tire.dae</uri><scale>0.001 0.001 0.001</scale></mesh></geometry></visual></link><link name="wheel_right_link"><inertial><pose>0.0 -0.144 0.023 -1.57 0 0</pose><inertia><ixx>0.001</ixx><ixy>0.000</ixy><ixz>0.000</ixz><iyy>0.001</iyy><iyz>0.000</iyz><izz>0.001</izz></inertia><mass>0.1</mass></inertial><collision name="wheel_right_collision"><pose>0.0 -0.144 0.023 -1.57 0 0</pose><geometry><cylinder><radius>0.033</radius><length>0.018</length></cylinder></geometry><surface><!-- This friction pamareter don't contain reliable data!! --><friction><ode><mu>100000.0</mu><mu2>100000.0</mu2><fdir1>0 0 0</fdir1><slip1>0.0</slip1><slip2>0.0</slip2></ode></friction><contact><ode><soft_cfm>0</soft_cfm><soft_erp>0.2</soft_erp><kp>1e+5</kp><kd>1</kd><max_vel>0.01</max_vel><min_depth>0.001</min_depth></ode></contact></surface></collision><visual name="wheel_right_visual"><pose>0.0 -0.144 0.023 0 0 0</pose><geometry><mesh><uri>model://turtlebot3_waffle/meshes/right_tire.dae</uri><scale>0.001 0.001 0.001</scale></mesh></geometry></visual></link><link name='caster_back_right_link'><pose>-0.177 -0.064 -0.004 0 0 0</pose><inertial><mass>0.001</mass><inertia><ixx>0.00001</ixx><ixy>0.000</ixy><ixz>0.000</ixz><iyy>0.00001</iyy><iyz>0.000</iyz><izz>0.00001</izz></inertia></inertial><collision name='collision'><geometry><sphere><radius>0.005000</radius></sphere></geometry><surface><contact><ode><soft_cfm>0</soft_cfm><soft_erp>0.2</soft_erp><kp>1e+5</kp><kd>1</kd><max_vel>0.01</max_vel><min_depth>0.001</min_depth></ode></contact></surface></collision></link><link name='caster_back_left_link'><pose>-0.177 0.064 -0.004 0 0 0</pose><inertial><mass>0.001</mass><inertia><ixx>0.00001</ixx><ixy>0.000</ixy><ixz>0.000</ixz><iyy>0.00001</iyy><iyz>0.000</iyz><izz>0.00001</izz></inertia></inertial><collision name='collision'><geometry><sphere><radius>0.005000</radius></sphere></geometry><surface><contact><ode><soft_cfm>0</soft_cfm><soft_erp>0.2</soft_erp><kp>1e+5</kp><kd>1</kd><max_vel>0.01</max_vel><min_depth>0.001</min_depth></ode></contact></surface></collision></link><link name="realsense_link"><inertial><pose>0.069 -0.047 0.107 0 0 0</pose><inertia><ixx>0.001</ixx><ixy>0.000</ixy><ixz>0.000</ixz><iyy>0.001</iyy><iyz>0.000</iyz><izz>0.001</izz></inertia><mass>0.035</mass></inertial><collision name="collision"><pose>0 0.047 0 0 0 0</pose><geometry><box><size>0.008 0.130 0.022</size></box></geometry></collision><pose>0.069 -0.047 0.107 0 0 0</pose><sensor name="intel_realsense_r200_depth" type="depth"><always_on>1</always_on><update_rate>30</update_rate><pose>0.064 -0.047 0.107 0 0 0</pose><camera name="realsense_depth_camera"><image><width>640</width><height>480</height><!-- <width>1920</width> --><!-- <height>1080</height> --><format>R8G8B8</format></image><clip><near>0.02</near><far>10</far></clip></camera><plugin name="intel_realsense_r200_depth_driver" filename="libgazebo_ros_camera.so"><ros><!-- <argument>custom_camera/image_raw:=custom_camera/custom_image</argument><argument>custom_camera/image_depth:=custom_camera/custom_image_depth</argument><argument>custom_camera/camera_info:=custom_camera/custom_info_raw</argument><argument>custom_camera/camera_info_depth:=custom_camera/custom_info_depth</argument><argument>custom_camera/points:=custom_camera/custom_points</argument> --></ros><camera_name>intel_realsense_r200_depth</camera_name><frame_name>realsense_depth_frame</frame_name><hack_baseline>0.07</hack_baseline><min_depth>0.001</min_depth></plugin></sensor><sensor name="intel_realsense_r200_rgb" type="camera"><always_on>true</always_on><visualize>true</visualize><update_rate>30</update_rate><pose>0.064 -0.047 0.107 0 0 0</pose><camera name="realsense_rgb_camera"><horizontal_fov>1.02974</horizontal_fov><image><width>640</width><height>480</height><!-- <width>1920</width> --><!-- <height>1080</height> --><format>R8G8B8</format></image><clip><near>0.02</near><far>300</far></clip><noise><type>gaussian</type><mean>0.0</mean><stddev>0.007</stddev></noise></camera><plugin name="intel_realsense_r200_rgb_driver" filename="libgazebo_ros_camera.so"><ros><!--<namespace>custom_ns</namespace><argument>image_raw:=custom_image</argument><argument>camera_info:=custom_info_raw</argument> --></ros><camera_name>intel_realsense_r200_rgb</camera_name><frame_name>realsense_rgb_frame</frame_name><hack_baseline>0.07</hack_baseline></plugin></sensor></link>  <joint name="base_joint" type="fixed"><parent>base_footprint</parent><child>base_link</child><pose>0.0 0.0 0.010 0 0 0</pose></joint><joint name="wheel_left_joint" type="revolute"><parent>base_link</parent><child>wheel_left_link</child><pose>0.0 0.144 0.023 -1.57 0 0</pose><axis><xyz>0 0 1</xyz></axis></joint><joint name="wheel_right_joint" type="revolute"><parent>base_link</parent><child>wheel_right_link</child><pose>0.0 -0.144 0.023 -1.57 0 0</pose><axis><xyz>0 0 1</xyz></axis></joint><joint name='caster_back_right_joint' type='ball'><parent>base_link</parent><child>caster_back_right_link</child></joint><joint name='caster_back_left_joint' type='ball'><parent>base_link</parent><child>caster_back_left_link</child></joint><joint name="lidar_joint" type="fixed"><parent>base_link</parent><child>base_scan</child><pose>-0.064 0 0.121 0 0 0</pose><axis><xyz>0 0 1</xyz></axis></joint><joint name="camera_joint" type="fixed"><parent>base_link</parent><child>realsense_link</child></joint><plugin name="turtlebot3_diff_drive" filename="libgazebo_ros_diff_drive.so"><ros><!-- <namespace>/tb3</namespace> --></ros><update_rate>30</update_rate><!-- wheels --><left_joint>wheel_left_joint</left_joint><right_joint>wheel_right_joint</right_joint><!-- kinematics --><wheel_separation>0.287</wheel_separation><wheel_diameter>0.066</wheel_diameter><!-- limits --><max_wheel_torque>20</max_wheel_torque><max_wheel_acceleration>1.0</max_wheel_acceleration><command_topic>cmd_vel</command_topic><!-- output --><publish_odom>true</publish_odom><publish_odom_tf>true</publish_odom_tf><publish_wheel_tf>false</publish_wheel_tf><odometry_topic>odom</odometry_topic><odometry_frame>odom</odometry_frame><robot_base_frame>base_footprint</robot_base_frame></plugin><plugin name="turtlebot3_joint_state" filename="libgazebo_ros_joint_state_publisher.so"><ros><!-- <namespace>/tb3</namespace> --><argument>~/out:=joint_states</argument></ros><update_rate>30</update_rate><joint_name>wheel_left_joint</joint_name><joint_name>wheel_right_joint</joint_name></plugin>    </model>
</sdf>

2. 修改urdf文件

2.1 路径位置

/opt/ros/humble/share/turtlebot3_description/urdf/turtlebot3_waffle.urdf

2.2 修改代码

同样，原代码先备份一个。同时用下面代码全部替换。

<?xml version="1.0" ?>
<robot name="turtlebot3_waffle" xmlns:xacro="http://ros.org/wiki/xacro"><!-- <xacro:include filename="$(find turtlebot3_description)/urdf/common_properties.urdf"/><xacro:property name="r200_cam_rgb_px" value="0.005"/><xacro:property name="r200_cam_rgb_py" value="0.018"/><xacro:property name="r200_cam_rgb_pz" value="0.013"/><xacro:property name="r200_cam_depth_offset" value="0.01"/> --><!-- Init colour --><material name="black"><color rgba="0.0 0.0 0.0 1.0"/></material><material name="dark"><color rgba="0.3 0.3 0.3 1.0"/></material><material name="light_black"><color rgba="0.4 0.4 0.4 1.0"/></material><material name="blue"><color rgba="0.0 0.0 0.8 1.0"/></material><material name="green"><color rgba="0.0 0.8 0.0 1.0"/></material><material name="grey"><color rgba="0.5 0.5 0.5 1.0"/></material><material name="orange"><color rgba="1.0 0.4235 0.0392 1.0"/></material><material name="brown"><color rgba="0.8706 0.8118 0.7647 1.0"/></material><material name="red"><color rgba="0.8 0.0 0.0 1.0"/></material><material name="white"><color rgba="1.0 1.0 1.0 1.0"/></material><link name="base_footprint"/><joint name="base_joint" type="fixed"><parent link="base_footprint"/><child link="base_link" /><origin xyz="0 0 0.010" rpy="0 0 0"/></joint><link name="base_link"><visual><origin xyz="-0.064 0 0.0" rpy="0 0 0"/><geometry><mesh filename="package://turtlebot3_description/meshes/bases/waffle_base.stl" scale="0.001 0.001 0.001"/></geometry><material name="light_black"/></visual><collision><origin xyz="-0.064 0 0.047" rpy="0 0 0"/><geometry><box size="0.266 0.266 0.094"/></geometry></collision><inertial><origin xyz="0 0 0" rpy="0 0 0"/><mass value="1.3729096e+00"/><inertia ixx="8.7002718e-03" ixy="-4.7576583e-05" ixz="1.1160499e-04"iyy="8.6195418e-03" iyz="-3.5422299e-06"izz="1.4612727e-02" /></inertial></link><joint name="wheel_left_joint" type="continuous"><parent link="base_link"/><child link="wheel_left_link"/><origin xyz="0.0 0.144 0.023" rpy="-1.57 0 0"/><axis xyz="0 0 1"/></joint><link name="wheel_left_link"><visual><origin xyz="0 0 0" rpy="1.57 0 0"/><geometry><mesh filename="package://turtlebot3_description/meshes/wheels/left_tire.stl" scale="0.001 0.001 0.001"/></geometry><material name="dark"/></visual><collision><origin xyz="0 0 0" rpy="0 0 0"/><geometry><cylinder length="0.018" radius="0.033"/></geometry></collision><inertial><origin xyz="0 0 0" /><mass value="2.8498940e-02" /><inertia ixx="1.1175580e-05" ixy="-4.2369783e-11" ixz="-5.9381719e-09"iyy="1.1192413e-05" iyz="-1.4400107e-11"izz="2.0712558e-05" /></inertial></link><joint name="wheel_right_joint" type="continuous"><parent link="base_link"/><child link="wheel_right_link"/><origin xyz="0.0 -0.144 0.023" rpy="-1.57 0 0"/><axis xyz="0 0 1"/></joint><link name="wheel_right_link"><visual><origin xyz="0 0 0" rpy="1.57 0 0"/><geometry><mesh filename="package://turtlebot3_description/meshes/wheels/right_tire.stl" scale="0.001 0.001 0.001"/></geometry><material name="dark"/></visual><collision><origin xyz="0 0 0" rpy="0 0 0"/><geometry><cylinder length="0.018" radius="0.033"/></geometry></collision><inertial><origin xyz="0 0 0" /><mass value="2.8498940e-02" /><inertia ixx="1.1175580e-05" ixy="-4.2369783e-11" ixz="-5.9381719e-09"iyy="1.1192413e-05" iyz="-1.4400107e-11"izz="2.0712558e-05" /></inertial></link><joint name="caster_back_right_joint" type="fixed"><parent link="base_link"/><child link="caster_back_right_link"/><origin xyz="-0.177 -0.064 -0.004" rpy="-1.57 0 0"/></joint><link name="caster_back_right_link"><collision><origin xyz="0 0.001 0" rpy="0 0 0"/><geometry><box size="0.030 0.009 0.020"/></geometry></collision><inertial><origin xyz="0 0 0" /><mass value="0.005" /><inertia ixx="0.001" ixy="0.0" ixz="0.0"iyy="0.001" iyz="0.0"izz="0.001" /></inertial></link><joint name="caster_back_left_joint" type="fixed"><parent link="base_link"/><child link="caster_back_left_link"/><origin xyz="-0.177 0.064 -0.004" rpy="-1.57 0 0"/></joint><link name="caster_back_left_link"><collision><origin xyz="0 0.001 0" rpy="0 0 0"/><geometry><box size="0.030 0.009 0.020"/></geometry></collision><inertial><origin xyz="0 0 0" /><mass value="0.005" /><inertia ixx="0.001" ixy="0.0" ixz="0.0"iyy="0.001" iyz="0.0"izz="0.001" /></inertial></link><joint name="imu_joint" type="fixed"><parent link="base_link"/><child link="imu_link"/><origin xyz="0.0 0 0.068" rpy="0 0 0"/></joint><link name="imu_link"/><joint name="scan_joint" type="fixed"><parent link="base_link"/><child link="base_scan"/><origin xyz="-0.064 0 0.122" rpy="0 0 0"/></joint><link name="base_scan"><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://turtlebot3_description/meshes/sensors/lds.stl" scale="0.001 0.001 0.001"/></geometry><material name="dark"/></visual><collision><origin xyz="0.015 0 -0.0065" rpy="0 0 0"/><geometry><cylinder length="0.0315" radius="0.055"/></geometry></collision><inertial><mass value="0.114" /><origin xyz="0 0 0" /><inertia ixx="0.001" ixy="0.0" ixz="0.0"iyy="0.001" iyz="0.0"izz="0.001" /></inertial></link><joint name="camera_joint" type="fixed"><origin xyz="0.064 -0.065 0.094" rpy="0 0 0"/><parent link="base_link"/><child link="realsense_link"/></joint><link name="realsense_link"><visual><origin xyz="0 0 0" rpy="1.57 0 1.57"/><geometry><mesh filename="package://turtlebot3_description/meshes/sensors/r200.dae" /></geometry></visual><collision><origin xyz="0.003 0.065 0.007" rpy="0 0 0"/><geometry><box size="0.012 0.132 0.020"/></geometry></collision><!-- This inertial field needs doesn't contain reliable data!! -->
<!--   <inertial><mass value="0.564" /><origin xyz="0 0 0" /><inertia ixx="0.003881243" ixy="0.0" ixz="0.0"iyy="0.000498940" iyz="0.0"izz="0.003879257" /></inertial>--></link><joint name="camera_rgb_joint" type="fixed"><origin xyz="0.005 0.018 0.013" rpy="-1.57 0 -1.57"/><!-- <origin xyz="${r200_cam_rgb_px} ${r200_cam_rgb_py} ${r200_cam_rgb_pz}" rpy="0 0 0"/> --><parent link="realsense_link"/><child link="camera_rgb_frame"/></joint><link name="camera_rgb_frame"/><joint name="camera_rgb_optical_joint" type="fixed"><origin xyz="0 0 0" rpy="-1.57 0 -1.57"/><parent link="camera_rgb_frame"/><child link="camera_rgb_optical_frame"/></joint><link name="camera_rgb_optical_frame"/><joint name="realsense_depth_joint" type="fixed"><origin xyz="0.005 0.028 0.013" rpy="-1.57 0 -1.57"/><!-- <origin xyz="${r200_cam_rgb_px} ${r200_cam_rgb_py + r200_cam_depth_offset} ${r200_cam_rgb_pz}" rpy="0 0 0"/> --><parent link="realsense_link"/><child link="realsense_depth_frame"/></joint><link name="realsense_depth_frame"/><joint name="camera_depth_optical_joint" type="fixed"><origin xyz="0 0 0" rpy="-1.57 0 -1.57"/><parent link="realsense_depth_frame"/><child link="camera_depth_optical_frame"/></joint><link name="camera_depth_optical_frame"/></robot>

加载gazebo需要等待几分钟，因为深度相机中有点云发布，比较占资源。
好了，有问题就评论区回复。