def get_odom(self):
try:
(trans,rot) = self.tf_listener.looupTransform(self.odom_frame,self.base_frame,rospy.Time(0))
except (tf.Exception,tf.ConectivityException,tf.lookupException):
rospy.loginfo("TF exception")
return
return (Point(*trans),quat_to_angle(Quaternion(*rot)))
def get_odom(self):
# Get the current transform between the odom and base frames
try:
(trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("TF Exception")
return
return (Point(*trans), quat_to_angle(Quaternion(*rot)))
def get_odom(self):
# Get the current transform between the odom and base frames
try:
(trans,rot)=self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("TF Exception")
return
return (Point(*trans), quat_to_angle(Quaternion(*rot)))
def get_odom(self):
# Get the current transform between the odom and base frames
try:
(trans,
rot) = self.tf_listener.lookupTransform(self.odom_frame,
self.base_frame,
rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("TF Exception")
return
# used * is Python's a notion for passing a list of numbers to a function
# trans is a list of x, y, and z coordinates
# rot is a list of x, y, z and w quaternion components.
return (Point(*trans), quat_to_angle(Quaternion(*rot))
) #current location Point and the current angel
def get_odom_angle(self):
# Get the current transform between the odom and base frames
try:
(trans,
rot) = self.tf_listener.lookupTransform(self.odom_frame,
self.base_frame,
rospy.Time(0))
# print "trans:", trans, " rot: ", rot
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("TF Exception")
return
# Convert the rotation from a quaternion to an Euler angle
angle = quat_to_angle(Quaternion(*rot))
# print "get angle: ", angle
return angle
def __init__(self):
# 初始化节点
rospy.init_node("ma_path_following", anonymous=False)
# 注册回调函数
rospy.on_shutdown(self.shut_down)
# 发布控制消息的句柄
self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
# 机器人机体坐标系
self.base_frame = rospy.get_param('~base_frame', '/base_link')
# 里程计坐标系
self.odom_frame = rospy.get_param('~odom_frame', '/odom')
# 初始化一个tf监听器
self.tf_listener = tf.TransformListener()
# tf需要时间填满缓存
rospy.sleep(2)
# Find out if the robot uses /base_link or /base_footprint
try:
self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
rospy.signal_shutdown("tf Exception")
# 期望路径
self.desire_path = 'x**2 + y**2 -4 = 0'
# 初始状态
self.s = 0
theta_es0 = 0.1
self.p1_es = sin(theta_es0)
self.p2_es = cos(theta_es0)
# 机器人车前一点的长度
self.L = 0.1 # 误差变化和L有关，L越大，收敛越快
# 机器人的线速度设为常数
self.v = 0.3
# 键盘输入的设置
self.in_ch = None # 用于保存输入的字符
self.k_v = {'d': 0.5, 'u': 1.6}
# 最大线速度和角速度
self.max_angular_vel = 1
self.max_linear_vel = 0.7
# 设置仿真时间
self.sim_time = 100
# 仿真步长
self.dT = 0.01
# 控制参数
self.k = 0.5
self.beta = 1
# 存储机器人的位置信息
self.x_list = list()
self.y_list = list()
self.theta_list = list()
# 期望路径信息
self.xd_list = list()
self.yd_list = list()
# 保存速度和角速度
self.v_list = list()
self.w_list = list()
# 保存误差信息的list
self.ex_list = []
self.ey_list = []
# 保存仿真时间信息
self.sim_time_list = []
# 记录当前时间
sim_start_time = rospy.Time.now().secs
# How fast will we check the odometry values?
rate = 10 # 发布频率
# Set the equivalent ROS rate variable
r = rospy.Rate(rate)
# 开始等待输入的子线程
wait_for_input = threading.Thread(target=self.get_key)
wait_for_input.start()
# 循环
while rospy.Time.now().secs - sim_start_time < self.sim_time and not rospy.is_shutdown():
if self.in_ch is not None:
if self.in_ch in self.k_v:
self.v = self.v * self.k_v[self.in_ch]
self.in_ch = None # 重置为None
print("after change: v = %s\n" % self.v)
elif ord(self.in_ch) == 0x3: # ctrl-c 退出循环
break
# 得到机器人当前位姿
(position, rotation) = self.get_odom()
# 位姿信息
self.x = position.x
self.y = position.y
self.theta = quat_to_angle(rotation)
# 存入list
self.x_list.append(self.x+self.L*cos(self.theta))
self.y_list.append(self.y+self.L*sin(self.theta))
# self.x_list.append(self.x)
# self.y_list.append(self.y)
self.theta_list.append(self.theta)
# 期望路径
h1 = 1.5
h2 = 1
self.x_d = h1*cos(h2*self.s)
self.y_d = h1*sin(h2*self.s)
# 对s求偏导数
dx_d = -h1*h2*sin(h2*self.s)
dy_d = h1*h2*cos(h2*self.s)
# 存入list
self.xd_list.append(self.x_d)
self.yd_list.append(self.y_d)
# 误差
self.ex = self.x + self.L*cos(self.theta) - self.x_d
self.ey = self.y + self.L*sin(self.theta) - self.y_d
# 保存到list
self.ex_list.append(self.ex)
self.ey_list.append(self.ey)
# 计算控制输入
# 系数矩阵
Q_es = np.array([[-self.L * self.p1_es, -dx_d], [self.L*self.p2_es, -dy_d]])
B = np.array([[-self.v*self.p2_es - self.k * self.ex], [-self.v*self.p1_es - self.k * self.ey]])
# 控制矩阵
U = np.dot(inv(Q_es), B) # NumPy中的乘法运算符*指示按元素计算，矩阵乘法可以使用dot函数或创建矩阵对象实现
# 控制输入
self.w = U[0, 0] # U是一个矩阵，必须这样取值，不然U[0]取得的是一个list
self.ds = U[1, 0]
# 更新路径变量和自适应控制律
self.adaptive_controller(self.dT, self.beta)
# 室内机器人，对控制器进行限幅
self.w = self.limit_velocity(self.w, self.max_angular_vel)
self.v = self.limit_velocity(self.v, self.max_linear_vel)
# 保存到list
self.v_list.append(self.v)
self.w_list.append(self.w)
# 保存仿真时间到list
self.sim_time_list.append(rospy.Time.now().secs - sim_start_time)
# 初始化一个空的运动控制消息
self.cmd_vel = Twist()
# 填充
self.cmd_vel.angular.z = self.w
self.cmd_vel.linear.x = self.v
self.cmd_vel_pub.publish(self.cmd_vel)
# r.sleep()
rospy.sleep(self.dT)
# 循环结束，停下机器人
rospy.loginfo("循环结束，停下机器人")
self.cmd_vel_pub.publish(Twist())
