# 安装gazebo仿真软件
sudo apt install gazebo
# 安装gazebo-ros功能包（所有）
sudo apt install ros-humble-gazebo-*
# 安装gazebo_ros插件
sudo apt install ros-humble-gazebo-ros
# 测试是否安装成功
gazebo --verbose -s libgazebo_ros_init.so -s libgazebo_ros_factory.so 

<?xml version="1.0"?>
<robot name="myfirst">
  <link name="base_link">
    <visual>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
    </visual>
  </link>
</robot>

<?xml version="1.0"?>
<robot name="multipleshapes">
  <link name="base_link">
    <visual>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
    </visual>
  </link>

  <link name="right_leg">
    <visual>
      <geometry>
        <box size="0.6 0.1 0.2"/>
      </geometry>
    </visual>
  </link>

  <joint name="base_to_right_leg" type="fixed">
    <parent link="base_link"/>
    <child link="right_leg"/>
  </joint>

</robot>

<?xml version="1.0"?>
<robot name="origins">
  <link name="base_link">
    <visual>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
    </visual>
  </link>

  <link name="right_leg">
    <visual>
      <geometry>
        <box size="0.6 0.1 0.2"/>
      </geometry>
      <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
    </visual>
  </link>

  <joint name="base_to_right_leg" type="fixed">
    <parent link="base_link"/>
    <child link="right_leg"/>
    <origin xyz="0 -0.22 0.25"/>
  </joint>

</robot>

  <material name="blue">
    <color rgba="0 0 0.8 1"/>
  </material>

  <material name="white">
    <color rgba="1 1 1 1"/>
  </material>

  <link name="base_link">
    <visual>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
      <material name="blue"/>
    </visual>
  </link>

<link name="left_gripper">
  <visual>
    <origin rpy="0.0 0 0" xyz="0 0 0"/>
    <geometry>
      <mesh filename="package://urdf_tutorial/meshes/l_finger.dae"/>
    </geometry>
  </visual>
</link>

<joint name="head_swivel" type="continuous">
  <parent link="base_link"/>
  <child link="head"/>
  <axis xyz="0 0 1"/>
  <origin xyz="0 0 0.3"/>
</joint>

<joint name="left_gripper_joint" type="revolute">
  <axis xyz="0 0 1"/>
  <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
  <origin rpy="0 0 0" xyz="0.2 0.01 0"/>
  <parent link="gripper_pole"/>
  <child link="left_gripper"/>
</joint>

<joint name="gripper_extension" type="prismatic">
  <parent link="base_link"/>
  <child link="gripper_pole"/>
  <limit effort="1000.0" lower="-0.38" upper="0" velocity="0.5"/>
  <origin rpy="0 0 0" xyz="0.19 0 0.2"/>
</joint>

<link name="base_link">
    <visual>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
      <material name="blue">
        <color rgba="0 0 .8 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
    </collision>
  </link>

<link name="base_link">
  <visual>
    <geometry>
      <cylinder length="0.6" radius="0.2"/>
    </geometry>
    <material name="blue">
      <color rgba="0 0 .8 1"/>
    </material>
  </visual>
  <collision>
    <geometry>
      <cylinder length="0.6" radius="0.2"/>
    </geometry>
  </collision>
  <inertial>
    <mass value="10"/>
    <inertia ixx="1e-3" ixy="0.0" ixz="0.0" iyy="1e-3" iyz="0.0" izz="1e-3"/>
  </inertial>
</link>

path_to_urdf = get_package_share_path('pr2_description') / 'robots' / 'pr2.urdf.xacro'
robot_state_publisher_node = launch_ros.actions.Node(
    package='robot_state_publisher',
    executable='robot_state_publisher',
    parameters=[{
        'robot_description': ParameterValue(
            Command(['xacro ', str(path_to_urdf)]), value_type=str
        )
    }]
)

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter">

<xacro:property name="width" value="0.2" />
<xacro:property name="bodylen" value="0.6" />

<link name="base_link">
    <visual>
        <geometry>
            <cylinder radius="${width}" length="${bodylen}"/>
        </geometry>
        <material name="blue"/>
    </visual>
    <collision>
        <geometry>
            <cylinder radius="${width}" length="${bodylen}"/>
        </geometry>
    </collision>
</link>

<xacro:property name=”robotname” value=”marvin” />
<link name=”${robotname}s_leg” />

