从零搭建一套ROS-Stage仿真

环境

系统版本信息：Ubuntu18.04
 ROS版本 ：      melodic

ROS安装

推荐小鱼的一件安装

wget http://fishros.com/install -O fishros && . fishros

gedit ~/.bashrc 在末尾添加添加

source ~/turtlebot_ws/devel/setup.bash
export TURTLEBOT_STAGE_MAP_FILE=/home/agv/turtlebot_ws/src/sim_agv/test.yaml
export TURTLEBOT_STAGE_WORLD_FILE=/home/agv/turtlebot_ws/src/sim_agv/stage/test.world
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:/home/agv/turtlebot_ws/src/sim_agv

Turtlebot安装

GitHub - gaunthan/Turtlebot2-On-Melodic: Make your Turtlebot2 runs on ROS Melodic (Ubuntu 18.04).可参考install turtlebot on ubuntu 18.04 + ros melodic_ros-melodic-move_base install-CSDN博客文章浏览阅读3.7k次，点赞2次，收藏33次。计算机：华为matebook 14系统：ubuntu 18.04 + ros melodic目的：在以上计算机与系统上安装turtlebot(注意：不是turtlebot 3)1. 安装教程参照 gaunthan/Turtlebot2-On-Melodicgaunthan/Turtlebot2-On-Melodic/install_all.sh1.1 构造工作空间mkdir -p ..._ros-melodic-move_base installhttps://blog.csdn.net/u013468614/article/details/103394215

网盘连接：百度网盘 请输入提取码

提取码：LZZH

执行安装脚本install_basic.sh

添加环境

echo "source ~/turtlebot_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc

安装依赖

sudo apt-get install ros-melodic-move-base*
sudo apt-get install ros-melodic-map-server*
sudo apt-get install ros-melodic-amcl*
sudo apt-get install ros-melodic-navigation*

---

搭建ros包

mkdir -p turtlebot_ws/src
cd turtlebot_ws
catkin_make
cd src
catkin_create_pkg agv_sim roscpp rospy std_msgs
cd sim_agv
mkdir stage

在Turtlebot2-On-Melodic/src/turtlebot_simulator/turtlebot_stage/maps/stage找到turtlebot.inc文件复制到stage目录下

define kinect ranger

(

sensor

(

range_max 6.5

fov 58.0

samples 640

)

# generic model properties

color "black"

size [ 0.06 0.15 0.03 ]

)

define turtlebot position

(

pose [ 0.0 0.0 0.0 0.0 ]

odom_error [0.03 0.03 999999 999999 999999 0.02]

size [ 1.0 0.83 0.50 ]

origin [ 0.0 0.0 0.0 0.0 ]

drive "diff"

color "gray"

kinect(pose [ -0.1 0.0 -0.11 0.0 ])

)

在stage目录下新建一个test.world文件

world的语法Stage Manual

.world手册 stage_ros - ROS Wiki

include "turtlebot.inc"
# include "myBlock.inc"

define floorplan model
(
  # sombre, sensible, artistic
  color "gray30"

  # most maps will need a bounding box
  boundary 0

  gui_nose 0
  gui_grid 0
  gui_outline 0
  gripper_return 0
  fiducial_return 0
  laser_return 1
)

resolution 0.02
interval_sim 100  # simulation timestep in milliseconds

window
(
#  size [ 600.0 700.0 ]
  size [ 761 430 ]
  center [ 14.464 32.714 ]
  rotate [ 14.000 0.000 ]
  scale 7.513
)

#floorplan
#(
#  name "test"
#  bitmap "../maps/empty1.png"
#  size [ 15.0 15.0 2.0 ]
#  pose [  7.5  7.5 0.0 0.0 ]
#)

turtlebot
(
  pose [ 21.6 21.6 0.0 180.0 ]
  name " turtlebot0"
  color "green"
)

turtlebot: 小车参数，需要多台小车复制即可，注意修改name和pose防止重复重叠

例如：

include "turtlebot.inc"
# include "myBlock.inc"

define floorplan model
(
  # sombre, sensible, artistic
  color "gray30"

  # most maps will need a bounding box
  boundary 0

  gui_nose 0
  gui_grid 0
  gui_outline 0
  gripper_return 0
  fiducial_return 0
  laser_return 1
)

resolution 0.02
interval_sim 100  # simulation timestep in milliseconds

