多机器人Gazebo仿真

准备：

git clone -b kinetic-devel https://gitee.com/kay2020/turtlebot3_simulations.git

　　

cd ~/catkin_ws/src/turtlebot3_simulations/turtlebot3_gazebo/launch/

　　

sudo vim multi_turtlebot3_empty_world.launch

　

内容如下：　

<launch>
  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>

  <arg name="first_tb3"  default="tb3_0"/>
  <arg name="second_tb3"  default="tb3_1"/>
  <arg name="third_tb3"  default="tb3_2"/>
  <arg name="fourth_tb3"  default="tb3_3"/>

  <arg name="first_tb3_x_pos" default="0.0"/>
  <arg name="first_tb3_y_pos" default="0.0"/>
  <arg name="first_tb3_z_pos" default="0.0"/>
  <arg name="first_tb3_yaw"   default="1.57"/>

  <arg name="second_tb3_x_pos" default="0.0"/>
  <arg name="second_tb3_y_pos" default="0.5"/>
  <arg name="second_tb3_z_pos" default="0.0"/>
  <arg name="second_tb3_yaw"   default="1.57"/>

  <arg name="third_tb3_x_pos" default="0.0"/>
  <arg name="third_tb3_y_pos" default="1.0"/>
  <arg name="third_tb3_z_pos" default="0.0"/>
  <arg name="third_tb3_yaw"   default="1.57"/>

  <arg name="fourth_tb3_x_pos" default="0.0"/>
  <arg name="fourth_tb3_y_pos" default="1.5"/>
  <arg name="fourth_tb3_z_pos" default="0.0"/>
  <arg name="fourth_tb3_yaw"   default="1.57"/>

  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find turtlebot3_gazebo)/worlds/empty.world"/>
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="gui" value="true"/>
    <arg name="headless" value="false"/>
    <arg name="debug" value="false"/>
  </include>

  <group ns = "$(arg first_tb3)">
    <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />

     <node pkg="gazebo_ros" type="spawn_model" name="spawn_urdf"  args="-urdf -model $(arg first_tb3) -x $(arg first_tb3_x_pos) -y $(arg first_tb3_y_pos) -z $(arg first_tb3_z_pos) -Y $(arg first_tb3_yaw) -param robot_description" />
  </group>

  <group ns = "$(arg second_tb3)">
     <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />

     <node pkg="gazebo_ros" type="spawn_model" name="spawn_urdf"  args="-urdf -model $(arg second_tb3) -x $(arg second_tb3_x_pos) -y $(arg second_tb3_y_pos) -z $(arg second_tb3_z_pos) -Y $(arg second_tb3_yaw) -param robot_description" />
  </group>

  <group ns = "$(arg third_tb3)">
    <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />

     <node pkg="gazebo_ros" type="spawn_model" name="spawn_urdf"  args="-urdf -model $(arg third_tb3) -x $(arg third_tb3_x_pos) -y $(arg third_tb3_y_pos) -z $(arg third_tb3_z_pos) -Y $(arg third_tb3_yaw) -param robot_description" />
  </group>

  <group ns = "$(arg fourth_tb3)">
    <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />

     <node pkg="gazebo_ros" type="spawn_model" name="spawn_urdf"  args="-urdf -model $(arg fourth_tb3) -x $(arg fourth_tb3_x_pos) -y $(arg fourth_tb3_y_pos) -z $(arg fourth_tb3_z_pos) -Y $(arg fourth_tb3_yaw) -param robot_description" />
  </group>
</launch>

　　

测试

roslaunch turtlebot3_gazebo multi_turtlebot3_empty_world.launch

 

到达指定点：

加中文编码

# -*- coding: utf-8 -*-

　　

import rospy
import math
import nav_msgs.msg
import geometry_msgs.msg
from tf.transformations import euler_from_quaternion

# 定义全局变量
x = 0
y = 0
w_o = 0
x_o = 0
y_o = 0
z_o = 0
yaw_t = 0
liner_speed = 0
angular_speed = 0
liner_speed_old = 0
angular_speed_old = 0

X_t = 0
Y_t = 0
X_sim = 5        # 目标点x坐标
Y_sim = 5        # 目标点y坐标


def Trans_robot_pose(msg):  # 回调函数
    # 位置坐标声明
    global x
    global y
    global w_o    # 当前小车位姿的四元数信息
    global x_o
    global y_o
    global z_o
    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y
    w_o = msg.pose.pose.orientation.w
    x_o = msg.pose.pose.orientation.x
    y_o = msg.pose.pose.orientation.y
    z_o = msg.pose.pose.orientation.z
    return w_o, y_o, z_o, x_o, x, y