<cylinder radius="${wheeldiam/2}" length="0.1"/>
<origin xyz="${reflect*(width+.02)} 0 0.25" />

<xacro:macro name="default_inertial" params="mass">
    <inertial>
            <mass value="${mass}" />
            <inertia ixx="1e-3" ixy="0.0" ixz="0.0"
                 iyy="1e-3" iyz="0.0"
                 izz="1e-3" />
    </inertial>
</xacro:macro>

<xacro:default_inertial mass="10"/>

<xacro:macro name="leg" params="prefix reflect">
    <link name="${prefix}_leg">
        <visual>
            <geometry>
                <box size="${leglen} 0.1 0.2"/>
            </geometry>
            <origin xyz="0 0 -${leglen/2}" rpy="0 ${pi/2} 0"/>
            <material name="white"/>
        </visual>
        <collision>
            <geometry>
                <box size="${leglen} 0.1 0.2"/>
            </geometry>
            <origin xyz="0 0 -${leglen/2}" rpy="0 ${pi/2} 0"/>
        </collision>
        <xacro:default_inertial mass="10"/>
    </link>

    <joint name="base_to_${prefix}_leg" type="fixed">
        <parent link="base_link"/>
        <child link="${prefix}_leg"/>
        <origin xyz="0 ${reflect*(width+.02)} 0.25" />
    </joint>
    <!-- A bunch of stuff cut -->
</xacro:macro>
<xacro:leg prefix="right" reflect="1" />
<xacro:leg prefix="left" reflect="-1" />

<robot>
  ... robot description ...
  <gazebo>
    <plugin name="differential_drive_controller" filename="libdiffdrive_plugin.so">
      ... plugin parameters ...
    </plugin>
  </gazebo>
  ... robot description ...
</robot>

<robot>
  ... robot description ...
  <link name="sensor_link">
    ... link description ...
  </link>

  <gazebo reference="sensor_link">
    <sensor type="camera" name="camera1">
      ... sensor parameters ...
      <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
        ... plugin parameters ..
      </plugin>
    </sensor>
  </gazebo>

</robot>

  <joint name="camera_joint" type="fixed">
    <axis xyz="0 1 0" />
    <origin xyz="${camera&#95;link} 0 ${height3 - axel_offset*2}" rpy="0 0 0"/>
    <parent link="link3"/>
    <child link="camera_link"/>
  </joint>

  <!-- Camera -->
  <link name="camera_link">
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
    <box size="${camera&#95;link} ${camera_link} ${camera_link}"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
    <box size="${camera&#95;link} ${camera_link} ${camera_link}"/>
      </geometry>
      <material name="red"/>
    </visual>

    <inertial>
      <mass value="1e-5" />
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
    </inertial>
  </link>

 <xacro:property name="camera_link" value="0.05" /> <!-- Size of square 'camera' box -->

  <!-- camera -->  
  <!-- The link name "camera_link" must match the name of the link we added to the Xacro URDF. -->
  <gazebo reference="camera_link">

    <!-- The sensor name "camera1" must be unique from all other sensor names. The name is not used many places except for within Gazebo plugins you can access.   -->
    <sensor type="camera" name="camera1">

      <!-- Gazebo内每秒拍摄新相机图像的次数。这是传感器在模拟过程中尝试的最大更新速率，但如果物理模拟运行速度快于传感器生成速度，则可能会落后于此目标速率。 -->
      <update_rate>30.0</update_rate>

        <!-- 填写这些值以匹配制造商对物理相机硬件的规格。需要注意的一点是，像素被假定为正方形。 -->
<!-- 此外，the near and far clips是模拟特定的参数，它们为摄影机在模拟中看到对象的距离提供上限和下限。这是在相机的验光框架中指定的。 -->
      <camera name="head">
        <horizontal_fov>1.3962634</horizontal_fov>
        <image>
          <width>800</width>
          <height>800</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.02</near>
          <far>300</far>
        </clip>


        <noise>
          <type>gaussian</type>
          <!-- Noise is sampled independently per pixel on each frame.
               That pixel's noise value is added to each of its color
               channels, which at that point lie in the range [0,1]. -->
          <mean>0.0</mean>
          <stddev>0.007</stddev>
        </noise>
      </camera>