window
(
#  size [ 600.0 700.0 ]
  size [ 761 430 ]
  center [ 14.464 32.714 ]
  rotate [ 14.000 0.000 ]
  scale 7.513
)

#floorplan
#(
#  name "test"
#  bitmap "../maps/empty1.png"
#  size [ 15.0 15.0 2.0 ]
#  pose [  7.5  7.5 0.0 0.0 ]
#)
turtlebot
(
  pose [ 21.6 23.4 0.0 180.0 ]
  name " turtlebot1"
  color "green"
)
turtlebot
(
  pose [ 21.6 21.6 0.0 180.0 ]
  name " turtlebot0"
  color "green"
)

floorplan: 地图参数

注意在melodic环境下是不支持gui_nose的，想看小车小车方向可以点击stage->View->data，将data选项打钩。

运行roscore

roscore

新开终端运行stage

rosrun stage_ros stageros turtlebot_ws/src/sim_agv/stage/test.world

可以看到stage出现

---

节点介绍

运行rostopic list查看stage的消息

/base_pose_ground_truth        小车位置信息

/cmd_vel                                           控制命令

如果只有一台agv是没有前面的/robot_x的

---

小车控制：

简单的小车控制样例

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
from geometry_msgs.msg import Twist

def move_forward():
    # 创建一个发布者，发布速度指令给小车
    pub = rospy.Publisher('/robot_1/cmd_vel', Twist, queue_size=10)
    rospy.init_node('control_node', anonymous=True)
    rate = rospy.Rate(10)  # 发布频率为10Hz

    while not rospy.is_shutdown():
        # 创建Twist消息
        cmd_vel_msg = Twist()
        cmd_vel_msg.linear.x = 0.5  # 设置线速度为0.5 m/s
        cmd_vel_msg.angular.z = 0.0  # 设置角速度为0.0 rad/s

        # 发布速度指令
        pub.publish(cmd_vel_msg)
        rate.sleep()

def rotate():
    # 创建一个发布者，发布速度指令给小车
    pub = rospy.Publisher('/robot_1/cmd_vel', Twist, queue_size=10)
    rospy.init_node('control_node', anonymous=True)
    rate = rospy.Rate(10)  # 发布频率为10Hz

    while not rospy.is_shutdown():
        # 创建Twist消息
        cmd_vel_msg = Twist()
        cmd_vel_msg.linear.x = 0.0  # 设置线速度为0.0 m/s
        cmd_vel_msg.angular.z = 0.5  # 设置角速度为0.5 rad/s

        # 发布速度指令
        pub.publish(cmd_vel_msg)
        rate.sleep()

if __name__ == '__main__':
    try:
        # 运行小车运动控制函数
        # move_forward()
        rotate()
    except rospy.ROSInterruptException:
        pass

小车是以发布topic的形式控制的，我这里增加了位置转发节点，在使用的时候需要注意发布频率和控制频率的关系

转发节点脚本turtlebot_agv.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import numpy as np
import math
import rospy
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion
from sim_agv.srv import AgvPose, AgvPoseResponse


class Turtlebot():
    def __init__(self, agv_num=0):
        self.agv_prefix = '/robot_'+ str(agv_num)
        self.sub_cmd = '/base_pose_ground_truth'
        self.transmit_pose = '/get_pose'
        self.pre_pose = []
        self.agv_pose = []
        # print("agv_pub: {}, self.agv_sub: {}".format(self.agv_prefix+self.pub_cmd, self.agv_prefix+self.sub_cmd))
        self.rate = 0.5
         
    def _odom_callback(self, msg):
        _odom = msg.pose.pose
        angle = euler_from_quaternion([
            _odom.orientation.x,
            _odom.orientation.y,
            _odom.orientation.z,
            _odom.orientation.w,
        ])
        self.agv_pose = [_odom.position.x,
                         _odom.position.y,
                         math.degrees(angle[-1])]

    def subscrib_agv_pose(self):
        rate = rospy.Rate(self.rate)
        self.agv_sub = rospy.Subscriber(self.agv_prefix+self.sub_cmd, Odometry, self._odom_callback)
        
        
    def transmit_pose_callback(self, req):
        res = AgvPoseResponse()
        if self.pre_pose == self.agv_pose:
            print("[Turtlebot]: Error agv {} get same pose {}"\
                  .format(self.agv_prefix, self.agv_pose))
        self.pre_pose = self.agv_pose
        res.x = self.agv_pose[0]
        res.y = self.agv_pose[1]
        res.angle = self.agv_pose[2]
        return res
    
      
    def transmit_server(self):
        s1 = rospy.Service(self.agv_prefix+self.transmit_pose, AgvPose, self.transmit_pose_callback)
        print("Ready to get {} pose from server".format(self.agv_prefix))
        assert self.agv_pose is not None, "self.agv_pose is None" 

    def run(self):
        self.subscrib_agv_pose()
        self.transmit_server()