if __name__ == '__main__':
    rospy.init_node('item1')

    turtle_vel = rospy.Publisher('/tb3_0/cmd_vel', geometry_msgs.msg.Twist, queue_size=1)
    rate = rospy.Rate(10.0)

    while not rospy.is_shutdown():
        msg = geometry_msgs.msg.Twist()
        (roll, pitch, yaw) = euler_from_quaternion([x_o,y_o,z_o,w_o])  # 将四元数转化为roll, pitch, yaw
        if yaw < 0:
            yaw = yaw + 2 * math.pi

        X_t = X_sim
        Y_t = Y_sim

        D_err = math.sqrt(math.pow((X_t - x), 2) + math.pow((Y_t - y), 2))

        # 判断坐标象限
        if (Y_t - y) == 0 and (X_t - x) > 0:
            yaw_t = 0
        if (Y_t - y) > 0 and (X_t - x) > 0:
            yaw_t = math.atan((Y_t - y) / (X_t - x))
        if (Y_t - y) > 0 and (X_t - x) == 0:
            yaw_t = 0.5 * math.pi
        if (Y_t - y) > 0 and (X_t - x) < 0:
            yaw_t = math.atan((Y_t - y) / (X_t - x)) + math.pi
        if (Y_t - y) == 0 and (X_t - x) < 0:
            yaw_t = math.pi
        if (Y_t - y) < 0 and (X_t - x) < 0:
            yaw_t = math.atan((Y_t - y) / (X_t - x)) + math.pi
        if (Y_t - y) < 0 and (X_t - x) == 0:
            yaw_t = 1.5 * math.pi
        if (Y_t - y) < 0 and (X_t - x) > 0:
            yaw_t = math.atan((Y_t - y) / (X_t - x)) + 2 * math.pi

        Theta_err = yaw_t - yaw
        if Theta_err < -math.pi:
            Theta_err = Theta_err + 2 * math.pi
        if Theta_err > math.pi:
            Theta_err = Theta_err - 2 * math.pi

        # 目前先只使用最简单的比例控制
        liner_speed = 0.1 * D_err
        angular_speed = 0.7 * Theta_err

        msg.linear.x = liner_speed
        msg.angular.z = angular_speed

        liner_speed_old = liner_speed
        angular_speed_old = angular_speed
        turtle_vel.publish(msg)    # 发布运动指令
        rospy.Subscriber('/tb3_0/odom', nav_msgs.msg.Odometry,  Trans_robot_pose) # 获取位姿信息

        rate.sleep()
    rospy.spin()

　　

给定轨迹

import rospy
import math
import nav_msgs.msg
import geometry_msgs.msg
from tf.transformations import euler_from_quaternion

# 定义全局变量
x = 0
y = 0
w_o = 0
x_o = 0
y_o = 0
z_o = 0
yaw_t = 0
liner_speed = 0
angular_speed = 0
liner_speed_old = 0
angular_speed_old = 0

X_t = 0
Y_t = 0
X_t_Pre = 0
Y_t_Pre = 0
X_sim = [5,  1, 2.5,  4, 0,   0,   5,  5,  0, 0]        # 目标点x坐标
Y_sim = [0, -4, 2.5, -4, 0, 2.5, 2.5, -4, -4, 0]        # 目标点y坐标
r = 0


def Trans_robot_pose(msg):  # 回调函数
    # 位置坐标声明
    global x
    global y
    global w_o    # 当前小车位姿的四元数信息
    global x_o
    global y_o
    global z_o
    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y
    w_o = msg.pose.pose.orientation.w
    x_o = msg.pose.pose.orientation.x
    y_o = msg.pose.pose.orientation.y
    z_o = msg.pose.pose.orientation.z
    return w_o, y_o, z_o, x_o, x, y


if __name__ == '__main__':
    rospy.init_node('item1')

    turtle_vel = rospy.Publisher('/tb3_0/cmd_vel', geometry_msgs.msg.Twist, queue_size=1)
    rate = rospy.Rate(10.0)

    while not rospy.is_shutdown():
        msg = geometry_msgs.msg.Twist()
        (roll, pitch, yaw) = euler_from_quaternion([x_o,y_o,z_o,w_o])  # 将四元数转化为roll, pitch, yaw
        if yaw < 0:
            yaw = yaw + 2 * math.pi

        X_t = X_sim[r]
        Y_t = Y_sim[r]

        D_err = math.sqrt(math.pow((X_t - x), 2) + math.pow((Y_t - y), 2))

        # 判断坐标象限
        if (Y_t - y) == 0 and (X_t - x) > 0:
            yaw_t = 0
        if (Y_t - y) > 0 and (X_t - x) > 0:
            yaw_t = math.atan((Y_t - y) / (X_t - x))
        if (Y_t - y) > 0 and (X_t - x) == 0:
            yaw_t = 0.5 * math.pi
        if (Y_t - y) > 0 and (X_t - x) < 0:
            yaw_t = math.atan((Y_t - y) / (X_t - x)) + math.pi
        if (Y_t - y) == 0 and (X_t - x) < 0:
            yaw_t = math.pi
        if (Y_t - y) < 0 and (X_t - x) < 0:
            yaw_t = math.atan((Y_t - y) / (X_t - x)) + math.pi
        if (Y_t - y) < 0 and (X_t - x) == 0:
            yaw_t = 1.5 * math.pi
        if (Y_t - y) < 0 and (X_t - x) > 0:
            yaw_t = math.atan((Y_t - y) / (X_t - x)) + 2 * math.pi

        Theta_err = yaw_t - yaw

        if Theta_err < -math.pi:
            Theta_err = Theta_err + 2 * math.pi
        if Theta_err > math.pi:
            Theta_err = Theta_err - 2 * math.pi

        if D_err < 0.3:        # 当距当前点小于一定值的时候去下一个点
            X_t_Pre = X_t
            Y_t_Pre = Y_t
            r = r + 1
            print r
            if r == 10:
                r = 0

            # 仍然先只使用最简单的比例控制
        liner_speed = 0.1 * D_err
        angular_speed = 0.7 * Theta_err

        msg.linear.x = liner_speed
        msg.angular.z = angular_speed

        liner_speed_old = liner_speed
        angular_speed_old = angular_speed
        turtle_vel.publish(msg)    # 发布运动指令
        rospy.Subscriber('/tb3_0/odom', nav_msgs.msg.Odometry,  Trans_robot_pose) # 获取位姿信息

        rate.sleep()
    rospy.spin()

　　

参考：