<!-- This is where the actual file is linked to, as a shared object. -->
      <plugin name="camera_controller" filename="libgazebo_ros_camera.so">

        <alwaysOn>true</alwaysOn>
        <updateRate>0.0</updateRate>

        <!-- Here we define the rostopic the camera will be publishing to, for both the image topic and the camera info topic.  -->
        <cameraName>rrbot/camera1</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>

        <!-- 在tf树中发布图像的坐标系。 -->
        <frameName>camera_link</frameName>
        <hackBaseline>0.07</hackBaseline>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
      </plugin>
    </sensor>
  </gazebo>

  <gazebo reference="left_camera_frame">
    <sensor type="multicamera" name="stereo_camera">
      <update_rate>30.0</update_rate>
      <camera name="left">
        <horizontal_fov>1.3962634</horizontal_fov>
        <image>
          <width>800</width>
          <height>800</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>
      <camera name="right">
        <pose>0 -0.07 0 0 0 0</pose>
        <horizontal_fov>1.3962634</horizontal_fov>
        <image>
          <width>800</width>
          <height>800</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="stereo_camera_controller" filename="libgazebo_ros_multicamera.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>0.0</updateRate>
        <cameraName>multisense_sl/camera</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <frameName>left_camera_optical_frame</frameName>
        <!--<rightFrameName>right_camera_optical_frame</rightFrameName>-->
        <hackBaseline>0.07</hackBaseline>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
      </plugin>
    </sensor>
  </gazebo>

  <joint name="hokuyo_joint" type="fixed">
    <axis xyz="0 1 0" />
    <origin xyz="0 0 ${height3 - axel_offset/2}" rpy="0 0 0"/>
    <parent link="link3"/>
    <child link="hokuyo_link"/>
  </joint>

  <!-- Hokuyo Laser -->
  <link name="hokuyo_link">
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
    <box size="0.1 0.1 0.1"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="package://rrbot_description/meshes/hokuyo.dae"/>
      </geometry>
    </visual>

    <inertial>
      <mass value="1e-5" />
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
    </inertial>
  </link>

  <!-- hokuyo -->
  <gazebo reference="hokuyo_link">
    <sensor type="gpu_ray" name="head_hokuyo_sensor">
      <pose>0 0 0 0 0 0</pose>

      <!-- 当为true时，在gpu激光器的扫描区域内可见半透明激光射线。这可能是一个有用的信息可视化，也可能是一种麻烦。 -->
      <visualize>false</visualize>

      <update_rate>40</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>720</samples>
            <resolution>1</resolution>
            <min_angle>-1.570796</min_angle>
            <max_angle>1.570796</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.10</min>
          <max>30.0</max>
          <resolution>0.01</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <!-- Noise parameters based on published spec for Hokuyo laser
               achieving "+-30mm" accuracy at range < 10m.  A mean of 0.0m and
               stddev of 0.01m will put 99.7% of samples within 0.03m of the true
               reading. -->
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_gpu_laser.so">

      <!-- 将这些设置为您希望发布激光扫描的ROS主题名称，以及您希望TF使用的转换帧。 -->
        <topicName>/rrbot/laser/scan</topicName>
        <frameName>hokuyo_link</frameName>


      </plugin>
    </sensor>
  </gazebo>

<robot>
:
  <gazebo>
    <plugin name="imu_plugin" filename="libgazebo_ros_imu.so">
      <alwaysOn>true</alwaysOn>
      <bodyName>base_footprint</bodyName>
      <topicName>imu</topicName>
      <serviceName>imu_service</serviceName>
      <gaussianNoise>0.0</gaussianNoise>
      <updateRate>20.0</updateRate>
    </plugin>
  </gazebo>
</robot>

  <gazebo reference="imu_link">
    <gravity>true</gravity>
    <sensor name="imu_sensor" type="imu">
      <always_on>true</always_on>
      <update_rate>100</update_rate>
      <visualize>true</visualize>
      <topic>__default_topic__</topic>
      <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
        <topicName>imu</topicName>
        <bodyName>imu_link</bodyName>
        <updateRateHZ>10.0</updateRateHZ>
        <gaussianNoise>0.0</gaussianNoise>
        <xyzOffset>0 0 0</xyzOffset>
        <rpyOffset>0 0 0</rpyOffset>
        <frameName>imu_link</frameName>
        <initialOrientationAsReference>false</initialOrientationAsReference>
      </plugin>
      <pose>0 0 0 0 0 0</pose>
    </sensor>
  </gazebo>