if __name__ == "__main__":
    rospy.init_node('agv_control_node', anonymous=True)
    agv_num = 72
    for i in range(agv_num):
        agv = Turtlebot(i)
        agv.run()
    rospy.spin()

创建srv文件夹。添加AgvPose.srv

---
float64 x
float64 y
float64 angle

修改CMakeLists.txt，添加

add_service_files(

FILES

AgvPose.srv

)

generate_messages(

DEPENDENCIES

std_msgs

)

catkin_package(

# INCLUDE_DIRS include

# LIBRARIES sim_agv

CATKIN_DEPENDS roscpp rospy std_msgs message_runtime

# DEPENDS system_lib

)

include_directories(

# include

${catkin_INCLUDE_DIRS}

)

find_package(catkin REQUIRED COMPONENTS

roscpp

rospy

std_msgs

message_generation

)

修改package.xml,添加以下两行

<build_depend>message_generation</build_depend>

<exec_depend>message_runtime</exec_depend>

运行

source ~/sim_agv/devel/setup.bash
sudo chmod +x turtlebot_agv.py
rosrun sim_agv turtlebot_agv.py

之后运行rosservice list可以看到转发的节点已经发布

---

封装小车类：

agv

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import numpy as np
import math
from sim_agv.srv import AgvPose
from pid_control import PIDControl
import tf.transformations as tfm
from geometry_msgs.msg import Twist
from std_srvs.srv import Empty
import time
import threading
class AGV:
    def __init__(self, agv_num):
        self.agv_prefix = '/robot_'+ str(agv_num)
        self.pub_cmd = self.agv_prefix + '/cmd_vel'
        self.gp_service_name = self.agv_prefix + '/get_pose'
        self.res_cmd = '/reset_positions'
        rospy.wait_for_service(self.gp_service_name)
        self.vel_msg = Twist()
        self.rate = 50
        self.agv_pub = rospy.Publisher(self.pub_cmd, Twist, queue_size=10)
        self.gp_clinet = rospy.ServiceProxy(self.gp_service_name, AgvPose)
        self.reset_client = rospy.ServiceProxy(self.res_cmd, Empty)
        print("[AGV]: Create agv {}".format(agv_num))
        self.lock = threading.Lock()
        self.latest_pose = None

    def reset_positions(self):
        self.reset_client()

    def get_pose(self):
        request = self.gp_clinet()
        x = request.x
        y = request.y
        angle = request.angle
        return [x, y, angle]
    
    def pose_matrix(self, cur_pose, angle):
        pose = [cur_pose[0],cur_pose[1],0,0,0,angle]
        cur_matrix = np.eye(4)
        rot_matrix = tfm.euler_matrix(0,0,pose[-1],'sxyz')
        cur_matrix[:3,:3] = rot_matrix[:3,:3]
        cur_matrix[:2,3] = pose[:2]
        return cur_matrix

    def matrix_pose(self, matrix):
        """
        return pose:[x, y, angle]
        """
        pose = list(matrix[:2,3])
        angle = tfm.euler_from_matrix(matrix[:3,:3])
        pose.append(math.degrees(angle[-1]))
        return pose
    
    def move_l(self, tar_pose, speed=1.0):
        with self.lock:
            start = time.time()
            distance = 10
            angular_pid = PIDControl(Kp=0.1, Ki=0.000, Kd=0.002)
            speed_pid = PIDControl(Kp=0.05, Ki=0.0, Kd=0.0005)
            rospy.Rate(self.rate)
            agv_pose = self.get_pose()
            car_radians = np.radians(agv_pose[-1])
            rotation_matrix = np.array([[np.cos(car_radians), -np.sin(car_radians), 0],
                                        [np.sin(car_radians), np.cos(car_radians), 0],
                                        [0, 0, 1]])
            vector_ = np.array(
                [tar_pose[0] - agv_pose[0], tar_pose[1] - agv_pose[1], 0])
            # 将向量差转换到B坐标系中
            transformed_vector = np.dot(rotation_matrix.T, vector_)
            dif_angle = np.degrees(np.arctan2(
                        transformed_vector[1], transformed_vector[0]))
            # tf_base_car = self.pose_matrix(agv_pose[0:2], agv_pose[-1])
            # tf_base_new = self.pose_matrix(tar_pose, agv_pose[-1])
            # tf_car_new = np.dot(np.linalg.inv(tf_base_car), tf_base_new)
            # distance = math.sqrt(tf_car_new[0,3] ** 2 + tf_car_new[1,3] ** 2)
            while distance>0.1:  
                agv_pose = self.get_pose()
                car_radians = np.radians(agv_pose[-1])
                rotation_matrix = np.array([[np.cos(car_radians), -np.sin(car_radians), 0],
                                            [np.sin(car_radians), np.cos(car_radians), 0],
                                            [0, 0, 1]])
                vector_ = np.array(
                    [tar_pose[0] - agv_pose[0], tar_pose[1] - agv_pose[1], 0])
                # 将向量差转换到B坐标系中
                transformed_vector = np.dot(rotation_matrix.T, vector_)
                dif_angle = np.degrees(np.arctan2(
                            transformed_vector[1], transformed_vector[0]))
                distance = np.linalg.norm(vector_)
                angular = angular_pid.update(0, dif_angle, self.rate)
                self.vel_msg.angular.z = -angular
                # speed =abs(speed) if pose_car_new[1] > 0 else -abs(speed)
                if distance<0.1:
                    speed = speed_pid.update(0, distance, self.rate)
                    #TODO 判断距离
                # print("distance: {}, cur_pose: {}, tar_pose: {}, speed: {}"\
                #                 .format(distance, agv_pose, tar_pose, speed))
                self.vel_msg.linear.x = speed
                self.agv_pub.publish(self.vel_msg)
                rospy.sleep(0.1)
            self.vel_msg.linear.x = 0
            self.vel_msg.angular.z = 0
            # rospy.wait_for_message(self.pub_cmd, Twist) 
            self.agv_pub.publish(self.vel_msg)
            rospy.sleep(0.5)
            print("[AGV]: current pose {}".format(self.get_pose()))
        return time.time()-start

    def move_c(self, angle, speed_c=0.5, e=0.1):
        with self.lock:
            start = time.time()
            pid = PIDControl(Kp=0.1, Ki=0.0, Kd=0.0)
            rate = rospy.Rate(self.rate)
            # 计算角度差
            cur_angle = self.get_pose()[-1]
            tf_base_car = tfm.euler_matrix(0,0,math.radians(cur_angle),'sxyz')
            tf_car_new = tfm.euler_matrix(0,0,math.radians(angle),'sxyz')
            tf_base_new = np.dot(tf_base_car, tf_car_new)
            C = np.dot(np.linalg.inv(tf_base_car), tf_base_new)
            angle_dif = tfm.euler_from_matrix(C, 'sxyz')[-1]
            angle_dif = math.degrees(angle_dif)
            speed = speed_c
            while abs(angle_dif) > e:
                # 计算角度差
                cur_angle = self.get_pose()[-1]
                tf_base_car = tfm.euler_matrix(0,0,math.radians(cur_angle),'sxyz')
                tf_car_new = np.dot(np.linalg.inv(tf_base_car), tf_base_new)
                angle_dif = tfm.euler_from_matrix(tf_car_new, 'sxyz')[-1]
                angle_dif = math.degrees(angle_dif)

                if angle_dif < 1 :
                    speed = pid.update(0, angle_dif, self.rate)
                    speed = speed if angle_dif > 0 else -speed
                else:
                    speed = speed_c if angle_dif > 0 else -speed_c
                # print("angle_dif: {}  cur_angle: {} input angle:{} speed: {}".format(angle_dif, cur_angle, angle, speed))
                self.vel_msg.angular.z = speed
                self.agv_pub.publish(self.vel_msg)
                # rate.sleep()
            self.vel_msg.angular.z = 0  
            self.agv_pub.publish(self.vel_msg)
            print("[AGV]: current pose {}".format(self.get_pose()))
        return time.time()-start

    def move_ofsc(self, tar_angle, speed_c=1, e=1):
        with self.lock:
            start = time.time()
            pid = PIDControl(Kp=0.1, Ki=0.0, Kd=0.0)
            rospy.Rate(self.rate)
            # 计算角度差
            cur_angle = self.get_pose()[-1]
            tf_base_car = tfm.euler_matrix(0,0,math.radians(cur_angle),'sxyz')
            tf_base_new = tfm.euler_matrix(0,0,math.radians(tar_angle),'sxyz')
            C = np.dot(np.linalg.inv(tf_base_car), tf_base_new)
            angle_dif = tfm.euler_from_matrix(C, 'sxyz')[-1]
            angle_dif = math.degrees(angle_dif)
            speed = speed_c
            while abs(angle_dif) > e:
                # 计算角度差
                cur_angle = self.get_pose()[-1]
                tf_base_car = tfm.euler_matrix(0,0,math.radians(cur_angle),'sxyz')
                tf_car_new = np.dot(np.linalg.inv(tf_base_car), tf_base_new)
                angle_dif = tfm.euler_from_matrix(tf_car_new, 'sxyz')[-1]
                angle_dif = math.degrees(angle_dif)
                # # use pid    
                if angle_dif < 1 :
                    speed = pid.update(0, angle_dif, 0.2)
                    speed = speed if angle_dif > 0 else -speed
                else:
                    speed = speed_c if angle_dif > 0 else -speed_c
                # print("angle_dif: {}  cur_angle: {} input tar_angle:{} speed: {}"\
                #       .format(angle_dif, cur_angle, tar_angle, speed))
                # speed = angle_dif / 180 * 3.14159
                # print("angle_dif: {}, speed: {}".format(angle_dif,speed))
                self.vel_msg.angular.z = speed
                # rospy.wait_for_message(self.pub_cmd, Twist) 
                self.agv_pub.publish(self.vel_msg)
                rospy.sleep(0.1)
            self.vel_msg.linear.x = 0
            self.vel_msg.angular.z = 0  
            # rospy.wait_for_message(self.pub_cmd, Twist) 
            self.agv_pub.publish(self.vel_msg)
            rospy.sleep(0.5)
            print("[AGV]: current pose {}".format(self.get_pose()))
        return time.time()-start


