PlannerPRM doesn't want to validate start point

Question

Zakaria Boussaid 2022-6-2

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1732275-plannerprm-doesn-t-want-to-validate-start-point

回答： Zakaria Boussaid 2022-6-2

Hey guys

I have a little problem with my code. So I want to move my robot from my start pose, provided by odom topic, to the target pose I specified.

I get this error and I am not able to solve it:

let's imagine that my startpose is [-1 , -1 , 0.012] = [x y theta]

goalPose = [2 0 0.012]

After some investigation i notice that my start point is outside the world grid that's why i changed. So my parameters look like this:

This is the code that i wrote. I'll be really happy if someone was able to help me

I = imread('turtlebot3world.pgm')
[rows, columns, numberOfColorChannels] = size(I);
I1 = imcrop(I,[140 120 120 120])
turtlebot3world = ~logical(I1);
tb3w_res= 20
tb3w_map = binaryOccupancyMap(turtlebot3world,tb3w_res)	
show(tb3w_map)
rosshutdown
rosinit
odom = rossubscriber('odom');
odomMsg = receive(odom,10);
[ x, y, theta ] = OdometryMsg2Pose( odomMsg );
tb3w_map.GridLocationInWorld =[-rows/2,-columns/2]
hold off
StartPose=[x y theta]
goallocation = [0, 2, theta]
stateSpace_n = stateSpaceSE2
stateSpace_n.StateBounds = [tb3w_map.XWorldLimits;tb3w_map.YWorldLimits; [-pi pi]];
stateValidator=validatorOccupancyMap(stateSpace_n);
stateValidator.Map = tb3w_map %map2
stateValidator.ValidationDistance = 0.5;
planner_n = plannerPRM(stateSpace_n,stateValidator,"MaxConnectionDistance",2.0,"MaxNumNodes",50)
[pathObj_n,solInfo_n]= planner_n.plan(StartPose,goallocation);%error here 