<gazebo>
  <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">

    <!-- Plugin update rate in Hz -->
    <updateRate>${update_rate}</updateRate>

    <!-- Name of left joint, defaults to `left_joint` -->
    <leftJoint>base_link_left_wheel_joint</leftJoint>

    <!-- Name of right joint, defaults to `right_joint` -->
    <rightJoint>base_link_right_wheel_joint</rightJoint>

    <!-- The distance from the center of one wheel to the other, in meters, defaults to 0.34 m -->
    <wheelSeparation>0.5380</wheelSeparation>

    <!-- Diameter of the wheels, in meters, defaults to 0.15 m -->
    <wheelDiameter>0.2410</wheelDiameter>

    <!-- Wheel acceleration, in rad/s^2, defaults to 0.0 rad/s^2 -->
    <wheelAcceleration>1.0</wheelAcceleration>

    <!-- Maximum torque which the wheels can produce, in Nm, defaults to 5 Nm -->
    <wheelTorque>20</wheelTorque>

    <!-- Topic to receive geometry_msgs/Twist message commands, defaults to `cmd_vel` -->
    <commandTopic>cmd_vel</commandTopic>

    <!-- Topic to publish nav_msgs/Odometry messages, defaults to `odom` -->
    <odometryTopic>odom</odometryTopic>

    <!-- Odometry frame, defaults to `odom` -->
    <odometryFrame>odom</odometryFrame>

    <!-- Robot frame to calculate odometry from, defaults to `base_footprint` -->
    <robotBaseFrame>base_footprint</robotBaseFrame>

    <!-- Odometry source, 0 for ENCODER, 1 for WORLD, defaults to WORLD -->
    <odometrySource>1</odometrySource>

    <!-- Set to true to publish transforms for the wheel links, defaults to false -->
    <publishWheelTF>true</publishWheelTF>

    <!-- Set to true to publish transforms for the odometry, defaults to true -->
    <publishOdom>true</publishOdom>

    <!-- Set to true to publish sensor_msgs/JointState on /joint_states for the wheel joints, defaults to false -->
    <publishWheelJointState>true</publishWheelJointState>

    <!-- Set to true to swap right and left wheels, defaults to true -->
    <legacyMode>false</legacyMode>
  </plugin>
</gazebo>

#在src目录下
ros2 pkg create --build-type ament_cmake --node-name avoid_node johnbot2 --dependencies rclcpp sensor_msgs geometry_msgs

# 阅读帮助文档
ros2 pkg create -h

cd pro_ws/src/package_name
mkdir models
cd models

install(DIRECTORY
  models
  DESTINATION share/${PROJECT_NAME}/
)

  <!-- 原雷达（Laser）插件模板-->
  <gazebo reference="hokuyo_link">
    <sensor type="ray" name="head_hokuyo_sensor">
      <pose>0 0 0 0 0 0</pose>
      <visualize>false</visualize>
      <update_rate>40</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>720</samples>
            <resolution>1</resolution>
            <min_angle>-1.570796</min_angle>
            <max_angle>1.570796</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.10</min>
          <max>30.0</max>
          <resolution>0.01</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <!-- Noise parameters based on published spec for Hokuyo laser
               achieving "+-30mm" accuracy at range < 10m.  A mean of 0.0m and
               stddev of 0.01m will put 99.7% of samples within 0.03m of the true
               reading. -->
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so">
        <topicName>/rrbot/laser/scan</topicName>
        <frameName>hokuyo_link</frameName>
      </plugin>
    </sensor>
  </gazebo>

  <!--超声波模块-->
  <!-- Link名称更换为模型中的名称 -->
  <gazebo reference="ultrasonic_sensor_link">
    <!-- 传感器类型是ray，插件名称自定义，不能重复 -->
    <sensor type="ray" name="ultrasonic_sensor">
      <pose>0 0 0 0 0 0</pose>
      <visualize>true</visualize>
      <update_rate>40</update_rate>
      <ray>
        <scan>
          <horizontal>
          <!-- 水平方向 -->
              <samples>50</samples>
              <!-- 每圈产生的激光数量num = samples * resolution -->
              <resolution>1</resolution>
              <!-- 0.8弧度约为45度角度，这里根据实际自定义即可 -->
              <min_angle>-0.4</min_angle>
              <max_angle>0.4</max_angle>
          </horizontal>
          <vertical>
          <!-- 竖直方向，不需要扫描 -->
              <samples>1</samples>
              <resolution>1</resolution>
              <min_angle>0</min_angle>
              <max_angle>0</max_angle>
          </vertical>
        </scan>
        <!-- 超声波检测的范围和数据分辨率单位m，考虑实际情况修改测量范围 -->
        <range>
          <min>0.02</min>
          <max>4.0</max>
          <resolution>0.01</resolution>
        </range>
        <!-- 数据噪声采用高斯噪声 -->
        <noise>
          <type>gaussian</type>
          <!-- Noise parameters based on published spec for Hokuyo laser
               achieving "+-30mm" accuracy at range < 10m.  A mean of 0.0m and
               stddev of 0.01m will put 99.7% of samples within 0.03m of the true
               reading.此处可以考虑修改，在实际工程应用中参考实际模块参数即可 -->
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin name="ultrasonic_sensor_controller" filename="libgazebo_ros_ray_sensor.so">
        <ros>
          <!-- 重映射输出话题 注意这里的话题名称，在后面设计节点时需要用到-->
          <remapping>~/out:=ultrasonic_sensor_range</remapping>
        </ros>
        <!-- 输出消息的类型,参阅/opt/ros/humble/include/gazebo_plugins/gazebo_ros_ray_sensor.hpp文件 -->
        <output_type>sensor_msgs/Range</output_type>
        <!-- 射线类型有两种，参阅opt/ros/humble/include/sensor_msgs/msg/detail/range_struct.h文件-->
        <radiation_type>ultrasound</radiation_type>
        <!-- 坐标系名称，要与reference名称一致 -->
        <frame_name>ultrasonic_sensor_link</frame_name>
      </plugin>
    </sensor>
  </gazebo>

