UbuntuROSOPENCV. path based on the traffic situation,; drivable area that the vehicle can move (defined in the path msg),; turn signal command to be sent to the vehicle interface. The rospy client API enables Python programmers to quickly interface with ROS Topics, Services, and Parameters. sampling pose proposal distribution (k) measurement 2.0. WebThe current implementation of the map_server converts color values in the map image data into ternary occupancy values: free (0), occupied (100), and unknown (-1). Webnav_msgs defines the common messages used to interact with the navigation stack. tf2 is an iteration on tf providing generally the same feature set more efficiently. As tf2 is a major change the tf API has been maintained in its current form. sampling pose proposal distribution (k) measurement 2.0. All for free. %% 2.5 % velocity = temp_v; %% 2.6 % stopDist = temp_v*temp_v/(2*maxVelAcc); % if dist>stopDist evalDB=[evalDB;[temp_v temp_w heading dist velocity 0]]; end end end %% 3 % evalDB0 if isempty(evalDB) evalDB = [0 0 0 0 0 0]; end % if sum(evalDB(:,3))~=0 evalDB(:,3)=evalDB(:,3)/sum(evalDB(:,3)); end if sum(evalDB(:,4))~=0 evalDB(:,4)=evalDB(:,4)/sum(evalDB(:,4)); end if sum(evalDB(:,5))~=0 evalDB(:,5)=evalDB(:,5)/sum(evalDB(:,5)); end % for i=1:length(evalDB(:,1)) evalDB(i,6)=alpha*evalDB(i,3)+beta*evalDB(i,4)+gama*evalDB(i,5); end [~,ind]=max(evalDB(:,6)); % nextVR=evalDB(ind,1:2)'; % %% 4 matE = [1 0 0 0 0;0 1 0 0 0;0 0 1 0 0;0 0 0 0 0;0 0 0 0 0]; matV = [dt*cos(rob_perState(3)) 0;dt*sin(rob_perState(3)) 0;0 dt;1 0;0 1]; robot_NextState = matE*[robotXY(1),robotXY(2),robotT,robotV,robotW]'+matV*nextVR; % DWA robotXY(1) = robot_NextState(1); robotXY(2) = robot_NextState(2); robotT = robot_NextState(3); robotV = robot_NextState(4); robotW = robot_NextState(5); % path = [path;[robotXY(1),robotXY(2)]]; %% 5 field(dy_obsIndex) = 1; dy_obsSub = coord2sub(dy_obsCoord); dy_obsR = dy_obsSub(:,1);dy_obsC = dy_obsSub(:,2); dy_obsIndex = sub2ind([rows,cols],dy_obsR,dy_obsC); field(dy_obsIndex) = 3; image(1.5,1.5,field); scatter(robotXY(1)+0.5,robotXY(2)+0.5,'r','LineWidth',1.5); % plot(path(:,1)+0.5,path(:,2)+0.5,'-b'); % drawnow; %% 6 if mod(step,20) == 0 movpos = [0,1; 0,-1; -1,0; 1,0]; % for i = 1:length(dy_obsCoord(:,1)) temp_obs = dy_obsCoord(i,:); temp_pos = randi(4); % if dy_obsCoord(i,1) + movpos(temp_pos,1) > 0 && dy_obsCoord(i,1) + movpos(temp_pos,1) < cols if dy_obsCoord(i,2) + movpos(temp_pos,2) > 0 && dy_obsCoord(i,2) + movpos(temp_pos,2) < rows dy_obsCoord(i,1) = dy_obsCoord(i,1) + movpos(temp_pos,1); dy_obsCoord(i,2) = dy_obsCoord(i,2) + movpos(temp_pos,2); end end end end step = step + 1;end, import numpy as npimport copyimport matplotlib.pyplot as pltimport PathPlanning, '''# rowscols# MATLAB1-->rowsPython10-->rows-1# pythonMATLAB'''rows = 15cols = 20startSub = [14, 1]goalSub = [1, 3]dy_obsSub = [[3, 3], [12, 4], [8, 13], [1, 17], [11, 15]], field = np.ones([rows, cols])field[startSub[0], startSub[1]] = 4field[goalSub[0], goalSub[1]] = 5for i in range(len(dy_obsSub)): field[dy_obsSub[i][0], dy_obsSub[i][1]] = 3, robotXY = PathPlanning.sub2coord(startSub)goalCoord = PathPlanning.sub2coord(goalSub), dy_obsCoord = []for i in range(len(dy_obsSub)): dy_obsCoord.append(PathPlanning.sub2coord(dy_obsSub[i])), '''# # DWAXYXY# '''robotT = np.pi/2robotV = 0robotW = 0obstacleR = 0.6dt = 0.1, maxVel = 1.0maxRot = 20.0/180*np.pimaxVelAcc = 0.2maxRotAcc = 50.0/180*np.pi, alpha = 0.05beta = 0.2gama = 0.1periodTime = 3.0, path = []pathx = []pathy = []PathPlanning.drawmap(field), '''# MATLAB'''while True: # dat = np.sqrt((robotXY[0]-goalCoord[0])*(robotXY[0]-goalCoord[0])+(robotXY[1]-goalCoord[1])*(robotXY[1]-goalCoord[1])) if dat < 0.5: break, # 1============================================== vel_rangeMin = max(0, robotV-maxVelAcc*dt) vel_rangeMax = min(maxVel, robotV+maxVelAcc*dt) rot_rangeMin = max(-maxRot, robotW-maxRotAcc*dt) rot_rangeMax = min(maxRot, robotW+maxRotAcc*dt), # # evalDBn*66 evalDB = [], ## 2****************** for temp_v in np.arange(vel_rangeMin,vel_rangeMax,0.01): for temp_w in np.arange(rot_rangeMin,rot_rangeMax,np.pi/180): # 2.1 rob_perState = [robotXY[0],robotXY[1],robotT,robotV,robotW], # 2.2 sim_period for time in np.arange(dt,periodTime,dt): matE = np.array([[1,0,0,0,0],[0,1,0,0,0],[0,0,1,0,0],[0,0,0,0,0],[0,0,0,0,0]]) matV = np.array([[dt*np.cos(rob_perState[2]),0],[dt*np.sin(rob_perState[2]),0],[0,dt],[1,0],[0,1]]) # 5*5*5*1+5*2*2*1 = 5*1 # 1dttdt0.1 rob_perState = np.dot(matE,rob_perState) + np.dot(matV,np.array([temp_v,temp_w])) # 2.3 , # # goalTheta = np.arctan2(goalCoord[1]-rob_perState[1],goalCoord[0]-rob_perState[0]) evalTheta = np.abs(rob_perState[2]-goalTheta)/np.pi*180 heading = 180 - evalTheta, # 2.4 # # # dist = np.inf for i in range(len(dy_obsCoord)): disttmp = np.sqrt((dy_obsCoord[i][0]-rob_perState[0])*(dy_obsCoord[i][0]-rob_perState[0]) + (dy_obsCoord[i][1]-rob_perState[1])*(dy_obsCoord[i][1]-rob_perState[1])) - obstacleR # dist if dist > disttmp: dist = disttmp, # ,2 if dist > 2*obstacleR: dist = 2*obstacleR, # 2.5 # velocity = temp_v, # 2.6 # stopDist = temp_v*temp_v/(2*maxVelAcc), # if dist>stopDist: evalDB.append([temp_v,temp_w,heading,dist,velocity,0]), # 3 # evalDB0 if len(evalDB) == 0: evalDB = [[0,0,0,0,0,0],[0,0,0,0,0,0]], # 4 evalDB = np.array(evalDB), sum_h = sum(evalDB[:,2]) sum_d = sum(evalDB[:,3]) sum_v = sum(evalDB[:,4]), if sum_h != 0: for i in range(len(evalDB)): evalDB[i][2] = evalDB[i][2]/sum_h, if sum_d != 0: for i in range(len(evalDB)): evalDB[i][3] = evalDB[i][3]/sum_d, if sum_v != 0: for i in range(len(evalDB)): evalDB[i][4] = evalDB[i][4]/sum_v for i in range(len(evalDB)): evalDB[i][5] = alpha*evalDB[i][2]+beta*evalDB[i][3]+gama*evalDB[i][4], idx = np.argmax(evalDB[:,5]) nextVR = evalDB[idx,0:2], # 5 matE = np.array([[1,0,0,0,0],[0,1,0,0,0],[0,0,1,0,0],[0,0,0,0,0],[0,0,0,0,0]]) matV = np.array([[dt*np.cos(rob_perState[2]),0],[dt*np.sin(rob_perState[2]),0],[0,dt],[1,0],[0,1]]) robot_NextState = np.dot(matE,[robotXY[0],robotXY[1],robotT,robotV,robotW]) + np.dot(matV,np.array(nextVR)), robotXY[0] = robot_NextState[0] robotXY[1] = robot_NextState[1] robotT = robot_NextState[2] robotV = robot_NextState[3] robotW = robot_NextState[4], path.append([robotXY[0],robotXY[1]]) pathx.append(robotXY[0]+0.5) pathy.append(robotXY[1]+0.5), plt.plot(pathx,pathy,'b') plt.pause(0.01), clear;% robotX = 1; % XrobotY = 1; % YrobotT = pi/2; % (0->2pi)robotV = 0; % robotW = 0; % , % goal=[9,9]; % [x(m),y(m)]obstacle=[2,2;4,4;6,6;8,8]; % [x(m) y(m)]obstacleR=0.6; % dt = 0.1; % [s], area=[0 10 0 10]; % [xmin xmax ymin ymax]path = []; % , %% DWAwhile true % if norm([robotX,robotY]-goal') < 0.5 break; end %% 1============================================== vel_rangeMin = max(0, robotV-maxVelAcc*dt); vel_rangeMax = min(maxVel, robotV+maxVelAcc*dt); rot_rangeMin = max(-maxRot, robotW-maxRotAcc*dt); rot_rangeMax = min(maxRot, robotW+maxRotAcc*dt); % % evalDBn*66 evalDB = []; %% 2****************** for temp_v = vel_rangeMin : 0.01 : vel_rangeMax for temp_w = rot_rangeMin : pi/180 : rot_rangeMax %% 2.1 rob_perState = [robotX,robotY,robotT,robotV,robotW]'; %% 2.2 sim_period for time = dt:dt:periodTime matE = [1 0 0 0 0;0 1 0 0 0;0 0 1 0 0;0 0 0 0 0;0 0 0 0 0]; matV = [dt*cos(rob_perState(3)) 0;dt*sin(rob_perState(3)) 0;0 dt;1 0;0 1]; % 5*5*5*1+5*2*2*1 = 5*1 % 1dttdt0.1 rob_perState = matE*rob_perState+matV*[temp_v;temp_w]; end %% 2.3 , % % goalTheta=atan2(goal(2)-rob_perState(2),goal(1)-rob_perState(1));% evalTheta = abs(rob_perState(3)-goalTheta)/pi*180; heading = 180 - evalTheta; %% 2.4 % % % dist = inf; for i=1:length(obstacle(:,1)) % disttmp=norm(obstacle(i,:)-rob_perState(1:2)')-obstacleR; % dist if dist>disttmp dist=disttmp; end end % ,2 if dist>=2*obstacleR dist=2*obstacleR; end, %% 2.6 % stopDist = temp_v*temp_v/(2*maxVelAcc); % if dist>stopDist evalDB=[evalDB;[temp_v temp_w heading dist velocity 0]]; end end end %% 3 % evalDB0 if isempty(evalDB) evalDB = [0 0 0 0 0 0]; end % if sum(evalDB(:,3))~=0 evalDB(:,3)=evalDB(:,3)/sum(evalDB(:,3)); end if sum(evalDB(:,4))~=0 evalDB(:,4)=evalDB(:,4)/sum(evalDB(:,4)); end if sum(evalDB(:,5))~=0 evalDB(:,5)=evalDB(:,5)/sum(evalDB(:,5)); end % for i=1:length(evalDB(:,1)) evalDB(i,6)=alpha*evalDB(i,3)+beta*evalDB(i,4)+gama*evalDB(i,5); end [~,ind]=max(evalDB(:,6)); % nextVR=evalDB(ind,1:2)'; % %% 4 matE = [1 0 0 0 0;0 1 0 0 0;0 0 1 0 0;0 0 0 0 0;0 0 0 0 0]; matV = [dt*cos(rob_perState(3)) 0;dt*sin(rob_perState(3)) 0;0 dt;1 0;0 1]; robot_NextState = matE*[robotX,robotY,robotT,robotV,robotW]'+matV*nextVR; % DWA robotX = robot_NextState(1); robotY = robot_NextState(2); robotT = robot_NextState(3); robotV = robot_NextState(4); robotW = robot_NextState(5); % path = [path;[robotX,robotY]]; %% 5 hold off; scatter(robotX,robotY,'r','LineWidth',1.5);hold on; % plot(goal(1),goal(2),'*r');hold on; % scatter(obstacle(:,1),obstacle(:,2),200,'k');hold on; % plot(path(:,1),path(:,2),'-b');hold on; % axis(area); grid on; drawnow; %% 6 movpos = [0,0.2; 0,-0.2; -0.2,0; 0.2,0]; % for i = 1:length(obstacle(:,1)) temp_obs = obstacle(i,:); temp_pos = randi(4); % if obstacle(i,1) + movpos(temp_pos,1) > 0 && obstacle(i,1) + movpos(temp_pos,1) < 10 if obstacle(i,2) + movpos(temp_pos,2) > 0 && obstacle(i,2) + movpos(temp_pos,2) < 10 obstacle(i,1) = obstacle(i,1) + movpos(temp_pos,1); obstacle(i,2) = obstacle(i,2) + movpos(temp_pos,2); end end end end, DWAXY DWA[3.21,4.56], -(dynamic window approach). ROS. http://www.guyuehome.com/35854, https://blog.csdn.net/weixin_43259286/article/details/107226736, https://blog.csdn.net/wesigj/article/details/111334726. map_server
Squishmallow Shipping Weight, Gta 5 Vapid Sadler Customization, Spider-man Silver Lining Gadget Challenge, Jackpocket Lottery App, Used Honda Near Singapore, Gigawatt Hours To Joules, Are Apples Bad For Ibs, Steak Frites Recipe French,