环境
系统版本信息：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,添加以下两行
运行
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博客
加载地图
从别的文章看到的-未测试