/// Plugin to attach to a gazebo ray or gpu_ray sensor and publish its data to ROS
/**
  \details
  SDF parameters:
  \verbatim
  <output_type>: Optional. The message type of the plugin's output. Can be any of the following:
    * sensor_msgs/PointCloud2: 3D cloud of points, Default
    * sensor_msgs/PointCloud:  3D cloud of points
    * sensor_msgs/LaserScan:   2D scan, uses center vertical ray if there are multiple
    * sensor_msgs/Range:       Single distance value, minimum of all ray ranges of parent sensor
  \endverbatim

  \verbatim
  <frame_name>: TF Frame id of output header.
               If not set, frame id will be name of sensor's parent link
  \endverbatim

  \verbatim
  <min_intensity>: Clip intensity values for output to this value.
                  Default: 0.0
  \endverbatim

  \verbatim
  <radiation_type>: The radiation type to publish when the output type is sensor_msgs/Range.
                   Can be either "infrared"（红外线） or "ultrasonic"（超声波）.
                   Default: "infrared".
  \endverbatim

  Example SDF:
  \code{.xml}
    <plugin name="my_ray_sensor_plugin" filename="libgazebo_ros_ray_sensor.so">
      <ros>
        <!-- Configure namespace and remap to publish to /ray/pointcloud2 -->
        <namespace>/ray</namespace>
        <remapping>~/out:=pointcloud2</remapping>
      </ros>
      <!-- Output as a PointCloud2, see above for other types -->
      <output_type>sensor_msgs/PointCloud2</output_type>
      <!-- Clip intensity values so all are above 100, optional -->
      <min_intensity>100.0</min_intensity>
      <!-- Frame id for header of output, defaults to sensor's parent link name -->
      <frame_name>ray_link</frame_name>
    </plugin>
  \endcode
*/

/ultrasonic_sensor_controller/out

/// Constant 'ULTRASOUND'.
/**
  * Radiation type enums
  * If you want a value added to this list, send an email to the ros-users list
 */
enum
{
  sensor_msgs__msg__Range__ULTRASOUND = 0
};

/// Constant 'INFRARED'.
enum
{
  sensor_msgs__msg__Range__INFRARED = 1
};

install(DIRECTORY
  launch
  DESTINATION share/${PROJECT_NAME}/
)

from launch_ros.actions import Node       # 节点启动的描述类
import os
from ament_index_python.packages import get_package_share_directory  # 查询功能包路径的方法
from launch import LaunchDescription                 # launch文件的描述类
from launch.actions import ExecuteProcess