if __name__=="__main__":
    rospy.init_node('agv_test')
    agv = AGV(0)
    agv.move_ofsc(90)

pid

#!/usr/bin/env python
# -*- coding: utf-8 -*-

class PIDControl:
    def __init__(self, Kp=0.25, Ki=0.0, Kd=0.1, integral_upper_limit=20, integral_lower_limit=-20):  
        self.Kp = Kp  
        self.Ki = Ki  
        self.Kd = Kd  
        self.integral_upper_limit = integral_upper_limit  
        self.integral_lower_limit = integral_lower_limit  
        self.prev_error = 0  
        self.integral = 0  
  
    def update(self, setpoint, actual_position, dt):  
        error = setpoint - actual_position  
        self.integral += error * dt  
        derivative = self.Kd * (error - self.prev_error) / dt  
        self.prev_error = error  
        proportional = self.Kp * error 
        integral = self.Ki * self.clip(self.integral, self.integral_lower_limit, self.integral_upper_limit)
        control_increment = proportional + derivative + integral
        # print("[PID]: 比例:{:.3f}, 积分:{:.3f}, 微分:{:.3f}, error:{:.3f}, p: {:.3f}"\
        #     .format(proportional,integral, derivative,error,control_increment))
        return control_increment  
  
    @staticmethod  
    def clip(value, lower_limit, upper_limit):  
        if value < lower_limit:  
            # print(f"lower_limit:{lower_limit}, upper_limit:{upper_limit}")
            return lower_limit  
        elif value > upper_limit:  
            # print(f"lower_limit:{lower_limit}, upper_limit:{upper_limit}")
            return upper_limit  
        else:  
            return value
    
    def clear(self):  
        self.integral = 0  
        self.prev_error = 0

运行测试效果

sudo chmod +x agv.py
cd ~/sim_agv
catkin_make
rosrun sim_agv agv.py

---

记录一些问题

stage小车发布频率

使用此方法未修改成功，怀疑是小车模型的原因，我这边使用的是turtlebot模型，文中使用的是robot模型

解决roslaunch启动stage_ros节点仿真时输出rostopic hz速率较低的问题（始终为10Hz）_linux ros topic频率异常-CSDN博客

加载地图

从别的文章看到的-未测试