PlannerPRM doesn't want to validate start point
2 次查看(过去 30 天)
显示 更早的评论
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:
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1019530/image.png)
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:
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1019535/image.png)
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
Thanks in advance
0 个评论
采纳的回答
更多回答(0 个)
另请参阅
类别
在 Help Center 和 File Exchange 中查找有关 Motion Planning 的更多信息
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!