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())