赞
踩
我们做视觉slam时会用到深度相机,但是gazebo的turtlebot3中只有rgb相机,没有深度,因此本节会修改代码,来给我们的小乌龟增加一个rgbd相机。
/opt/ros/humble/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf
最好将原来的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>
/opt/ros/humble/share/turtlebot3_description/urdf/turtlebot3_waffle.urdf
同样,原代码先备份一个。同时用下面代码全部替换。
<?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需要等待几分钟,因为深度相机中有点云发布,比较占资源。
好了,有问题就评论区回复。
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。