├── AStarPlanner.py ├── LICENSE ├── README.md ├── Vplanner.py ├── dwa.py ├── gazebo ├── CMakeLists.txt ├── course_agv_control │ ├── CMakeLists.txt │ ├── config │ │ └── course_agv_control.yaml │ ├── launch │ │ └── course_agv_control.launch │ ├── package.xml │ └── scripts │ │ ├── keyboard_velocity.py │ │ └── kinematics.py ├── course_agv_description │ ├── CMakeLists.txt │ ├── launch │ │ ├── course_agv.rviz │ │ └── course_agv_rviz.launch │ ├── meshes │ │ └── hokuyo.dae │ ├── package.xml │ └── urdf │ │ ├── course_agv.gazebo │ │ ├── course_agv.xacro │ │ └── materials.xacro ├── course_agv_gazebo │ ├── CMakeLists.txt │ ├── config │ │ ├── map.yaml │ │ └── map │ │ │ ├── map.png │ │ │ └── map_backup2.png │ ├── launch │ │ ├── course_agv.rviz │ │ ├── course_agv_world.launch │ │ └── course_agv_world_rviz.launch │ ├── models │ │ └── ground_plane_for_agv │ │ │ ├── map │ │ │ ├── map.png │ │ │ └── map_backup2.png │ │ │ ├── materials │ │ │ └── textures │ │ │ │ ├── flat_normal.png │ │ │ │ └── grey.png │ │ │ ├── model.config │ │ │ └── model.sdf │ ├── package.xml │ ├── scripts │ │ └── robot_tf.py │ └── worlds │ │ └── course_agv.world └── course_agv_nav │ ├── CMakeLists.txt │ ├── launch │ ├── nav.launch │ └── nav.rviz │ ├── package.xml │ ├── scripts │ ├── A_star.py │ ├── RRT_plan.py │ ├── RRT_rewire.py │ ├── __pycache__ │ │ ├── A_star.cpython-39.pyc │ │ └── dwaplanner.cpython-39.pyc │ ├── dwaplanner.py │ ├── global_planner.py │ └── local_planner.py │ └── srv │ └── Plan.srv └── main.py /AStarPlanner.py: -------------------------------------------------------------------------------- 1 | from heapq import heappush, heappop 2 | from numpy import sqrt 3 | import numpy as np 4 | class AStarPlanner: 5 | def __init__(self, unit_step = 0.1): 6 | self.unit_step = unit_step 7 | def planning(self, planning_obs_x, planning_obs_y, planning_obs_radius, planning_start_x, planning_start_y, planning_target_x, planning_target_y, planning_minx, planning_miny, planning_maxx, planning_maxy): 8 | heap = [] 9 | # 离散化 10 | planning_start_x_int = int((planning_start_x - planning_minx) / self.unit_step) 11 | planning_start_y_int = int((planning_start_y - planning_miny) / self.unit_step) 12 | planning_target_x_int = int((planning_target_x - planning_minx) / self.unit_step) 13 | planning_target_y_int = int((planning_target_y - planning_miny) / self.unit_step) 14 | planning_max_x_int = int((planning_maxx - planning_minx) / self.unit_step) 15 | planning_max_y_int = int((planning_maxy - planning_miny) / self.unit_step) 16 | map = np.zeros((planning_max_x_int, planning_max_y_int)) 17 | 18 | # 设置障碍物 19 | planning_obs_radius *= 1.0 20 | obs_radius_int = int(planning_obs_radius / self.unit_step) 21 | for i in range(planning_obs_x.shape[0]): 22 | obs_x_int = int((planning_obs_x[i] - planning_minx) / self.unit_step) 23 | obs_y_int = int((planning_obs_y[i] - planning_miny) / self.unit_step) 24 | for x in range(obs_x_int - obs_radius_int, obs_x_int + obs_radius_int): 25 | for y in range(obs_y_int - obs_radius_int, obs_y_int + obs_radius_int): 26 | real_x = planning_minx + x * self.unit_step 27 | real_y = planning_miny + y * self.unit_step 28 | if x >= 0 and x < planning_max_x_int and y >= 0 and y < planning_max_y_int and (real_x - planning_obs_x[i]) ** 2 + (real_y - planning_obs_y[i]) ** 2 <= obs_radius_int ** 2: 29 | map[x, y] = 1 30 | 31 | # 初始化 32 | heappush(heap, (0, planning_start_x_int, planning_start_y_int)) 33 | came_from = {} 34 | cost_so_far = {} 35 | # came_from[(planning_start_x_int, planning_start_y_int)] = None 36 | cost_so_far[(planning_start_x_int, planning_start_y_int)] = 0 37 | while len(heap) > 0: 38 | current = heappop(heap) 39 | # 如果已经到达目标点，直接返回 40 | if current[1] == planning_target_x_int and current[2] == planning_target_y_int: 41 | break 42 | #只允许上下左右,斜向走 43 | for next in [(current[1] + 1, current[2]), (current[1] - 1, current[2]), (current[1], current[2] + 1), (current[1], current[2] - 1), (current[1] + 1, current[2] + 1), (current[1] - 1, current[2] - 1), (current[1] + 1, current[2] - 1), (current[1] - 1, current[2] + 1)]: 44 | # 若超出地图范围，则跳过 45 | if next[0] < 0 or next[0] >= planning_max_x_int or next[1] < 0 or next[1] >= planning_max_y_int: 46 | continue 47 | # 换算next实际坐标 48 | next_x = next[0] * self.unit_step + planning_minx 49 | next_y = next[1] * self.unit_step + planning_miny 50 | # 若为障碍物，则跳过 51 | if map[next[0], next[1]] == 1: 52 | continue 53 | # if ((next_x - planning_obs_x) ** 2 + (next_y - planning_obs_y) ** 2 <= planning_obs_radius ** 2).any(): 54 | # continue 55 | # for i in range(len(planning_obs_x)): 56 | # if (next_x - planning_obs_x[i]) ** 2 + (next_y - planning_obs_y[i]) ** 2 < planning_obs_radius ** 2: 57 | # continue 58 | # 换算current实际坐标 59 | current_x = current[1] * self.unit_step + planning_minx 60 | current_y = current[2] * self.unit_step + planning_miny 61 | # 计算新的cost,因为只允许向四个方向走，不加根号也可以!!!!!!!!!!!!!!!(加上了斜向走，所以不加根号不可以) 62 | new_cost = cost_so_far[(current[1], current[2])] + np.sqrt((next_x - current_x) ** 2 + (next_y - current_y) ** 2) 63 | if next not in cost_so_far or new_cost < cost_so_far[next]: 64 | cost_so_far[next] = new_cost 65 | #这里也忘了sqrt 66 | priority = new_cost + sqrt((planning_target_x - next_x) ** 2 + (planning_target_y - next_y) ** 2) 67 | heappush(heap, (priority, next[0], next[1])) 68 | came_from[next] = (current[1], current[2]) 69 | #如果到达不了 70 | if (planning_target_x_int, planning_target_y_int) not in cost_so_far: 71 | return [],[] 72 | #回溯路径，返回的是非离散空间的路径 73 | current = (planning_target_x_int, planning_target_y_int) 74 | path = [] 75 | while current in came_from: 76 | path.append((current[0] * self.unit_step + planning_minx, current[1] * self.unit_step + planning_miny)) 77 | current = came_from[current] 78 | path.append((planning_start_x, planning_start_y)) 79 | path.reverse() 80 | pathnp = np.array(path) 81 | return pathnp[:,0], pathnp[:,1] -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 kaliv 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Robot 2 | - 实现Astar算法和DWA算法的结合 3 | 1. main.py: 文件可以通过Astar算法实现两点间的路径规划 4 | 2. dwa.py: 文件在main.py文件的基础上增加了dwa动态窗口算法，可以实现小车在运行过程中动态避障功能 5 | 3. Vplanner.py: dwa算法实现 6 | 4. AStarPlanner.py: astar算法实现 7 | 8 | - 关键控制指令： 9 | 1. 按下鼠标左键放置起始点 10 | 2. 按下鼠标右键放置终点 11 | 3. 按下鼠标中键放置障碍物 12 | 4. 按下空格键开始规划路径 13 | 14 | 15 | ## Astar算法实现的示意图 16 | 17 | 18 | 19 | 20 | 21 | ## dwa算法实现的示意图 22 | 23 | 24 | ## dwa算法带雷达显示 25 | 26 | 27 | 28 | 29 | 由于dwa算法的缺陷，容易陷入局部最优解，改进的方法是在上述情况下重新规划路径，实现动态dwa 30 | 31 | ## Gazebo仿真环境 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /Vplanner.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | class DWA(): 5 | def __init__(self): 6 | pass 7 | 8 | def plan(self, x, info, midpos, planning_obs): 9 | vw = self.vw_generate(x, info) 10 | min_score = 1000.0 11 | # 速度v,w都被限制在速度空间里 12 | all_ctral = [] 13 | all_scores = [] 14 | for v in np.arange(vw[0], vw[1], info.v_reso): 15 | for w in np.arange(vw[2], vw[3], info.yawrate_reso): 16 | # cauculate traj for each given (v,w) 17 | ctraj = self.traj_cauculate(x, [v, w], info) 18 | # 计算评价函数 19 | goal_score = info.to_goal_cost_gain * \ 20 | self.goal_evaluate(ctraj, midpos) 21 | vel_score = info.speed_cost_gain * \ 22 | self.velocity_evaluate(ctraj, info) 23 | traj_score = info.obstacle_cost_gain * \ 24 | self.traj_evaluate(ctraj, planning_obs, info) 25 | # 可行路径不止一条，通过评价函数确定最佳路径 26 | # 路径总分数 = 距离目标点 + 速度 + 障碍物 27 | # 分数越低，路径越优 28 | ctraj_score = goal_score + vel_score + traj_score 29 | # print(goal_score, vel_score, traj_score) 30 | 31 | ctraj = np.reshape(ctraj, (-1, 5)) 32 | 33 | # evaluate current traj (the score smaller,the traj better) 34 | if min_score >= ctraj_score: 35 | min_score = ctraj_score 36 | u = np.array([v, w]) 37 | best_ctral = ctraj 38 | all_ctral.append(ctraj) 39 | all_scores.append(ctraj_score) 40 | 41 | 42 | return u, best_ctral, all_ctral, all_scores 43 | 44 | # 定义机器人运动模型 45 | # 返回坐标(x,y),偏移角theta,速度v,角速度w 46 | def motion_model(self, x, u, dt): 47 | # robot motion model: x,y,theta,v,w 48 | x[0] += u[0] * dt * np.cos(x[2]) 49 | x[1] += u[0] * dt * np.sin(x[2]) 50 | x[2] += u[1] * dt 51 | x[3] = u[0] 52 | x[4] = u[1] 53 | return x 54 | 55 | # 依据当前位置及速度，预测轨迹 56 | def traj_cauculate(self, x, u, info): 57 | ctraj = np.array(x) 58 | xnew = np.array(x) 59 | time = 0 60 | 61 | while time <= info.predict_time: # preditc_time作用 62 | xnew = self.motion_model(xnew, u, info.dt) 63 | ctraj = np.vstack([ctraj, xnew]) 64 | time += info.dt 65 | 66 | # print(ctraj) 67 | return ctraj 68 | 69 | 70 | 71 | # 产生速度空间 72 | def vw_generate(self, x, info): 73 | # generate v,w window for traj prediction 74 | Vinfo = [info.min_speed, info.max_speed, 75 | info.min_yawrate, info.max_yawrate] 76 | 77 | Vmove = [x[3] - info.max_accel * info.dt, 78 | x[3] + info.max_accel * info.dt, 79 | x[4] - info.max_dyawrate * info.dt, 80 | x[4] + info.max_dyawrate * info.dt] 81 | 82 | # 保证速度变化不超过info限制的范围 83 | vw = [max(Vinfo[0], Vmove[0]), min(Vinfo[1], Vmove[1]), 84 | max(Vinfo[2], Vmove[2]), min(Vinfo[3], Vmove[3])] 85 | 86 | return vw 87 | 88 | # 距离目标点评价函数 89 | def goal_evaluate(self, traj, goal): 90 | # cauculate current pose to goal with euclidean distance 91 | goal_score = np.sqrt((traj[-1, 0]-goal[0]) ** 92 | 2 + (traj[-1, 1]-goal[1])**2) 93 | return goal_score 94 | 95 | # 速度评价函数 96 | def velocity_evaluate(self, traj, info): 97 | # cauculate current velocty score 98 | vel_score = info.max_speed - traj[-1, 3] 99 | return vel_score 100 | 101 | # 轨迹距离障碍物的评价函数 102 | def traj_evaluate(self, traj, obstacles, info): 103 | # evaluate current traj with the min distance to obstacles 104 | min_dis = float("Inf") 105 | for i in range(len(traj)): 106 | for ii in range(len(obstacles)): 107 | current_dist = np.sqrt( 108 | (traj[i, 0] - obstacles[ii, 0])**2 + (traj[i, 1] - obstacles[ii, 1])**2) 109 | 110 | if current_dist <= 0.75: 111 | return float("Inf") 112 | 113 | if min_dis >= current_dist: 114 | min_dis = current_dist 115 | 116 | return 1 / min_dis 117 | -------------------------------------------------------------------------------- /dwa.py: -------------------------------------------------------------------------------- 1 | import time 2 | import math 3 | import matplotlib 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | from AStarPlanner import AStarPlanner 7 | from Vplanner import DWA 8 | 9 | # from planner import AStarPlanner,RRTPlanner 10 | # from localplanner import dwa 11 | 12 | plt.rcParams["figure.figsize"] = [8.0, 8.0] 13 | plt.rcParams["figure.autolayout"] = True 14 | plt.rcParams['keymap.save'].remove('s') 15 | 16 | 17 | def transformation_matrix(x, y, theta): 18 | return np.array([ 19 | [np.cos(theta), -np.sin(theta), x], 20 | [np.sin(theta), np.cos(theta), y], 21 | [0, 0, 1] 22 | ]) 23 | 24 | 25 | class DWAConfig: 26 | robot_radius = 0.25 27 | 28 | def __init__(self, obs_radius): 29 | self.obs_radius = obs_radius 30 | self.dt = 0.1 # [s] Time tick for motion prediction 31 | 32 | self.max_speed = 1.5 # [m/s] 最大线速度 33 | self.min_speed = -0.5 # [m/s] 最小线速度 34 | self.max_accel = 0.5 # [m/ss] 加速度 35 | self.v_reso = self.max_accel*self.dt/10.0 # [m/s] 速度增加的步长 36 | 37 | self.min_yawrate = -100.0 * math.pi / 180.0 # [rad/s] 最小角速度 38 | self.max_yawrate = 100.0 * math.pi / 180.0 # [rad/s] 最大角速度 39 | self.max_dyawrate = 500.0 * math.pi / 180.0 # [rad/ss] 角加速度 40 | self.yawrate_reso = self.max_dyawrate*self.dt/10.0 # [rad/s] 角速度增加的步长 41 | 42 | # 模拟轨迹的持续时间 43 | self.predict_time = 2 # [s] 44 | 45 | # 三个比例系数 46 | self.to_goal_cost_gain = 1.0 # 距离目标点的评价函数的权重系数 47 | self.speed_cost_gain = 0.7 # 速度评价函数的权重系数 48 | self.obstacle_cost_gain = 1.0 # 距离障碍物距离的评价函数的权重系数 49 | 50 | self.tracking_dist = self.predict_time*self.max_speed 51 | self.arrive_dist = 0.1 52 | 53 | 54 | class Playground: 55 | planning_obs_radius = 0.5 56 | 57 | def __init__(self, planner=None, vplanner=None): 58 | self.x = 0.0 59 | self.y = 0.0 60 | self.theta = 0.0 61 | self.vx = 0.0 62 | self.vw = 0.0 63 | self.x_traj = [] 64 | self.y_traj = [] 65 | 66 | self.dwaconfig = DWAConfig(self.planning_obs_radius) 67 | self.dt = self.dwaconfig.dt 68 | 69 | self.fig, self.ax = plt.subplots() 70 | 71 | self.fig.canvas.mpl_connect('button_press_event', self.on_mousepress) 72 | self.fig.canvas.mpl_connect('key_press_event', self.on_press) 73 | self.fig.canvas.mpl_connect('motion_notify_event', self.on_mousemove) 74 | self.NEED_EXIT = False 75 | 76 | ############################################ 77 | 78 | self.planning_obs = np.empty(shape=(0, 2)) 79 | 80 | self.planning_path = np.empty(shape=(0, 2)) 81 | 82 | self.planning_target = None 83 | 84 | self.planner = planner 85 | self.vplanner = vplanner 86 | 87 | self.vplanner_midpos_index = None 88 | 89 | ##################################### 90 | self.temp_obs = [0, 0] 91 | 92 | def run(self): 93 | while True: 94 | if self.NEED_EXIT: 95 | plt.close("all") 96 | break 97 | self.vplanner_midpos_index = self.check_path() 98 | all_traj = [] 99 | all_u = [] 100 | best_traj = None 101 | if self.vplanner_midpos_index >= 0: 102 | midpos = self.planning_path[self.vplanner_midpos_index] 103 | [self.vx,self.vw], best_traj, all_traj, all_u = self.vplanner.plan([self.x,self.y,self.theta,self.vx,self.vw], 104 | self.dwaconfig,midpos,self.planning_obs) 105 | else: 106 | self.vx, self.vw = 0.0, 0.0 107 | 108 | dx, dy, dw = self.vx*self.dt, 0, self.vw*self.dt 109 | T = transformation_matrix(self.x, self.y, self.theta) 110 | p = np.matmul(T, np.array([dx, dy, 1])) 111 | self.x = p[0] 112 | self.y = p[1] 113 | self.theta += dw 114 | self.x_traj.append(self.x) 115 | self.y_traj.append(self.y) 116 | 117 | plt.cla() 118 | self.__draw(all_traj, all_u, best_traj=best_traj) 119 | 120 | def check_path(self): 121 | if self.planning_path is None or self.planning_path.shape[0] == 0: 122 | return -1 123 | if self.vplanner_midpos_index is not None and self.vplanner_midpos_index >= 0: 124 | midindex = self.vplanner_midpos_index 125 | while True: 126 | midpos = self.planning_path[midindex] 127 | dist = np.hypot(self.x-midpos[0], self.y-midpos[1]) 128 | if dist > self.dwaconfig.tracking_dist: 129 | break 130 | if midindex + 1 == self.planning_path.shape[0]: 131 | return midindex 132 | midindex += 1 133 | return midindex 134 | else: 135 | return 0 136 | 137 | def add_obs(self, x, y): 138 | self.planning_obs = np.append(self.planning_obs, [[x, y]], axis=0) 139 | 140 | def add_obss(self, xs, ys): 141 | self.planning_obs = np.append( 142 | self.planning_obs, np.vstack([xs, ys]).T, axis=0) 143 | 144 | def __draw(self, all_traj, all_value, best_traj): 145 | assert(self.planning_path is None or self.planning_path.shape[1] == 2, 146 | "the shape of planning path should be '[x,2]', please check your algorithm.") 147 | assert(self.planning_obs is None or self.planning_obs.shape[1] == 2, 148 | "the shape of self.planning_obs(obstacles) should be '[x,2]', please check your algorithm.") 149 | 150 | p1_i = np.array([0.5, 0, 1]).T 151 | p2_i = np.array([-0.5, 0.25, 1]).T 152 | p3_i = np.array([-0.5, -0.25, 1]).T 153 | 154 | T = transformation_matrix(self.x, self.y, self.theta) 155 | p1 = np.matmul(T, p1_i) 156 | p2 = np.matmul(T, p2_i) 157 | p3 = np.matmul(T, p3_i) 158 | 159 | plt.plot([p1[0], p2[0], p3[0], p1[0]], [ 160 | p1[1], p2[1], p3[1], p1[1]], 'k-') 161 | 162 | if self.planning_target is not None: 163 | self.ax.plot( 164 | self.planning_target[0], self.planning_target[1], "r*", markersize=20) 165 | 166 | if len(all_traj) > 0: 167 | all_value = np.array(all_value,dtype=float) 168 | all_value = (all_value-all_value.min())/(all_value.max()-all_value.min()) 169 | for i,traj in enumerate(all_traj): 170 | color = plt.cm.jet(all_value[i]) 171 | self.ax.plot(traj[:,0],traj[:,1],".",color=color,markersize=1) 172 | self.ax.plot(traj[-1,0],traj[-1,1],"+",color=color,markersize=3) 173 | 174 | if best_traj is not None: 175 | self.ax.plot(best_traj[:,0],best_traj[:,1],color="green",linewidth=3) 176 | 177 | if self.planning_path is not None: 178 | self.ax.plot(self.planning_path[:, 0], 179 | self.planning_path[:, 1], 'b--') 180 | if self.vplanner_midpos_index is not None and self.vplanner_midpos_index >= 0: 181 | midpos = self.planning_path[self.vplanner_midpos_index] 182 | self.ax.plot(midpos[0], midpos[1], "g+", markersize=20) 183 | if len(self.x_traj) > 0: 184 | plt.plot(self.x_traj, self.y_traj, 'g-') 185 | for obs in self.planning_obs: 186 | self.ax.add_artist(plt.Circle( 187 | (obs[0], obs[1]), self.planning_obs_radius, fill=False)) 188 | 189 | self.ax.set_xlim(-10, 10) 190 | self.ax.set_ylim(-10, 10) 191 | 192 | plt.pause(self.dt) 193 | 194 | def on_mousepress(self, event): 195 | if not event.dblclick: 196 | if event.button == 1: 197 | self.x, self.y = event.xdata, event.ydata 198 | if event.button == 3: 199 | self.planning_target = np.array([event.xdata, event.ydata]) 200 | if event.button == 2: 201 | self.add_obs(event.xdata, event.ydata) 202 | self.temp_obs = [event.xdata, event.ydata] 203 | 204 | def on_mousemove(self, event): 205 | if hasattr(event, "button") and event.button == 2: 206 | dx = event.xdata-self.temp_obs[0] 207 | dy = event.ydata-self.temp_obs[1] 208 | if np.hypot(dx, dy) > self.planning_obs_radius*0.8: 209 | self.temp_obs = [event.xdata, event.ydata] 210 | self.add_obs(*self.temp_obs) 211 | 212 | def on_press(self, event): 213 | if(event.key == 'escape'): 214 | self.set_exit() 215 | if(event.key == ' '): 216 | self.planning_path = None 217 | self.x_traj, self.y_traj = [], [] 218 | self.vplanner_midpos_index = None 219 | if self.planning_target is not None and self.planner is not None: 220 | print("do planning...") 221 | px, py = planner.planning(self.planning_obs[:, 0], self.planning_obs[:, 1], Playground.planning_obs_radius + 222 | DWAConfig.robot_radius, self.x, self.y, self.planning_target[0], self.planning_target[1], -10, -10, 10, 10) 223 | self.planning_path = np.vstack([px, py]).T 224 | print("pathLength : ", self.planning_path.shape[0]) 225 | 226 | def set_exit(self): 227 | self.NEED_EXIT = True 228 | 229 | 230 | if __name__ == "__main__": 231 | planner = None 232 | planner = AStarPlanner(0.2) 233 | vplanner = DWA() 234 | # planner = RRTPlanner(0.2,1.5) 235 | pg = Playground(planner, vplanner) 236 | pg.run() 237 | -------------------------------------------------------------------------------- /gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/noetic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /gazebo/course_agv_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(course_agv_control) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a exec_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if your package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES course_agv_control 104 | # CATKIN_DEPENDS other_catkin_pkg 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | include_directories( 115 | # include 116 | # ${catkin_INCLUDE_DIRS} 117 | ) 118 | 119 | ## Declare a C++ library 120 | # add_library(${PROJECT_NAME} 121 | # src/${PROJECT_NAME}/course_agv_control.cpp 122 | # ) 123 | 124 | ## Add cmake target dependencies of the library 125 | ## as an example, code may need to be generated before libraries 126 | ## either from message generation or dynamic reconfigure 127 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Declare a C++ executable 130 | ## With catkin_make all packages are built within a single CMake context 131 | ## The recommended prefix ensures that target names across packages don't collide 132 | # add_executable(${PROJECT_NAME}_node src/course_agv_control_node.cpp) 133 | 134 | ## Rename C++ executable without prefix 135 | ## The above recommended prefix causes long target names, the following renames the 136 | ## target back to the shorter version for ease of user use 137 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 138 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 139 | 140 | ## Add cmake target dependencies of the executable 141 | ## same as for the library above 142 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 143 | 144 | ## Specify libraries to link a library or executable target against 145 | # target_link_libraries(${PROJECT_NAME}_node 146 | # ${catkin_LIBRARIES} 147 | # ) 148 | 149 | ############# 150 | ## Install ## 151 | ############# 152 | 153 | # all install targets should use catkin DESTINATION variables 154 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 155 | 156 | ## Mark executable scripts (Python etc.) for installation 157 | ## in contrast to setup.py, you can choose the destination 158 | # install(PROGRAMS 159 | # scripts/my_python_script 160 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark executables for installation 164 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 165 | # install(TARGETS ${PROJECT_NAME}_node 166 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | # ) 168 | 169 | ## Mark libraries for installation 170 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 171 | # install(TARGETS ${PROJECT_NAME} 172 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 175 | # ) 176 | 177 | ## Mark cpp header files for installation 178 | # install(DIRECTORY include/${PROJECT_NAME}/ 179 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 180 | # FILES_MATCHING PATTERN "*.h" 181 | # PATTERN ".svn" EXCLUDE 182 | # ) 183 | 184 | ## Mark other files for installation (e.g. launch and bag files, etc.) 185 | # install(FILES 186 | # # myfile1 187 | # # myfile2 188 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 189 | # ) 190 | 191 | ############# 192 | ## Testing ## 193 | ############# 194 | 195 | ## Add gtest based cpp test target and link libraries 196 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_course_agv_control.cpp) 197 | # if(TARGET ${PROJECT_NAME}-test) 198 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 199 | # endif() 200 | 201 | ## Add folders to be run by python nosetests 202 | # catkin_add_nosetests(test) 203 | -------------------------------------------------------------------------------- /gazebo/course_agv_control/config/course_agv_control.yaml: -------------------------------------------------------------------------------- 1 | course_agv: 2 | # Publish all joint states ----------------------------------- 3 | joint_state_controller: 4 | type: joint_state_controller/JointStateController 5 | publish_rate: 50 6 | 7 | # Velocity Controllers --------------------------------------- 8 | left_wheel_velocity_controller: 9 | type: velocity_controllers/JointVelocityController 10 | joint: course_agv__left_wheel_joint 11 | # pid: {p: 100.0, i: 0.01, d: 10.0} 12 | right_wheel_velocity_controller: 13 | type: velocity_controllers/JointVelocityController 14 | joint: course_agv__right_wheel_joint 15 | # pid: {p: 100.0, i: 0.01, d: 10.0} 16 | gazebo_ros_control/pid_gains: 17 | course_agv__left_wheel_joint: {p: 5.0, i: 0.1, d: 0.0} 18 | course_agv__right_wheel_joint: {p: 5.0, i: 0.1, d: 0.0} 19 | -------------------------------------------------------------------------------- /gazebo/course_agv_control/launch/course_agv_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 11 | 12 | 13 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /gazebo/course_agv_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | course_agv_control 4 | 0.0.0 5 | The course_agv_control package 6 | 7 | 8 | 9 | 10 | zjunlict 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /gazebo/course_agv_control/scripts/keyboard_velocity.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from pynput.keyboard import Key, Listener 5 | import geometry_msgs.msg 6 | 7 | command = { 8 | "vx":0.0, 9 | "vw":0.0, 10 | "step":0.1, 11 | "vmax":3.0 12 | } 13 | def limitNum(num,minNum,maxNum): 14 | if num > maxNum: 15 | return maxNum 16 | if num < minNum: 17 | return minNum 18 | return num 19 | def cmd_num_func(cmd,key,value,minValue,maxValue): 20 | def function(): 21 | cmd[key] = limitNum(cmd[key]+value,minValue,maxValue) 22 | return function 23 | def cmd_reset_func(cmd): 24 | def function(): 25 | cmd["vx"] = 0 26 | cmd["vw"] = 0 27 | return function 28 | KEY_MAP_TABLE={ 29 | 'w' : cmd_num_func(command,"vx", command["step"],-command["vmax"],command["vmax"]), 30 | 's' : cmd_num_func(command,"vx",-command["step"],-command["vmax"],command["vmax"]), 31 | 'a' : cmd_num_func(command,"vw", command["step"],-command["vmax"],command["vmax"]), 32 | 'd' : cmd_num_func(command,"vw",-command["step"],-command["vmax"],command["vmax"]), 33 | Key.space : cmd_reset_func(command) 34 | } 35 | 36 | class Publisher: 37 | def __init__(self): 38 | self.vel_pub = rospy.Publisher( 39 | '/course_agv/velocity', 40 | geometry_msgs.msg.Twist, queue_size=1) 41 | self.cmd = geometry_msgs.msg.Twist() 42 | 43 | def pub(self,command): 44 | if rospy.is_shutdown(): 45 | exit() 46 | print("publish command : vx - %.2f vw - %.2f"%(command['vx'],command['vw'])) 47 | self.cmd.linear.x = command["vx"] 48 | self.cmd.angular.z = command["vw"] 49 | self.vel_pub.publish(self.cmd) 50 | 51 | def press_function(map_table,publisher): 52 | def on_press(key): 53 | global NEED_EXIT 54 | if key == Key.esc: 55 | NEED_EXIT = True 56 | return False 57 | try: 58 | convert_key = key.char 59 | except AttributeError: 60 | convert_key = key 61 | # print('{0} pressed'.format(convert_key)) 62 | if convert_key in map_table: 63 | map_table[convert_key]() 64 | publisher.pub(command) 65 | else: 66 | print('\n{0} not in table'.format(convert_key)) 67 | return on_press 68 | 69 | def main(): 70 | node_name = "velocity_publisher" 71 | print("node : ",node_name) 72 | rospy.init_node(node_name,anonymous=True) 73 | publisher = Publisher() 74 | with Listener(on_press=press_function(KEY_MAP_TABLE,publisher)) as listener: 75 | listener.join() 76 | 77 | if __name__ == '__main__': 78 | main() 79 | -------------------------------------------------------------------------------- /gazebo/course_agv_control/scripts/kinematics.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import geometry_msgs.msg 5 | from std_msgs.msg import Float64 6 | 7 | class Kinematics: 8 | left_wheel_vel = 0.0 9 | right_wheel_vel = 0.0 10 | def __init__(self): 11 | # call rospy.Subscriber() and rospy.Publisher() to define receiver and publisher 12 | # type: geometry_msgs.msg.Twist, Float64, Float64 13 | self.publ = rospy.Publisher('/course_agv/left_wheel_velocity_controller/command', Float64, queue_size=10) 14 | self.pubr = rospy.Publisher('/course_agv/right_wheel_velocity_controller/command', Float64, queue_size=10) 15 | # self.subs = rospy.Subscriber('course_agv/velocity', geometry_msgs.msg.Twist, self.callback) 16 | # input: 17 | # data: 18 | # (data.linear.x, data.angular.z) 19 | # call self.kinematics(vx, vw) to compute wheel velocities 20 | def callback(self, data): 21 | # Compute self.left_wheel_vel and self.right_wheel_vel according to input 22 | self.kinematics(data.linear.x,data.angular.z) 23 | self.publish() 24 | 25 | # input: vx and vw of the robot 26 | # output: wheel velocities of two wheels 27 | def kinematics(self,vx,vw): 28 | # too simple method , TODO 29 | print(vx,vw) 30 | self.left_wheel_vel = (vx - (0.08/3+0.1)*vw)/0.08 31 | self.right_wheel_vel = (vx + (0.08/3+0.1)*vw)/0.08 32 | 33 | # Call publisher to publish wheel velocities 34 | def publish(self): 35 | self.publ.publish(self.left_wheel_vel) 36 | self.pubr.publish(self.right_wheel_vel) 37 | # print(self.left_wheel_vel,self.right_wheel_vel) 38 | 39 | def main(): 40 | node_name = "course_agv_kinematics" 41 | print("node : ",node_name) 42 | 43 | try: 44 | rospy.init_node(node_name) 45 | k = Kinematics() 46 | rate = rospy.Rate(rospy.get_param('~publish_rate',200)) 47 | while not rospy.is_shutdown(): 48 | k.publish() 49 | rospy.Subscriber('/course_agv/velocity', geometry_msgs.msg.Twist, k.callback) 50 | rospy.spin() 51 | rate.sleep() 52 | except rospy.ROSInterruptException: 53 | pass 54 | 55 | if __name__ == '__main__': 56 | main() 57 | -------------------------------------------------------------------------------- /gazebo/course_agv_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(course_agv_description) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a exec_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if your package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES course_agv_description 104 | # CATKIN_DEPENDS other_catkin_pkg 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | include_directories( 115 | # include 116 | # ${catkin_INCLUDE_DIRS} 117 | ) 118 | 119 | ## Declare a C++ library 120 | # add_library(${PROJECT_NAME} 121 | # src/${PROJECT_NAME}/course_agv_description.cpp 122 | # ) 123 | 124 | ## Add cmake target dependencies of the library 125 | ## as an example, code may need to be generated before libraries 126 | ## either from message generation or dynamic reconfigure 127 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Declare a C++ executable 130 | ## With catkin_make all packages are built within a single CMake context 131 | ## The recommended prefix ensures that target names across packages don't collide 132 | # add_executable(${PROJECT_NAME}_node src/course_agv_description_node.cpp) 133 | 134 | ## Rename C++ executable without prefix 135 | ## The above recommended prefix causes long target names, the following renames the 136 | ## target back to the shorter version for ease of user use 137 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 138 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 139 | 140 | ## Add cmake target dependencies of the executable 141 | ## same as for the library above 142 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 143 | 144 | ## Specify libraries to link a library or executable target against 145 | # target_link_libraries(${PROJECT_NAME}_node 146 | # ${catkin_LIBRARIES} 147 | # ) 148 | 149 | ############# 150 | ## Install ## 151 | ############# 152 | 153 | # all install targets should use catkin DESTINATION variables 154 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 155 | 156 | ## Mark executable scripts (Python etc.) for installation 157 | ## in contrast to setup.py, you can choose the destination 158 | # install(PROGRAMS 159 | # scripts/my_python_script 160 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark executables for installation 164 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 165 | # install(TARGETS ${PROJECT_NAME}_node 166 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | # ) 168 | 169 | ## Mark libraries for installation 170 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 171 | # install(TARGETS ${PROJECT_NAME} 172 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 175 | # ) 176 | 177 | ## Mark cpp header files for installation 178 | # install(DIRECTORY include/${PROJECT_NAME}/ 179 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 180 | # FILES_MATCHING PATTERN "*.h" 181 | # PATTERN ".svn" EXCLUDE 182 | # ) 183 | 184 | ## Mark other files for installation (e.g. launch and bag files, etc.) 185 | # install(FILES 186 | # # myfile1 187 | # # myfile2 188 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 189 | # ) 190 | 191 | ############# 192 | ## Testing ## 193 | ############# 194 | 195 | ## Add gtest based cpp test target and link libraries 196 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_course_agv_description.cpp) 197 | # if(TARGET ${PROJECT_NAME}-test) 198 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 199 | # endif() 200 | 201 | ## Add folders to be run by python nosetests 202 | # catkin_add_nosetests(test) 203 | -------------------------------------------------------------------------------- /gazebo/course_agv_description/launch/course_agv.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /RobotModel1 10 | Splitter Ratio: 0.5 11 | Tree Height: 549 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: "" 31 | Preferences: 32 | PromptSaveOnExit: true 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 1 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: true 43 | Line Style: 44 | Line Width: 0.029999999329447746 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 10 54 | Reference Frame: 55 | Value: true 56 | - Alpha: 1 57 | Class: rviz/RobotModel 58 | Collision Enabled: false 59 | Enabled: true 60 | Links: 61 | All Links Enabled: true 62 | Expand Joint Details: false 63 | Expand Link Details: false 64 | Expand Tree: false 65 | Link Tree Style: Links in Alphabetic Order 66 | course_agv__left_wheel: 67 | Alpha: 1 68 | Show Axes: false 69 | Show Trail: false 70 | Value: true 71 | course_agv__right_wheel: 72 | Alpha: 1 73 | Show Axes: false 74 | Show Trail: false 75 | Value: true 76 | robot_base: 77 | Alpha: 1 78 | Show Axes: false 79 | Show Trail: false 80 | robot_chassis: 81 | Alpha: 1 82 | Show Axes: false 83 | Show Trail: false 84 | Value: true 85 | Name: RobotModel 86 | Robot Description: robot_description 87 | TF Prefix: "" 88 | Update Interval: 0 89 | Value: true 90 | Visual Enabled: true 91 | - Class: rviz/TF 92 | Enabled: true 93 | Frame Timeout: 15 94 | Frames: 95 | All Enabled: true 96 | course_agv__left_wheel: 97 | Value: true 98 | course_agv__right_wheel: 99 | Value: true 100 | robot_base: 101 | Value: true 102 | robot_chassis: 103 | Value: true 104 | Marker Scale: 1 105 | Name: TF 106 | Show Arrows: true 107 | Show Axes: true 108 | Show Names: true 109 | Tree: 110 | robot_base: 111 | course_agv__left_wheel: 112 | {} 113 | course_agv__right_wheel: 114 | {} 115 | robot_chassis: 116 | {} 117 | Update Interval: 0 118 | Value: true 119 | Enabled: true 120 | Global Options: 121 | Background Color: 48; 48; 48 122 | Default Light: true 123 | Fixed Frame: robot_base 124 | Frame Rate: 30 125 | Name: root 126 | Tools: 127 | - Class: rviz/Interact 128 | Hide Inactive Objects: true 129 | - Class: rviz/MoveCamera 130 | - Class: rviz/Select 131 | - Class: rviz/FocusCamera 132 | - Class: rviz/Measure 133 | - Class: rviz/SetInitialPose 134 | Theta std deviation: 0.2617993950843811 135 | Topic: /initialpose 136 | X std deviation: 0.5 137 | Y std deviation: 0.5 138 | - Class: rviz/SetGoal 139 | Topic: /move_base_simple/goal 140 | - Class: rviz/PublishPoint 141 | Single click: true 142 | Topic: /clicked_point 143 | Value: true 144 | Views: 145 | Current: 146 | Class: rviz/Orbit 147 | Distance: 2.859943151473999 148 | Enable Stereo Rendering: 149 | Stereo Eye Separation: 0.05999999865889549 150 | Stereo Focal Distance: 1 151 | Swap Stereo Eyes: false 152 | Value: false 153 | Focal Point: 154 | X: 0 155 | Y: 0 156 | Z: 0 157 | Focal Shape Fixed Size: true 158 | Focal Shape Size: 0.05000000074505806 159 | Invert Z Axis: false 160 | Name: Current View 161 | Near Clip Distance: 0.009999999776482582 162 | Pitch: 0.785398006439209 163 | Target Frame: 164 | Value: Orbit (rviz) 165 | Yaw: 0.785398006439209 166 | Saved: ~ 167 | Window Geometry: 168 | Displays: 169 | collapsed: false 170 | Height: 846 171 | Hide Left Dock: false 172 | Hide Right Dock: false 173 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 174 | Selection: 175 | collapsed: false 176 | Time: 177 | collapsed: false 178 | Tool Properties: 179 | collapsed: false 180 | Views: 181 | collapsed: false 182 | Width: 1200 183 | X: 60 184 | Y: 27 185 | -------------------------------------------------------------------------------- /gazebo/course_agv_description/launch/course_agv_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /gazebo/course_agv_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | course_agv_description 4 | 0.0.0 5 | The course_agv_description package 6 | 7 | 8 | 9 | 10 | zjunlict 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /gazebo/course_agv_description/urdf/course_agv.gazebo: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | /course_agv 8 | gazebo_ros_control/DefaultRobotHWSim 9 | 10 | 11 | 12 | 24 | 25 | 26 | 0.0 27 | 0.0 28 | Gazebo/DarkYellow 29 | 30 | 31 | 32 | 33 | 0 34 | 0 35 | 1.0 36 | 1.0 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 1 49 | 1 50 | 0 51 | 0 52 | 53 | 54 | 55 | 56 | Gazebo/DarkGrey 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 1 65 | 1 66 | 0 67 | 0 68 | 69 | 70 | 71 | 72 | Gazebo/DarkGrey 73 | 74 | 75 | 76 | 77 | 78 | 0 0 0 0 0 0 79 | true 80 | 5 81 | 82 | 83 | 84 | 200 85 | 1 86 | -3.14159 87 | 3.14159 88 | 89 | 90 | 91 | 0.10 92 | 20.0 93 | 0.05 94 | 95 | 96 | gaussian 97 | 101 | 0.0 102 | 0.01 103 | 104 | 105 | 106 | /course_agv/laser/scan 107 | course_agv__hokuyo__link 108 | 109 | 110 | 111 | 112 | 113 | 114 | true 115 | Gazebo/Red 116 | 117 | true 118 | 100 119 | true 120 | __default_topic__ 121 | 122 | /course_agv/imu 123 | imu_link 124 | 10.0 125 | 0.0 126 | 0 0 0 127 | 0 0 0 128 | course_agv__imu 129 | 130 | 0 0 0 0 0 0 131 | 132 | 133 | 134 | -------------------------------------------------------------------------------- /gazebo/course_agv_description/urdf/course_agv.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | transmission_interface/SimpleTransmission 200 | 201 | hardware_interface/VelocityJointInterface 202 | 203 | 204 | hardware_interface/VelocityJointInterface 205 | 1 206 | 207 | 208 | 209 | 210 | transmission_interface/SimpleTransmission 211 | 212 | hardware_interface/VelocityJointInterface 213 | 214 | 215 | hardware_interface/VelocityJointInterface 216 | 1 217 | 218 | 219 | 220 | -------------------------------------------------------------------------------- /gazebo/course_agv_description/urdf/materials.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /gazebo/course_agv_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(course_agv_gazebo) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a exec_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if your package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES course_agv_gazebo 104 | # CATKIN_DEPENDS other_catkin_pkg 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | include_directories( 115 | # include 116 | # ${catkin_INCLUDE_DIRS} 117 | ) 118 | 119 | ## Declare a C++ library 120 | # add_library(${PROJECT_NAME} 121 | # src/${PROJECT_NAME}/course_agv_gazebo.cpp 122 | # ) 123 | 124 | ## Add cmake target dependencies of the library 125 | ## as an example, code may need to be generated before libraries 126 | ## either from message generation or dynamic reconfigure 127 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Declare a C++ executable 130 | ## With catkin_make all packages are built within a single CMake context 131 | ## The recommended prefix ensures that target names across packages don't collide 132 | # add_executable(${PROJECT_NAME}_node src/course_agv_gazebo_node.cpp) 133 | 134 | ## Rename C++ executable without prefix 135 | ## The above recommended prefix causes long target names, the following renames the 136 | ## target back to the shorter version for ease of user use 137 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 138 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 139 | 140 | ## Add cmake target dependencies of the executable 141 | ## same as for the library above 142 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 143 | 144 | ## Specify libraries to link a library or executable target against 145 | # target_link_libraries(${PROJECT_NAME}_node 146 | # ${catkin_LIBRARIES} 147 | # ) 148 | 149 | ############# 150 | ## Install ## 151 | ############# 152 | 153 | # all install targets should use catkin DESTINATION variables 154 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 155 | 156 | ## Mark executable scripts (Python etc.) for installation 157 | ## in contrast to setup.py, you can choose the destination 158 | # install(PROGRAMS 159 | # scripts/my_python_script 160 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark executables for installation 164 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 165 | # install(TARGETS ${PROJECT_NAME}_node 166 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | # ) 168 | 169 | ## Mark libraries for installation 170 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 171 | # install(TARGETS ${PROJECT_NAME} 172 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 175 | # ) 176 | 177 | ## Mark cpp header files for installation 178 | # install(DIRECTORY include/${PROJECT_NAME}/ 179 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 180 | # FILES_MATCHING PATTERN "*.h" 181 | # PATTERN ".svn" EXCLUDE 182 | # ) 183 | 184 | ## Mark other files for installation (e.g. launch and bag files, etc.) 185 | # install(FILES 186 | # # myfile1 187 | # # myfile2 188 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 189 | # ) 190 | 191 | ############# 192 | ## Testing ## 193 | ############# 194 | 195 | ## Add gtest based cpp test target and link libraries 196 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_course_agv_gazebo.cpp) 197 | # if(TARGET ${PROJECT_NAME}-test) 198 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 199 | # endif() 200 | 201 | ## Add folders to be run by python nosetests 202 | # catkin_add_nosetests(test) 203 | -------------------------------------------------------------------------------- /gazebo/course_agv_gazebo/config/map.yaml: -------------------------------------------------------------------------------- 1 | image: map/map.png 2 | resolution: 0.155 3 | origin: [-10.0, -10.0, 0.0] 4 | occupied_thresh: 0.65 5 | free_thresh: 0.196 6 | negate: 1 7 | -------------------------------------------------------------------------------- /gazebo/course_agv_gazebo/config/map/map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Friedrich-M/Wheeled-robot-path-planning-algorithm/566230578d03db637da2251d6d82ca3f3d36ec68/gazebo/course_agv_gazebo/config/map/map.png -------------------------------------------------------------------------------- /gazebo/course_agv_gazebo/config/map/map_backup2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Friedrich-M/Wheeled-robot-path-planning-algorithm/566230578d03db637da2251d6d82ca3f3d36ec68/gazebo/course_agv_gazebo/config/map/map_backup2.png -------------------------------------------------------------------------------- /gazebo/course_agv_gazebo/launch/course_agv.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | - /RobotModel1 11 | - /TF1 12 | - /Map1 13 | - /LaserScan1 14 | - /Imu1 15 | Splitter Ratio: 0.5 16 | Tree Height: 695 17 | - Class: rviz/Selection 18 | Name: Selection 19 | - Class: rviz/Tool Properties 20 | Expanded: 21 | - /2D Pose Estimate1 22 | - /2D Nav Goal1 23 | - /Publish Point1 24 | Name: Tool Properties 25 | Splitter Ratio: 0.5886790156364441 26 | - Class: rviz/Views 27 | Expanded: 28 | - /Current View1 29 | Name: Views 30 | Splitter Ratio: 0.5 31 | - Class: rviz/Time 32 | Experimental: false 33 | Name: Time 34 | SyncMode: 0 35 | SyncSource: LaserScan 36 | Preferences: 37 | PromptSaveOnExit: true 38 | Toolbars: 39 | toolButtonStyle: 2 40 | Visualization Manager: 41 | Class: "" 42 | Displays: 43 | - Alpha: 0.5 44 | Cell Size: 1 45 | Class: rviz/Grid 46 | Color: 160; 160; 164 47 | Enabled: true 48 | Line Style: 49 | Line Width: 0.029999999329447746 50 | Value: Lines 51 | Name: Grid 52 | Normal Cell Count: 0 53 | Offset: 54 | X: 0 55 | Y: 0 56 | Z: 0 57 | Plane: XY 58 | Plane Cell Count: 20 59 | Reference Frame: 60 | Value: true 61 | - Alpha: 1 62 | Class: rviz/RobotModel 63 | Collision Enabled: false 64 | Enabled: true 65 | Links: 66 | All Links Enabled: true 67 | Expand Joint Details: false 68 | Expand Link Details: false 69 | Expand Tree: false 70 | Link Tree Style: Links in Alphabetic Order 71 | course_agv__hokuyo__link: 72 | Alpha: 1 73 | Show Axes: false 74 | Show Trail: false 75 | Value: true 76 | course_agv__imu: 77 | Alpha: 1 78 | Show Axes: false 79 | Show Trail: false 80 | Value: true 81 | course_agv__left_wheel: 82 | Alpha: 1 83 | Show Axes: false 84 | Show Trail: false 85 | Value: true 86 | course_agv__right_wheel: 87 | Alpha: 1 88 | Show Axes: false 89 | Show Trail: false 90 | Value: true 91 | robot_base: 92 | Alpha: 1 93 | Show Axes: false 94 | Show Trail: false 95 | robot_chassis: 96 | Alpha: 1 97 | Show Axes: false 98 | Show Trail: false 99 | Value: true 100 | Name: RobotModel 101 | Robot Description: robot_description 102 | TF Prefix: "" 103 | Update Interval: 0 104 | Value: true 105 | Visual Enabled: true 106 | - Class: rviz/TF 107 | Enabled: true 108 | Frame Timeout: 15 109 | Frames: 110 | All Enabled: true 111 | course_agv__hokuyo__link: 112 | Value: true 113 | course_agv__imu: 114 | Value: true 115 | course_agv__left_wheel: 116 | Value: true 117 | course_agv__right_wheel: 118 | Value: true 119 | map: 120 | Value: true 121 | robot_base: 122 | Value: true 123 | robot_chassis: 124 | Value: true 125 | world_base: 126 | Value: true 127 | Marker Scale: 0.30000001192092896 128 | Name: TF 129 | Show Arrows: true 130 | Show Axes: true 131 | Show Names: true 132 | Tree: 133 | world_base: 134 | map: 135 | {} 136 | robot_base: 137 | course_agv__hokuyo__link: 138 | {} 139 | course_agv__imu: 140 | {} 141 | course_agv__left_wheel: 142 | {} 143 | course_agv__right_wheel: 144 | {} 145 | robot_chassis: 146 | {} 147 | Update Interval: 0 148 | Value: true 149 | - Alpha: 0.5 150 | Class: rviz/Map 151 | Color Scheme: map 152 | Draw Behind: false 153 | Enabled: true 154 | Name: Map 155 | Topic: /map 156 | Unreliable: false 157 | Use Timestamp: false 158 | Value: true 159 | - Alpha: 0.6000000238418579 160 | Autocompute Intensity Bounds: true 161 | Autocompute Value Bounds: 162 | Max Value: 10 163 | Min Value: -10 164 | Value: true 165 | Axis: Z 166 | Channel Name: intensity 167 | Class: rviz/LaserScan 168 | Color: 255; 255; 255 169 | Color Transformer: Intensity 170 | Decay Time: 0 171 | Enabled: true 172 | Invert Rainbow: false 173 | Max Color: 255; 255; 255 174 | Max Intensity: 0 175 | Min Color: 0; 0; 0 176 | Min Intensity: 0 177 | Name: LaserScan 178 | Position Transformer: XYZ 179 | Queue Size: 10 180 | Selectable: true 181 | Size (Pixels): 3 182 | Size (m): 0.10000000149011612 183 | Style: Flat Squares 184 | Topic: /course_agv/laser/scan 185 | Unreliable: false 186 | Use Fixed Frame: true 187 | Use rainbow: true 188 | Value: true 189 | - Alpha: 0.20000000298023224 190 | Class: rviz_plugin_tutorials/Imu 191 | Color: 32; 74; 135 192 | Enabled: true 193 | History Length: 1 194 | Name: Imu 195 | Topic: /course_agv/imu 196 | Unreliable: false 197 | Value: true 198 | Enabled: true 199 | Global Options: 200 | Background Color: 85; 87; 83 201 | Default Light: true 202 | Fixed Frame: world_base 203 | Frame Rate: 30 204 | Name: root 205 | Tools: 206 | - Class: rviz/Interact 207 | Hide Inactive Objects: true 208 | - Class: rviz/MoveCamera 209 | - Class: rviz/Select 210 | - Class: rviz/FocusCamera 211 | - Class: rviz/Measure 212 | - Class: rviz/SetInitialPose 213 | Theta std deviation: 0.2617993950843811 214 | Topic: /initialpose 215 | X std deviation: 0.5 216 | Y std deviation: 0.5 217 | - Class: rviz/SetGoal 218 | Topic: /move_base_simple/goal 219 | - Class: rviz/PublishPoint 220 | Single click: true 221 | Topic: /clicked_point 222 | Value: true 223 | Views: 224 | Current: 225 | Class: rviz/Orbit 226 | Distance: 7.358518600463867 227 | Enable Stereo Rendering: 228 | Stereo Eye Separation: 0.05999999865889549 229 | Stereo Focal Distance: 1 230 | Swap Stereo Eyes: false 231 | Value: false 232 | Focal Point: 233 | X: 0 234 | Y: 0 235 | Z: 0 236 | Focal Shape Fixed Size: true 237 | Focal Shape Size: 0.05000000074505806 238 | Invert Z Axis: false 239 | Name: Current View 240 | Near Clip Distance: 0.009999999776482582 241 | Pitch: 0.6803986430168152 242 | Target Frame: 243 | Value: Orbit (rviz) 244 | Yaw: 5.888577461242676 245 | Saved: ~ 246 | Window Geometry: 247 | Displays: 248 | collapsed: false 249 | Height: 992 250 | Hide Left Dock: false 251 | Hide Right Dock: false 252 | QMainWindow State: 000000ff00000000fd00000004000000000000015600000342fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000342000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000342fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000342000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000050f0000034200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 253 | Selection: 254 | collapsed: false 255 | Time: 256 | collapsed: false 257 | Tool Properties: 258 | collapsed: false 259 | Views: 260 | collapsed: false 261 | Width: 1920 262 | X: 0 263 | Y: 27 264 | -------------------------------------------------------------------------------- /gazebo/course_agv_gazebo/launch/course_agv_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /gazebo/course_agv_gazebo/launch/course_agv_world_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /gazebo/course_agv_gazebo/models/ground_plane_for_agv/map/map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Friedrich-M/Wheeled-robot-path-planning-algorithm/566230578d03db637da2251d6d82ca3f3d36ec68/gazebo/course_agv_gazebo/models/ground_plane_for_agv/map/map.png -------------------------------------------------------------------------------- /gazebo/course_agv_gazebo/models/ground_plane_for_agv/map/map_backup2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Friedrich-M/Wheeled-robot-path-planning-algorithm/566230578d03db637da2251d6d82ca3f3d36ec68/gazebo/course_agv_gazebo/models/ground_plane_for_agv/map/map_backup2.png -------------------------------------------------------------------------------- /gazebo/course_agv_gazebo/models/ground_plane_for_agv/materials/textures/flat_normal.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Friedrich-M/Wheeled-robot-path-planning-algorithm/566230578d03db637da2251d6d82ca3f3d36ec68/gazebo/course_agv_gazebo/models/ground_plane_for_agv/materials/textures/flat_normal.png -------------------------------------------------------------------------------- /gazebo/course_agv_gazebo/models/ground_plane_for_agv/materials/textures/grey.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Friedrich-M/Wheeled-robot-path-planning-algorithm/566230578d03db637da2251d6d82ca3f3d36ec68/gazebo/course_agv_gazebo/models/ground_plane_for_agv/materials/textures/grey.png -------------------------------------------------------------------------------- /gazebo/course_agv_gazebo/models/ground_plane_for_agv/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Ground Plane AGV 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Mark 10 | hzypp@sina.cn 11 | 12 | 13 | 14 | A simple test ground for AGV. 15 | 16 | 17 | -------------------------------------------------------------------------------- /gazebo/course_agv_gazebo/models/ground_plane_for_agv/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | model://ground_plane_for_agv/map/map.png 10 | 20 20 0.4 11 | 0 0 0.01 12 | 13 | 14 | 15 | 16 | 17 | 18 | false 19 | 24 | 29 | 30 | model://ground_plane_for_agv/materials/textures/grey.png 31 | model://ground_plane_for_agv/materials/textures/flat_normal.png 32 | 1 33 | 34 | 35 | 2 36 | 5 37 | 38 | 39 | 4 40 | 5 41 | 42 | model://ground_plane_for_agv/map/map.png 43 | 20 20 0.8 44 | 0 0 0.01 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /gazebo/course_agv_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | course_agv_gazebo 4 | 0.0.0 5 | The course_agv_gazebo package 6 | 7 | 8 | 9 | 10 | zjunlict 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /gazebo/course_agv_gazebo/scripts/robot_tf.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import tf 5 | from gazebo_msgs.msg import LinkStates 6 | from geometry_msgs.msg import Pose 7 | from geometry_msgs.msg import Quaternion 8 | 9 | class GazeboLinkPose: 10 | link_name = '' 11 | link_pose = Pose() 12 | 13 | def __init__(self, robot_name, link_name): 14 | self.robot_name = robot_name 15 | self.link_name = link_name 16 | # self.link_name_rectified = link_name.replace("::", "_") 17 | 18 | if not self.link_name: 19 | raise ValueError("'link_name' is an empty string") 20 | 21 | self.states_sub = rospy.Subscriber( 22 | "/gazebo/link_states", LinkStates, self.callback) 23 | self.pose_pub = rospy.Publisher( 24 | "/gazebo/" + (self.robot_name + '__' + self.link_name), Pose, queue_size=10) 25 | self.tf_pub = tf.TransformBroadcaster() 26 | 27 | def callback(self, data): 28 | try: 29 | ind = data.name.index(self.robot_name + "::" + self.link_name) 30 | self.link_pose = data.pose[ind] 31 | except ValueError: 32 | pass 33 | 34 | def publish_tf(self): 35 | p = self.link_pose.position 36 | o = self.link_pose.orientation 37 | self.tf_pub.sendTransform((p.x,p.y,p.z),(o.x,o.y,o.z,o.w), 38 | rospy.Time.now(),self.link_name,"map") 39 | self.tf_pub.sendTransform((0,0,0),tf.transformations.quaternion_from_euler(0,0,0), 40 | rospy.Time.now(),"world_base","map") 41 | 42 | if __name__ == '__main__': 43 | try: 44 | rospy.init_node('robot_pose_tf_publisher') 45 | gp = GazeboLinkPose(rospy.get_param('~robot_name', 'course_agv'), 46 | rospy.get_param('~link_name', 'robot_base')) 47 | publish_rate = rospy.get_param('~publish_rate', 100) 48 | 49 | rate = rospy.Rate(publish_rate) 50 | while not rospy.is_shutdown(): 51 | gp.pose_pub.publish(gp.link_pose) 52 | gp.publish_tf() 53 | rate.sleep() 54 | 55 | except rospy.ROSInterruptException: 56 | pass 57 | -------------------------------------------------------------------------------- /gazebo/course_agv_gazebo/worlds/course_agv.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://ground_plane_for_agv 7 | 8 | 9 | 10 | 11 | model://sun 12 | 13 | 14 | 15 | 16 | 17 | 4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190 18 | orbit 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /gazebo/course_agv_nav/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(course_agv_nav) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | rospy 12 | std_msgs 13 | message_generation 14 | ) 15 | ## System dependencies are found with CMake's conventions 16 | # find_package(Boost REQUIRED COMPONENTS system) 17 | 18 | 19 | ## Uncomment this if the package has a setup.py. This macro ensures 20 | ## modules and global scripts declared therein get installed 21 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 22 | # catkin_python_setup() 23 | 24 | ################################################ 25 | ## Declare ROS messages, services and actions ## 26 | ################################################ 27 | 28 | ## To declare and build messages, services or actions from within this 29 | ## package, follow these steps: 30 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 31 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 32 | ## * In the file package.xml: 33 | ## * add a build_depend tag for "message_generation" 34 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 35 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 36 | ## but can be declared for certainty nonetheless: 37 | ## * add a exec_depend tag for "message_runtime" 38 | ## * In this file (CMakeLists.txt): 39 | ## * add "message_generation" and every package in MSG_DEP_SET to 40 | ## find_package(catkin REQUIRED COMPONENTS ...) 41 | ## * add "message_runtime" and every package in MSG_DEP_SET to 42 | ## catkin_package(CATKIN_DEPENDS ...) 43 | ## * uncomment the add_*_files sections below as needed 44 | ## and list every .msg/.srv/.action file to be processed 45 | ## * uncomment the generate_messages entry below 46 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 47 | 48 | ## Generate messages in the 'msg' folder 49 | # add_message_files( 50 | # FILES 51 | # Message1.msg 52 | # Message2.msg 53 | # ) 54 | 55 | ## Generate services in the 'srv' folder 56 | add_service_files( 57 | FILES 58 | Plan.srv 59 | # Service1.srv 60 | # Service2.srv 61 | ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | generate_messages( 72 | DEPENDENCIES 73 | std_msgs # Or other packages containing msgs 74 | ) 75 | 76 | ################################################ 77 | ## Declare ROS dynamic reconfigure parameters ## 78 | ################################################ 79 | 80 | ## To declare and build dynamic reconfigure parameters within this 81 | ## package, follow these steps: 82 | ## * In the file package.xml: 83 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 84 | ## * In this file (CMakeLists.txt): 85 | ## * add "dynamic_reconfigure" to 86 | ## find_package(catkin REQUIRED COMPONENTS ...) 87 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 88 | ## and list every .cfg file to be processed 89 | 90 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 91 | # generate_dynamic_reconfigure_options( 92 | # cfg/DynReconf1.cfg 93 | # cfg/DynReconf2.cfg 94 | # ) 95 | 96 | ################################### 97 | ## catkin specific configuration ## 98 | ################################### 99 | ## The catkin_package macro generates cmake config files for your package 100 | ## Declare things to be passed to dependent projects 101 | ## INCLUDE_DIRS: uncomment this if your package contains header files 102 | ## LIBRARIES: libraries you create in this project that dependent projects also need 103 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 104 | ## DEPENDS: system dependencies of this project that dependent projects also need 105 | catkin_package( 106 | # INCLUDE_DIRS include 107 | # LIBRARIES course_agv_nav 108 | CATKIN_DEPENDS message_runtime 109 | # DEPENDS system_lib 110 | ) 111 | 112 | ########### 113 | ## Build ## 114 | ########### 115 | 116 | ## Specify additional locations of header files 117 | ## Your package locations should be listed before other locations 118 | include_directories( 119 | # include 120 | # ${catkin_INCLUDE_DIRS} 121 | ) 122 | 123 | ## Declare a C++ library 124 | # add_library(${PROJECT_NAME} 125 | # src/${PROJECT_NAME}/course_agv_nav.cpp 126 | # ) 127 | 128 | ## Add cmake target dependencies of the library 129 | ## as an example, code may need to be generated before libraries 130 | ## either from message generation or dynamic reconfigure 131 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 132 | 133 | ## Declare a C++ executable 134 | ## With catkin_make all packages are built within a single CMake context 135 | ## The recommended prefix ensures that target names across packages don't collide 136 | # add_executable(${PROJECT_NAME}_node src/course_agv_nav_node.cpp) 137 | 138 | ## Rename C++ executable without prefix 139 | ## The above recommended prefix causes long target names, the following renames the 140 | ## target back to the shorter version for ease of user use 141 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 142 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 143 | 144 | ## Add cmake target dependencies of the executable 145 | ## same as for the library above 146 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 147 | 148 | ## Specify libraries to link a library or executable target against 149 | # target_link_libraries(${PROJECT_NAME}_node 150 | # ${catkin_LIBRARIES} 151 | # ) 152 | 153 | ############# 154 | ## Install ## 155 | ############# 156 | 157 | # all install targets should use catkin DESTINATION variables 158 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 159 | 160 | ## Mark executable scripts (Python etc.) for installation 161 | ## in contrast to setup.py, you can choose the destination 162 | # install(PROGRAMS 163 | # scripts/my_python_script 164 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 165 | # ) 166 | 167 | ## Mark executables for installation 168 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 169 | # install(TARGETS ${PROJECT_NAME}_node 170 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 171 | # ) 172 | 173 | ## Mark libraries for installation 174 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 175 | # install(TARGETS ${PROJECT_NAME} 176 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 178 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 179 | # ) 180 | 181 | ## Mark cpp header files for installation 182 | # install(DIRECTORY include/${PROJECT_NAME}/ 183 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 184 | # FILES_MATCHING PATTERN "*.h" 185 | # PATTERN ".svn" EXCLUDE 186 | # ) 187 | 188 | ## Mark other files for installation (e.g. launch and bag files, etc.) 189 | # install(FILES 190 | # # myfile1 191 | # # myfile2 192 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 193 | # ) 194 | 195 | ############# 196 | ## Testing ## 197 | ############# 198 | 199 | ## Add gtest based cpp test target and link libraries 200 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_course_agv_nav.cpp) 201 | # if(TARGET ${PROJECT_NAME}-test) 202 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 203 | # endif() 204 | 205 | ## Add folders to be run by python nosetests 206 | # catkin_add_nosetests(test) 207 | -------------------------------------------------------------------------------- /gazebo/course_agv_nav/launch/nav.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /gazebo/course_agv_nav/launch/nav.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /RobotModel1 10 | - /Path1 11 | - /Goal1 12 | - /Mid Goal1 13 | - /Odometry1 14 | - /Map2 15 | Splitter Ratio: 0.5 16 | Tree Height: 719 17 | - Class: rviz/Selection 18 | Name: Selection 19 | - Class: rviz/Tool Properties 20 | Expanded: 21 | - /2D Pose Estimate1 22 | - /2D Nav Goal1 23 | - /Publish Point1 24 | Name: Tool Properties 25 | Splitter Ratio: 0.5886790156364441 26 | - Class: rviz/Views 27 | Expanded: 28 | - /Current View1 29 | Name: Views 30 | Splitter Ratio: 0.5 31 | - Class: rviz/Time 32 | Experimental: false 33 | Name: Time 34 | SyncMode: 0 35 | SyncSource: LaserScan 36 | Preferences: 37 | PromptSaveOnExit: true 38 | Toolbars: 39 | toolButtonStyle: 2 40 | Visualization Manager: 41 | Class: "" 42 | Displays: 43 | - Alpha: 0.5 44 | Cell Size: 1 45 | Class: rviz/Grid 46 | Color: 160; 160; 164 47 | Enabled: true 48 | Line Style: 49 | Line Width: 0.029999999329447746 50 | Value: Lines 51 | Name: Grid 52 | Normal Cell Count: 0 53 | Offset: 54 | X: 0 55 | Y: 0 56 | Z: 0 57 | Plane: XY 58 | Plane Cell Count: 20 59 | Reference Frame: 60 | Value: true 61 | - Alpha: 0.699999988079071 62 | Class: rviz/Map 63 | Color Scheme: map 64 | Draw Behind: false 65 | Enabled: false 66 | Name: Map 67 | Topic: /map 68 | Unreliable: false 69 | Use Timestamp: false 70 | Value: false 71 | - Alpha: 0.699999988079071 72 | Class: rviz/RobotModel 73 | Collision Enabled: false 74 | Enabled: true 75 | Links: 76 | All Links Enabled: true 77 | Expand Joint Details: false 78 | Expand Link Details: false 79 | Expand Tree: false 80 | Link Tree Style: Links in Alphabetic Order 81 | course_agv__hokuyo__link: 82 | Alpha: 1 83 | Show Axes: false 84 | Show Trail: false 85 | Value: true 86 | course_agv__imu: 87 | Alpha: 1 88 | Show Axes: false 89 | Show Trail: false 90 | Value: true 91 | course_agv__left_wheel: 92 | Alpha: 1 93 | Show Axes: false 94 | Show Trail: false 95 | Value: true 96 | course_agv__right_wheel: 97 | Alpha: 1 98 | Show Axes: false 99 | Show Trail: false 100 | Value: true 101 | robot_base: 102 | Alpha: 1 103 | Show Axes: false 104 | Show Trail: false 105 | robot_chassis: 106 | Alpha: 1 107 | Show Axes: false 108 | Show Trail: false 109 | Value: true 110 | Name: RobotModel 111 | Robot Description: robot_description 112 | TF Prefix: "" 113 | Update Interval: 0 114 | Value: true 115 | Visual Enabled: true 116 | - Alpha: 0.6000000238418579 117 | Autocompute Intensity Bounds: true 118 | Autocompute Value Bounds: 119 | Max Value: 10 120 | Min Value: -10 121 | Value: true 122 | Axis: Z 123 | Channel Name: intensity 124 | Class: rviz/LaserScan 125 | Color: 255; 255; 255 126 | Color Transformer: Intensity 127 | Decay Time: 0 128 | Enabled: true 129 | Invert Rainbow: false 130 | Max Color: 255; 255; 255 131 | Min Color: 0; 0; 0 132 | Name: LaserScan 133 | Position Transformer: XYZ 134 | Queue Size: 10 135 | Selectable: true 136 | Size (Pixels): 3 137 | Size (m): 0.05000000074505806 138 | Style: Flat Squares 139 | Topic: /course_agv/laser/scan 140 | Unreliable: false 141 | Use Fixed Frame: true 142 | Use rainbow: true 143 | Value: true 144 | - Alpha: 1 145 | Buffer Length: 1 146 | Class: rviz/Path 147 | Color: 237; 212; 0 148 | Enabled: true 149 | Head Diameter: 0.15000000596046448 150 | Head Length: 0.10000000149011612 151 | Length: 0.30000001192092896 152 | Line Style: Lines 153 | Line Width: 0.029999999329447746 154 | Name: Path 155 | Offset: 156 | X: 0 157 | Y: 0 158 | Z: 0 159 | Pose Color: 255; 85; 255 160 | Pose Style: None 161 | Queue Size: 10 162 | Radius: 0.029999999329447746 163 | Shaft Diameter: 0.05000000074505806 164 | Shaft Length: 0.10000000149011612 165 | Topic: /course_agv/global_path 166 | Unreliable: false 167 | Value: true 168 | - Alpha: 1 169 | Axes Length: 1 170 | Axes Radius: 0.10000000149011612 171 | Class: rviz/Pose 172 | Color: 117; 80; 123 173 | Enabled: true 174 | Head Length: 0.30000001192092896 175 | Head Radius: 0.10000000149011612 176 | Name: Goal 177 | Queue Size: 10 178 | Shaft Length: 1 179 | Shaft Radius: 0.05000000074505806 180 | Shape: Arrow 181 | Topic: /course_agv/goal 182 | Unreliable: false 183 | Value: true 184 | - Alpha: 1 185 | Axes Length: 0.20000000298023224 186 | Axes Radius: 0.10000000149011612 187 | Class: rviz/Pose 188 | Color: 255; 25; 0 189 | Enabled: true 190 | Head Length: 0.30000001192092896 191 | Head Radius: 0.10000000149011612 192 | Name: Mid Goal 193 | Queue Size: 10 194 | Shaft Length: 1 195 | Shaft Radius: 0.05000000074505806 196 | Shape: Axes 197 | Topic: /course_agv/mid_goal 198 | Unreliable: false 199 | Value: true 200 | - Angle Tolerance: 0.10000000149011612 201 | Class: rviz/Odometry 202 | Covariance: 203 | Orientation: 204 | Alpha: 0.5 205 | Color: 255; 255; 127 206 | Color Style: Unique 207 | Frame: Local 208 | Offset: 1 209 | Scale: 1 210 | Value: true 211 | Position: 212 | Alpha: 0.30000001192092896 213 | Color: 204; 51; 204 214 | Scale: 1 215 | Value: true 216 | Value: true 217 | Enabled: true 218 | Keep: 100 219 | Name: Odometry 220 | Position Tolerance: 0.10000000149011612 221 | Queue Size: 10 222 | Shape: 223 | Alpha: 1 224 | Axes Length: 1 225 | Axes Radius: 0.10000000149011612 226 | Color: 255; 25; 0 227 | Head Length: 0.30000001192092896 228 | Head Radius: 0.10000000149011612 229 | Shaft Length: 1 230 | Shaft Radius: 0.05000000074505806 231 | Value: Arrow 232 | Topic: /icp_odom 233 | Unreliable: false 234 | Value: true 235 | - Alpha: 0.699999988079071 236 | Class: rviz/Map 237 | Color Scheme: map 238 | Draw Behind: false 239 | Enabled: true 240 | Name: Map 241 | Topic: /map 242 | Unreliable: false 243 | Use Timestamp: false 244 | Value: true 245 | - Alpha: 1 246 | Arrow Length: 0.30000001192092896 247 | Axes Length: 0.30000001192092896 248 | Axes Radius: 0.009999999776482582 249 | Class: rviz/PoseArray 250 | Color: 255; 25; 0 251 | Enabled: true 252 | Head Length: 0.07000000029802322 253 | Head Radius: 0.029999999329447746 254 | Name: PoseArray 255 | Queue Size: 10 256 | Shaft Length: 0.23000000417232513 257 | Shaft Radius: 0.009999999776482582 258 | Shape: Arrow (Flat) 259 | Topic: /test/obs 260 | Unreliable: false 261 | Value: true 262 | Enabled: true 263 | Global Options: 264 | Background Color: 48; 48; 48 265 | Default Light: true 266 | Fixed Frame: map 267 | Frame Rate: 30 268 | Name: root 269 | Tools: 270 | - Class: rviz/Interact 271 | Hide Inactive Objects: true 272 | - Class: rviz/MoveCamera 273 | - Class: rviz/Select 274 | - Class: rviz/FocusCamera 275 | - Class: rviz/Measure 276 | - Class: rviz/SetInitialPose 277 | Theta std deviation: 0.2617993950843811 278 | Topic: /initialpose 279 | X std deviation: 0.5 280 | Y std deviation: 0.5 281 | - Class: rviz/SetGoal 282 | Topic: /course_agv/goal 283 | - Class: rviz/PublishPoint 284 | Single click: true 285 | Topic: /clicked_point 286 | Value: true 287 | Views: 288 | Current: 289 | Class: rviz/Orbit 290 | Distance: 18.78539276123047 291 | Enable Stereo Rendering: 292 | Stereo Eye Separation: 0.05999999865889549 293 | Stereo Focal Distance: 1 294 | Swap Stereo Eyes: false 295 | Value: false 296 | Field of View: 0.7853981852531433 297 | Focal Point: 298 | X: 0.8559381365776062 299 | Y: -1.2515515089035034 300 | Z: -0.07588538527488708 301 | Focal Shape Fixed Size: true 302 | Focal Shape Size: 0.05000000074505806 303 | Invert Z Axis: false 304 | Name: Current View 305 | Near Clip Distance: 0.009999999776482582 306 | Pitch: 1.0353978872299194 307 | Target Frame: 308 | Yaw: 0.6653981804847717 309 | Saved: ~ 310 | Window Geometry: 311 | Displays: 312 | collapsed: false 313 | Height: 1016 314 | Hide Left Dock: false 315 | Hide Right Dock: false 316 | QMainWindow State: 000000ff00000000fd00000004000000000000015f0000035afc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000004fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073010000003d0000035a0000005c00fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073010000003d000002b00000000000000000fb0000000a0056006900650077007300000000ec00000201000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007280000003efc0100000002fb0000000800540069006d0065010000000000000728000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004ae0000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 317 | Selection: 318 | collapsed: false 319 | Time: 320 | collapsed: false 321 | Tool Properties: 322 | collapsed: false 323 | Views: 324 | collapsed: false 325 | Width: 1832 326 | X: 88 327 | Y: 27 328 | -------------------------------------------------------------------------------- /gazebo/course_agv_nav/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | course_agv_nav 4 | 0.0.0 5 | The course_agv_nav package 6 | 7 | 8 | 9 | 10 | zjunlict 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | message_generation 43 | 44 | 45 | 46 | message_runtime 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /gazebo/course_agv_nav/scripts/A_star.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | import numpy as np 3 | import math as m 4 | 5 | 6 | class A_star: 7 | """A*算法类 8 | 9 | 通过A*算法求解二维栅格地图中指定起点、终点的最短路径序列 10 | 11 | Attributes: 12 | closed: 类型：list，已经探索过且封闭的顶点 13 | unknown：类型：list. 未探索过的顶点 14 | now: 类型：Vertex，当前所处顶点 15 | start_point: 类型：Point，起点 16 | end_point: 类型：Point 终点 17 | map: 类型：array 栅格地图信息 18 | end_Vertex: 类型：Vertex 终点所对应的顶点 19 | max_iter: 类型：int 算法的最大迭代次数，超过此次数终止计算 20 | Vertex: 定义一个顶点类（class）,用于描述图中的顶点 21 | 22 | """ 23 | 24 | def __init__(self,obstacles_point, start_point, end_point, planning_minx,planning_miny,planning_maxx,planning_maxy,grid,dist): 25 | """"由给定起点、终点以及地图初始化A*的数据""" 26 | self.closed = [] # 封闭的顶点 27 | self.unknown = [] # 未探索过的顶点 28 | self.now = 0 # 现在的Vertex 29 | self.start_point = (int((start_point[0]-planning_minx)/grid),int((start_point[1]-planning_miny)/grid)) 30 | self.end_point = (int((end_point[0]-planning_minx)/grid),int((end_point[1]-planning_miny)/grid)) 31 | self.end_Vertex = self.Vertex(self.end_point, self.end_point, m.inf) 32 | self.max_iter = 50000 33 | self.grid = grid 34 | self.dist = dist 35 | self.planning_minx = planning_minx 36 | self.planning_miny = planning_miny 37 | self.planning_maxx = planning_maxx 38 | self.planning_maxy = planning_maxy 39 | self.width = int((planning_maxx - planning_minx)/grid+1) 40 | self.height = int((planning_maxy - planning_miny)/grid+1) 41 | self.map = np.empty([self.height, self.width], dtype=float) 42 | for i in range(self.height): 43 | for j in range(self.width): 44 | self.temp=np.hypot(obstacles_point[:,0]-(i*grid+planning_minx),obstacles_point[:,1]-(j*grid+planning_miny)) 45 | self.map[i,j] = min(self.temp) 46 | if self.map[i,j] > dist: 47 | self.map[i,j] = 0 48 | else: 49 | self.map[i,j] = 1 50 | 51 | class Vertex: 52 | """ 图中顶点类 53 | 54 | 将栅格地图中的坐标点抽象为“图”中的顶点，储存更多信息，便于实现A* 55 | 56 | Attributes: 57 | point: 类型：Point 当前顶点所对应的坐标点 58 | endpoint: 类型： Point 终点坐标（本质上和上面的end_point一致，这里定义成类内变量更加方便后续的编程 59 | father: 类型：Vertex 类似一个指针，指向自己的上一个顶点，用于回溯 60 | g：类型：float A*算法中的g(n)函数值，表示现有已知代价 61 | h: 类型：float A*算法中的h(n)函数值，表示对当前节点的未来代价的评估 62 | f: 类型：float g(n)函数 + f(n)函数，用来评估该节点的总代价 63 | """ 64 | 65 | def __init__(self, point, endpoint, g): 66 | """"使用给定坐标、终点坐标、已得g值进行初始化""" 67 | self.point = point # 自己坐标 68 | self.endpoint = endpoint # 终点坐标 69 | self.father = None # 父节点，用来回溯最短路 70 | self.g = g # 现有代价g值，是由起点到该点的累加值 71 | self.h =m.hypot((point[0]-endpoint[0]),(point[1]-endpoint[1])) 72 | self.f = self.g + self.h 73 | 74 | def search_next(self, vertical, horizontal): 75 | """搜寻并确定下一个可到达的顶点Vertex 76 | 77 | 通过传入的水平位移dx - horizontal和竖直位移dy-vertical来确定下一个可到达的顶点 78 | 79 | Args: 80 | self: Vertex类所有变量均已知，包括当前坐标self.point 81 | vertical: 类型：int 表示竖直方向的坐标增量 dy 82 | horizontal： 类型：int 表示水平方向坐标增量 dx 83 | 84 | return: 85 | nearVertex: 类型：Vertex 下一个可探索的顶点 86 | """ 87 | nextpoint =(self.point[0] + horizontal, 88 | self.point[1] + vertical) 89 | if vertical != 0 and horizontal != 0: # 走斜对角线，g值增加根号2 90 | nearVertex = A_star.Vertex( 91 | nextpoint, self.endpoint, self.g + 1.414) 92 | else: # 走十字方向，g值增加1 93 | nearVertex = A_star.Vertex( 94 | nextpoint, self.endpoint, self.g + 1) 95 | return nearVertex 96 | 97 | def Process(self): 98 | """A*算法核心函数，整个算法的流程控制 99 | 100 | 每次从未探索区域中找到f(n)最小的顶点，并进行探索和回溯 101 | 最终得到终点的顶点end_Vertex,通过father指针逆向索引得到最优路径 102 | 103 | Args: 104 | self: A_star类所有变量均已知 105 | 106 | return: 107 | self.closed : 类型：list 列表中的每个元素为Vertex, 是所有曾探索过的点 108 | best_path: 类型：list 列表中的每个元素为Vertex，是从起点到终点的路径序列顶点集 109 | """ 110 | start_Vertex = self.Vertex(self.start_point, self.end_point, 0) 111 | self.unknown.append(start_Vertex) 112 | arrive = 0 113 | count = 0 114 | while arrive != 1 and count < self.max_iter: # arrive = 1时到达终点 115 | self.now = self.min_cost_Vertex() # 每次从unknown中选取一个f(n)最小的Vertex 116 | self.closed.append(self.now) # 起点不计入 117 | arrive = self.explore_next(self.now) # 对选中的Vertex进行周边探索 118 | count += 1 119 | best_path_X = [] 120 | best_path_Y = [] 121 | temp_Vertex = self.end_Vertex # 拿到终点顶点 122 | # while (temp_Vertex.point[0] != self.start_plloint[0] or temp_Vertex.point[1] != self.start_point[1]): 123 | while (temp_Vertex): 124 | best_path_X.append(temp_Vertex.point[0]) 125 | best_path_Y.append(temp_Vertex.point[1]) 126 | temp_Vertex = temp_Vertex.father 127 | best_path_X.reverse() # 逆转链表 128 | best_path_Y.reverse() # 逆转链表 129 | best_path_X=[float(i)*self.grid+self.planning_minx for i in best_path_X] 130 | best_path_Y=[float(i)*self.grid+self.planning_miny for i in best_path_Y] 131 | # print(best_path_X,best_path_Y) 132 | return best_path_X,best_path_Y 133 | 134 | def min_cost_Vertex(self): 135 | """在unknown list中找到最小f(n)的顶点 136 | 将该顶点从unknown list中移除并添加到closed list中 137 | 138 | Args: 139 | self: A_star类所有变量均已知 140 | 141 | return: 142 | Vertex_min: 类型：Vertex unknown顶点集中f(n)最小的顶点 143 | """ 144 | Vertex_min = self.unknown[0] 145 | for Vertex_temp in self.unknown: 146 | if Vertex_temp.f < Vertex_min.f: 147 | Vertex_min = Vertex_temp 148 | self.unknown.remove(Vertex_min) 149 | return Vertex_min 150 | 151 | def is_unknown(self, Vertex): 152 | """判断顶点是否在未探索区域 153 | 154 | 如果改顶点属于为探索区域，则返回未更新过的顶点 155 | 156 | Args: 157 | self: A_star类所有变量均已知 158 | Vertex: 类型：Vertex 待判断的顶点 159 | 160 | return: 161 | 0: 表示不属于未探索区域 162 | Vertex_temp：unknown列表中未更新的Vertex 163 | """ 164 | for Vertex_temp in self.unknown: 165 | if Vertex_temp.point == Vertex.point: 166 | return Vertex_temp 167 | return 0 168 | 169 | def is_closed(self, Vertex): 170 | """判断顶点是否在已探索区域 171 | 172 | Args: 173 | self: A_star类所有变量均已知 174 | Vertex: 类型：Vertex 待判断的顶点 175 | 176 | return: 177 | 0/1: 1表示该顶点属于已探索区域，0表示不属于 178 | """ 179 | for closeVertex_temp in self.closed: 180 | if closeVertex_temp.point[0] == Vertex.point[0] and closeVertex_temp.point[1] == Vertex.point[1]: 181 | return 1 182 | return 0 183 | 184 | def is_obstacle(self, Vertex): 185 | """判断顶点是否位于障碍物区域 186 | 187 | Args: 188 | self: A_star类所有变量均已知 189 | Vertex: 类型：Vertex 待判断的顶点 190 | 191 | return: 192 | 0/1: 1表示该顶点位于障碍物区域，0表示不位于 193 | """ 194 | if self.map[Vertex.point[0]][Vertex.point[1]] == 1: 195 | return 1 196 | return 0 197 | 198 | def explore_next(self, Vertex): 199 | """探索下一个顶点 200 | 201 | 栅格地图中可以探索的有8个方向，可以用vertical和horizontal的循环表示 202 | 相应地更新unknown序列，可有配合Vertex类中的函数search_next来实现 203 | 204 | 注意事项:如果探索到的节点已经处于unknown list中,那么需要对比一下原有的f值 205 | 与当前的f值，选取最小f值的保存到unknown list中 206 | 207 | Args: 208 | self: A_star类所有变量均已知 209 | Vertex: 类型：Vertex 当前所在的顶点 210 | 211 | return: 212 | 0：表示还未探索到终点 213 | 1：表示已经探索到终点，A*即将结束 214 | """ 215 | num = {-1, 0, 1} 216 | for vertical in num: 217 | for horizontal in num: 218 | if vertical == 0 and horizontal == 0: 219 | continue 220 | if Vertex.point[0] + horizontal < 0: 221 | continue 222 | if Vertex.point[0] + horizontal >= self.width: 223 | continue 224 | if Vertex.point[1] + vertical < 0: 225 | continue 226 | if Vertex.point[1] + vertical >= self.height: 227 | continue 228 | Vertex_next = Vertex.search_next(vertical, horizontal) 229 | if Vertex_next.point[0] == self.end_point[0] and Vertex_next.point[1] == self.end_point[1]: 230 | Vertex_next.father = Vertex 231 | self.end_Vertex = Vertex_next 232 | return 1 233 | if self.is_closed(Vertex_next): 234 | continue 235 | if self.is_obstacle(Vertex_next): 236 | continue 237 | if self.is_unknown(Vertex_next) == 0: 238 | Vertex_next.father = Vertex 239 | self.unknown.append(Vertex_next) 240 | else: 241 | Vertex_old = self.is_unknown(Vertex_next) 242 | if Vertex_next.f < Vertex_old.f: 243 | self.unknown.remove(Vertex_old) 244 | Vertex_next.father = Vertex 245 | self.unknown.append(Vertex_next) 246 | return 0 247 | -------------------------------------------------------------------------------- /gazebo/course_agv_nav/scripts/RRT_plan.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | from cmath import inf 3 | import numpy as np 4 | import math as m 5 | 6 | 7 | 8 | class RRT: 9 | """RRT算法类 10 | 11 | Attributes: 12 | known：类型：Tree. 探索过的顶点 13 | start_point: 类型：Point，起点 14 | end_point: 类型：Point 终点 15 | start_node: 类型：Tree 起点所对应的顶点 16 | Vertex: 定义一个顶点类（class）,用于描述图中的顶点 17 | 18 | """ 19 | 20 | def __init__(self,obstacles_point, start_point, end_point, planning_minx,planning_miny,planning_maxx,planning_maxy,grid,dist): 21 | """"由给定起点、终点初始化""" 22 | self.node_set = [] 23 | self.start_point = start_point 24 | self.end_point = end_point 25 | self.start_node = self.RRT_Tree(start_point,[],[],end_point,start_point) 26 | self.planning_minx = planning_minx 27 | self.planning_miny = planning_miny 28 | self.planning_maxx = planning_maxx 29 | self.planning_maxy = planning_maxy 30 | self.obstacles_point = obstacles_point 31 | self.grid = grid 32 | self.dist = dist 33 | 34 | class RRT_Tree: 35 | """RRT树 36 | 37 | Attributes: 38 | point 当前节点的坐标 39 | father 父节点 40 | child 子节点 41 | """ 42 | def __init__(self,point,father,child,endpoint,startpoint): 43 | self.point = point 44 | self.father = father 45 | self.child = child 46 | self.endpoint = endpoint # 终点坐标 47 | self.startpoint = startpoint # 起点坐标 48 | 49 | 50 | def Process(self): 51 | """RRT算法核心函数，整个算法的流程控制 52 | 53 | 通过发散找点，最终到终点附近的时候进行回溯 54 | 55 | Args: 56 | self: RRT类所有变量均已知 57 | 58 | return: 59 | best_path_X:最有路径的x坐标 60 | best_path_Y:最有路径的y坐标 61 | """ 62 | new_node = self.start_node 63 | self.node_set.append(new_node) 64 | while self.if_nearend(new_node) == 0: 65 | new_node = self.search_next() 66 | self.node_set.append(new_node) 67 | end_node = self.RRT_Tree(self.end_point,new_node,[],self.end_point,self.start_point) 68 | best_path_X = [] 69 | best_path_Y = [] 70 | temp_node = end_node # 拿到终点顶点 71 | while (temp_node): 72 | best_path_X.append(temp_node.point[0]) 73 | best_path_Y.append(temp_node.point[1]) 74 | temp_node = temp_node.father 75 | best_path_X.reverse() # 逆转链表 76 | best_path_Y.reverse() # 逆转链表 77 | return best_path_X,best_path_Y 78 | def search_next(self): 79 | """搜寻并确定下一个可到达的顶点near_node 80 | 81 | Args: 82 | self: RRT_Tree类所有变量均已知，包括当前坐标self.point 83 | 84 | return: 85 | near_node: 类型：Tree 下一个可探索的顶点 86 | """ 87 | flag =1 88 | while flag: 89 | flag = 0 90 | # 利用rand()函数在[0,1]区间内随机生成一个数 91 | if np.random.rand() < self.grid: 92 | # 如果小于0.5，则在图 img_binary 的范围内随机采样一个点 93 | x_temp = np.random.randint(self.planning_minx,self.planning_maxx, dtype=int) 94 | y_temp = np.random.randint(self.planning_miny,self.planning_maxy, dtype=int) 95 | temp_point = (x_temp,y_temp) 96 | else: 97 | # 否则用目标点作为采样点 98 | temp_point = self.end_point 99 | dist,nearest_node = self.min_distance_node(temp_point) 100 | d = ([temp_point[i]-nearest_node.point[i] for i in range(len(temp_point))])/np.linalg.norm([temp_point[i]-nearest_node.point[i] for i in range(len(temp_point))]) 101 | near_node = RRT.RRT_Tree(nearest_node.point+d*self.grid,nearest_node,[],nearest_node.endpoint,nearest_node.startpoint) 102 | for obstacles_point in self.obstacles_point: 103 | if np.linalg.norm(obstacles_point-near_node.point)node.value+self.grid: 108 | near_node.father = node 109 | if near_node.value+self.grid np.sqrt((robot_info[0]-midpos[0])**2+(robot_info[1]-midpos[1])**2): 90 | return [0,0]#,best_traj,all_traj,all_u 91 | # time.sleep(1) 92 | # 遍历 93 | tot = 100000.0 94 | tot_info=robot_info 95 | cost = np.zeros((possibleV_num, possibleW_num)) 96 | for vw in possibleW: 97 | for vx in possibleV: 98 | traj = [] 99 | robot_info_temp = [robot_info[0], robot_info[1], robot_info[2], vx, vw] 100 | robot_info_temp = self.motion(robot_info_temp, dwaconfig) 101 | # 计算cost 102 | cost1 = self.heading_cost(robot_info_temp[0], robot_info_temp[1], midpos[0], midpos[1], robot_info_temp) 103 | cost2 = self.dist_cost(dwaconfig, midpos,robot_info_temp, planning_obs, 0.1) 104 | cost3 = self.velocity_cost(robot_info_temp[3], dwaconfig.max_speed) 105 | cost = cost1 * dwaconfig.to_goal_cost_gain + cost2 * dwaconfig.obstacle_cost_gain + cost3 * dwaconfig.speed_cost_gain 106 | # print(vx, vw, cost1, cost2, cost3, cost) 107 | if(cost < tot): 108 | tot = cost 109 | nvx = vx 110 | nvw = vw 111 | tot_info = robot_info_temp 112 | where = robot_info_temp 113 | for i in range(int(dwaconfig.predict_time / dwaconfig.dt)): 114 | where = self.motion(where, dwaconfig) 115 | traj.append([where[0],where[1]]) 116 | all_traj.append(traj) 117 | all_u.append(cost) 118 | where = tot_info 119 | for i in range(int(dwaconfig.predict_time / dwaconfig.dt)): 120 | where = self.motion(where, dwaconfig) 121 | best_traj.append([where[0],where[1]]) 122 | best_traj = np.array(best_traj) 123 | all_traj = np.array(all_traj) 124 | return [nvx, nvw]#,best_traj,all_traj,all_u 125 | -------------------------------------------------------------------------------- /gazebo/course_agv_nav/scripts/global_planner.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | import tf 4 | from nav_msgs.srv import GetMap 5 | from nav_msgs.msg import Path 6 | from geometry_msgs.msg import PoseStamped 7 | from course_agv_nav.srv import Plan,PlanResponse 8 | from nav_msgs.msg import OccupancyGrid 9 | from std_msgs.msg import Bool 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | import A_star as A 13 | 14 | class GlobalPlanner: 15 | def __init__(self): 16 | self.plan_sx = 0.0 17 | self.plan_sy = 0.0 18 | self.plan_gx = 8.0 19 | self.plan_gy = -8.0 20 | self.plan_grid_size = 0.3 21 | self.plan_robot_radius = 0.6 22 | self.plan_ox = [] 23 | self.plan_oy = [] 24 | self.plan_rx = [] 25 | self.plan_ry = [] 26 | 27 | # count to update map 28 | self.map_count = 0 29 | 30 | self.tf = tf.TransformListener() 31 | self.goal_sub = rospy.Subscriber('/course_agv/goal',PoseStamped,self.goalCallback) 32 | self.plan_srv = rospy.Service('/course_agv/global_plan',Plan,self.replan) 33 | self.path_pub = rospy.Publisher('/course_agv/global_path',Path,queue_size = 1) 34 | self.map_sub = rospy.Subscriber('/slam_map',OccupancyGrid,self.mapCallback) 35 | self.collision_sub = rospy.Subscriber('/collision_checker_result',Bool,self.collisionCallback) 36 | 37 | self.updateMap() 38 | # self.updateGlobalPose() 39 | 40 | pass 41 | def goalCallback(self,msg): 42 | self.plan_goal = msg 43 | self.plan_gx = msg.pose.position.x 44 | self.plan_gy = msg.pose.position.y 45 | # print("get new goal!!! ",self.plan_goal) 46 | self.replan(0) 47 | pass 48 | 49 | def collisionCallback(self,msg): 50 | self.replan(0) 51 | def updateGlobalPose(self): 52 | try: 53 | self.tf.waitForTransform("/map", "/robot_base", rospy.Time(), rospy.Duration(4.0)) 54 | (self.trans,self.rot) = self.tf.lookupTransform('/map','/robot_base',rospy.Time(0)) 55 | except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 56 | print("get tf error!") 57 | self.plan_sx = self.trans[0] 58 | self.plan_sy = self.trans[1] 59 | 60 | def replan(self,req): 61 | print('get request for replan!!!!!!!!') 62 | self.initAStar() 63 | self.updateGlobalPose() 64 | self.plan_rx,self.plan_ry = self.planner.Process() 65 | # self.plan_rx,self.plan_ry = self.a_star.planning(self.plan_ox,self.plan_oy,self.plan_sx,self.plan_sy,self.plan_gx,self.plan_gy,np.array(self.plan_ox).min(),np.array(self.plan_oy).min(),np.array(self.plan_ox).max(),np.array(self.plan_oy).max()) 66 | self.publishPath() 67 | res = True 68 | return PlanResponse(res) 69 | def initAStar(self): 70 | map_data = np.array(self.map.data).reshape((-1,self.map.info.height)).transpose() 71 | ox,oy = np.nonzero(map_data > 50)#大于50是障碍物 72 | self.plan_ox = (ox*self.map.info.resolution+self.map.info.origin.position.x).tolist() 73 | self.plan_oy = (oy*self.map.info.resolution+self.map.info.origin.position.y).tolist() 74 | obs = np.stack((self.plan_ox,self.plan_oy),axis=1) 75 | self.planner = A.A_star(obs,[self.plan_sx,self.plan_sy],[self.plan_gx,self.plan_gy],np.array(self.plan_ox).min(),np.array(self.plan_oy).min(),np.array(self.plan_ox).max(),np.array(self.plan_oy).max(),self.plan_grid_size,self.plan_robot_radius) 76 | # self.a_star = AStarPlanner(self.plan_grid_size,self.plan_robot_radius) 77 | 78 | def mapCallback(self,msg): 79 | self.map = msg 80 | pass 81 | def updateMap(self): 82 | rospy.wait_for_service('/static_map') 83 | try: 84 | getMap = rospy.ServiceProxy('/static_map',GetMap) 85 | msg = getMap().map 86 | except: 87 | e = sys.exc_info()[0] 88 | print('Service call failed: %s'%e) 89 | # Update for planning algorithm 90 | self.mapCallback(msg) 91 | 92 | def publishPath(self): 93 | path = Path() 94 | path.header.seq = 0 95 | path.header.stamp = rospy.Time(0) 96 | path.header.frame_id = 'map' 97 | for i in range(len(self.plan_rx)): 98 | pose = PoseStamped() 99 | pose.header.seq = i 100 | pose.header.stamp = rospy.Time(0) 101 | pose.header.frame_id = 'map' 102 | pose.pose.position.x = self.plan_rx[i]#reverse 103 | pose.pose.position.y = self.plan_ry[i]#reverse 104 | pose.pose.position.z = 0.01 105 | pose.pose.orientation.x = 0 106 | pose.pose.orientation.y = 0 107 | pose.pose.orientation.z = 0 108 | pose.pose.orientation.w = 1 109 | path.poses.append(pose) 110 | self.path_pub.publish(path) 111 | 112 | 113 | def main(): 114 | rospy.init_node('global_planner') 115 | gp = GlobalPlanner() 116 | rospy.spin() 117 | pass 118 | 119 | if __name__ == '__main__': 120 | main() 121 | -------------------------------------------------------------------------------- /gazebo/course_agv_nav/scripts/local_planner.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | import math 4 | import numpy as np 5 | import rospy 6 | import tf 7 | from nav_msgs.srv import GetMap 8 | from nav_msgs.msg import Path 9 | from geometry_msgs.msg import PoseStamped,Twist,PoseArray,Pose 10 | from sensor_msgs.msg import LaserScan 11 | 12 | 13 | import dwaplanner 14 | 15 | from threading import Lock,Thread 16 | import time 17 | 18 | def limitVal(minV,maxV,v): 19 | if v < minV: 20 | return minV 21 | if v > maxV: 22 | return maxV 23 | return v 24 | 25 | class LocalPlanner: 26 | def __init__(self): 27 | self.arrive = 0.1 28 | self.x = 0.0 29 | self.y = 0.0 30 | self.yaw = 0.0 31 | self.vx = 0.0 32 | self.vw = 0.0 33 | self.test_seq_count = 0 34 | # init plan_config for once 35 | self.laser_lock = Lock() 36 | self.plan_config = dwaplanner.Config(0.3)#0.1550000011920929 37 | c = self.plan_config 38 | self.threshold = c.max_speed*c.predict_time 39 | 40 | self.path = Path() 41 | self.tf = tf.TransformListener() 42 | self.path_sub = rospy.Subscriber('/course_agv/global_path',Path,self.pathCallback) 43 | self.vel_pub = rospy.Publisher('/course_agv/velocity',Twist, queue_size=1) 44 | self.midpose_pub = rospy.Publisher('/course_agv/mid_goal',PoseStamped,queue_size=1) 45 | self.laser_sub = rospy.Subscriber('/course_agv/laser/scan',LaserScan,self.laserCallback) 46 | self.obs_pub = rospy.Publisher('/test/obs',PoseArray, queue_size=1) 47 | self.planner_thread = None 48 | 49 | self.need_exit = False 50 | 51 | self.updateMap() 52 | pass 53 | 54 | def updateMap(self): 55 | rospy.wait_for_service('/static_map') 56 | try: 57 | getMap = rospy.ServiceProxy('/static_map',GetMap) 58 | msg = getMap().map 59 | except: 60 | e = sys.exc_info()[0] 61 | print('Service call failed: %s'%e) 62 | # Update for planning algorithm 63 | self.mapCallback(msg) 64 | 65 | def mapCallback(self,msg): 66 | self.map = msg 67 | pass 68 | 69 | def updateGlobalPose(self): 70 | try: 71 | self.tf.waitForTransform("/map", "/robot_base", rospy.Time(), rospy.Duration(4.0)) 72 | (self.trans,self.rot) = self.tf.lookupTransform('/map','/robot_base',rospy.Time(0)) 73 | except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 74 | print("get tf error!") 75 | euler = tf.transformations.euler_from_quaternion(self.rot) 76 | roll,pitch,yaw = euler[0],euler[1],euler[2] 77 | self.x = self.trans[0] 78 | self.y = self.trans[1] 79 | self.yaw = yaw 80 | ind = self.goal_index 81 | self.goal_index = len(self.path.poses)-1 82 | while ind < len(self.path.poses): 83 | p = self.path.poses[ind].pose.position 84 | dis = math.hypot(p.x-self.x,p.y-self.y) 85 | # print('mdgb;; ',len(self.path.poses),ind,dis) 86 | self.goal_index = ind 87 | if dis > self.threshold: 88 | break 89 | ind += 1 90 | goal = self.path.poses[self.goal_index] 91 | self.midpose_pub.publish(goal) 92 | lgoal = self.tf.transformPose("/robot_base", goal) 93 | self.plan_goal = np.array([lgoal.pose.position.x,lgoal.pose.position.y]) 94 | self.goal_dis = math.hypot(self.x-self.path.poses[-1].pose.position.x,self.y-self.path.poses[-1].pose.position.y) 95 | 96 | def laserCallback(self,msg): 97 | # print("get laser msg!!!!",msg) 98 | self.laser_lock.acquire() 99 | # preprocess 100 | self.ob = [[100,100]] 101 | angle_min = msg.angle_min 102 | angle_increment = msg.angle_increment 103 | for i in range(len(msg.ranges)): 104 | a = angle_min + angle_increment*i 105 | r = msg.ranges[i] 106 | if r < self.threshold: 107 | self.ob.append([math.cos(a)*r,math.sin(a)*r]) 108 | self.laser_lock.release() 109 | pass 110 | 111 | def calculate(self): 112 | map_data = np.array(self.map.data).reshape((-1,self.map.info.height)).transpose() 113 | ox,oy = np.nonzero(map_data > 50)#大于50是障碍物 114 | self.plan_ox = (ox*self.map.info.resolution+self.map.info.origin.position.x).tolist() 115 | self.plan_oy = (oy*self.map.info.resolution+self.map.info.origin.position.y).tolist() 116 | th = np.ones(len(self.plan_ox)) 117 | obs = np.stack((self.plan_ox,self.plan_oy,th),axis=1) 118 | th = self.yaw 119 | R = np.array([[math.cos(th), math.sin(th), -self.x*math.cos(th)-self.y*math.sin(th)],[-math.sin(th), math.cos(th), self.x*math.sin(th)-self.y*math.cos(th)],[0,0,1]]) 120 | now_obs = [] 121 | for i in range(len(self.plan_ox)): 122 | now_obs.extend((np.matmul(R,np.transpose(obs[i,:])))) 123 | now_obs = np.array(now_obs) 124 | now_obs=now_obs.reshape(len(self.plan_ox),3) 125 | return now_obs[:,0:2] 126 | 127 | def updateObstacle(self): 128 | self.laser_lock.acquire() 129 | self.plan_ob = [] 130 | self.plan_ob = np.array(self.ob) 131 | # self.plan_ob = self.calculate() 132 | # print(self.plan_ob) 133 | self.laser_lock.release() 134 | pass 135 | def pathCallback(self,msg): 136 | self.need_exit = True 137 | time.sleep(0.1) 138 | self.path = msg 139 | self.planner_thread = Thread(target=self.planThreadFunc) 140 | self.initPlanning() 141 | self.planner_thread.start() 142 | 143 | def initPlanning(self): 144 | self.goal_index = 0 145 | self.vx = 0.0 146 | self.vw = 0.0 147 | self.dis = 99999 148 | self.updateGlobalPose() 149 | cx = [] 150 | cy = [] 151 | for pose in self.path.poses: 152 | cx.append(pose.pose.position.x) 153 | cy.append(pose.pose.position.y) 154 | self.goal = np.array([cx[0],cy[0]]) 155 | self.plan_cx,self.plan_cy = np.array(cx),np.array(cy) 156 | self.plan_goal = np.array([cx[-1],cy[-1]]) 157 | self.plan_x = np.array([0.0,0.0,0.0,self.vx,self.vw]) 158 | pass 159 | def planThreadFunc(self): 160 | print("running planning thread!!") 161 | self.need_exit = False 162 | while not self.need_exit: 163 | self.planOnce() 164 | self.publishTestObs(None) 165 | if self.goal_dis < self.arrive: 166 | print("arrive goal!") 167 | break 168 | time.sleep(0.001) 169 | print("exit planning thread!!") 170 | self.publishVel(True) 171 | self.planner_thread = None 172 | pass 173 | def planOnce(self): 174 | self.updateGlobalPose() 175 | # Update plan_x [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)] 176 | self.plan_x = [0.0,0.0,0.0,self.vx,self.vw] 177 | # Update obstacle 178 | self.updateObstacle() 179 | cal = dwaplanner.DWA() 180 | u= cal.plan(self.plan_x,self.plan_config,self.plan_goal,self.plan_ob) 181 | # print(u) 182 | alpha = 0.5 183 | self.vx = u[0]*alpha+self.vx*(1-alpha) 184 | self.vw = u[1]*alpha+self.vw*(1-alpha) 185 | # print(self.vx,self.vw) 186 | # print("mdbg; ",u) 187 | self.publishVel() 188 | pass 189 | def publishTestObs(self,obs): 190 | test_obs = PoseArray() 191 | test_obs.header.seq = self.test_seq_count 192 | test_obs.header.frame_id = "robot_base" 193 | test_obs.header.stamp = rospy.Time(0) 194 | 195 | for i in self.plan_ob: 196 | pose = Pose() 197 | pose.position.x = i[0] 198 | pose.position.y = i[1] 199 | pose.position.z = 0.01 200 | pose.orientation.x = 0 201 | pose.orientation.y = 0 202 | pose.orientation.z = 0 203 | pose.orientation.w = 1 204 | test_obs.poses.append(pose) 205 | 206 | self.obs_pub.publish(test_obs) 207 | 208 | self.test_seq_count += 1 209 | 210 | def publishVel(self,zero = False): 211 | if zero: 212 | self.vx = 0 213 | self.vw = 0 214 | cmd = Twist() 215 | cmd.linear.x = self.vx 216 | cmd.angular.z = self.vw 217 | self.vel_pub.publish(cmd) 218 | 219 | def main(): 220 | rospy.init_node('path_Planning') 221 | lp = LocalPlanner() 222 | rospy.spin() 223 | pass 224 | 225 | if __name__ == '__main__': 226 | main() 227 | -------------------------------------------------------------------------------- /gazebo/course_agv_nav/srv/Plan.srv: -------------------------------------------------------------------------------- 1 | --- 2 | bool res -------------------------------------------------------------------------------- /main.py: -------------------------------------------------------------------------------- 1 | import threading 2 | import time 3 | import matplotlib 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | from AStarPlanner import AStarPlanner 7 | # from Astar import AStarPlanner 8 | 9 | ## TODO 10 | # from planner import AStarPlanner,RRTPlanner 11 | 12 | plt.rcParams["figure.figsize"] = [8.0,8.0] 13 | plt.rcParams["figure.autolayout"] = True 14 | plt.rcParams['keymap.save'].remove('s') 15 | 16 | class Playground: 17 | def __init__(self,planner=None): 18 | self.dt = 0.2 19 | 20 | self.fig, self.ax = plt.subplots() 21 | 22 | self.fig.canvas.mpl_connect('button_press_event', self.on_mousepress) 23 | self.fig.canvas.mpl_connect('key_press_event', self.on_press) 24 | self.fig.canvas.mpl_connect('motion_notify_event',self.on_mousemove) 25 | self.NEED_EXIT = False 26 | 27 | ############################################ 28 | 29 | self.planning_minx = -10 30 | self.planning_miny = -10 31 | self.planning_maxx = 10 32 | self.planning_maxy = 10 33 | 34 | self.planning_obs = np.empty(shape=(0,2)) 35 | self.planning_obs_radius = 1.5 36 | self.planning_path = np.empty(shape=(0,2)) 37 | 38 | self.planning_start = np.array([0.0,0.0]) 39 | self.planning_target = None 40 | 41 | self.planner = planner 42 | 43 | ##################################### 44 | self.temp_obs = [0,0] 45 | 46 | def run(self): 47 | while True: 48 | if self.NEED_EXIT: 49 | plt.close("all") 50 | break 51 | plt.cla() 52 | self.__draw() 53 | 54 | def add_obs(self,x,y): 55 | self.planning_obs = np.append(self.planning_obs,[[x,y]],axis=0) 56 | def add_obss(self,xs,ys): 57 | self.planning_obs = np.append(self.planning_obs,np.vstack([xs,ys]).T,axis=0) 58 | def __draw(self): 59 | assert(self.planning_path.shape[1]==2,"the shape of planning path should be '[x,2]', please check your algorithm.") 60 | assert(self.planning_obs.shape[1]==2,"the shape of self.planning_obs(obstacles) should be '[x,2]', please check your algorithm.") 61 | 62 | self.ax.plot(self.planning_start[0],self.planning_start[1],"k>",markersize=12) 63 | if self.planning_target is not None: 64 | self.ax.plot(self.planning_target[0],self.planning_target[1],"r*",markersize=20) 65 | 66 | self.ax.plot(self.planning_path[:,0], self.planning_path[:,1], 'b--') 67 | for obs in self.planning_obs: 68 | self.ax.add_artist(plt.Circle((obs[0],obs[1]), self.planning_obs_radius,fill=False)) 69 | 70 | self.ax.set_xlim(self.planning_minx, self.planning_maxx) 71 | self.ax.set_ylim(self.planning_miny, self.planning_maxy) 72 | 73 | plt.pause(self.dt) 74 | 75 | def on_mousepress(self,event): 76 | if not event.dblclick: 77 | if event.button == 1: 78 | self.planning_start = np.array([event.xdata,event.ydata]) 79 | if event.button == 3: 80 | self.planning_target = np.array([event.xdata,event.ydata]) 81 | if event.button == 2: 82 | self.add_obs(event.xdata,event.ydata) 83 | self.temp_obs = [event.xdata,event.ydata] 84 | def on_mousemove(self,event): 85 | if hasattr(event,"button") and event.button == 2: 86 | dx = event.xdata-self.temp_obs[0] 87 | dy = event.ydata-self.temp_obs[1] 88 | if np.hypot(dx,dy) > self.planning_obs_radius*0.8: 89 | self.temp_obs = [event.xdata,event.ydata] 90 | self.add_obs(*self.temp_obs) 91 | def on_press(self,event): 92 | if(event.key == 'escape'): 93 | self.set_exit() 94 | if(event.key == ' '): 95 | print("---------------------------------\ndo planning...") 96 | print("obstacles : ",self.planning_obs) 97 | print("plan start : ",self.planning_start) 98 | print("plan target : ",self.planning_target) 99 | if self.planning_target is not None and self.planner is not None: 100 | ##### TODO 101 | # px,py = planner.planning(self.planning_obs[:,0],self.planning_obs[:,1], self.planning_start[0],self.planning_start[1],self.planning_target[0],self.planning_target[1],self.planning_minx,self.planning_miny,self.planning_maxx,self.planning_maxy) 102 | px,py = planner.planning(self.planning_obs[:,0],self.planning_obs[:,1], 1.5, self.planning_start[0],self.planning_start[1],self.planning_target[0],self.planning_target[1],self.planning_minx,self.planning_miny,self.planning_maxx,self.planning_maxy) 103 | 104 | self.planning_path = np.vstack([px,py]).T 105 | print("plan path : ",self.planning_path) 106 | else: 107 | print("planner or target is None,please check again.") 108 | 109 | def set_exit(self): 110 | self.NEED_EXIT = True 111 | 112 | if __name__ == "__main__": 113 | planner = None 114 | # planner = AStarPlanner(0.1, 1.5) 115 | planner = AStarPlanner(0.05) 116 | 117 | # planner = RRTPlanner(0.2,1.5) 118 | pg = Playground(planner) 119 | pg.run() --------------------------------------------------------------------------------