def generate_launch_description():        # 自动生成launch文件的函数
    ld=LaunchDescription()
    package_name = 'johnbot2'
    pkg_path = os.path.join(get_package_share_directory(package_name))

    world_file_path = 'models/wall01.world'
    world_path = os.path.join(pkg_path, world_file_path) 
    robot_name_in_model = 'johnbot2'

    urdf_file_path = 'models/avoid_bot2.urdf'
    urdf_model_path = os.path.join(pkg_path,urdf_file_path)


    # Start Gazebo server
    start_gazebo_cmd = ExecuteProcess(
        cmd=['gazebo', '--verbose','-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so', world_path],
        output='screen'
    )
    ld.add_action(start_gazebo_cmd)

    # # Launch the robot
    # Pose where we want to spawn the robot,这一组参数指定起始位置和姿态，否则默认机器人出生在原点，有点类似MC游戏《我的世界》中“出生点”的概念
    spawn_x_val = '0.0'
    spawn_y_val = '-2.0'
    spawn_z_val = '0.0'
    spawn_yaw_val = '0.00'
    spawn_entity_cmd = Node(
        package='gazebo_ros', 
        executable='spawn_entity.py',
        arguments=['-entity', robot_name_in_model,  '-file', urdf_model_path ,'-x', spawn_x_val,
                                '-y', spawn_y_val,
                                '-z', spawn_z_val,
                                '-Y', spawn_yaw_val], output='screen')
    ld.add_action(spawn_entity_cmd)

    # Start Robot State publisher
    start_robot_state_publisher_cmd = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        arguments=[urdf_model_path]
    )
    ld.add_action(start_robot_state_publisher_cmd)

    # Launch RViz
    start_rviz_cmd = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        # arguments=['-d', default_rviz_config_path]
        )
    ld.add_action(start_rviz_cmd)

    # start_avoid_node = Node(
    #     package='johnbot2',
    #     executable= 'avoid_node',
    #     # arguments=[output = 'screen']
    # )
    # ld.add_action(start_avoid_node)
    # Launch them all!
    return ld

# 在proj_ws文件夹下
colcon build
source install/setup.bash
ros2 launch johnbot2 avoid.launch.py

ros2 interface show geometry_msgs/msg/Twist

#include <cstdio>
#include <chrono>
#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "sensor_msgs/msg/range.hpp"

class Node_avoid : public rclcpp::Node
{
    public:
        Node_avoid(std::string name) : Node(name)
        {
            RCLCPP_INFO(this -> get_logger(), "Node_avoid init here,name: %s",name.c_str());
            //创建发布者

            command_publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel",10);
            // timer_ = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&Node_avoid::timer_callback, this));

            distance_monitor_ = this->create_subscription<sensor_msgs::msg::Range>("ultrasonic_sensor_range",10,
            std::bind(&Node_avoid::distance_callback, this, std::placeholders::_1));

        }
    private:
        rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr command_publisher_;
        rclcpp::TimerBase::SharedPtr timer_;

        //超声波，距离监视器
        rclcpp::Subscription<sensor_msgs::msg::Range>::SharedPtr distance_monitor_;

        void distance_callback(const sensor_msgs::msg::Range distance_real)
        {
            geometry_msgs::msg::Twist run_command;
            float divide = 0.3;
            if(distance_real.range >= divide)
            {
                run_command = direct_run();
            }
            else{
                run_command = turn_left();
            }
            command_publisher_->publish(run_command);
        }


        //直行
        geometry_msgs::msg::Twist direct_run()
        {
            geometry_msgs::msg::Twist loc;
            loc.linear.x = 0.2;
            loc.linear.y = 0.0;
            loc.linear.z = 0.0;
            loc.angular.x = 0.0;
            loc.angular.y = 0.0;
            loc.angular.z = 0.0;
            return loc;
        }

        //转弯
        geometry_msgs::msg::Twist turn_left()
        {
            geometry_msgs::msg::Twist loc;
            loc.linear.x = 0.0;
            loc.linear.y = 0.0;
            loc.linear.z = 0.0;
            loc.angular.x = 0.0;
            loc.angular.y = 0.0;
            loc.angular.z = 0.8;
            return loc;
        }


};

/*
 @brief: 避障节点，没有障碍就直行，有则转弯。
 @note:订阅超声波ultrasonic_sensor_range话题，话题接口类型为sensor_msgs/msg/range，
 发布机器人运动的cmd_vel话题，话题接口类型geometry_msgs/msg/twist

*/
int main(int argc, char ** argv)
{
  rclcpp::init(argc,argv);
  auto node = std :: make_shared<Node_avoid>("avoid_node");
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

colcon build
source install/setup.bash
ros2 launch johnbot2 avoid.launch.py